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

说明书

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

技术领域

本发明涉及三维空间环境感知建模与路径规划技术，尤其涉及一种基于动态球形窗口在三维空间进行快速路径规划的方法。

背景技术

路径规划技术是机器人、无人机等无人系统投入实际应用时所必需的重点技术。随着当前传感器技术的不断突破，对于环境感知的速度与精度也不断提高，在此基础上可以更为准确安全的进行无人系统的路径规划。

路径规划的目的是在有障碍物的环境中按照某种评价指标寻找一条从起始点到目标点的最优或次优无碰撞路径。现有技术可大致分为全局路径规划与局部路径规划两大类。全局路径规划是指在有先验地图信息的基础上，利用全局地图信息找到符合要求的从起始点到达目标点的最优路径。全局路径规划的优势是其充分考虑到了整体地图信息，并且可以得到全局唯一的最优路径。但是由于其严重依赖于先验信息以及有着庞大的计算量，因此实时性较差。局部路径规划技术则侧重考虑无人系统当前位置周围的环境信息，这使得无人系统有着较好的避障能力。因此，与全局路径规划相比，局部规划技术有着更好的实时性以及实用性，对动态环境也有着较强的适应能力。

动态窗口法是一种局部路径规划方法，相比于遗传算法、模糊算法等其他局部规划算法，动态窗口法有着计算量小、实时性好的优点。基于速度、角度空间(v，ω)采样的动态窗口法已经在二维地面机器人上进行了应用，取得了不错的避障效果。但是随着诸如无人机、无人潜水器等新兴智能的无人系统出现，无人系统的运动自由度已经从二维平面扩展到三维空间，为解决此类问题，一种常见的技术路线是：

1)首先根据传感器数据对三维环境进行建模，重点勾勒障碍物模型。

2)将有着障碍物模型的三维环境地图向下投影建立二维平面上的栅格化代价地图。

3)在上述代价地图的基础上，运用现有路径规划技术进行处理，得到一条到达目标点的无碰撞路线。

上述技术路线的核心思想是三维空间向下投影得到二维栅格地图，这种方法在一般情况下的确能够快速得到可行的无碰撞路径，但是也不可避免的存在着三维空间中存在路径但在二维空间中无法得到有效结果的情况。此外，这种方法的实质是忽略了三维空间垂直方向上的自由度，实际使用范围有限，此外还使得无人机、潜水器等无人系统灵活机动的优势荡然无存，无法得到令人满意的结果。

发明内容

本发明的目的在于针对现有技术的不足，提出一种基于动态球形窗口的三维空间路径规划的方法，综合考虑了三维空间高自由度的特性，极大地提升了规划路径的质量与可靠性。

本发明的目的是通过以下技术方案来实现的：一种基于动态球形窗口的三维空间路径规划方法，该方法包含以下步骤：

(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)：