CN111813100B - 一种局部路径规划算法及装置 - Google Patents

Abstract

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

Description

一种局部路径规划算法及装置

技术领域

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

背景技术

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

发明内容

(一)要解决的技术问题

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

(二)技术方案

第一方面，本发明提供了一种局部路径规划算法，用于移动设备的路径规划，移动设备装载有传感器，算法包括：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具体包括：判断目的地与候选间隙集合中间隙的关系：若目的地在候选间隙集合的一间隙内，则间隙即为目标间隙；若目的地不在候选间隙集合的任一间隙内，则将与移动设备和目的地连线夹角最小的间隙设为目标间隙。

可选地，将与移动设备和目的地连线夹角最小的间隙设为目标间隙具体为：与移动设备和目的地连线相邻的两间隙均包括一与连线夹角最小的一边界，判断两间隙的边界与连线的夹角，将夹角较小的边界对应的间隙设为目标间隙。

可选地，步骤S4具体包括：S41，获取临界偏移角，其中所述临界偏移角_δ的计算公式为：

其中，R_rob为移动设备的半径，D_s为安全距离，θ_g为间隙的角度宽度，D_g为组成该间隙的障碍物与障碍物之间的距离宽度；S42，根据临界偏移角确定安全区域；S43，根据安全区域确定所述移动设备的移动方向。

可选地，步骤S42中根据所述临界偏移角确定安全区域具体为：间隙的两边界分别向间隙内移动δ后形成两新边界，两新边界之间的区域即为安全区域。

可选地，步骤S43具体为：若移动设备与目的地连线方向处于安全区域内，则直接将连线方向作为移动方向；否则，获取两新边界与移动设备和目的地之间连线的夹角，将夹角较小的新边界作为移动设备的移动方向。

可选地，改进的动态窗口法包括：S51，根据移动设备的速度约束及环境约束确定速度采样空间，速度采样空间V_r的计算公式为：V_r＝V_s∩V_a∩V_d，其中，V_s＝{(v，w)|v∈[0，v_max]∧w∈[w_min，w_max]}，v_max为移动设备的最大线速度，w_min和w_max分别为移动设备的最小角速度和最大角速度；

dist(v，w)为移动设备的制动距离，

和

分别为移动设备的最大加速度和最大角加速度；

v_a和w_a为当前移动设备的线速度和角速度；S52，对速度采样空间中的每一速度进行轨迹预测；S53，根据评价函数对所述轨迹进行评价，将评价最高的轨迹所对应的速度作为移动设备的速度，评价函数的计算公式为：G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)，其中，α、β、γ为系数，

θ为移动设备在轨迹终点位置的朝向与目的地方向的夹角；

R_obs为障碍物到移动设备轨迹中心的最小距离，R_rob为移动设备的半径，若R_obs小于R_rob，则删除轨迹对应的速度，D_s为安全距离，

另一方面，本发明提供了一种局部路径规划装置，包括：判断模块，用于判断移动设备是否到达目的地，若到达，则结束，否则执行获取模块，；获取模块，用于根据传感器数据，得到候选间隙集合；选择模块，用于从候选间隙集合中选择一个最近间隙作为目标间隙；确定模块，用于根据目标间隙确定所述移动设备的移动方向；控制模块，用于采用改进的动态窗口法控制移动设备沿目标方向移动；重复模块，用于重复执行判断模块、获取模块、选择模块、确定模块以及控制模块的功能，直至移动设备达到目的地或没有可行间隙。

(三)有益效果

本发明提供了一种局部路径规划算法及装置，至少达到如下有益效果：

通过在移动设备上装载传感器以对周围环境进行障碍检测，通过分析传感器所采集的数据得到当前移动设备可以通过的间隙，而后得出与目的地最接近的间隙，进一步算出移动设备的移动方向，最后利用改进的动态窗口法使移动设备朝着移动方向移动，这就使得移动设备不总是朝着目的地方向移动，更多的取决于当前的障碍分布，从而极大地减少了陷入局部极小值点的情况；

本发明对典型的动态窗口法的评价函数进行了优化，本算法将预测轨迹方向的评价项改为轨迹终点方向与移动方向的夹角大小，同时，为提高避障的安全性，直接排除有障碍物阻碍预测轨迹的那组速度，并将轨迹安全范围内的障碍点与轨迹的最小距离作为评价项，从而改善了动态窗口法的性能。

附图说明

图1示意性示出了本公开实施例的移动设备的运动环境；

图2示意性示出了本公开实施例的局部路径规划算法的流程图；

图3示意性示出了本公开实施例的局部路径规划算法的步骤图；

图4示意性示出了本公开实施例的移动设备在某时刻的扫描范围示意图；

图5示意性示出了本公开实施例的移动设备的行驶的安全区域计算示意图；

