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

摘要

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

主权项

1.一种基于魔方模型的多窗口启发式三维空间路径规划方法，其特征在于，包含以下步骤：(1)利用传感器感知环境得到环境三维点云，对此三维点云进行滤波整合；(2)设置障碍物特征地图的分辨率Res，将滤波整合后的三维点云插入特征地图，最终得到障碍物特征地图M(Res)；同时根据场景、目标物大小确定路径规划的空间栅格尺寸L≥Res，将边长为L的空间立方体作为路径规划三维空间栅格地图的基本组成单元，用其中心点来表示该立方体，立方体的空间位置即为中心点的三维坐标；(3)建立候选列表Candidate，存储下次可以访问的三维栅格点；建立已访问列表Visit，存储已经访问过的三维栅格点；以当前点O′所表示的立方体为中心，在三维空间中建立边长为3L的魔方模型窗口S，则该窗口区域内共有27个小立方体；除去中心点O′表示的立方体外，其余的26个小立方体的中心均可作为候选点P，然后执行步骤(4)；(4)建立多窗口判断模型Wi，j，其中i表示优先级数，j表示当前优先级下对应的窗口；将26个候选点划分等级，优先级从高到低；每一级分别设置多个窗口，窗口尺寸互不相同，每个窗口对应多个候选点；按照优先级高低的顺序，利用不同的窗口对候选点进行可访问性判断；筛选出可以访问的候选点，执行步骤(5)；(5)建立启发式评价函数模型F(P)：F(P)＝α·G(P)+β·H(P)其中，G(P)表征从起点Start到达候选点P的累积路径距离，H(P)表征从候选点P到达终点End的预计路径距离，α、β为其权重参数，取值均不超过2；因此F(P)越大，则从当前点O′途径候选点P继而到达终点End的代价越大；F(P)的数值大小与点P的评价优劣成负相关关系，F(P)越大，点P评价越低，将其作为下一步目标点的可能性越低；利用该评价函数模型F(P)对第(4)步得到的候选点进行考察筛选；如果第i个候选点Pi已存在于候选列表Candidate中，那么对其评价函数F(P)的值进行更新；如果该候选点存在于已访问列表Visit中，则跳过该候选点；完成对所有候选点的考察之后，执行步骤(6)；(6)在候选列表Candidate中根据评价函数F(P)的数值选取最优候选点，将其作为局部目标点并到达该局部目标点；判断当前是否已到达终点End，计算当前点O′距离终点End的直线距离d；设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，将当前点添加进已访问列表Visit中，返回步骤(3)继续执行。