CN108803659A - 基于魔方模型的多窗口启发式三维空间路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

现有技术在解决三维栅格空间路径规划问题时，一般将自由度约束到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)建立多窗口判断模型W_i，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个候选点P_i已存在于候选列表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′)+λD_O′P

其中G(O′)为从起点Start到达当前点O′的累积路径距离，D_O′P为当前点O′到候选点P的距离，λ为权重参数，取值为[1,2]，以表征当前点O′到达候选点P的难易程度，λ越大表示越难到达。

本发明的有益效果是：在传统的启发式三维空间路径规划方法中，规划路径的灵活性与算法实时性往往不可兼得；本方法创新性地对基于魔方模型的26个候选点进行分级，优先考虑最基本的前、后、左、右、上、下六维运动，继而再分级考察其他运动方向的可行性。并建立多窗口模型，配合三维空间栅格地图边界迭代器，批量快速完成对相关运动方向候选点的可访问性判断，大幅节约了时间，提高了算法效率。本方法在充分保证路径规划结果灵活、实用的前提下，同时还兼顾了算法效率，实时性良好，在实际测试中表现优异。

附图说明

图1为基于魔方模型的多窗口启发式三维空间路径规划方法的流程图；

图2为魔方模型候选点分级示意图

图3为实际测试场景的三维空间特征地图；

图4为本发明提出的方法在实际路径规划中的效果图。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

如图1所示，本发明提供的一种基于动态球形窗口的三维空间路径规划方法，具体实施方式如下：

(1)决定所要进行路径规划的场景，针对不同场景选择不同的环境传感器作为前端感知器。例如，室外场景可选择激光雷达、双目成像系统；室内场景可选择单\双目相机或者RGB-D相机。通过对三维环境进行感知，得到含有深度信息的三维点云。使用体素滤波器、统计滤波器方法对含有杂点点云进行滤波处理。

(2)设置障碍物特征地图分辨率Res，本实施例设置为0.01m，使用射线插入方法将三维点云插入特征地图，最终得到障碍物特征地图M(Res)。根据场景、目标物大小确定三维空间栅格尺寸L≥Res，本实施例根据应用场景设置为栅格尺寸L＝0.2m。将边长为L的空间立方体作为路径规划三维空间栅格地图的基本组成单元，用其中心点来表示该立方体，立方体的空间位置即为中心点的三维坐标；

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

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

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

本实例中，使用最小堆存储候选列表Candidate，以提升算法执行效率。

(4)建立多窗口判断模型W_i，j，其中i表示优先级数，j表示当前优先级下对应的窗口。根据候选点距离中心点的远近以及到达该候选点的难易程度等因素，将26个候选点划分等级，本发明中设置划分为3级，如图2所示。具体的将上、下、左、右、前、后方向上的6个候选点作为第一优先级候选点，将前侧斜上方、前侧斜下方、后侧斜上方、后侧斜下方、左侧斜上方、左侧斜下方、右侧斜上方、右侧斜下方上的共8个候选点作为第二优先级候选点，余下12个方向上的候选点作为第三优先级候选点。

对于第一优先级候选点，本发明专门设置了与各个方向上候选点一一对应的6个窗口，用来判断其可访问性。

对于第二优先级候选点，本发明设置了4个窗口，每个窗口对应于两个候选点，用来批量判断每个窗口对应的候选点的可访问性。

对于第三优先级候选点，本发明设置了4个窗口，每个窗口对应于三个候选点，用来批量判断每个窗口对应的候选点的可访问性。

同时为了探测候选点周围的障碍物情况，保障安全性。所有窗口的尺寸设置时应具有一定的冗余，一般为空间栅格尺寸的1～2倍，以保证同时检测候选点和其周围的格点的可通行性。

一旦窗口内存在障碍物，即认为当前窗口所对应的候选点均为不可访问。

通过上述方法快速候选点P的可访问性，筛选出所有可以访问的候选点，执行步骤(5)；

(5)建立启发式评价函数模型F(P)：

F(P)＝α·G(P)+β·H(P)

G(P)表征从起点Start到达候选点P的累积路径距离，H(P)表征从候选点P到达终点End的预计路径距离，α、β为其权重参数，一般不超过2。本实施例中设置为α＝1，β＝1.5。

G(P)使用以下公式求解：

G(P)＝G(O′)+λD_O′P

G(O′)为从起点Start到达当前点O′的累积路径距离，D_O′P为当前点O′到候选点P的距离，λ为权重参数，取值为[1,2]，以表征当前点O′到达候选点P的难易程度，λ越大表示越难到达。本实施例中，统一设置λ＝1。

H(P)表征从点P到达终点End的预计路径距离，本实施例中，统一使用曼哈顿距离来衡量该预计代价：

H(P)＝|X_Pi-X_end|+|Y_Pi-Y_end|+|Z_Pi-Z_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＝||End-O′||₂

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

实施例1

以下利用本发明方法针对实际三维空间场景进行路径规划，同时对比了实时性较好的局部路径规划方法，结果如下：

图3为本例的测试场景模型，模型刻画了办公室内一台放有电脑的桌子模型。其中，x坐标指向右侧，y坐标指向下方，z坐标指向桌子模型。

在三维场景中，设置起点Start为(0,0,-1)，终点End为(1.5,-0.6,4)。详细路径分析结果见表1，规划结果如图4所示。

其中，规划用时指路径规划的总体耗时，中间节点个数是在最终路径结果中所包含的途径的节点数目，路径长度是指最终路径在三维空间中的总长度。可以发现，由于本方法在兼顾实时性的同时，能得到鲁棒、灵活、高质量的全局最优路径。

表1

以上仅为本发明具体实施方式，不能以此来限定本发明的范围，本技术领域内的一般技术人员根据本创作所作的均等变化，以及本领域内技术人员熟知的改变，都应仍属本发明涵盖的范围。

Claims (9)

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表示当前优先级下对应的窗口；将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个候选点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)中根据中心点O′距离候选点P的远近以及到达该候选点P的难易程度，将26个候选点划分多个等级，优先级从高到低，具体为：将上、下、左、右、前、后方向上的6个候选点作为第一优先级候选点，将前侧斜上方、前侧斜下方、后侧斜上方、后侧斜下方、左侧斜上方、左侧斜下方、右侧斜上方、右侧斜下方上的共8个候选点作为第二优先级候选点，余下12个方向上的候选点作为第三优先级候选点。

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

7.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于：所述步骤(4)中为加快对候选点可访问性的判断速度，使用窗口模型进行查询，单个窗口对应于多个候选点，一旦某一级的某个窗口内存在障碍物，即认为当前窗口对应的候选点均不可访问。

8.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于：所述步骤(4)中所建立的窗口，其尺寸设置为空间栅格尺寸的1～2倍，以同时检测候选点及其周围格点的可通行情况，以保证路径的安全性。

9.根据权利要求1所述的魔方模型的多窗口启发式三维空间路径规划方法，其特征在于：所述步骤(5)中评价函数模型F(P)的子函数G(P)使用以下公式求解：

G(P)＝G(O′)+λD_O′P

其中G(O′)为从起点Start到达当前点O′的累积路径距离，D_O′P为当前点O′到候选点P的距离，λ为权重参数，取值为[1，2]，以表征当前点O′到达候选点P的难易程度，λ越大表示越难到达。