图6示意性示出了本公开实施例的评价函数的计算示意图；

图7示意性示出了本公开实施例的机器人经过该局部路径规划方法的示意性路径。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

移动设备会装载360度扫描的传感器，如激光雷达等，用于对局部环境感知，本发明实施例，以该移动设备为机器人为例对本方案进行详细说明，机器人在地面移动时，周围环境中往往会存在许多散乱的障碍(如图1所示)，因此机器人在移动过程中需要有效的规避这些障碍，到达目的地。

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

S1，判断移动设备是否到达目的地，若到达，则结束，否则执行步骤S2；

具体的，需要对机器人进行初始化，读取机器人传感器中的数据以备后续参考使用，并判断当前位置是否为目的地，若是，则机器人不再移动，若不是，则执行以下步骤S2。

S2，根据传感器数据，得到候选间隙集合；

若机器人初始化后，发现不在目的地，需要对机器人的路径进行规划，使得机器人能够有效的规避障碍物到达目的地。机器人在行进的过程中不断的对周围的环境进行扫描以收集周围环境数据，如图4所示，传感器设置采样点，通过扫描采样点得到传感器数据，具体分析方式如下：

S21，分析传感器数据，获取传感器数据中障碍物与障碍物之间的间隙，获取多个间隙数据，多个间隙数据组成间隙集合；

具体的实现方式为，如图4所示，以该时刻机器人的朝向为起点，沿逆时针方向每间隔预设角度扫描采样点，并编号为0、1、…、i、i+1、…、N-1，若扫描点在障碍物上，则得到的扫描点到传感器的距离小于传感器的扫描半径，若扫描点在障碍物与障碍物的间隙，则得到的采样点为传感器的扫描半径，因此障碍物的边界成为我们要分析的重点，因此定义，若第i个采样点与第i+1个采样点满足：ρ_i＜R_max&&ρ_i+1＝R_max，则第i个采样点为第一类采样点，也即第i个采样点为障碍物的边界，第i+1个采样点在间隙内，则称这样的采样点为第一类采样点；若第i个采样点与第i+1个采样点满足：ρ_i＝R_max&&ρ_i+1＜R_max，则第i+1个采样点为第二类采样点，也即，第i个采样点为间隙内的点，第i+1个点在障碍物上，其中，R_max为传感器扫描的最大距离，ρ_i为第i个采样点与所述机器人的距离，ρ_i+1为第i+1个采样点与机器人的距离。由此可得到障碍物与障碍物之间的间隙。通过分析某时刻的传感器数据即可得到在该时刻传感器扫描范围内的间隙。

将相邻的且顺序为第一类采样点和第二类采样点之间的间隙依次设为第一间隙、第二间隙、……、第M间隙，所述第一间隙、第二间隙、……组成间隙集合。本发明实施例中为表述方便，若第i点为第一类扫描点，将第j点设为第二类扫描点，也即第j-1点在间隙内，第j点在障碍物上，由上可知，j＝i+m，其中间隙内扫描点数为m-1，即可得到间隙(i，j)(见图4)。

S22，删除间隙集合中宽度小于移动设备直径的间隙，得到候选间隙集合。

由于间隙小于机器人直径的间隙机器人是肯定过不去的，因此直接将间隙宽度小于机器人直径的间隙删除，以对间隙集合中的间隙进行初步筛选，以减少后续运算量，得到候选间隙集合。

S3，从候选间隙集合中选择一个最近间隙作为目标间隙；

具体的，由步骤S2可得到候选间隙集合，因此需要进一步对机器人的路径进行规划，确定从哪个间隙通过，因此此时需要判断目的地与候选间隙集合中间隙的关系：

若目的地在候选间隙集合的某一间隙内，则该间隙即为目标间隙；

若目的地不在候选间隙集合的任一间隙内，则将与机器人和目的地连线夹角最小的间隙设为目标间隙。

具体的，若目的地在间隙集合的某间隙两边界组成的扇形范围内，则该间隙即为目标间隙，机器人直接按此目标间隙通过即可。

若目的地不在候选间隙集合的任意一间隙内，则将间隙边界与机器人和目的地连线夹角最小的间隙设为目标间隙。

S4，根据目标间隙确定移动设备的移动方向；

由步骤S3可知，在通过目标间隙时，还需要与障碍物保持一定的安全距离，因此需要对安全距离进行计算，具体包括如下步骤：

S41，获取临界偏移角，其中临界偏移角δ的计算公式为：

其中，R_rob为机器人的半径，D_s为安全距离，θ_g为间隙的角度宽度，D_g为组成该间隙的障碍物与障碍物之间的距离宽度；

S42，根据临界偏移角确定安全区域；

间隙的两边界分别以传感器为圆心向间隙内旋转δ后形成两新边界，两新边界之间的扇形区域即为安全区域。

