CN115741686A - 一种基于变概率约束采样的机器人路径规划方法 - Google Patents

Abstract

本发明属于机器人路径选择技术领域，具体地说，是一种基于变概率约束采样的机器人路径规划方法，首先，通过将目标偏置机制引入采样过程，减少首条路径形成时间，从而快速生成椭球采样集，其作用可以减少搜索路径数；采用可变步长策略，减少机器人难以搜索到路径的概率；对所生成的近优路径进行回溯处理，弱化较为生硬的路径线条。藉此确保算法可以有效的提升搜索效率、减少搜索时间，并保证搜索到的路径是近优路径。

Description

一种基于变概率约束采样的机器人路径规划方法

技术领域

本发明属于机器人路径选择技术领域，具体地说，是一种基于变概率约束采样的机器人路径规划方法。

背景技术

近些年来，路径规划在机器人技术的发展过程中占据着重要的地位，其主要原因是使机器人能够在当前工作环境中有效的规划出一条链接起始点和目标点的可行路径，该路径不仅需要满足一定的约束条件同时也需要达到一定的性能指标。

目前路径规划方法在机器人领域中主要分为以下几类：(1)基于搜索的路径规划，例如A*和Dijkstra算法，这类算法通过比较当前格栅图的8个近邻启发值来确定下一步的位置，当存在多个极小近邻启发值时，则不能保证下一步位置的解是最优的。与此同时这类算法难以在多维的环境中达到令人满意的规划效果。(2)基于智能算法的路径规划，例如遗传算法和蚁群算法，遗传算法的效率通常较低且具有较差的局部搜索能力。蚁群算法收敛速度慢，容易出现局部最优的情况，且具有较差的环境适应能力。(3)基于采样的路径规划，例如扩展空间树ESTs(Expansive Space Trees)，概率路线图法PRM(Probabilisticroadmap method)和快速搜索随机树RRT(Rapidly-exploringrandom trees)，这类算法在复杂环境下进行采样时可以避免一些低效率和低成功率问题的发生。PRM算法计算的速度较快，但是在采样点稀疏或者分布不均时，不能够较好的执行搜索任务。相较于PRM算法，RRT算法更加灵活，但所搜索的路径通常并不是最优的。

通过上述分析讨论，不难发现RRT算法也存在一定的不足之处。为了优化RRT算法，许多研究者提出了不同的改进算法，Kuffner等人提出了RRT Connect算法，与RRT算法相比，RRTConnect算法搜索路径时采用了双树的搜索机制，一棵树是以起点为起始点，另一棵树以终点为起始点，当两棵树相连时停止搜索。因此，该算法能够提升搜索路径的效率，减少路径搜索时间。

然而上述这些算法仍有一定的局限性，不能更好地优化搜索路径的搜索过程，缩短路径长度以及降低路径搜索成本。

首先，该算法在生成新节点时，约束了目标采样点的偏向采样条件，使得在搜索过程中能够有效的减少搜索时间；其次，采用可变步长策略，提高搜索到路径的概率；最后，对搜索到的路径进行回溯，重新布线，找到一条更优的路线；最后，反复迭代直到找到一条近优路径

发明内容

为了解决上述问题，本发明披露了一种基于变概率约束采样的机器人路径规划方法，首先，该算法在生成新节点时，约束了目标采样点的偏向采样条件，使得在搜索过程中能够有效的减少搜索时间；其次，采用可变步长策略，提高搜索到路径的概率；最后，对搜索到的路径进行回溯，重新布线，找到一条更优的路线；最后，反复迭代直到找到一条近优路径。

本发明具体的技术方案如下：

一种基于变概率约束采样的机器人路径规划方法，包括以下步骤：

步骤1：生成地图及障碍物信息，给定机器人的起始位置Q_s和目标位置Q_g；

具体的，设置机器人运动的地图大小为20单位×17单位的二维空间，机器人的起始位置Q_s，目标位置Q_g，最后再生成障碍物。

步骤2：将Q_s、Q_g分别作为两棵树的初始起点，相向扩展随机树，根据随机生成的概率p来控制采样方向；

具体的，设置一个范围在0-100之间的随机概率p，以及可变值ε，若p小于ε，则以目标点为采样点，否则进行椭圆约束采样，若周围判定有障碍物，则改变ε的值，使得采样方式更倾向于椭圆约束采样。

