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

权利要求书

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间隙组成所述间隙集合。