[发明专利]基于魔方模型的多窗口启发式三维空间路径规划方法

说明书

本发明公开了一种基于魔方模型的多窗口启发式三维空间路径规划方法。该方法包括：通过传感器感知环境信息，在此基础上滤波整合进而转化为三维空间栅格地图；建立基于魔方模型的26自由度搜索空间；使用多窗口模型按照优先级顺序分批次快速确定26自由度的可访问性；建立启发式评价函数，根据权重参数计算函数值，利用评价函数筛选得到局部目标；逐步迭代最终得到全局唯一的最优路径。本发明可应用于含有先验信息的无人机\无人潜水器在三维空间中的快速路径规划问题，充分考虑到了三维空间中的高自由度，可快速得到全局最优的无碰撞三维路径。

技术领域

本发明涉及三维空间环境感知建模与路径规划技术，尤其涉及一种基于魔方模型的多窗口启发式三维空间路径规划方法。

背景技术

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

路径规划的目的是在有障碍物的环境中按照某种评价指标寻找一条从起点到终点的最优或次优无碰撞路径。现有技术可大致分为全局路径规划与局部路径规划两大类。局部路径规划技术则侧重考虑无人系统当前位置周围的环境信息，这使得无人系统有着较好的避障能力。但是由于没有全局先验信息的指引，有时候会陷入局部极值点从而反复震荡，无法到达目标点。此外，局部路径规划技术虽然每一步都保证局部最优，但是其最终得到的路径并不一定是全局最优的，可能出现质量较差的情况。全局路径规划技术是指在有先验地图信息的基础上，利用全局地图信息找到符合要求的从起始点到达终点的一条路径。全局路径规划的优势在于其充分考虑到了整体地图信息，并且可以得到全局唯一的最优路径。相比于局部路径规划技术而言，全局规划技术充分利用了先验信息，不会发生陷入局部极值震荡而无法到达终点的情况，有着较强的鲁棒性，路径质量方面也是有着显著的优势。

全局路径规划技术的根本问题在于环境模型的建立与路径搜索策略的制定。以常见的二维栅格环境模型为例，使用迪杰斯特拉算法进行路径搜索所耗费的时间远远大于启发式搜索算法。如果将二维模型进一步扩展到三维，则由于搜索空间的增大，算法的实时性方面将面临更大的挑战。

现有技术在解决三维栅格空间路径规划问题时，一般将自由度约束到6维即：上、下、左、右、前、后6个方向，以此来限制路径搜索范围，保证算法效率。但是，这种方法的不足也是显而易见的，首先，将三维空间中原本的26维自由度约束到6维导致最终规划结果受到约束，无法发挥出三维空间内无人机\无人潜水器等应用设备的灵活性。其次，现有方法在进行候选栅格点探索时只关注该格点的占用情况，往往忽略候选格点周围的情况，因此为保障安全还需要事先对先验信息中的障碍物逐一进行膨胀操作，步骤比较繁琐，导致算法效率低下，实用性不高。

发明内容

本发明的目的在于针对现有技术的不足，提出一种基于魔方模型的多窗口启发式三维空间路径规划方法，综合考虑了三维空间所有方向上26维自由度，利用多窗口模型灵活快速的批量判断候选格点的可访问性，在兼顾实时性的同时得到更为灵活、实用的路径规划结果。

本发明的目的是通过以下技术方案来实现的：一种基于魔方模型的多窗口启发式三维空间路径规划方法，包含以下步骤：

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

(2)设置障碍物特征地图的分辨率Res，将滤波整合后的三维点云插入特征地图，最终得到障碍物特征地图M(Res)；同时根据场景、目标物大小确定路径规划的空间栅格尺寸L≥Res，将边长为L的空间立方体作为路径规划三维空间栅格地图的基本组成单元，用其中心点来表示该立方体，立方体的空间位置即为中心点的三维坐标；

(3)建立候选列表Candidate，存储下次可以访问的三维栅格点；

建立已访问列表Visit，存储已经访问过的三维栅格点；