步骤3：当p大于ε时，进行椭圆约束采样；

具体的，起点Q_s和终点Q_g分别为椭圆的两个焦点，椭圆的共轭直径为d_i，其中d_i被定义为：

偏心率由cmin/cbest计算得到，在双向搜索树条件下，机器人从Q_s到Q_g的路径成本可以看作由两段组成，即从Q_s到x的成本，以及从x到Q_g的成本，使用欧几里得距离得到两个焦点的距离c_min.椭圆的横向直径为c_best，它代表当前所记录的最优路径，因此椭圆采样集可以如公式(1)这样定义：

使用欧几里得距离得到两个焦点的距离c_min.椭圆的横向直径为c_best，它代表当前所记录的最优路径椭圆约束采样通过将均匀分布的样本从一单位的n维超球面转变成n维椭圆子集x_ellipse-μ(X_ellipse)，从而在椭圆子集内完成均匀分布采样，即

x_ellipse＝Lx_ball+x_center， (3)

x_center＝(x_f1+x_f2)/2， (4)

x_ball＝{x∈X|||x||₂≤1}， (5)

其中x_center为椭圆的中心，x_f1，x_f2分别为椭圆的两个焦点，x_ball为随机采样点；

之后通过椭圆矩阵S的Cholesky分解来计算上述变换，S∈Rn×n，有

LL^T≡S， (6)

(x-x_center)^TS(x-x_centet)＝I， (7)

S的特征值为椭圆半径的平方{r_i ²}，r_i的值为

r₁＝c_best/2， (8)

采样点子集的变化可通过横截半径和长轴得到，其中diag{.}为对角矩阵，其中长轴的对角线矩阵和分解运算分别为

在取得采样集之后，需要将此时的椭圆转到世界坐标系下，可通过Kabsch算法求解，即

C＝Udiag{1,...1,det(U)det(V)}V_t (12)

本发明直接使用旋转矩阵C(θ)来计算，从而求得采样集并返回。

步骤4：在树中选取最近邻点Q_nest，根据最近邻点Q_nest以及障碍物的位置判断得到动态步长L，从而生成新节点，其中包括：

(1)确定动态步长L：

对于树一，在随机寻找一个采样点Q_r1后，搜索距Q_r1最近的点Q_nest1，接着以步长L向点Q_r1扩展，其运算为

对于树二，在新节点Q_new1生成后，搜索得到Q_new1距离树二最近的点Q_nest2，然后以步长L向点Q_new1扩展，其运算为

运算表达式中的步长L为动态步长，相较于静态步长，此处理使得机器人面对复杂路径时更加灵活，减少搜索路径失败的概率，加快了搜索近优路径的收敛时间。设计如下：

当两树之间的距离D_tree小于预设值s₁时，使用小步长L_s来扩展；当D_tree大于预设值s₁时，则表示两树之间距离较远，由机器人与障碍物环境决定步长；如果机器人与障碍物之间的距离D_obs大于预设值s₂时，认为机器人附近无杂物，采用大步长L_m；反之，当D_obs小于于预设值s₂时，认为机器人周围环境复杂，采用小步长Ls，其表达式为

(2)生成新节点：

Q_s、Q_g分别为路径的起点和终点，Q_r为空间中的随机采样点，Q_nest为与Q_r距离最近的节点，Q_new为在Q_nest到Q_r的延长线上延伸一个步长之后得到的点。函数首先在地图中随机寻找一个采样点Q_r，其次寻找距离采样点Q_r最近的点Q_nest，且该点为树中已存在的节点，判断最近邻点Q_nest到采样点Q_r的距离是否小于算法所设步长，如果小于就将Q_r设为新节点Q_new，如果超出步长大小，则将在最近邻点Q_nest路径的基础上向采样点Q_r延伸一个步长后所得到的点设为新节点Q_new。此时有邻近节点Q_near、潜在父节点Q_{potential-parent}以及真父节点Q_parent。以新节点Q_new为圆心一定半径作圆，该圆内所有的点构成一个新的集合。接着遍历构成的集合，找到所有邻近节点Q_near并逐一将其作为潜在父节点Q_{potential-parent}与新节点Q_new连接，同时计算起点Q_s、潜在父节点、新节点的路径长度并与原路径长度进行比较，从而筛选出最优的潜在父节点Q_{potential-parent}，并将其设为新的父节点Q_parent。最后判断新节点Q_new与最近邻点Q_nest之间的连线是否与障碍物碰撞，若判断为否，则此时将新路径添加进树并且删除原始的连接路径。

