CN116009527A - 基于动态场景结构膨胀感知的路径规划算法 - Google Patents

Abstract

本发明公开了一种基于动态场景结构膨胀感知的路径规划算法。首先，通过栅格地图的稀疏拓扑结构图使随机树的扩展更具方向性，采用改进的双向快速搜索随机树算法得到初始路径，减少移动机器人的寻路时间。其次，根据动态障碍物的大小和数量、静态障碍物之间的最短距离和地图的大小，通过自适应算子建立机器人的膨胀模型，让其在动态的场景下预碰撞到移动的障碍物。最后，基于随机采样算法得到的规划路径特点，构建移动机器人的运动模型。本发明大大缩短路径规划的时间，有效的解决了室内环境下机器人路径规划时间长以及狭长空间无法快速进行路径规划的问题。具有很强的实时性和灵活性，可以在动态的场景中有效的进行避障导航。

Description

基于动态场景结构膨胀感知的路径规划算法

技术领域

本发明涉及机器人自动路径规划技术领域，尤其涉及一种基于动态场景结构膨胀感知的路径规划算法。

背景技术

如今智能机器人的应用已经越来越受到人们的关注。为了能够在复杂的场景中完成特定的任务，需要移动机器人具有自主导航的能力。路径规划技术作为自主导航的核心技术之一，目的是为了让移动机器人在有障碍物的环境中能够快速并且无碰撞的从起始位置移动到目标位置。在目前的生产生活中，场景通常都会包含移动的障碍物。在面对动态场景时，目前已有的局部路径规划算法往往很难快速的做出反应并且重新规划路径，对环境变化的适应性较低，导致无法对室内的突发情况进行判断以及处理。

发明内容

本发明目的就是为了弥补已有技术的缺陷，提供一种基于动态场景结构膨胀感知的路径规划算法，首先，该算法通过栅格地图的稀疏拓扑结构图使随机树的扩展更具方向性，在此基础上，该算法采用改进的双向快速搜索随机树算法得到初始路径，减少移动机器人的寻路时间。其次，该算法根据动态障碍物的大小和数量、静态障碍物之间的最短距离和地图的大小，通过自适应算子建立机器人的膨胀模型，让其在动态的场景下预碰撞到移动的障碍物。最后，该算法基于随机采样算法得到的规划路径特点，构建移动机器人的运动模型。

本发明是通过以下技术方案实现的：

一种基于动态场景结构膨胀感知的路径规划算法，具体包括如下步骤：

(1)对于实际场景建立一个二维的直角坐标系XOY，同时将场景划分成M×N的二维栅格地图，其中M表示所述栅格地图的横向栅格数，N表示栅格地图的纵向栅格数。

(2)利用Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，保留下来的特征点成为稀疏拓扑节点。根据栅格地图的大小采用如下公式对所获得的角点进行自适应选取：

其中a为仿真栅格地图的高，b为仿真栅格地图的宽，ε为地图大小的数量级。以当前特征点为圆心、Threshold为半径，在其所组成的圆内，剔除圆心以外的特征点。

(3)通过稀疏拓扑节点构建稀疏化拓扑结构图。在此基础上，通过改进的双向快速搜索随机树算法得到初始路径。具体方法如下：

(3.1)：所有闭合环路生成为一颗树；

(3.2)：在交叉支路上定义稀疏拓扑节点的父节点为parent₁，parent₂，……，parent_n，以这样的方式生成一颗树；

(3.3)：统计由稀疏拓扑节点所组成的所有可行路径，并对其进行比较，选取长度最短的作为初始路径；

(3.4)：当起点和最近的稀疏拓扑节点的子节点的连线穿过障碍物时，令起点为最近的稀疏拓扑节点的父节点，然后把起点插入到初始树中。如果起点和最近的稀疏拓扑节点之间的连线仍穿过障碍物，则另起点为稀疏拓扑节点父节点的父节点，以此往复直至其连线未穿过障碍物，令起点为该节点的父节点；

(3.5)：当起点和最近的稀疏拓扑节点的子节点的连线没有穿过障碍物时，令起点为最近的稀疏拓扑节点的子节点的父节点，同样把起点插入到树中。

(4)设定移动机器人自适应膨胀模型和运动模型使其在动态场景中进行移动仿真并进行障碍物提前感知。首先采用自适应膨胀半径在物体周围进行膨胀感知以提前预知障碍物，在感知到障碍物后，立即进行路径重规划。具体公式如下：

