一种机器人的行走路径规划方法

1.本发明属于路径规划技术领域，具体涉及一种机器人的行走路径规划方法。


背景技术：

2.移动机器人在如今人工智能发展越来越主流的时代作为机器人学习的重要对象，在自动驾驶、智能工厂等领域越来越受欢迎，而移动机器人的路径规划作为移动机器人的重要研究方向之一，成为多位专家学者探讨的主题。
3.常用的移动机器人的路径规划方法为基于采样的规划方法。
4.基于采样的搜索算法中应用最广泛的是快速探索随机树方法。基于节点采样的快速探索随机树方法具有对障碍物建模简单，既能用于完整系统机器人机械臂路径规划，也可以用于非完整系统的轮式移动机器人或者无人机路径规划等优点，它的最大优点是不需要对空间进行预处理并且具有概率完备性。
5.然而现有的的基于采样的规划方法在生成路径的时候，是通过不断的随机选择环境地图中的节点，之后根据随机选择的节点在环境地图上确定路径节点，将路径节点连接起来生成路径的。其在节点的选择上面具有盲目性，不能很好的得到距离短又能绕开障碍物的最佳路径。


技术实现要素：

6.为了克服上述现有技术存在的不足，本发明提供了一种机器人的行走路径规划方法。
7.为了实现上述目的，本发明提供如下技术方案：
8.一种机器人的行走路径规划方法，包括：
9.获取环境地图；
10.在环境地图中确定机器人运动的起始点x
init
和目标点x
goal
；
11.在环境地图中随意设置多个点；
12.对环境地图中的点进行随机采样，检测新采样到的节点a在目标点方向与节点a的最近障碍物之间的距离d；若距离d大于或者等于阈值step，则把目标点当成采样点x
rand
；若距离d小于阈值step，将节点a作为采样点x
rand
；
13.获取采样点x
rand
后，寻找出距离采样点x
rand
最近的点，将其定义为节点x
near
；
14.在节点x
near
与节点x
rand
的连线上，距离节点x
near
一个阈值step处设置新的节点x
new
作为新的路径节点；
15.不断生成新的路径节点x
new
，直到路径节点之间的连线构成机器人从起始点x
init
到目标点x
goal
的路径；
16.机器人沿着生成的路径从起始点x
init
运动到目标点x
goal
。
17.进一步，获取采样点x
rand
的表达式为：
[0018][0019]
进一步，还包括：设置阈值step，其包括：
[0020][0021]
其中，s为初始步长，k为控制系数，n1为有效采样次数，n为总采样次数，每采样3次重新计数。
[0022]
进一步，还包括：对从起始点x
init
到目标点x
goal
的路径进行剪枝和平滑处理，其包括：
[0023]
如果路径中有两个节点满足直接连接且同时满足移动机器人的转角和无碰撞的条件，剔除两个节点中的所有节点，并将两个节点相连；
[0024]
将路径中节点的连线替换为贝塞尔曲线。
[0025]
进一步，所述贝塞尔曲线为：
[0026]
贝塞尔曲线为：
[0027][0028]
式中，i表示样条曲线的序号，i＝1,2,3...n，n表示样条曲线为n次的基函数参数曲线，p
i+k
表示第i段的贝塞尔曲线的第k个控制点，f
k,n
(u)为n次贝塞尔曲线的基函数，u表示贝塞尔曲线的参数，0≤u≤1；
[0029][0030]
式中，n值表示贝塞尔曲线的平滑度。
[0031]
进一步，选择三阶贝塞尔曲线对从起始点x
init
到目标点x
goal
的路径进行平滑处理，包括：
[0032][0033]
进一步，所述机器人在环境地图中运动时的位置坐标和偏航角公式为：
[0034][0035]
其中，x0、y0、θ0分别为初始的x坐标、y坐标、偏航角，v为机器人的线速度，w为机器人运行时的角速度，t为时间。
[0036]
本发明提供的一种机器人的行走路径规划方法具有以下有益效果：
[0037]
本发明在规划机器人的运动路径时，充分考虑到目标点方向以及障碍物的影响，
当选取的节点a在目标点方向与节点a的最近障碍物的距离d大于或者等于阈值step时，将目标点作为采样点x
rand
而不是随机选取，当遇到障碍物时，随机选取采样点x
rand
绕过障碍物，本发明能够生成距离短又能绕开障碍物的最优路径。解决了现有技术中，在节点x
rand
的选择上面具有盲目性，不能很好的得到距离短又能绕开障碍物的最佳路径的问题。
附图说明
[0038]
为了更清楚地说明本发明实施例及其设计方案，下面将对本实施例所需的附图作简单地介绍。下面描述中的附图仅仅是本发明的部分实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0039]
图1为本发明的一种机器人的行走路径规划方法示意图；
[0040]
图2为本发明实施例的rrt算法示意图；
[0041]
图3为本发明实施例的目标偏置采样示意图；
[0042]
图4为本发明实施例的改进算法对比图；
[0043]
图5为本发明实施例的路径剪枝示意图；
[0044]
图6为本发明实施例的无障碍物环境下的算法效果对比图，其中，图6(a)为rrt算法路径图、图6(b)为rrt-connect算法路径图、图6(c)为rrt*算法路径图、图6(d)为本发明算法路径图；
[0045]
图7为本发明实施例的简单障碍物环境下的算法效果对比图，其中，图7(a)为rrt算法路径图、图7(b)为rrt-connect算法路径图、图7(c)为rrt*算法路径图、图7(d)为本发明算法路径图；
[0046]
图8为本发明实施例的狭窄通道环境下的算法效果对比图，其中，图8(a)为rrt算法路径图、图8(b)为rrt-connect算法路径图、图8(c)为rrt*算法路径图、图8(d)为本发明算法路径图；
[0047]
图9为本发明实施例的狭窄出入口环境下的算法效果对比图，其中，图9(a)为rrt算法路径图、图9(b)为rrt-connect算法路径图、图9(c)为rrt*算法路径图、图9(d)为本发明算法路径图。
具体实施方式
[0048]
为了使本领域技术人员更好的理解本发明的技术方案并能予以实施，下面结合附图和具体实施例对本发明进行详细说明。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0049]
实施例：
[0050]
本发明提供了一种机器人的行走路径规划方法，具体如图1-5所示，包括：
[0051]
获取环境地图。
[0052]
在环境地图中确定机器人运动的起始点x
init
和目标点x
goal
。
[0053]
在环境地图中随意设置多个点。
[0054]
对环境地图中的点进行随机采样，检测新采样到的节点a在目标点方向与节点a的最近障碍物之间的距离d；若距离d大于或者等于阈值step，则把目标点当成采样点x
rand
；若距离d小于阈值step，将节点a作为采样点x
rand
。
[0055]
获取采样点x
rand
后，寻找出距离采样点x
rand
最近的点，将其定义为节点x
near
。
[0056]
在节点x
near
与节点x
rand
的连线上，距离节点x
near
一个阈值step处设置新的节点x
new
作为新的路径节点。
[0057]
不断生成新的路径节点x
new
，直到路径节点之间的连线构成机器人从起始点x
init
到目标点x
goal
的路径。
[0058]
机器人沿着生成的路径从起始点x
init
运动到目标点x
goal
。
[0059]
以下为本发明详细说明：
[0060]
1、基于自适应步长策略的rrt算法
[0061]
1.1rrt算法
[0062]
移动机器人在实现路径规划移动的过程中，需要对地图
[18]
进行基本判断选择。为了对环境的信息进行识别和处理，首先对地图模型进行选择，常见的地图模型有栅格地图模型、特征地图模型和拓扑地图模型等。
[0063]
表1地图模型对比
[0064][0065]
本文为了能够表示地图信息、障碍物信息以及路径规划情况的随机性，将移动机器人所处地图作为算法研究的基础。
[0066]
在全局路径规划中使用确定起始点和目标点，构建随机地图大小和静态障碍物规则与不规则共存的环境，证实算法的普遍性和科学性，采用拓扑地图，使用点线结构来表示
地图，地图直观形象，充分利用发挥基于采样点的路径规划算法的优势。
[0067]
传统rrt(rapidly-exploring random tree，rrt)算法
[19]
在得到场景的地图后，可以根据起始位姿和目标位姿构造搜索随机树。
[0068]
rrt算法示意图如图2所示，以起始点作为随机树的根节点，通过随机采样，将随机树的扩展引导到自由区域进行随意撒点直到搜索到目标点，最终形成一条路径扩展的随机树，从而生成一条从起始点到目标点的全局规划路径。
[0069]
基本步骤如下：
[0070]
(1)移动机器人在二维状态空间的起点x
init
作为根节点构造随机树；
[0071]
(2)自由搜索空间内产生随机采样点x
rand
，用于引导随机树的扩展；
[0072]
(3)遍历整个随机树上已经产生的节点，寻找选出距离随机点x
rand
最近的树节点，将其定义为x
near
；
[0073]
(4)从x
near
节点沿着x
rand
节点的伸展的方向作为扩展方向，扩展适当步长，设置一个适当步长作为树枝长度，生成新的节点x
new
作为新的树节点。
[0074]
(5)如果扩展过程中遇到障碍物则取消本次扩展，并进行再次采样。路径不断重复上述迭代过程，直到目标节点超过规定的迭代次数时搜索结束，最终形成快速扩展随机树路径，rrt算法基本伪代码如下所示：
[0075][0076]
但是，快速探索随机树算法在规划路径时还存在几个缺点：搜索时具有盲目性、随机性和无方向性；节点利用率低，在搜索的过程中会产生许多冗余节点；收敛速度慢和搜索效率低，得到的路径只是可行而不是最优等缺点。
[0077]
面对算法中出现的问题，很多专家学者都致力于改进rrt算法的缺陷。针对收敛速度慢的问题，laval le和kuffner提出了rrt-connect算法，该算法通过贪婪搜索连接两棵随机树达到减少采样节点的数目的目的，最终实现收敛速度提高。针对rrt算法得到的路径只是可行而不是最优的问题，karama和frazzol i提出了rrt*算法，算法中实现了路径代价信息和重布线操作，实现的路径具备渐进最优性，在时间条件允许下，能够收敛到最优路径，缺点就是收敛时间也会随之增加，快速性差。
[0078]
在研究过程中除了上述方法之外还有很多rrt系列算法，例如kdt-rrt算法、pg-rrt算法和dgabi-rrt算法等方法。
[0079]
为了提高轨迹的质量，chang-bae moon等提出了kdt-rrt算法，即动态双树快速探索随机树算法，算法修改生成树结构的重连接树而不需要对传统rrt结构重新传播，利用树结构的不同实现灵活的节点扩展，最终实现路径质量的提升。
[0080]
针对收敛速度慢的问题，sharma paras等提出一种智能概率高斯混合模型驱动的
算法，该算法称为pg-rrt算法，在模型里算法于最可能的区域生成节点，实现更快的收敛。为了获得更好的路径，尽可能减少路径代价和获得平滑轨迹，li等提出dgabi-rrt算法，算法针对随机性采用更优的采样点策略和重力自适应步长方法，使移动机器人的运动具有目标导向性和避障功能，路径优化策略缩短平滑路径，使移动机器人能够在工作空间中平稳移动，本文受到采样点改进方法和自适应步长策略的启发，实现了一种更为直接高效的方法实现轨迹优化，计算有效点占比最大实现冗余点剔除最优化，根据环境自适应调整探索扩展长度，并不依赖本身质量调整扩展距离。
[0081]
同时还有很多rrt系列算法与其他算法融合后形成新算法实现优化。冯来春等针对采样效率低，速度慢等问题，利用a*算法和rrt算法相结合的方法，发挥a*算法在栅格图中生成的最短路径来构建引导域提升rrt算法的采样效率。
[0082]
不仅是rrt算法上，在改进rrt*算法方面，臧强等通过改进相对距离势场法，提出自适应人工势场法与rrt*算法进行融合，显著提高了路径效率和避障的灵活性。
[0083]
2、目标偏置采样法
[0084]
传统rrt算法是一种随机算法，算法在采样的过程中会有大量的冗余节点产生。为了减少rrt算法在搜索过程中的盲目性，解决采样点选择过多的问题，rrt算法通过对整个地图进行随机采样来控制随机树的生长，但由于其采样的随机性，大量的采样节点的产生导致算法的收敛速度慢，所以采样点的筛选要尽可能让随机树向目标点方向生长，从而提高搜索速度。
[0085]
在rrt算法节点的扩展过程中，x
new
是由x
near
向x
rand
扩展过程中得到的，表达式可以表示为：
[0086][0087]
式中，step为扩展步长，d(x
rand
,x
near
)为x
rand
与x
near
状态节点之间的欧式距离。
[0088]
由此可以看出每一次节点的扩展生成过程都伴随着大量的采样。
[0089]
因此本文为了改进状态采样点提出了一种贪婪目标偏置采样方法，x
rand
的选择减少了盲目性，如图3所示。
[0090]
算法在每一次采样的过程中都会产生新节点，在每一次迭代的过程中检测新节点在目标点方向与最近障碍物的距离d，若距离d大于或者等于生长步长，则把目标点当成下一次采样的随机点；若d小于生长步长，则在地图内随机取样，随机树随机生长，以最快速的方式绕过障碍物。
[0091]
如图4情形为距离d小于生长步长的情况，需要进行随机采样，生成采样点x
rand
。目标偏置采样改进公式如下：
[0092][0093]
3、自适应步长策略
[0094]
rrt算法采用固定步长策略均匀采样，固定的采样方式会使得新节点在生成和扩展的过程中，遇到障碍物较多的情况时不能完全遍历环境，障碍物较少的情况时有多余节点采样时间，不能够快速的遍历地图，算法随机性大，收敛速度慢。
[0095]
因此可以改变随机树步长来适应各种障碍物环境，根据具体情况障碍物的多少以及复杂程度生成自适应步长。
[0096]
本文采用一种自适应步长策略，利用算法收集到的信息，实时调整步长的大小，快速通过复杂狭窄区域。
[0097]
自适应步长选择公式如下：
[0098][0099]
s为初始步长，k为控制系数，n1为有效采样次数，n为总采样次数，为了能够实时反映当前环境的复杂性，采用采样遗忘式计数方案，每采样3次重新计数。当有效采样的次数n1越多，即障碍物环境越简单，附近障碍物越少，生成的步长取值就越大；反之，步长取值会较小。
[0100]
在简单无障碍环境下，设置少量圆形障碍物环境，固定步长和改进后动态步长的搜索产生的路径对比如图4所示。
[0101]
从图4可以看出加入自适应步长后，调整扩展路径生成步长长短，(b)图生成的路径明显优于在图4(a)改进前的路径，减少了路径节点多余的选择，虽然轨迹转折不够平滑，但是在避障的同时可以规划出一条低代价、少折点的路径。
[0102]
从仿真结果可以看出，通过有效节点集中处理可以大大提高路径规划质量，减少收敛时间，遍历实时障碍物环境探索生成合理轨迹。
[0103]
4、路径剪枝优化与平滑处理
[0104]
改进rrt算法可以探索到移动机器人的避障路径，但一条无碰撞可实现的路径仍然受限于多个转折点，为了减少移动过程中的卡顿感，保证移动机器人行驶过程中的安全，需要对搜索到的路径进行剪枝和平滑处理，得到移动机器人可跟踪的平滑路径。
[0105]
改进rrt算法初始采样到的路径集合点为x{x1,x2,x3,x4...xn}，如图5所示，如果采样点x1与节点x4之间能够直接连接且同时可以满足移动机器人的转角和无碰撞的约束条件，则将x1和x4之间的所有节点即x2和x3称为冗余节点，即可以将两点从路径采样节点集合中剔除。每一次迭代对得到的全局路径进行剪枝处理[24]，遍历整个路径采样点集合，将有效采样点直接连接，可以得到一条路径长度更短、与目标点方向连接更明确的轨迹。
[0106]
经过剪枝处理后得到路径仍是由若干条路径线段连接而成，并不是平滑的路径轨迹，不能够满足车体对平滑移动和没有急停等安全性要求，所以生成后的路径仍然需要进行平滑处理。
[0107]
b样条曲线是样条曲线中的一种灵活且常见的曲线，也称为贝塞尔曲线，是jacob schoenberg创造出的一种由形状数学进行描述曲线的方法。样条曲线的转折点在一定范围内受到局部顶点的控制，所以需要选择合适的控制顶点。
[0108]
b样条曲线的函数公式如下所示：
[0109][0110]
式(15)中，i表示样条曲线的序号，i＝1,2,3...n，n表示样条曲线为n次的基函数参数曲线，p
i+k
表示第i段的贝塞尔曲线的第k个控制点，f
k,n
(u)为n次贝塞尔曲线的基函数，
u(0≤u≤1)表示贝塞尔曲线的参数，其表达式如下：
[0111][0112]
式中，n值表示贝塞尔曲线的平滑度，n值越大表示平滑程度越高，但是计算的复杂度也会随之提高。为了兼顾计算的复杂性和路径的平滑需求，本文选择三次b样条曲线进行平滑处理，三阶贝塞尔曲线可以满足本文所需的位姿需求和路径平滑需求，车体的三次b样条曲线基函数f
k,n
(u)将n＝3带入上式可得：
[0113][0114]
5、移动机器人运动学建模：
[0115]
rrt算法有且仅可以寻找一条能够连接起点和终点路径，但是规划出来的路径曲折且不符合移动机器人车体运动学约束，从而车辆无法完成跟随规划的全局路径行驶。机器人运动平台虽然是四轮运动底盘，但是其结构方式是在两轮差分式底盘的基础上增加固定轮，车体两侧为了能够爬坡等经过凹凸地形安装了链条，实现了两个电机带动四个轮子进行移动，最终使用两轮差分底盘运动学模型对其进行分析建模。
[0116]
在移动机器人差速模型中，机器人以半径r作圆周运动，2d为左右两轮之间的距离，v
l
、vr表示左右轮的线速度，w
l
、wr表示左右轮的角速度，v表示移动机器人在运行时的线速度，w表示移动机器人运行时的角速度，θ表示运行时的旋转角度。移动机器人底盘是刚性连接的方式，左轮和右轮分别以半径r-d和r+d的方式绕同一圆心作圆周运动，可得动力学方程：
[0117][0118]
w＝w
l
＝wrꢀꢀ
(2)
[0119]vl
(r+d)＝vr(r-d)
ꢀꢀ
(3)
[0120]
(v
r-v
l
)r＝(vr+v
l
)d
ꢀꢀ
(4)
[0121][0122][0123]
将式(6)带入角速度公式可得：
[0124][0125]
将式(5)和式(7)带入线速度公式可得：
[0126][0127]
旋转半径r可以表示为：
[0128][0129]
移动机器人以初始时刻的坐标系作为世界坐标系，机器人的位姿从坐标系原点开始计算，推算过程为：通过电机上里程计得到左轮速度和右轮速度分别为v
l
和vr，带入式(8)中求得线速度，在短时间δt内，移动机器人可以默认为做匀速直线运动，机器人位姿变化量如式(10)所示：
[0130][0131]
由上述公式可以得到移动机器人的位置坐标和偏航角如式(11)所示：
[0132][0133]
6、仿真算法实验对比分析
[0134]
使用matlab对本文所提出的改进rrt算法进行仿真实验，测试主机为联想lenovor9000p，处理器为amd r7-5800h，主频3.0ghz，内存大小为32gb。
[0135]
为了验证本文改进rrt算法可以达到更好的性能指标，将rrt算法、rrt-connect算法、rrt*算法和改进rrt算法在无障碍环境、简单障碍物环境、狭窄通道障碍物环境和狭窄起始点环境4种二维地图环境下进行全局路径规划对比试验，4种仿真地图大小为(1097，1059)，从一般障碍物环境到本文研究的复杂狭窄环境，逐步验证算法的合理性。在4种环境下每种方法均进行50次重复实验，并统计算法各个指标的平均值。4种地图环境下各算法的规划结果如图6-8所示，其中(a)图为传统rrt算法规划效果，(b)图为rrt-connect算法规划效果，(c)图为rrt*算法的路径规划效果，(d)图为改进rrt算法路径优化后的规划效果。
[0136]
图6为无障碍物环境的算法规划对比图，环境中设置了起始点和目标点的位置分别为(100，100)和(950，950)，没有其他障碍物干扰，环境相对简单。图(a)为基本rrt算法规划的一条可行路径，由于rrt算法的搜索盲目的特性，生长树会随机分布在无障碍区域，所以算法效率较低并且得到路径质量很差，即使是无障碍环境下也会在盲目搜索。图(b)为rrt-connect算法规划的一条可行路径，明显看出rrt-connect算法采取起始点目标点同时采样的方式大量减少了采样点的选取，但是仍然存在冗余点的选择颇多以及路径代价高等问题。图(c)为rrt*算法规划的一条可行路径，rrt*算法在rrt算法基础上做了改进处理，具有渐进最优的特性，所以rrt*能够得到较优的路径长度，减少部分路径代价，但是仍然需要大量反复迭代，节点利用率和搜索效率较差，存在盲目性以及迭代时间会相对较长。图(d)是本文改进rrt算法在进行路径规划后实现的一条路径，可以看出路径剔除了多余采样点在极短时间内无障碍环境下实现了最优路径的选择，效果明显优于之前路径规划的方法。
[0137]
从表2的实验数据中可以得出，无障碍物地图简单，改进后的算法优势明显，剔除了无用节点的扩展采样，加快了搜索速度，在无障碍的环境下可以准确实现起始点和目标点的快速寻找扩展，实现最优路径选择。结果相比于rrt算法、rrt-connect算法和rrt*算法，扩展节点数目分别减少了96.42％、70.25％和96.40％，搜索时间分别减少了96.74％、39.79％和84.81％。在经过节点剪枝和路径平滑优化处理后，改进rrt算法的路径长度较优，其路径长度相比于rrt算法的路径长度缩短了23.29.％，相比于rrt-connect算法缩短了17.67％，相比于rrt*算法缩短了13.90％。由仿真结果可以看出改进后算法效果明显优于其它采样算法。
[0138]
表2无障碍物环境下实验结果对比
[0139][0140]
图7为简单障碍物下地图的规划效果，设置起始点和目标点位置为(100，100)和(950，950)。和图8无障碍环境不同的是，增加了障碍物的介入，验证算法在有基本障碍物环境下的效果。环境中存在大小不一、形状不同、相对规则的障碍物，不同算法分别在同一张地图中进行静态轨迹规划。图(a)中为rrt算法规划出的轨迹路径，可以发现随机树的扩展路径有相对杂乱曲折，有多余节点冗余，多节点轨迹车体无法跟随。图(b)中rrt-connect算法能够从起始点和目标点分别进行采样大大减少采样点的选择，但是仍然存在路径曲折无法跟随以及会产生多冗余节点等问题。图(c)的规划效果优于rrt算法和rrt-connect算法，虽然路径规划迭代时间相对增长但是路径代价减少且路径相对合理。图(d)为改进rrt算法规划出的路径效果，路径相对平滑并且在路径代价长度、节点数目和规划时间上都有明显优化和改善。
[0141]
表3为简单障碍物环境下的实验结果，由实验结果可知，本文改进的rrt算法在简单障碍物环境下路径依然有较好的规划效果。相比于rrt算法，rrt-connect算法和rrt*算法在规划时间上分别缩短了97.77％、89.60％和97.33％，快速性显著提升；在采样节点数目上分别减少了98.87％、80.75％和96.90％，明显提高了采样效率；在路径长度上分别减少了29.97％、23.40％和25.49％，路径代价减少，降低成本，优化了初始生成的路径。因此本文的改进rrt算法该环境下能够保证路径长度达到相对较小的同时提高算法的规划效率，路径在有障碍物的环境下的同样具有平滑性。
[0142]
表3简单障碍物环境下实验结果对比
[0143]
[0144]
图8为狭窄通道环境下的仿真效果，设置起始点和目标点的位置为(200，200)和(900，900)。和图9简单障碍物环境不同的是，图8并没有设置多余障碍物，而是在一条单一的狭窄通道里进行路径规划，主要为了观察自适应步长策略在狭窄通道环境下是否会陷入局部最小值和在单一空间里的寻优效率。从4种算法的规划效果对比来看，rrt算法和rrt-connect算法在狭小空间容易陷入局部困境，无法快速搜索出口，路径寻迹采样时间偏长，多余路径较多。rrt*算法在狭窄复杂或者多障碍物环境下适当增加迭代次数可以调整路径效果，(c)图中由于1000次的迭代无法满足路径规划的需求，选择增加迭代的次数为4000次，路径长度少于rrt算法和rrt-connect算法，但是规划迭代时间较长且同样存在路径无法跟踪的问题。而本文的改进rrt算法利用贪心算法剔除冗余节点，采用目标偏置采样尽可能减少采样点的选择，提高规划效率，自适应步长提高路径规划收敛速度，具有明显的优化效果，在狭窄通道环境内快速得到一条安全无碰撞的可行全局路径。
[0145]
表4为狭窄通道环境下的实验数据，从数据中可以得出，改进rrt算法的规划时间为34.13s，比rrt算法缩短了76.05％，比rrt-connect算法缩短了55.09％，比rrt*算法缩短了59.24％，实现了更好的路径且有着更高的搜索效率；节点数目为5880个，比rrt算法减少了2.10％，比rrt-connect算法减少了24.34％，实现了的多余采样点的剔除效果；在路径长度上比rrt算法减少23.61％，比rrt-connect算法长度上减少了19.66％，比rrt*算法减少了6.68％，如图8所示，路径平滑但是路径代价较小，算法实现了很好的路径规划优化工作。
[0146]
表4狭窄通道环境下的实验结果对比
[0147][0148]
图9为在复杂狭窄出入口障碍物环境[26]下的仿真效果，设置起始点和目标点为(210，340)和(890，700)。地图不同于前三种情况，图9环境主要增加了始末位姿点附近的封闭性，出入口较小，路径采样搜索的困难提高。地图模拟现实中相对狭小的始末位置，例如在楼道走廊里狭小电梯口环境，目的证实改进后算法的现实一般性和适用性。rrt算法在狭窄通道环境下路径采样点选择迭代时间较长，有大量的多余采样点，生成的路径曲折，路径代价过大。rrt-connect算法相对rrt算法减少了采样点，缩短了路径规划时间，相对顺利通过狭窄环境但规划出的路径依然欠佳。rrt*算法在进行1000次迭代采样后生成路径轨迹概率较低，迭代采样次数增加到与图8一样的4000次，规划路径迭代时间增加，快速性下降，路径代价减少，路径质量提高。经过比较发现，本文改进的rrt算法能够快速搜索到一条平滑路径，路径选择合理并且路径代价少于其他算法，较短时间内避开障碍物环境，通过狭小通道，完成最优全局路径选择。
[0149]
表5为狭窄环境下的实验结果数据对比。从表中可以观察到，改进rrt算法的扩展节点数在对整个地图遍历的情况下略高于rrt*算法，比rrt算法减少了3.53％，比rrt-connect算法减少了47.97％，采样点数目减少，但是实现对更大空间的遍历搜索效果；在规划时间方面，比rrt算法缩短了42.98％，比rrt-connect算法缩短了33.53％，比rrt*算法缩
短了49.88％，改进后算法在时间上的效果证实了算法弥补了rrt在快速性上的不足，提高了快速性的需求；最后在路径长度上明显短于rrt和rrt-connect算法，稍短于rrt*算法，并且在图9中狭小始末点环境下规划出一条平滑可跟踪无碰撞的路径，路径仿真结果如图9(d)所示。
[0150]
表5狭窄通道环境下实验结果对比
[0151][0152]
由实验的路径规划图和仿真数据可以看出，本文的改进rrt算法无论是在路径规划时间、采样节点的扩展个数还是路径代价上都会相比rrt算法、rrt-connect算法和rrt*算法上都具有明显优势。改进rrt算法对路径进行剪枝和平滑处理后得到的路径平滑且不会受到障碍物环境约束，没有碰撞和转角限制，保障了移动机器人在移动过程中的安全性和平稳性。因此，本文提出的改进rrt算法在4种不同的障碍物环境下，具有路径探索效率更高，路径长度更短，轨迹光滑且安全等优点。
[0153]
7、总结：
[0154]
本文针对传统rrt算法在移动机器人全局路径规划领域采样冗余节点多，收敛速度慢，搜索效率低和路径转折不平滑等问题，提出了一种基于自适应步长调整的改进rrt算法。算法首先对环境初始化，确定始末位姿信息，引入目标偏置采样方法，生成距离目标点更接近的随机采样点，向目标方向扩展探索生成新节点；其次针对传统rrt算法里固定步长无法遍历全局的缺陷，采用自适应步长策略，根据障碍物环境调整树的扩展长度，相较于rrt算法，rrt-connect算法和rrt*算法分别缩短了42.98％、33.53％和49.88％的规划时间，路径更贴合障碍物环境，路径代价更少；对机器人移动过程中转角和碰撞约束检测，扩展生成初始路径；最后对初始路径采取剪枝处理和b样条采样的方法平滑转折路径，简化处理初始路径，剔除多余节点，最终规划出的路径更加光滑安全。将算法移植到移动机器人上在楼道环境里完成了实物验证工作，证实了算法可行性。通过实验数据的对比可以发现，本文的改进rrt算法4种环境下探索生成的路径在搜索时间、采样成本和路径代价上都有明显改善。在日常生活中，改进后算法可以在更短时间内完成在障碍物复杂、狭窄通道环境以及始末位置环境狭小等背景下的路径规划工作，更多保留原算法中优势部分，也为环境中出现动态障碍物融合局部避障算法工作打下基础，对实际生活中移动机器人全局路径规划具有现实意义。
[0155]
以上所述实施例仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换，均属于本发明的保护范围。