如图5所示，对于间隙(i，j)，若该间隙右侧边界的角度为θ_c，间隙左侧边界的角度为θ_f，则新边界的角度分别为：θ′_c＝θ_c+δ，θ′_f＝θ_f-δ，因此得到的安全区域的角度为[θ′_c，θ′_f]。

S43，根据安全区域确定移动设备的移动方向。

得到安全区域后，尤其是安全区域较大时需要进一步规划路径，以使机器人的行驶路径最短，需进一步获取两新边界与机器人和目的地之间连线的夹角，将夹角较小的新边界作为机器人的移动方向，进而使得机器人的行驶路径最短。若移动设备与目的地连线方向处于安全区域内，则直接将连线方向作为移动方向；否则，获取两新边界与所述移动设备和目的地之间连线的夹角，将夹角较小的新边界作为移动设备的移动方向。

由上可知，机器人的移动方向θ_sg为：

其中，θ_goal为目的地所在角度。

S5，采用改进的动态窗口法控制移动设备沿目标方向移动；

具体的，改进的动态窗口法包括：

S51，根据移动设备的速度约束及环境约束确定速度采样空间，速度采样空间V_r的计算公式为：

V_r＝V_s∩V_a∩V_d

其中，V_s为机器人可能的线速度和角速度组合，其范围由最大线速度、最小线速度、最大角速度以及最小角速度决定，V_s＝{(v，w)|v∈[0，v_max]∧w∈['w_min，w_max]}，v_max为机器人的最大线速度，w_min和w_max分别为机器人的最小角速度和最大角速度；V_a为考虑机器人制动距离后的可能的速度和角速度组合，在该范围内机器人均可以保证在撞到障碍物前停车，

dist(v，w)为机器人的制动距离，

和

分别为机器人的最大加速度和最大角加速度；

v_a和w_a为当前机器人的线速度和角速度。

S52，对速度采样空间中的每一速度进行轨迹预测；

S53，根据评价函数对所述轨迹进行评价，将评价最高的轨迹所对应的速度作为所述移动设备的速度，所述评价函数的计算公式为：

G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)

其中，α、β、γ为系数，

θ为机器人在轨迹终点位置的朝向与目的地方向的夹角，如图6所示；

R_obs为障碍物到所述机器人轨迹中心的最小距离，R_rob为所述机器人的半径，若R_obs小于R_rob，则删除轨迹对应的速度，D_s为安全距离，

S6，重复步骤S1～S5，直至移动设备达到目的地或没有可行间隙。

如图7所示，为该机器人使用该局部路径规划方法的示意性路径。

第二方面，本申请公开了一种局部路径规划装置，包括判断模块、获取模块、选择模块、确定模块、控制模块以及重复模块，其中：

判断模块，用于判断移动设备是否到达目的地，若到达，则结束，否则执行获取模块；

获取模块，用于根据传感器数据，得到候选间隙集合；

选择模块，用于从候选间隙集合中选择一个最近间隙作为目标间隙；

确定模块，用于根据目标间隙确定移动设备的移动方向；

控制模块，用于采用改进的动态窗口法控制移动设备沿目标方向移动；

重复模块，用于重复执行判断模块、获取模块、选择模块、确定模块以及控制模块的功能，直至移动设备达到目的地或没有可行间隙。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种局部路径规划算法，用于移动设备的路径规划，所述移动设备装载有传感器，所述算法包括：

S1，判断移动设备是否到达目的地，若到达，则结束，否则执行步骤S2；

S2，根据所述传感器数据，得到候选间隙集合；

S3，从所述候选间隙集合中选择一个最近间隙作为目标间隙；

S4，根据所述目标间隙确定所述移动设备的移动方向；

所述步骤S4具体包括：

S41，获取临界偏移角，其中所述临界偏移角δ的计算公式为：

其中，R_rob为移动设备的半径，D_s为安全距离，θ_g为间隙的角度宽度，D_g为组成该间隙的障碍物与障碍物之间的距离宽度；

S42，根据所述临界偏移角确定安全区域；

S43，根据所述安全区域确定所述移动设备的移动方向；

S5，采用改进的动态窗口法控制移动设备沿目标方向移动；

所述改进的动态窗口法包括：

S51，根据所述移动设备的速度约束及环境约束确定速度采样空间，速度采样空间V_r的计算公式为：

V_r＝V_s∩V_a∩V_d

其中，V_s＝{(v，w)|v∈[0，v_max]∧w∈[w_min，w_max]}，v_max为移动设备的最大线速度，w_min和w_max分别为移动设备的最小角速度和最大角速度；

dist(v，w)为移动设备的制动距离，

和

分别为移动设备的最大加速度和最大角加速度；

v_a和w_a为当前移动设备的线速度和角速度；

S52，对速度采样空间中的每一速度进行轨迹预测；

S53，根据评价函数对所述轨迹进行评价，将评价最高的轨迹所对应的速度作为所述移动设备的速度，所述评价函数的计算公式为：

G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)