其中k为自适应参数，动态调整膨胀半径。R表示自适应膨胀半径，argmin是一个数学函数，表示使目标函数f(x)取最小值时的变量值，

确保膨胀区间不会包含动态障碍物，l_min/2是为了让移动机器人的膨胀区域能在静态障碍物间进行正常的行走，防止其出现体积过大而进行无效的路径重规划。

然后是设定移动机器人运动模型，针对动态场景往往需要较好的运动模型以适应时刻变化的障碍物，本发明提出如下公式：

X_pos＝node_current.x+(node_next.x-node_current.x)/k

Y_pos＝node_current.y+(node_next.y-node_current.y)/k

其中node_current代表当前节点的位置，node_next代表规划路径相邻的下一个节点的位置，Xpos和Ypos包含机器人运动模型中的位置信息以及方向信息，x和y是机器人的坐标值，例如node_current.x就表示当前机器人节点的X轴的坐标值，node_next.x就表示下一节点的X轴的坐标值，y的含义以此类推，表示Y轴的坐标值。k值可以自适应调整移动机器人的速度，k值越大其移动速度越慢，k值越小其移动速度越快。在节点较为密集的地方，节点构成的路径平滑性较差，具体体现为移动速度较慢。同时在节点稀疏的地方，节点构成的路径的平滑性较好，具体体现为移动速度较快。

同时为了提高动态场景中路径规划的效率，针对移动的机器人遇到动态障碍物时需要重新规划。本章设定重规划的终点为向后探索W个节点(向后探索得到的节点应包含于路径节点，若超出，对W的取值进行自减一的操作直至探索的节点包含于路径节点)，并判断该节点是否在障碍物内。如果该节点在障碍物内，则继续向后探索一个节点直至探索的节点未被障碍物所包含，令该探索的节点为重新路径规划的终点。如果该节点未在障碍物内，则令该节点为重新路径规划的终点。这样得到新路径再与原先规划的路径组合在一起，可以提高路径重规划的效率。

(5)未感知到动态障碍物时，移动机器人继续按照步骤(3)中的规划路径进行移动。

(6)在感知到动态障碍物时，移动机器人进行路径重规划，其起点为当前节点，终点为当前节点向后探索W个节点，如果是由稀疏化拓扑结构图构成的路径，则为W个稀疏拓扑节点。

(7)在进行路径重规划时根据步骤(6)的起止点并采用最优双向快速搜索随机树法，快速规划出新的路径。本发明将动态的障碍物设定为1，静态的障碍物设定为0。当移动机器人在重规划的路径上进行循迹导航时，如果膨胀模型再次碰到静态障碍物，移动的机器人不进行重规划，有利于其在狭长的空间经过。如果碰到的为动态障碍物，需再次进行重规划。

(8)把新的路径与原先的路径进行有效地融合，移动机器人在融合后的路径上继续移动同时重复进行步骤(7)。

本发明的优点是：

1.本发明通过Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，保留稀疏拓扑节点。然后通过稀疏拓扑节点构建稀疏化拓扑结构图，并结合双向快速搜索随机树算法快速地得到初始路径。这样可以有效的引导路径规划的拓展方向，大大缩短路径规划的时间，有效的解决了室内环境下机器人路径规划时间长以及狭长空间无法快速进行路径规划的问题。

2.本发明通过自适应膨胀模型和运动模型，根据地图大小、障碍物数目等调整机器人膨胀半径和运动速度，具有很强的实时性和灵活性，可以在动态的场景中有效的进行避障导航。

3.本发明通过将双向快速搜索随机树算法与机器人自适应膨胀模型和运动模型相结合，既可以在周围没有动态障碍物时通过双向快速搜索随机数算法快速计算出初始路径，又可以在周围存在障碍物时通过自适应膨胀模型和运动模型快速做出规避和路径重规划。本发明所提出的当移动的机器人在进行路径重规划时向后只探索W个节点作为重规划的终点提高了算法的效率，使其规划路径的实时性得以保障。

附图说明

图1为本发明方法流程图；

图2为建立的带有动态障碍物的栅格地图场景图；

图3为稀疏化拓扑结构图；

图4为双向快速探索随机树的优化过程图；

图5为膨胀半径的设定示意图；

图6为本发明的仿真结果图。

具体实施方式

下面结合附图和实例对本发明进一步说明。

本实例中以室内机器人为例，总体按照如下过程实现基于动态场景结构膨胀感知的路径规划算法，如图1所示：