技术特征：
1.一种机器人的行走路径规划方法，其特征在于，包括：获取环境地图；在环境地图中确定机器人运动的起始点x
init
和目标点x
goal
；在环境地图中随意设置多个点；对环境地图中的点进行随机采样，检测新采样到的节点a在目标点方向与节点a的最近障碍物之间的距离d；若距离d大于或者等于阈值step，则把目标点当成采样点x
rand
；若距离d小于阈值step，将节点a作为采样点x
rand
；获取采样点x
rand
后，寻找出距离采样点x
rand
最近的点，将其定义为节点x
near
；在节点x
near
与节点x
rand
的连线上，距离节点x
near
一个阈值step处设置新的节点x
new
作为新的路径节点；不断生成新的路径节点x
new
，直到路径节点之间的连线构成机器人从起始点x
init
到目标点x
goal
的路径；机器人沿着生成的路径从起始点x
init
运动到目标点x
goal
。2.根据权利要求1所述的一种机器人的行走路径规划方法，其特征在于，获取采样点x
rand
的表达式为：3.根据权利要求1所述的一种机器人的行走路径规划方法，其特征在于，还包括：设置阈值step，其包括：其中，s为初始步长，k为控制系数，n1为有效采样次数，n为总采样次数，每采样3次重新计数。4.根据权利要求1所述的一种机器人的行走路径规划方法，其特征在于，还包括：对从起始点x
init
到目标点x
goal
的路径进行剪枝和平滑处理，其包括：如果路径中有两个节点满足直接连接且同时满足移动机器人的转角和无碰撞的条件，剔除两个节点中的所有节点，并将两个节点相连；将路径中节点的连线替换为贝塞尔曲线。5.根据权利要求4所述的一种机器人的行走路径规划方法，其特征在于，所述贝塞尔曲线为：贝塞尔曲线为：式中，i表示样条曲线的序号，i＝1,2,3...n，n表示样条曲线为n次的基函数参数曲线，p
i+k
表示第i段的贝塞尔曲线的第k个控制点，f
k,n
(u)为n次贝塞尔曲线的基函数，u表示贝塞尔曲线的参数，0≤u≤1；式中，n值表示贝塞尔曲线的平滑度。
6.根据权利要求5所述的一种机器人的行走路径规划方法，其特征在于，选择三阶贝塞尔曲线对从起始点x
init
到目标点x
goal
的路径进行平滑处理，包括：7.根据权利要求1所述的一种机器人的行走路径规划方法，其特征在于，所述机器人在环境地图中运动时的位置坐标和偏航角公式为：其中，x0、y0、θ0分别为初始的x坐标、y坐标、偏航角，v为机器人的线速度，w为机器人运行时的角速度，t为时间。

技术总结
本发明提供了一种机器人的行走路径规划方法，属于路径规划技术领域，包括：在环境地图中确定机器人运动的起始点X


技术研发人员：王丽娜 杨鑫 陈泽领 汪小峰 王斌锐
受保护的技术使用者：中国计量大学
技术研发日：2023.08.07
技术公布日：2023/10/15