步骤5：判断最近邻点Q_nest与新节点Q_new之间的路径有无障碍物；

将近邻点Q_nest与新节点Q_new分别设为待检测路段的两个端点，T为障碍物的圆心，o为障碍物到待检测路段的垂直点，则有

再根据机器人的模型大小，即可判断机器人在行进过程中是否有碰撞。

步骤6：随机树生成新节点后，判断两树的最新节点是否小于阈值，若满足条件则路径已被找到，进行回溯处理；

具体的，通过双树搜索路径策略搜索到路径后，判断两树是否符合连接条件，此时仅通过判断新节点与另外一棵树的距离是否小于设定的常数值是有一定缺陷的，这会使得整条路径折线过多、连接处转角过大。因此，为了搜索近优路径，需要对以生成的较优路径进行回溯处理，遍历整个较优路径的节点，从起点开始依次寻找路径上能最大程度保证不碰撞障碍物的节点，将起点与之相连并记录这个点；再从该点开始，寻找它之后的无碰撞点，直到可以与终点无碰撞连接。将这些点和路径信息保存下来，通过对比路径信息搜索到新的较优路径，反复迭代，最终选择总距离最短的路径为算法最终搜索路径。

本发明的有益效果：首先，通过将目标偏置机制引入采样过程，使得首条路径形成速度加快，从而快速生成椭球采样集，其作用可以减少搜索路径数，并有利于最优路径生成；采用可变步长策略，使得机器人面对复杂路径时更加灵活，减少机器人难以搜索到路径的概率；对所生成的近优路径进行回溯处理，优化路径折线过多、连接处转角过大的问题，对比路径信息搜索到更好的较优路径，从而确定最终路径，藉此确保算法可以有效的提升搜索效率、减少搜索时间，机器人在该路径的运行时间也得以缩短，并保证搜索到的路径是近优路径。

附图说明

图1为本发明实施例的路径规划方法流程示意图。

图2为本发明实施例的简单环境的仿真模型示意图。

图3为本发明实施例的椭圆子集采样概念示意图。

图4为本发明实施例的随机数生长过程示意图。

图5为本发明实施例的新父节点生成过程示意图。

图6为本发明实施例的场景1的仿真结果对比示意图。

图7为本发明实施例的场景2的仿真结果对比示意图。

具体实施方式

为了加深对本发明的理解，下面将结合附图和实施例对本发明做进一步详细描述，该实施例仅用于解释本发明，并不对本发明的保护范围构成限定。

实施例：如图1-图7所示，一种基于变概率约束采样的机器人路径规划方法，包括以下步骤：

步骤1：生成地图及障碍物信息，给定机器人的起始位置Q_s和目标位置Q_g；

具体的，设置机器人运动的地图大小为20单位×17单位的二维空间，机器人的起始位置Q_s，目标位置Q_g，最后再生成障碍物；如图2所示，图中阴影部分即为障碍物，机器人无法通过。

步骤2：将Q_s、Q_g分别作为两棵树的初始起点，相向扩展随机树，根据随机生成的概率p来控制采样方向；

具体的，设置一个范围在0-100之间的随机概率p，以及可变值ε，若p小于ε，则以目标点为采样点，否则进行椭圆约束采样，若周围判定有障碍物，则改变ε的值，使得采样方式更倾向于椭圆约束采样。

步骤3：当p大于ε时，进行椭圆约束采样；

具体的，椭圆约束采样如图3所示，起点Q_s和终点Q_g分别为椭圆的两个焦点，椭圆的共轭直径为d_i，其中d_i被定义为：

偏心率由cmin/cbest计算得到，在双向搜索树条件下，机器人从Q_s到Q_g的路径成本可以看作由两段组成，即从Q_s到x的成本，以及从x到Q_g的成本，使用欧几里得距离得到两个焦点的距离c_min.椭圆的横向直径为c_best，它代表当前所记录的最优路径，因此椭圆采样集可以如公式(1)这样定义：