1.根据实际场景建立栅格地图；

2.利用Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，形成拓扑结构图；

3.使用双向快速搜索随机树算法对拓扑结构图计算出初始路径；

4.自适应膨胀模型和运动模型使其在动态场景中进行移动仿真并进行障碍物提前感知；

5.进行路径重规划，直至到达终点。

综上所述，在移动机器人进行循迹导航时，基于动态场景结构膨胀感知的路径规划算法通过膨胀模型使移动机器人对障碍物进行提前感知，并且在感知到障碍物后，会通过双向快速搜索随机树算法进行路径的重规划。在保障算法有效性的同时，为了进一步提高算法在动态场景中的实时性，当移动机器人进行路径重规划时，设定起点为当前节点，终点为当前节点向后探索W个节点，而不是最初设定的目标点，把重规划得到的路径结果整合到原先规划的路径上得到新的避障路径。上述过程相比于终点是目标点的情况可以在一定程度上减少算法的迭代次数，提高算法的效率。

本实例具体过程如下：

步骤1：如图2，利用仿真的室内环境作为实验地图数据，并添加了7个动态障碍物。实验采用正方形膨胀模型，为了较好的进行碰撞检测，在进行仿真实验之前，需对实验地图数据进行预处理工作，将仿真室内环境中的圆形障碍物根据其半径膨胀设置为正方形障碍物。其地图的大小为734×402，根据地图的大小，对动态障碍物的大小进行了相应的调整，调整后的大小为18×18。同时建立一个二维的直角坐标系XOY，同时将场景划分成M×N的二维栅格地图，其中M表示所述栅格地图的横向栅格数，N表示栅格地图的纵向栅格数。

步骤2：如图3，利用Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，保留下来的特征点成为稀疏拓扑节点。根据栅格地图的大小采用如下公式对所获得的角点进行自适应选取：

其中a为仿真栅格地图的高，b为仿真栅格地图的宽，ε为地图大小的数量级。以当前特征点为圆心、Threshold为半径，在其所组成的圆内，剔除圆心以外的特征点。

步骤3：对稀疏拓扑节点图使用改进的双向快速搜索随机树算法得到初始路径。本发明提出了改进的采样策略，使采样的节点服从正态分布，即采样点有百分之五十的概率沿着两棵随机树中距离最近的节点之间的连线进行采样，另外百分之五十的概率在整个状态空间进行采样。

由于本发明在理论上是渐进最优的，因此随着迭代次数的增加可以找到最优路径。此外该算法从起点和终点同时进行扩展，减少了路径规划的探索时间。同时在算法进行为新节点选择代价最小的父节点和为新节点寻找子节点这两个过程时，本发明将判断函数由欧式距离改为曼哈顿距离以提高算法的运行效率，如下式所示，其中node代表当前节点，node_i代表以node为圆心以采样半径r(采样半径的大小根据下式所确定)构成的圆中的所有的节点。

dis＝|node.x-node_i.x|+|node.y-node_i.y|

该算法的主要流程如下：首先，分别以起点和目标点作为初始节点建立两颗快速扩展随机树，两颗随机生成树分别为T₁和T₂。当T₁树在栅格地图中的无障碍物的空间Map_free中随机采样一个节点node_r，从T₁树中最近的顶点向它延伸得到新节点node_new。其次，构建以新节点node_new为圆心采样半径为r的圆，得到圆内所有节点的集合Node_n。最后，在集合Node_n中寻找节点，使该节点与新节点node_new所构成的路径成本最低，同时对新节点node_new进行重新布线。

在T₁树完成上述过程之后，渐进最优双向快速探索随机树算法试图将T₂树中距离T₁树中的结果顶点node_new最近的顶点node_connect连接到结果顶点。接下来，针对T₂树路径优化的过程具体描述如下：首先，在T₂树中选取两个节点node₁和node₂作为输入，T₂树中的迭代过程和RRT^*算法的迭代过程一样，其中node₂扮演node_r的角色。从T₂树中以node₁为圆心，以一定长度作为半径构成一个圆，计算圆内的每一个节点和node₂之间的路径成本，并把路径成本存储在链表中。然后，该算法针对总的路径成本对链表进行遍历排序，并且仅当所得到的解决方案的成本低于当前最佳方案的路径成本时，才尝试连接到另一棵树上的顶点。最后，该算法更新当前的最佳解决方案，用分支界定算法修剪并交换树的扩展。

