基于魔方模型的多窗口启发式三维空间路径规划方法技术

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

【技术实现步骤摘要】
基于魔方模型的多窗口启发式三维空间路径规划方法
本专利技术涉及三维空间环境感知建模与路径规划技术，尤其涉及一种基于魔方模型的多窗口启发式三维空间路径规划方法。
技术介绍
路径规划技术是机器人、无人机等无人系统投入实际应用时所必需的重点技术。随着当前传感器技术的不断突破，对于环境感知的速度与精度也不断提高，在此基础上可以更为准确安全的进行无人系统的路径规划。路径规划的目的是在有障碍物的环境中按照某种评价指标寻找一条从起点到终点的最优或次优无碰撞路径。现有技术可大致分为全局路径规划与局部路径规划两大类。局部路径规划技术则侧重考虑无人系统当前位置周围的环境信息，这使得无人系统有着较好的避障能力。但是由于没有全局先验信息的指引，有时候会陷入局部极值点从而反复震荡，无法到达目标点。此外，局部路径规划技术虽然每一步都保证局部最优，但是其最终得到的路径并不一定是全局最优的，可能出现质量较差的情况。全局路径规划技术是指在有先验地图信息的基础上，利用全局地图信息找到符合要求的从起始点到达终点的一条路径。全局路径规划的优势在于其充分考虑到了整体地图信息，并且可以得到全局唯一的最优路径。相比于局部路径规划技术而言，全局规划技术充分利用了先验信息，不会发生陷入局部极值震荡而无法到达终点的情况，有着较强的鲁棒性，路径质量方面也是有着显著的优势。全局路径规划技术的根本问题在于环境模型的建立与路径搜索策略的制定。以常见的二维栅格环境模型为例，使用迪杰斯特拉算法进行路径搜索所耗费的时间远远大于启发式搜索算法。如果将二维模型进一步扩展到三维，则由于搜索空间的增大，算法的实时性方面将面临更大的挑战。现有技术在解决三维栅格空间路径规划问题时，一般将自由度约束到6维即：上、下、左、右、前、后6个方向，以此来限制路径搜索范围，保证算法效率。但是，这种方法的不足也是显而易见的，首先，将三维空间中原本的26维自由度约束到6维导致最终规划结果受到约束，无法发挥出三维空间内无人机\无人潜水器等应用设备的灵活性。其次，现有方法在进行候选栅格点探索时只关注该格点的占用情况，往往忽略候选格点周围的情况，因此为保障安全还需要事先对先验信息中的障碍物逐一进行膨胀操作，步骤比较繁琐，导致算法效率低下，实用性不高。
技术实现思路
本专利技术的目的在于针对现有技术的不足，提出一种基于魔方模型的多窗口启发式三维空间路径规划方法，综合考虑了三维空间所有方向上26维自由度，利用多窗口模型灵活快速的批量判断候选格点的可访问性，在兼顾实时性的同时得到更为灵活、实用的路径规划结果。本专利技术的目的是通过以下技术方案来实现的：一种基于魔方模型的多窗口启发式三维空间路径规划方法，包含以下步骤：(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)继续执行。进一步地，所述步骤(1)中，利用激光雷达/双目/深度相机感知环境得到环境三维点云，并对三维点云使用体素滤波器、统计滤波器进行滤波整合。进一步地，所述步骤(2)中，使用射线插入方法将三维点云插入特征地图。进一步地，所述步骤(3)中使用最小堆作为列表Candidate的数据结构，以提高算法效率。进一步地，所述步骤(4)中根据中心点O′距离候选点P的远近以及到达该候选点P的难易程度，将26个候选点划分多个等级，优先级从高到低，具体为：的将上、下、左、右、前、后方向上的6个候选点作为第一优先级候选点，将前侧斜上方、前侧斜下方、后侧斜上方、后侧斜下方、左侧斜上方、左侧斜下方、右侧斜上方、右侧斜下方上的共8个候选点作为第二优先级候选点，余下12个方向上的候选点作为第三优先级候选点。进一步地，所述步骤(4)中每一级建立不同数量的窗口，配合障碍物特征地图边界迭代器，按照优先级高低分批次的利用窗口快速查询候选点的可访问性。进一步地，所述步骤(4)中为加快对候选点可访问性的判断速度，使用窗口模型进行查询，单个窗口对应于多个候选点，一旦某一级的某个窗口内存在障碍物，即认为当前窗口对应的候选点均不可访问。进一步地，所述步骤(4)中所建立的窗口，其尺寸大小具有一定的冗余，一般为空间栅格尺寸的1～2倍，以同时检测候选点及其周围格点的可通行情况，以保证路径的安全性。进一步地，所述步骤(5)中评价函数模型F(P)的子函数G(P)使用以下公式求解：G(P)＝G(O′)+λDO′P其中G(O′)为从起点Start到达当前点O′的累积路径距离，DO′P为当前点O′到候选点P的距离，λ为权重参数，取值为[1,2]，以表征当前点O′到达候选点P的难易程度，λ越大表示越难到达。本专利技术的有益效果是：在传统的启发式三维空间路径规划方法中，规划路径的灵活性与算法实时性往往不可兼得；本方法创新性地对基于魔方模型的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)继续执行。...

【技术特征摘要】
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≤δ时，认为已到达目标点，结束路径规划；否则，将当前点添加进已访问列...

【专利技术属性】
技术研发人员：杨嘉珩，胡文学，沈会良，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33