使用欧几里得距离得到两个焦点的距离c_min.椭圆的横向直径为c_best，它代表当前所记录的最优路径椭圆约束采样通过将均匀分布的样本从一单位的n维超球面转变成n维椭圆子集x_ellipse-μ(X_ellipse)，从而在椭圆子集内完成均匀分布采样，即

x_ellipse＝Lx_ball+x_center， (3)

x_center＝(x_f1+x_f2)/2， (4)

x_ball＝{x∈X|||x||₂≤1}， (5)

其中x_center为椭圆的中心，x_f1，x_f2分别为椭圆的两个焦点，x_ball为随机采样点；

之后通过椭圆矩阵S的Cholesky分解来计算上述变换，S∈Rn×n，有

LL^T≡S， (6)

(x-x_center)^TS(x-x_centet)＝I， (7)

S的特征值为椭圆半径的平方{r_i ²}，r_i的值为

r₁＝c_best/2， (8)

采样点子集的变化可通过横截半径和长轴得到，其中diag{.}为对角矩阵，其中长轴的对角线矩阵和分解运算分别为

在取得采样集之后，需要将此时的椭圆转到世界坐标系下，可通过Kabsch算法求解，即

C＝Udiag{1,...1,det(U)det(V)}V_t (12)

本发明直接使用旋转矩阵C(θ)来计算，从而求得采样集并返回。

步骤4：在树中选取最近邻点Q_nest，根据最近邻点Q_nest以及障碍物的位置判断得到动态步长L，从而生成新节点，其中包括：

(1)确定动态步长L：

对于树一，在随机寻找一个采样点Q_r1后，搜索距Q_r1最近的点Q_nest1，接着以步长L向点Q_r1扩展，其运算为

对于树二，在新节点Q_new1生成后，搜索得到Q_new1距离树二最近的点Q_nest2，然后以步长L向点Q_new1扩展，其运算为

运算表达式中的步长L为动态步长，相较于静态步长，此处理使得机器人面对复杂路径时更加灵活，减少搜索路径失败的概率，加快了搜索近优路径的收敛时间。设计如下：

当两树之间的距离D_tree小于预设值s₁时，使用小步长L_s来扩展；当D_tree大于预设值s₁时，则表示两树之间距离较远，由机器人与障碍物环境决定步长；如果机器人与障碍物之间的距离D_obs大于预设值s₂时，认为机器人附近无杂物，采用大步长L_m；反之，当D_obs小于于预设值s₂时，认为机器人周围环境复杂，采用小步长Ls，其表达式为

(2)生成新节点：

Extend*函数如图4、5所示，Q_s、Q_g分别为路径的起点和终点，Q_r为空间中的随机采样点，Q_nest为与Q_r距离最近的节点，Q_new为在Q_nest到Q_r的延长线上延伸一个步长之后得到的点。函数首先在地图中随机寻找一个采样点Q_r，其次寻找距离采样点Q_r最近的点Q_nest，且该点为树中已存在的节点，判断最近邻点Q_nest到采样点Q_r的距离是否小于算法所设步长，如果小于就将Q_r设为新节点Q_new，如果超出步长大小，则将在最近邻点Q_nest路径的基础上向采样点Q_r延伸一个步长后所得到的点设为新节点Q_new。此时有邻近节点Q_near、潜在父节点Q_{potential-parent}以及真父节点Q_parent。以新节点Q_new为圆心一定半径作圆，该圆内所有的点构成一个新的集合。接着遍历构成的集合，找到所有邻近节点Q_near并逐一将其作为潜在父节点Q_{potential-parent}与新节点Q_new连接，同时计算起点Q_s、潜在父节点、新节点的路径长度并与原路径长度进行比较，从而筛选出最优的潜在父节点Q_{potential-parent}，并将其设为新的父节点Q_parent。最后判断新节点Q_new与最近邻点Q_nest之间的连线是否与障碍物碰撞，若判断为否，则此时将新路径添加进树并且删除原始的连接路径。

步骤5：判断最近邻点Q_nest与新节点Q_new之间的路径有无障碍物；

