[发明专利]基于动态球形窗口的三维空间路径规划方法

权利要求书

1.一种基于动态球形窗口的三维空间路径规划方法，其特征在于，包含以下步骤：

(1)感知环境得到环境三维点云，对此三维点云进行滤波整合；

(2)设置障碍物特征地图分辨率Res，将滤波整合后的三维点云插入地图，得到基于空间栅格的障碍物特征地图M(Res)；

(3)根据场景、目标物大小确定障碍物探测窗口W(a，b，h)的三维尺寸：长a，宽b，高h，以当前点O′为中心展开该窗口；

(4)建立空间球形窗口采样模型S(R)，初始化球体最大半径R，在球形窗口中，采样得到空间点记为P，在特征地图M中确定点P的可访问性，如果可访问则将其加入候选列表Candidate；

(5)建立统一评价函数模型F(P)：

F(P)＝α·direction(θ，φ)+β·dist(P，W)+γ·step(O′，P)

其中，direction(θ，φ)是俯仰角θ-方位角φ的方向评价函数，用于评价从当前点O′出发到候选点P的角度与当前点O′到最终目标点T的角度之间的差距；

dist(P，W)是安全性评价函数，计算候选点P到障碍物探测窗口W(a，b，h)内障碍物Obs_i的最小距离，以此作为度量候选点安全性的指标；

step(O′，P)为效率评价函数，计算从当前点O′到候选点P之间的步长，以此来表征路径效率；

α，β，γ是子评价函数的三个权重参数，均取(0，1]；

遍历候选列表Candidate，找到最优候选点作为局部目标点，并到达该局部目标点；

(6)判断当前是否到达目标点T，计算当前点O′距离目标点T的直线距离d，设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，执行步骤(7)；

(7)根据障碍物情况动态调整障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)。

2.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(1)中，利用激光雷达相机或双目相机或深度相机感知环境得到环境三维点云，并对三维点云使用体素滤波器和统计滤波器进行滤波整合。

3.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(2)中，使用射线插入方法将三维点云插入障碍物特征地图。

4.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(3)中，设置障碍物探测窗口后，使用障碍物特征地图边界迭代器快速遍历区域内部障碍物。

5.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(4)中使用球坐标建立空间球形窗口采样模型S(R)，按照相对球心距离ρ、俯仰角θ、方位角φ的顺序进行采样。

6.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，俯仰角θ-方位角φ的方向评价函数需和180°进行减法运算以保证结果的优劣与数值的大小成正比，俯仰角、方位角分别进行运算然后将最终结果进行求和，具体公式如下：

direction(θ，φ)＝(180°-δθ)+(180°-δφ)

分别指从第i个候选点P_i指向最终目标点T向量的俯仰角和方位角；

分别指从当前点O′指向第i个候选点P_i向量的俯仰角和方位角。

7.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，安全性评价函数需设置统一上限，防止在没有障碍物的情况下安全评价函数数值过大影响最终评价结果。

8.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，三项子评价函数的数值在分析对比前需进行统一的平滑化处理，将其限制在0～1之间。

9.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(7)中，在前一步中障碍物探测窗口W(a，b，h)内无障碍时，下一次规划时放大障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)；

如果前一步中障碍物探测窗口W(a，b，h)内存在障碍物，且障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ均大于初始设定值，则对上述三者进行适当缩小，然后执行步骤(3)，否则，不进行修改，直接执行步骤(3)。