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

摘要

本发明公开了一种基于动态球形窗口的三维空间路径规划方法。该方法包括：在通过传感器感知环境信息的基础上滤波整合进而转化为空间栅格地图；建立动态球形窗口模型，对球体内部及表面进行动态采样以得到局部候选目标点；建立统一评价函数，根据评价函数对候选点进行最优化筛选；通过局部目标点逐渐向终点逼近直至最终达到目的地。本发明可应用于无人机\无人潜水器在三维空间中的快速路径规划问题，充分考虑到了三维空间中的高自由度，可快速得到兼顾安全性与效率的无碰撞三维路径。

主权项

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)内障碍物Obsi的最小距离，以此作为度量候选点安全性的指标；step(O′，P)为效率评价函数，计算从当前点O′到候选点P之间的步长，以此来表征路径效率；α，β，γ是子评价函数的三个权重参数，均取(0,1]；遍历候选列表Candidate，找到最优候选点作为局部目标点，并到达该局部目标点；(6)判断当前是否到达目标点T，计算当前点O′距离目标点T的直线距离d，设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，执行步骤(7)；(7)根据障碍物情况动态调整障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)。