将近邻点Q_nest与新节点Q_new分别设为待检测路段的两个端点，T为障碍物的圆心，o为障碍物到待检测路段的垂直点，则有

再根据机器人的模型大小，即可判断机器人在行进过程中是否有碰撞。

步骤6：随机树生成新节点后，判断两树的最新节点是否小于阈值，若满足条件则路径已被找到，进行回溯处理；

具体的，通过双树搜索路径策略搜索到路径后，判断两树是否符合连接条件，此时仅通过判断新节点与另外一棵树的距离是否小于设定的常数值是有一定缺陷的，这会使得整条路径折线过多、连接处转角过大。因此，为了搜索近优路径，需要对以生成的较优路径进行回溯处理，遍历整个较优路径的节点，从起点开始依次寻找路径上能最大程度保证不碰撞障碍物的节点，将起点与之相连并记录这个点；再从该点开始，寻找它之后的无碰撞点，直到可以与终点无碰撞连接。将这些点和路径信息保存下来，通过对比路径信息搜索到新的较优路径，反复迭代，最终选择总距离最短的路径为算法最终搜索路径。

为了验证本文所提改进算法的性能优势，该实验场景1的仿真结果如图6所示，场景1的地图内从起点到终点有多条路径可供选择，从图中我们可以看出改进算法与Informed RRT*、Informed RRT*-Connect相比，搜索到的路径更加平滑，线条更简洁，主要原因是改进算法采用了回溯处理机制，减少了路径的搜索成本。从表1中我们不难看出改进算法的平均首次搜索到近优路径的时间约占Informed RRT*算法的70％，而对比InformedRRT*-Connect算法约占80％，由此可以看出采用双树进行搜索可以明显缩短路径收敛所需时间，但是三种算法搜索得出的最终近优路径均较为相似。下表中Apl表示平均路径长度，Afg表示平均首次生成近优路径时间，而Sn表示最短近优路径。

表1 3种不同算法在场景1中的实验结果对比

相较于场景1，场景2的地图设计更加复杂，机器人可以考虑进行选择的路径比较少，且障碍物干扰较强，目的是为了检验改进算法在复杂环境中的适应能力。该场景下的实验仿真结果如图7所示，场景1中改进算法所展现出来的性能在场景2中也呈现了出来，由此可以证明改进算法具有较高的适应能力。相较于场景1，场景2中首次搜索到的近优路径的平均时间消耗更少，改进算法的用时低于Informed RRT*算法用时的50％，与InformedRRT*-Connect算法耗时的差异更为明显，这显得目标偏向机制在复杂环境下的采样过程中尤为关键，而未改进的算法在寻找新节点时由于采样区域过大，容易导致搜索时间消耗增加。

表2 3种不同算法在场景2中的实验结果对比

作为一种实施方式，本发明引入了目标偏向的思想，对采样点起到约束作用，缩小采样区域，使采样点分布在能够到达目标点的路径周围，从而有效的提升搜索路径的效率和降低搜索路径所用的成本，同时解决了在复杂环境下，节点扩展方向不明确使得搜索效果不佳的问题。该改进算法还设置了概率偏向，防止搜索结果为局部最优。采用可变步长策略，提高搜索到路径的概率。最后对搜索到的路径进行回溯，减少路径转折点的数量，避免两树搜索连接处不规则的情况出现，进一步减少总路径长度，从而降低机器人的运行时间。

以上所述为本发明的示例性实施例，并非因此限制本发明专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (7)

1.一种基于变概率约束采样的机器人路径规划方法，其特征在于，包括以下步骤：

步骤1、生成地图及障碍物信息，给定机器人的起始位置Q_s和目标位置Q_g；

步骤2、将Q_s、Q_g分别作为两棵树的初始起点，相向扩展随机树，根据随机生成的概率p来控制采样方向；

步骤3、当p大于设定的可变值ε时，将采样节点从一单位的n维超球面转变成n维椭圆子集x_ellipse-μ(X_ellipse)，从而在椭圆子集内完成采样；

步骤4、在树中选取最近邻点Q_nest，根据最近邻点Q_nest以及障碍物的位置判断得到动态步长L，从而生成新节点；

