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

权利要求书

1.一种基于魔方模型的多窗口启发式三维空间路径规划方法，其特征在于，包含以下步骤：

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

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

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

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

以当前点O＇所表示的立方体为中心，在三维空间中建立边长为3L的魔方模型窗口S，则该窗口区域内共有27个小立方体；除去中心点O′表示的立方体外，其余的26个小立方体的中心均可作为候选点P，然后执行步骤(4)；

(4)建立多窗口判断模型W_i，j，其中i表示优先级数，j表示当前优先级下对应的窗口；根据中心点O′距离候选点P的远近以及到达该候选点P的难易程度，将26个候选点划分等级，优先级从高到低；具体为：将上、下、左、右、前、后方向上的6个候选点作为第一优先级候选点，将前侧斜上方、前侧斜下方、后侧斜上方、后侧斜下方、左侧斜上方、左侧斜下方、右侧斜上方、右侧斜下方上的共8个候选点作为第二优先级候选点，余下12个方向上的候选点作为第三优先级候选点；每一级分别设置多个窗口，窗口尺寸互不相同，每个窗口对应多个候选点；按照优先级高低的顺序，利用不同的窗口对候选点进行可访问性判断；筛选出可以访问的候选点，执行步骤(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个候选点P_i已存在于候选列表Candidate中，那么对其评价函数F(P)的值进行更新；如果该候选点存在于已访问列表Visit中，则跳过该候选点；

完成对所有候选点的考察之后，执行步骤(6)；

(6)在候选列表Candidate中根据评价函数F(P)的数值选取最优候选点，将其作为局部目标点并到达该局部目标点；

判断当前是否已到达终点End，计算当前点O′距离终点End的直线距离d；

设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，将当前点添加进已访问列表Visit中，返回步骤(3)继续执行。

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

3.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于，所述步骤(2)中，使用射线插入方法将三维点云插入特征地图。

4.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于：所述步骤(3)中使用最小堆作为列表Candidate的数据结构。

5.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于：所述步骤(4)中每一级建立不同数量的窗口，配合障碍物特征地图边界迭代器，按照优先级高低分批次的利用窗口快速查询候选点的可访问性。