图4表示了T₂树的优化过程，其中红色的节点为T₂树，黑色的节点为T₁树。从图中我们可以看到在没有选取node₂(节点a)之前，其规划的路径会沿着节点e-c-d的方式到达node_goal，其当前的路径成本为27。在添加node₂之后其路径成本会进行重新的计算，从图中我们可以看到沿着节点e-c-a其路径成本会变小，其成本为22。因此其会更新当前的解决方案，即通过节点e-c-a替代节点e-c-d来降低规划路径的成本。

步骤4：设定移动机器人自适应膨胀模型和运动模型使其在动态场景中进行移动仿真并进行障碍物提前感知。

由于进行动态场景的路径规划时，在移动的机器人由始点向目标点移动的过程中，动态障碍物也会移动，因此会出现原先所规划的路径，并不能适用于障碍物移动后的场景。因此我们采用自适应膨胀半径在物体周围进行膨胀感知以提前预知障碍物，如图5，在感知到障碍物后，立即进行路径的重规划。膨胀半径主要由以下参数所确定：静态障碍物之间的最短距离l_min、动态障碍物之间的数量q和地图的大小a,b相关，动态障碍物的面积s，其中a为栅格地图的宽，b为栅格地图中的长。具体公式如下：

其中k为自适应参数，动态调整膨胀半径。

确保膨胀区间不会包含动态障碍物，l_min/2是为了让移动机器人的膨胀区域能在静态障碍物间进行正常的行走，防止其出现体积过大而进行无效的路径重规划。

然后是设定移动机器人运动模型，针对动态场景往往需要较好的运动模型以适应时刻变化的障碍物，本发明提出如下公式：

X_pos＝node_current.x+(node_next.x-node_current.x)/k

Y_pos＝node_current.y+(node_next.y-node_current.y)/k

其中node_current代表当前节点的位置，node_next代表规划路径相邻的下一个节点的位置。k值可以自适应调整移动机器人的速度，k值越大其移动速度越慢，k值越小其移动速度越快。在节点较为密集的地方，节点构成的路径平滑性较差，具体体现为移动速度较慢。同时在节点稀疏的地方，节点构成的路径的平滑性较好，具体体现为移动速度较快。因此构建的移动机器人运动模型能够较好的适用于动态场景的路径导航。

同时为了提高动态场景中路径规划的效率，针对移动的机器人遇到动态障碍物时需要重新规划。本发明设定重规划的终点为向后探索W个节点(向后探索得到的节点应包含于路径节点，若超出，对W的取值进行自减一的操作直至探索的节点包含于路径节点)，并判断该节点是否在障碍物内。如果该节点在障碍物内，则继续向后探索一个节点直至探索的节点未被障碍物所包含，令该探索的节点为重新路径规划的终点。如果该节点未在障碍物内，则令该节点为重新路径规划的终点。这样得到新路径再与原先规划的路径组合在一起，可以提高路径重规划的效率。

步骤5：未感知到动态障碍物时，移动机器人继续按照步骤3中的规划路径进行移动。

步骤6：在感知到动态障碍物时，移动机器人进行路径重规划，其起点为当前节点，终点为当前节点向后探索W个节点，如果是由稀疏化拓扑结构图构成的路径，则为W个稀疏拓扑节点。

步骤7：在进行路径重规划时根据步骤6的起止点并采用最优双向快速搜索随机树法，快速规划出新的路径。本发明将动态的障碍物设定为1，静态的障碍物设定为0。当移动机器人在重规划的路径上进行循迹导航时，如果膨胀模型再次碰到静态障碍物，移动的机器人不进行重规划，有利于其在狭长的空间经过。如果碰到的为动态障碍物，需再次进行重规划；如图6，移动机器人在重规划路径进行循迹导航时如果再次遇到障碍物，将以当前位置为起点，终点仍为重规划时的终点。最后留下的绿色路径是机器人无碰撞的规划路径。

步骤8：把新的路径与原先的路径进行有效地融合，移动机器人在融合后的路径上继续移动同时重复进行步骤7。

Claims (6)

1.一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：具体包括如下步骤：

(1)根据实际场景建立栅格地图；

(2)利用Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，保留下来的特征点成为稀疏拓扑节点；

(3)通过稀疏拓扑节点构建稀疏化拓扑结构图，并通过双向快速搜索随机树算法对拓扑结构图计算出初始路径；

(4)设定移动机器人自适应膨胀模型和运动模型使其在动态场景中进行移动仿真并进行障碍物提前感知；