其中，α、β、γ为系数，

θ为所述移动设备在轨迹终点位置的朝向与所述目的地方向的夹角；

R_obs为障碍物到所述移动设备轨迹中心的最小距离，R_rob为所述移动设备的半径，若R_obs小于R_rob，则删除所述轨迹对应的速度，D_s为安全距离，

S6，重复步骤S1～S5，直至所述移动设备达到所述目的地或没有可行间隙。

2.根据权利要求1所述的局部路径规划算法，所述步骤S2具体包括：

S21，分析所述传感器数据，获取所述传感器数据中障碍物与障碍物之间的间隙，获取多个间隙数据，所述多个间隙数据组成间隙集合；

S22，删除所述间隙集合中宽度小于所述移动设备直径的间隙，得到所述候选间隙集合。

3.根据权利要求2所述的局部路径规划算法，所述步骤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间隙，所述第一间隙、第二间隙、……、第M间隙组成所述间隙集合。

4.根据权利要求1所述的局部路径规划算法，所述步骤S3具体包括：

判断所述目的地与所述候选间隙集合中间隙的关系：

若所述目的地在所述候选间隙集合的一间隙内，则所述间隙即为目标间隙；

若所述目的地不在所述候选间隙集合的任一间隙内，则将与所述移动设备和目的地连线夹角最小的间隙设为目标间隙。

5.根据权利要求4所述的局部路径规划算法，所述将与所述移动设备和目的地连线夹角最小的间隙设为目标间隙具体为：

与所述移动设备和目的地连线相邻的两间隙均包括一与所述连线夹角最小的一边界，判断所述两间隙的所述边界与所述连线的夹角，将夹角较小的边界对应的间隙设为目标间隙。

6.根据权利要求1所述的局部路径规划算法，所述步骤S42中根据所述临界偏移角确定安全区域具体为：

所述间隙的两边界分别向间隙内移动δ后形成两新边界，所述两新边界之间的区域即为安全区域。

7.根据权利要求6所述的局部路径规划算法，所述步骤S43具体为：

若所述移动设备与所述目的地连线方向处于所述安全区域内，则直接将连线方向作为移动方向；否则，获取所述两新边界与所述移动设备和目的地之间连线的夹角，将夹角较小的新边界作为所述移动设备的移动方向。

8.一种局部路径规划装置，包括：

判断模块，用于判断移动设备是否到达目的地，若到达，则结束，否则执行获取模块，其中，所述移动设备装载有传感器；

获取模块，用于根据所述传感器数据，得到候选间隙集合；

选择模块，用于从所述候选间隙集合中选择一个最近间隙作为目标间隙；

确定模块，用于根据所述目标间隙确定所述移动设备的移动方向；

其中，所述确定模块具体包括：

获取临界偏移角，其中所述临界偏移角δ的计算公式为：

其中，R_rob为移动设备的半径，D_s为安全距离，θ_g为间隙的角度宽度，D_g为组成该间隙的障碍物与障碍物之间的距离宽度；

根据所述临界偏移角确定安全区域；

根据所述安全区域确定所述移动设备的移动方向；

控制模块，用于采用改进的动态窗口法控制移动设备沿目标方向移动；

其中，所述改进的动态窗口法包括：

根据所述移动设备的速度约束及环境约束确定速度采样空间，速度采样空间V_r的计算公式为：

V_r＝V_s∩V_a∩V_d

其中，V_s＝{(v，w)|v∈[0，v_max]∧w∈[w_min，w_max]}，v_max为移动设备的最大线速度，w_min和w_max分别为移动设备的最小角速度和最大角速度；

dist(v，w)为移动设备的制动距离，

和

分别为移动设备的最大加速度和最大角加速度；

v_a和w_a为当前移动设备的线速度和角速度；

对速度采样空间中的每一速度进行轨迹预测；

根据评价函数对所述轨迹进行评价，将评价最高的轨迹所对应的速度作为所述移动设备的速度，所述评价函数的计算公式为：

G(v，w)＝α·heading(v，w)+β·obs(v，w)+γ·vel(v，w)

其中，α、β、γ为系数，

θ为所述移动设备在轨迹终点位置的朝向与所述目的地方向的夹角；

R_obs为障碍物到所述移动设备轨迹中心的最小距离，R_rob为所述移动设备的半径，若R_obs小于R_rob，则删除所述轨迹对应的速度，D_s为安全距离，

重复模块，用于重复执行判断模块、获取模块、选择模块、确定模块以及控制模块的功能，直至所述移动设备达到所述目的地或没有可行间隙。

Images