[发明专利]一种局部路径规划算法及装置

说明书

一种局部路径规划算法及装置，用于移动设备的路径规划，移动设备装载有传感器，算法包括：S1，判断移动设备是否到达目的地，若到达，则结束，否则执行步骤S2；S2，根据传感器数据，得到候选间隙集合；S3，从候选间隙集合中选择一个最近间隙作为目标间隙；S4，根据目标间隙确定移动设备的移动方向；S5，采用改进的动态窗口法控制移动设备沿目标方向移动；S6，重复步骤S1～S5，直至移动设备达到目的地或没有可行间隙。采用基于间隙和改进动态窗口法对移动设备路径进行规划以减少局部极值情况，并提高避障性能。

技术领域

本发明涉及移动设备路径规划技术领域，尤其涉及一种局部路径规划算法及装置。

背景技术

路径规划算法主要分为全局路径规划和局部路径规划，动态窗口法是一种典型的局部路径规划算法，动态窗口法考虑了机器人实际的动力学约束和运动学约束，在速度空间中采样速度窗口内的多组速度，并预测在一定时间内所形成的轨迹，然后利用评价函数对这些轨迹进行评价，最后选择评价最高的轨迹所对应的速度作为当前时刻的速度输入。动态窗口法主要利用评价函数来对各条轨迹做出评价，进而选择一条评价最高的轨迹所对应的速度作为速度输入，因此，最终速度的选择与评价函数有直接关系，而当轨迹方向与目标方向夹角越小则评价越高，这就使得动态窗口法更倾向于选择朝向目标方向运动的轨迹，然而，当机器人前方存在障碍物时，使得期望轨迹不仅需要朝向目标运动，还需要远离障碍，这一矛盾的产生使得机器人陷入局部极小值点，具体表现为在这一位置附近不断地循环往复甚至停滞；另外，动态窗口法在对轨迹进行评价时，并没有直接排除穿越障碍的轨迹所对应的速度，而是给了一个低的评价，这就可能出现其余评价函数项的评价仍然很高的情况，进而得出最优的轨迹将由这组速度产生，导致机器人大概率与障碍相撞。

发明内容

(一)要解决的技术问题

基于上述技术问题，本发明提供了一种局部路径规划算法及装置，采用基于间隙和改进动态窗口法对移动设备路径进行规划以减少局部极值情况，并提高避障性能。

(二)技术方案

第一方面，本发明提供了一种局部路径规划算法，用于移动设备的路径规划，移动设备装载有传感器，算法包括：S1，判断移动设备是否到达目的地，若到达，则结束，否则执行步骤S2；S2，根据传感器数据，得到候选间隙集合；S3，从候选间隙集合中选择一个最近间隙作为目标间隙；S4，根据目标间隙确定移动设备的移动方向；S5，采用改进的动态窗口法控制移动设备沿目标方向移动；S6，重复步骤S1～S5，直至移动设备达到目的地或没有可行间隙。

可选地，步骤S2具体包括：S21，分析传感器数据，获取传感器数据中障碍物与障碍物之间的间隙，获取多个间隙数据，多个间隙数据组成间隙集合；S22，删除间隙集合中宽度小于移动设备直径的间隙，得到候选间隙集合。

可选地，步骤S21具体为：以移动设备的朝向为起点，沿逆时针方向每间隔预设角度扫描采样点，并编号为0、1、…、i、i+1、…、N-1，其中，若第i个采样点与第i+1个采样点满足：ρ_i＜R_maxρ_i+1＝R_max，则第i个采样点为第一类采样点；若第i个采样点与第i+1个采样点满足：ρ_i＝R_maxρ_i+1＜R_max，则第i+1个采样点为第二类采样点，其中，R_max为传感器扫描的最大距离，ρ_i为第i个采样点与移动设备的距离，ρ_i+1为第i+1个采样点与移动设备的距离；将相邻的且顺序为第一类采样点和第二类采样点之间的间隙依次设为第一间隙、第二间隙、……、第M间隙，第一间隙、第二间隙、……组成间隙集合。

可选地，步骤S3具体包括：判断目的地与候选间隙集合中间隙的关系：若目的地在候选间隙集合的一间隙内，则间隙即为目标间隙；若目的地不在候选间隙集合的任一间隙内，则将与移动设备和目的地连线夹角最小的间隙设为目标间隙。