(5)未感知到动态障碍物时，移动机器人继续按照步骤(3)中的规划路径进行移动；

(6)在感知到动态障碍物时，移动机器人进行路径重规划，其起点为当前节点，终点为当前节点向后探索W个节点，如果是由稀疏化拓扑结构图构成的路径，则为W个稀疏拓扑节点；

(7)在进行路径重规划时根据步骤(6)的起止点并采用最优双向快速搜索随机树法，快速规划出新的路径；

(8)把新的路径与原先的路径进行融合，移动机器人在融合后的路径上继续移动同时重复进行步骤(7)。

2.根据权利要求1所述的一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：步骤(1)所述的根据实际场景建立栅格地图，具体如下：对于实际场景建立一个二维的直角坐标系XOY，同时将实际场景划分成M×N的二维栅格地图，其中M表示所述栅格地图的横向栅格数，N表示栅格地图的纵向栅格数。

3.根据权利要求2所述的一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：步骤(2)所述的利用Harris角点检测算法得到轮廓骨架中的特征点，并通过自适应阈值消除冗余的特征点，保留下来的特征点成为稀疏拓扑节点，具体如下：根据栅格地图的大小采用如下公式对所获得的角点进行自适应选取：

其中a为仿真栅格地图的高，b为仿真栅格地图的宽，ε为地图大小的数量级，以当前特征点为圆心、Threshold为半径，在其所组成的圆内，剔除圆心以外的特征点。

4.根据权利要求3所述的一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：步骤(3)所述的通过稀疏拓扑节点构建稀疏化拓扑结构图，并通过双向快速搜索随机树算法对拓扑结构图计算出初始路径，具体方法如下：

(3.1)：所有闭合环路生成为一颗树；

(3.2)：在交叉支路上定义稀疏拓扑节点的父节点为parent₁，parent₂，……，parent_n，以这样的方式生成一颗树；

(3.3)：统计由稀疏拓扑节点所组成的所有可行路径，并对其进行比较，选取长度最短的作为初始路径；

(3.4)：当起点和最近的稀疏拓扑节点的子节点的连线穿过障碍物时，令起点为最近的稀疏拓扑节点的父节点，然后把起点插入到初始树中，如果起点和最近的稀疏拓扑节点之间的连线仍穿过障碍物，则另起点为稀疏拓扑节点父节点的父节点，以此往复直至其连线未穿过障碍物，令起点为该节点的父节点；

(3.5)：当起点和最近的稀疏拓扑节点的子节点的连线没有穿过障碍物时，令起点为最近的稀疏拓扑节点的子节点的父节点，同样把起点插入到树中。

5.根据权利要求4所述的一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：步骤(4)所述的设定移动机器人自适应膨胀模型和运动模型使其在动态场景中进行移动仿真并进行障碍物提前感知，具体如下：

首先采用自适应膨胀半径在物体周围进行膨胀感知以提前预知障碍物，在感知到障碍物后，立即进行路径重规划，具体公式如下：

其中k为自适应参数，动态调整膨胀半径，l_min为静态障碍物之间的最短距离、q为动态障碍物之间的数量，s为动态障碍物的面积，R表示自适应膨胀半径，argmin是一个数学函数，表示使目标函数f(x)取最小值时的变量值；

然后设定移动机器人运动模型，提出如下公式：

X_pos＝node_current.x+(node_next.x-node_current.x)/k

Y_pos＝node_current.y+(node_next.y-node_current.y)/k

其中node_current代表当前节点的位置，node_next代表规划路径相邻的下一个节点的位置，Xpos和Ypos包含机器人运动模型中的位置信息以及方向信息，x和y是机器人的坐标值，k值自适应调整移动机器人的速度，k值越大其移动速度越慢，k值越小其移动速度越快；在节点较为密集的地方，节点构成的路径平滑性较差，具体体现为移动速度较慢，同时在节点稀疏的地方，节点构成的路径的平滑性较好，具体体现为移动速度较快。

6.根据权利要求5所述的一种基于动态场景结构膨胀感知的路径规划算法，其特征在于：步骤(7)中进行路径重规划时，将动态的障碍物设定为1，静态的障碍物设定为0，当移动机器人在重规划的路径上进行循迹导航时，如果膨胀模型再次碰到静态障碍物，移动的机器人不进行重规划，有利于其在狭长的空间经过，如果碰到的为动态障碍物，则再次进行重规划路径。

Images