CN115373381B 移动体以及移动体的路径决定方法 （丰田自动车株式会社）

US2020097010A1,2020.03.26US2017017237A1,2017.01.19US2019086226A1,201本发明提供移动体以及移动体的路径决定方法。该移动体从当前位置朝向目标位置而行先设定的第一设定时间内移动体能够从当前位在被预先设定的第二设定时间内移动体能够从第一区间候选的终点起而行驶的多个第二区间在方向的目标方向进行识别；障碍物位置识别路径决定部，其基于目标方向以及障碍物的位中决定作为移动体所行驶的路径而被使用的第2第一区间候选搜索部，其以假设在所述移动体的周围不存设定的第一设定时间内所述移动体能够从所述当前位置起而进行行驶的多个第一区间候第二区间候选搜索部，其以假设在所述移动体的周围不存在物区间候选的每个终点而搜索所述移动体在被预先设定的第二设定时间内能够从所述第一障碍物位置识别部，其对作为所述移动体的周路径决定部，其基于所述目标方向以及所述障碍物的位置，而从选以及多个所述第二区间候选之中决定作为所述移动体所行驶的路径来使用的第一区间所述第一区间候选搜索部基于被预先设定的所述移动体的平移速度以及所述移动体的旋转速度的多个组合，来搜索所述平移速度和/或所述旋转速度的条件不同的多个所述所述第二区间候选搜索部基于被预先设定的所述移动体的平移速度以及所述移动体的旋转速度的多个组合，来搜索所述平移速度和/或所述旋转速度的条件不同的多个所述所述路径决定部以作为将所决定的所述第一区间的终点设为起点的多个所述第二区间候选中的所述移动体的平移速度的第二平移速度与作为所述第一区间中的所述移动体并且，以作为将所决定的所述第一区间的终点设为起点的所述移动体的旋转速度的第二旋转速度与作为所述第一区间中的所述移动体的旋转速度还具备行驶状态识别部，所述行驶状态识别部对所述当速度进行识别，所述路径决定部以所述当前位置处的所述移动体的平移速度与作为多个所述第一区间候选中的所述移动体的平移速度的第一平移速度的差分小于第一平移速度阈值的方式，还具备行驶状态识别部，所述行驶状态识别部对所述当前位置处的所速度进行识别，所述路径决定部以所述当前位置处的所述移动体的旋转速度与作为多个所述第一区间候选中的所述移动体的旋转速度的第一旋转速度的差分小于第一旋转速度阈值的方式，所述路径决定部基于所述目标方向和所述障碍物的位置而对所述第一区间候选的障3基于所述目标方向和所述障碍物的位置而对所述第二区间候选的障碍物最接近距离第二区间候选的所述有效距离越大，则将所述第二区间候选的所述评价值计算为越大的从多个所述第一区间候选中将所述评价值最高的所述第一区间候选决定为所述第一从所述第一区间的所述第一旋转速度与所述第二旋转速度的差分小于所述第二旋转所述障碍物最接近距离是指，在所述第一区间候选或者所述第二区间候所述有效距离是指，将所述第一区间候选或者所述第二区间候选投影5.一种移动体的路径决定方法，其为从当第一区间候选搜索步骤，以假设在所述移动体的周围不存设定的第一设定时间内所述移动体能够从所述当前位置起而进行行驶的多个第一区间候第二区间候选搜索步骤，以假设在所述移动体的周围不存在物区间候选的每个终点而搜索在被预先设定的第二设定时间内所述移动体能够从所述第一障碍物位置识别步骤，对作为所述移动体的周路径决定步骤，基于所述目标方向以及所述障碍物的位置，而从选以及多个所述第二区间候选之中决定作为所述移动体所行驶的路径来使用的第一区间在所述第一区间候选搜索步骤中，基于被预先设定的所述移动体的旋转速度的多个组合，来搜索所述平移速度和/或所述旋转速度的条件不同的多在所述第二区间候选搜索步骤中，基于被预先设定的所述移动体的旋转速度的多个组合，来搜索所述平移速度和/或所述旋转速度的条件不同的多4在所述路径决定步骤中，以作为将所决定的所述第一区第二区间候选中的所述移动体的平移速度的第二平移速度与作为所述第一区间中的所述并且，以作为将所决定的所述第一区间的终点设为起点的所述移动体的旋转速度的第二旋转速度与作为所述第一区间中的所述移动体的旋转速度5collisionavoidance.IEEERoboticsandautomationmagazine(避免碰撞的动态窗口狭窄道路等的情况下无法获得能够顺畅地移动的路索在被预先设定的第二设定时间内移动体能够从第一区间候选的终点起而进行行驶的多6部以当前位置处的移动体的平移速度与作为多个第一区间候选中的移动体的平移速度的部以当前位置处的移动体的旋转速度与作为多个第一区间候选中的移动体的旋转速度的以作为将所决定的第一区间的终点设为起点的多个第二区间候选中的移动体的平移速度的第二平移速度与作为第一区间中的移动体的平移速度的第一平移速度的差分小于第二以作为将所决定的第一区间的终点设为起点的多个第二区间候选中的移动体的旋转速度的第二旋转速度与作为第一区间中的移动体的旋转速度的第一旋转速度的差分小于第二第一设定时间内移动体在被预先设定的第一速度条件下能够从当前位置起而进行行驶的先设定的第二设定时间内移动体在被预先设定的第二速度条件下能够从第一区间候选的间候选之中决定作为移动体所行驶的路径来使用的第一区个第一区间候选起而延伸的多个第二区间候选的搜索结果的一个7[0024]图4(a)为表示路径决定的一个示例的图。图4(b)为表示路径决定的其他示例的子控制单元。控制装置10也可以作为普通的计算机而被构成。存储部既可以为ROM(Read被设为RAID(RedundantArraysofInexpensiveDisks，独立磁盘冗余磁盘阵列)结构的[0036]雷达传感器3为被设置在移动体1上并利用电磁波而对移动体1的周围的障碍物进8[0038]速度传感器5为对移动体1的平移速度进行检测的检测设备。移动体1的平移速度是指移动体1的位置变化的速度。速度传感器5例如根据移动体1的车轮速而对平移速度进传感器5将移动体1的平移速度的检测结果发送至控制装[0039]加速度传感器6为对移动体1的加速度进行检测的检测设备。加速度传感器6也可转向致动器根据来自控制装置10的控制信号来控制车辆的电动助力转向系统中的对转向下所说明的控制装置10的一部分功能在能够与移动体1进行通信的服驶状态识别部11基于例如速度传感器5、加速度传感器6、以及旋转速度传感器7的检测结[0046]目标方向识别部12基于移动体1的当前位置和被预先设定的目标位置，来对移动标位置能够通过将目标坐标值输入至例如包含移动体1的当前位置的平面坐标上从而被设12根据以移动体1的当前位置为基准的平面坐标上的目标位置(目标坐标值)来对目标方向9进行识别。识别部12根据由GNSS接收部2接收的移动体1的位置信息来对地图上的移动体1的当前位置进行识别。目标方向识别部12也可以利用航位推算或SLAM(SimultaneousLocalization标方向识别部12能够基于地图上的移动体1的当前位置和在地图上被设定的目标位置来对目标方向进行识别。[0050]第一区间候选搜索部13以移动体1的当前位置为起点，来搜索作为第一区间的候为第一区间的候选。第一区间候选作为在被预先设定的第一设定时间内移动体1能够从当第一设定时间内移动体1能够从当前位置起而进行行驶的第一区间候选进行搜索。关于[0053]第一区间候选搜索部13以假设移动体1在周围不存在物体的平面上行驶的方式来sin或cos的条件)而以多个第一区间候选适当地分离能够追加搜索以起点Sa为中心而向X轴方向反转了的多个第一区间候选PA(向Y轴的负方向驶的路径的区间。第二区间候选作为在被预先设定的第二设定时间内移动体1能够从第一[0059]第二区间候选搜索部14还假设移动体1在周围不存在物体的平面上行驶，来搜索[0060]图2(b)为表示从第一区间候选(仅一个候选)起而延伸的多个第二区间候选的搜[0062]图2(c)为表示从多个第一区间候选起而延伸的多个第二区间候选的搜索结果的区间候选搜索部14搜索移动体1能够从第一区间候选PA的终点Ea起而进行行驶的多个第二[0064]障碍物位置识别部15基于雷达传感器3的障碍物的检测结果和/或摄像机4的摄像置识别部15也可以通过来自外部服务器(例如移动体1正在行驶的设施的服务器或交通信识别出的障碍物的位置，而从第一区间候选PA以及第二区间候选PB之中决定作为移动体1以及第二区间候选PB的评价。路径决定部16例如利用作为下述的式(1)而示出的评价函数障碍物的部位与障碍物之间的距离。障碍物最接近距离C在评价对象的第一区间候选PA以[0070]L为目标方向上的有效距离(例如将第一区间候选PA投影到目标方向上的情况下一区间候选PA向与目标方向T相反的一侧行进的情况下，会成为负评价。a以及b为调整参将即使稍微靠近壁W1但目标方向T1上的有效距离较长且靠近于目标位置G1的候选路径C3C6的评价值计算得最高。与过度接近壁W4且目标方向T2上的有效距离也较短的候选路径决定部16也可以利用例如作为下述的式(2)而表示的评价函数Gb，而对第一区间候选PA的[0078]具体而言，速度变化判断值Aa在当前位置处的移动体1的平移速度vc与作为第一区间候选PA中的移动体1的平移速度的第一平移速度v1的差分(绝对值)为第一平移速度阈[0079]此外，速度变化判断值Aa在当前位置处的移动体1的旋转速度wc与作为第一区间候选PA的移动体1的旋转速度的第一旋转速度w1的差分(绝对值)为第一旋转速度阈值wmax1max1以及第一旋转速度阈值wmax1为被预先设定的值的阈值。第一平移速度阈值vmax1也可以设为在移动体1的最大平移加速度上乘以控制大旋转加速度上乘以控制装置10的控制周期而移动体1的平移速度、旋转速度的变化急剧从而无法顺畅地行驶的第一区间候选PA从移动[0083]另外，也可以设为速度变化判断值Aa仅考虑平移速度和旋转速度中的一方的方速度的第一平移速度v1与作为第二区间候选PB中的移动体1的平移速度的第二平移速度v2[0088]此外，速度变化判断值Ab在作为第一区间候选PA中的移动体1的旋转速度的第一速度阈值wmax2为被预先设定的值的阈值。第二平移速度阈值vmax2既可以为与第一平移速度[0089]另一方面，速度变化判断值Ab在第一平移速度v1与第二二平移速度阈值vmax2、且第一旋转速度w1与第二旋转速度w2的差分小于第二旋转速度阈值[0091]另外，也可以设为速度变化判断值Ab仅考虑平移速度和旋转速度中的一方的方式。速度变化判断值Ab既可以仅在第一平移速度v1与第二平移速度v2的差分为第二平移速[0092]路径决定部16从多个第一区间候选PA以及多个第二区高的组合决定为第一区间以及第二区间。第一区间以及第二区间为作为移动体1所行驶的点设为起点的多个第二区间候选PB中的评价值最高的区第一区间以及第二区间的决定。路径决定部16也可以在移动体1正在行驶于一度所决定的[0095]移动体控制部17对移动体1的行驶进行控制。移动体控制部17根据路径决定部16[0096]例如在移动体1具有在左右轮被配置于对置轴上的对置二轮型的移动机构的情况轮的转速rr作为控制信号而发送至移动体驱动部8，来实施移动体1的沿着路径的行驶控[0104]在S2中，控制装置10通过第一区间候选搜索部13来搜索多个第一区间候选PA(第预先设定的第一设定时间内移动体1能够从当前位置起而进行行驶的多个第一区间候选[0105]在S3中，控制装置10通过第二区间候选搜索部14来搜索多个第二区间候选PB(第搜索在被预先设定的第二设定时间内移动体1能够从第一区间候选PA的终点Ea起而进行行[0106]在S4中，控制装置10通过障碍物位置识别部15而对移动体1的周围的障碍物的位置进行识别(障碍物位置识别步骤)。障碍物目标方向和障碍物的位置，而实施第一区间候选PA以及第二区间候选PB的评价(评价步[0108]在S6中，控制装置10通过路径决定部16而实施路径决定(第一区间以及第二区间的决定)(路径决定步骤)。路径决定部11的平移速度vc与作为第一区间候选PA的移动体1的平移速度的第一平移速度v1的差分(绝[0113]在S11中，控制装置10通过路径决定部16而将第一区间候选PA(上述S10的判断为将上述的式(2)的速度变化判断值Aa的值设为一∞,从而将相符的第一区间候选P1的旋转速度wc与作为第一区间候选PA的移动体1的旋转速度的第一旋转速度w1的差分(绝[0117]在S21中，控制装置10通过路径决定部16而将第一区间候选PA(上述S20的判断为将上述的式(2)的速度变化判断值Aa的值设为一∞,从而将相符的第一区间候选P的移动体1的平移速度的第一平移速度v1与作为第二区间候选PB的移动体1的平移速度的第二平移速度v2的差分(绝对值)是否为第二平移速度阈值vmax2以上[0121]在S31中，控制装置10通过路径决定部16而将第二区间候选PB(上述S30的判断为将上述的式(3)的速度变化判断值Ab的值设为一∞,从而将相符的第二区间候选P的移动体1的旋转速度的第一旋转速度w1与作为第二区间候选PB的移动体1的旋转速度的第二旋转速度w2的差分(绝对值)是否为第二旋转速度阈值wmax2以上[0125]在S41中，控制装置10通过路径决定部16而将第二区间候选PB(上述S40的判断为将上述的式(3)的速度变化判断值Ab的值设为一∞,从而将相符的第二区间候选P[0126]根据以上所说明的本实施方式所涉及的移动体1(以及移动通过实施至少包括第一区间候选PA和第二区间候选PB的使用DWA而进行的多阶段的路径搜索，且基于目标方向以及障碍物的位置来决定作为移动体1所行驶的路径来使用的第一区[0127]在此，图8(a)为表示由现有的移动体的路径决定方法获得的路径的一个示例的地行驶的路径。移动体1可以获得也能够顺畅地实现穿过障碍物或与障碍物错身而过的路候选PA中的移动体1的平移速度的第一平移速度v1的差分小于第一平移速度阈值vmax1的方间候选PA中的移动体1的旋转速度的第一旋转速度w1的差分小于第一旋转速度阈值wmax1的平移速度v1与作为第二区间候选PB中的移动体1的平移速度的第二平移速度v2的差分小于一