步骤5、判断最近邻点Qnest与新节点Qnew之间的路径有无障碍物；

步骤6、随机树生成新节点后，判断两树的最新节点是否小于阈值，若满足条件则路径已被找到，进行回溯处理。

2.根据权利要求1所述的基于变概率约束采样的机器人路径规划方法，其特征在于，在所述步骤2中，设置一个范围在0-100之间的随机概率p以及可变值ε，若p小于ε，则以目标点为采样点，否则进行椭圆约束采样，若周围判定有障碍物，则改变ε的值，使得采样方式更倾向于椭圆约束采样。

3.根据权利要求2所述的基于变概率约束采样的机器人路径规划方法，其特征在于，在所述步骤3中，起点Q_s和终点Q_g分别为椭圆的两个焦点，椭圆的共轭直径为d_i，其中d_i被定义为：

使用欧几里得距离得到两个焦点的距离c_min.椭圆的横向直径为c_best，它代表当前所记录的最优路径椭圆约束采样通过将均匀分布的样本从一单位的n维超球面转变成n维椭圆子集x_ellipse-μ(X_ellipse)，从而在椭圆子集内完成均匀分布采样，即

x_ellipse＝Lx_ball+x_center，

x_center＝(x_f1+x_f2)/2，

x_ball＝{x∈X|||x||₂≤1}，

其中x_center为椭圆的中心，x_f1，x_f2分别为椭圆的两个焦点，x_ball为随机采样点；

之后通过椭圆矩阵S的Cholesky分解来计算上述变换，S∈Rn×n，有

LL^T≡S，

(x-x_center)^TS(x-x_centet)＝I，

S的特征值为椭圆半径的平方{r_i ²}，r_i的值为

r₁＝c_best/2，

采样点子集的变化可通过横截半径和长轴得到，其中diag{.}为对角矩阵，其中长轴的对角线矩阵和分解运算分别为

在取得采样集之后，使用旋转矩阵C(θ)来计算，从而求得采样集并返回：

4.根据权利要求3所述的基于变概率约束采样的机器人路径规划方法，其特征在于，在所述步骤4中，在树中选取最近邻点Q_nest，根据最近邻点Q_nest以及障碍物的位置判断得到动态步长L，从而生成新节点，其中包括确定动态步长L和生成新节点两个流程。

5.根据权利要求3所述的基于变概率约束采样的机器人路径规划方法，其特征在于，在所述步骤4中，在确定动态步长L的流程中：

对于树一，在随机寻找一个采样点Q_r1后，搜索距Q_r1最近的点Q_nest1，接着以步长L向点Q_r1扩展，其运算为

对于树二，在新节点Q_new1生成后，搜索得到Q_new1距离树二最近的点Q_nest2，然后以步长L向点Q_new1扩展，其运算为

运算表达式中的步长L为动态步长，设计如下：

当两树之间的距离D_tree小于预设值s₁时，使用小步长L_s来扩展，当D_tree大于预设值s₁时，则表示两树之间距离较远，由机器人与障碍物环境决定步长，如果机器人与障碍物之间的距离D_obs大于预设值s₂时，认为机器人附近无杂物，采用大步长L_m，反之，当D_obs小于于预设值s₂时，认为机器人周围环境复杂，采用小步长Ls，其表达式为

6.根据权利要求5所述的基于变概率约束采样的机器人路径规划方法，其特征在于，在所述步骤5中，将近邻点Q_nest与新节点Q_new分别设为待检测路段的两个端点，T为障碍物的圆心，o为障碍物到待检测路段的垂直点，则有

再根据机器人的模型大小，即可判断机器人在行进过程中是否有碰撞。

7.根据权利要求6所述的基于变概率约束采样的机器人路径规划方法，其特征在于，所述步骤6的具体流程为；为了搜索近优路径，需要对以生成的较优路径进行回溯处理，遍历整个较优路径的节点，从起点开始依次寻找路径上能最大程度保证不碰撞障碍物的节点，将起点与之相连并记录这个点，再从该点开始，寻找它之后的无碰撞点，直到可以与终点无碰撞连接，将这些点和路径信息保存下来，通过对比路径信息搜索到新的较优路径，反复迭代，最终选择总距离最短的路径为算法最终搜索路径。

Images