摘要
机器人已被广泛应用于日常生活之中。路径规划作为机器人的主要技术之一,优秀的路径规划算法能提升机器人的工作效率、降低其使用成本,并为研究机器人的导航打下良好的基础。RRT(rapidly-exploring random trees)算法具有扩展性强的优点,但存在路径长度并非最优、光滑性差等不足,为此提出反向寻优和三次样条曲线插值以改进算法,并在MATLAB和ROS(robot operating system)系统中仿真。结果表明:改进后的RRT算法能降低路径长度,减少节点数目,提高光滑性,实现了算法的有效性。
进入21世纪后,随着前沿科学技术的发展,机器人技术被不少国家列为重点发展的一项高新技术,且相关产业被用作衡量综合国力强弱的标
快速扩展随机树(rapidly-exploring random trees,RRT)算法作为一种基于采样的路径规划算法,地图不需要做预处理,且可以迅速搜索。但是,RRT也有不足之处,譬如,搜索时较为盲目,在复杂或者动态环境里往往会出现计算极其复杂、所需的时间过长、易于陷入死区等情况。针对以上问题,康亮
以上对RRT算法的改进,使搜索能力进一步增强,搜索具有目标性,路径长度缩短或节点数目减少,但路径光滑性依旧很差,路径长度还是很长且不是最优,算法有待进一步改进。因此,笔者提出反向寻优和三次样条曲线插值改进算法,在MATLAB中仿真验证并进行实例分析,并在ROS系统中以餐厅为例做进一步仿真分析,验证了改进算法的有效性。
LaVall

图1 RRT算法扩展树构造过程
Fig. 1 Construction process of RRT algorithm extension tree
算法伪代码如
步骤 | 伪代码 |
---|---|
1 | |
2 | |
3 | |
4 | |
5 | |
6 | |
7 | |
8 | |
9 | |
10 | |
11 |
1)稳定性差。由于扩展树节点的生长方向是随机的,所以在相同的任务下重复规划,也会出现不同的路径,导致每次规划的结果都不同,甚至偏离最优路径。
2)随机性强。因为扩展树是在整个区域内随机搜索,不受目标点的引导和启发,因此,存在大量无效的扩展节点,使算法具有很强的随机性。
3)光滑性差。RRT算法生成的路径是很多折线段相连,并不是一条光滑的曲线。
4)不适合动态环境。RRT算法规划出一条可行路径后,当环境中出现动态障碍物时,RRT算法不能有效地检测,因此,不适合动态环境下的路径规划问
机器人的路径若不是最优,就会增加工作时间,降低运作效率。为了改善RRT算法的路径长度,通过反向寻优的方法来改进RRT算法,如

图2 反向寻优示意图
Fig. 2 Reverse optimization schematic
改进后的算法伪代码如
步骤 | 伪代码 |
---|---|
1 | procedure Improved RRT |
2 | |
3 | for do |
4 | |
5 | if: obstacle then |
6 | Yes: return AB |
7 | No: find |
8 | |
9 | if: obstacle then |
10 | Yes: return |
11 | No: find |
12 | …… |
13 | return Reached |
RRT算法规划出的路径往往不光滑,机器人在节点处需要先减速,然后再加速前行,增加了机器人的能量消耗和工作时间。为了让路径更加光滑,使机器人平稳移动,用三次样条曲线插值优化反向寻优改进算法。三次样条曲线是一种常用的插值平滑算法,通过一系列的控制点得到一条连续平滑的曲线。
三次样条曲线插值是将原始长序列分成若干段,每一段都是一个三次函数,在分段处左右两边函数值相等,一阶导数和二阶导数都连
假设有n+1个节点,把n+1个节点划分成以下n个区间:
。 |
每个分段区间的三次多项式构造形式如下:
所以,S(x)表达式是一个分段函数:
(1) |
总共n个区间,每个区间都有4个未知数,所以整个三次样条曲线共有4n个未知数,需要4n个方程才能求解。
分段函数满足以下4个条件。
条件1:函数穿过所有已知节点,可得n+1个方程。
条件2:节点处0阶连续,即前一段方程在节点处的函数值和后一段方程在相同节点处的函数值都相等,可得n-1个方程。
条件3:在所有节点(除了第1个节点和最后的节点)处1阶连续,保证节点处有相同的斜率,原函数曲线上没有剧烈的跳变,可得n-1个方程。
条件4:在所有节点(除了第1个节点和最后的节点)处2阶连续,保证节点处有相同的曲率,意味着每个点的曲率半径有定义,共n-1个方程。
以上总共4n-2个方程,由端点条件可以增加2个约束,增加2个方程,总共4n个方程,由数学软件可以求解线性方程组,得到的
将原始的RRT算法、反向寻优改进的RRT算法、三次样条曲线插值(自由边界)优化后的RRT算法在MATLAB(R2018a)中仿真分析。实验地图为二维场景图,仿真分为无障碍物、单个障碍物、复杂环境、狭窄通道4个不同的场景,这样可以使仿真结果更具有可比性,更能验证算法的有效性。起点坐标(0,0),终点坐标(750,750),搜索步长80。在下面的路径规划图中,白色区域表示算法可覆盖的区域。
在无障碍情况下,3种算法对应的结果如

图3 无障碍物情况路径规划图
Fig. 3 Path planning for a scenario without obstacles
在单个障碍情况下,3种算法对应的结果如

图4 单个障碍物情况路径规划图
Fig. 4 Path planning for a scenario with a single obstacle
在复杂环境情况下,即存在很多障碍物,3种算法对应的结果如

图5 复杂环境路径规划图
Fig. 5 Path planning for a scenario of complex environment
在狭窄通道情况下,即可搜索区域和障碍物之间构成很多的狭窄通道,3种算法对应结果的如

图6 狭窄通道路径规划图
Fig. 6 Path planning for a scenario with narrow channels
为了便于进一步和原始RRT算法的相关数据进行比较,验证反向寻优改进和三次样条曲线插值优化后的算法的有效性和准确性,在仿真条件下对以上4个不同的场景进行了200次路径规划,计算对应算法的平均路径长度和平均节点数目,结果如
算法 | 平均路径长度/mm | |||
---|---|---|---|---|
无障碍物 | 单个障碍物 | 复杂环境 | 狭窄通道 | |
原始RRT算法 | 1 430.1 | 1 644.1 | 1 822.3 | 1 798.7 |
反向寻优改进 | 1 057.7 | 1 287.6 | 1 410.5 | 1 450.1 |
三次样条曲线 | 1 068.9 | 1 307.3 | 1 453.9 | 1 477.1 |
算法 | 平均节点数 | |||
---|---|---|---|---|
无障碍物 | 单个障碍物 | 复杂环境 | 狭窄通道 | |
原始RRT算法 | 17.9 | 20.3 | 23.1 | 22.1 |
反向寻优改进 | 0 | 1.1 | 2.4 | 2.7 |
三次样条曲线 | 0 | 0 | 0 | 0 |
计算结果显示原始RRT算法路径长度较大,且节点数多;反向寻优改进后的算法路径长度明显减小,节点数急剧减少,说明光滑性变好,算法改进效果显著;三次样条曲线插值在反向寻优的基础上进行优化后,虽然路径长度略微增加,但是节点数归零,光滑性非常好。
结合以上改进算法验证结果分析,可得以下结论:移动机器人在路径规划中如果更偏重于路径长度的要求,可以选择反向寻优改进后的算法;如果更偏重于光滑性的要求,使移动机器人行走更加平稳,可以选择三次样条曲线插值优化后的算法。
为进一步验证改进算法的有效性,以餐厅环境为例在ROS平台进行仿真,使用gmapping实现机器人的SLAM建图,如

图7 rviz中的机器人状态
Fig. 7 The state of robot in rviz
启动相应的launch文件,启动gazebo并加载机器人URDF模型的餐厅环境,启动rviz并加载了gmapping建立的地

图8 基于激光雷达的gmapping建图结果
Fig. 8 Mapping results of gmapping based on lidar

图9 机器人导航启动界面
Fig. 9 Robot navigation startup interface
根据现实餐厅障碍物情况,分为空餐厅情况、障碍物较少、障碍物很多3个不同的场景进行仿真,设置相同的起点和终点,并对它们的路径长度和运行时间进行对比分析。
餐厅内部全空时,机器人规划出的从当前位置到目标位置的有效路径如

图10 空餐厅环境下的路径规划
Fig. 10 Path planning in empty restaurant environment
在障碍物较少的情况下,机器人迅速规划出一条路径,如

图11 障碍物较少情况下的路径规划
Fig. 11 Path planning with a few obstacles
在障碍物很多的情况下,机器人规划出的路径如

图12 障碍物很多情况下的路径规划
Fig. 12 Path planning with many obstacles
为了更好地对比以上3种场景,统计路径长度和运行时间,如
场景 | 路径长度/m | 运行时间/s |
---|---|---|
空餐厅情况 | 21.58 | 43.98 |
障碍物较少 | 21.71 | 47.96 |
障碍物很多 | 24.89 | 60.01 |
分析以上图可知,障碍物较少的情况相对于空餐厅情况,路径长度和运行时间略微增加,总体变化不大;在障碍物很多的情况下,路径长度和运行时间明显增加,原因在于机器人为躲避较多的障碍物,规划的路径并非最优,导致路径长度和运行时间明显增加。
1)针对RRT算法存在的缺点提出了反向寻优和三次样条曲线插值改进方法。
2)将改进的RRT算法在MATLAB中不同的场景下进行多次仿真,结果表明,改进后的算法可以缩短路径长度,减少节点数目,提高光滑性,验证了改进算法的有效性。
3)以餐厅环境为例在ROS系统中进一步仿真,结果表明,机器人能实现路径规划功能,验证了改进算法的有效性,后续重点研究工作将搭建实物机器人验证,未来有望应用到送餐机器人中,提高工作效率,降低人工成本,产生经济效益。
参考文献
金碚. 中国制造2025[M]. 北京: 中信出版集团, 2015. [百度学术]
Jin B. Made in China 2025[M]. Beijing: Citic Publishing Group, 2015. (in Chinese) [百度学术]
王淘淘. 中国机器人产业发展报告: 我国机器人市场进入高速增长期[J]. 今日制造与升级, 2018(8): 22. [百度学术]
Wang T T. China robot industry development report: China’s robot market has entered a period of rapid growth[J]. Manufacture & Upgrading Today, 2018(8): 22. (in Chinese) [百度学术]
中国电子学会. 机器人简史[M]. 北京: 电子工业出版社, 2015. [百度学术]
Chinese Society of Electronics. A brief history of robot Brief history of robot[M]. Beijing: Publishing House of Electronics Industry, 2015. (in Chinese) [百度学术]
Yang L, Qi J T, Xiao J Z, et al. A literature review of UAV 3D path planning[C]//Proceeding of the 11th World Congress on Intelligent Control and Automation, June 29 - July 4, 2014, Shenyang. IEEE, 2014: 2376-2381. [百度学术]
Karasev V, Ayvaci A, Heisele B, et al. Intent-aware long-term prediction of pedestrian motion[C]//2016 IEEE International Conference on Robotics and Automation (ICRA), May 16-21, 2016, Stockholm, Sweden. IEEE, 2016: 2543-2549. [百度学术]
Bakdi A, Hentout A, Boutami H, et al. Optimal path planning and execution for mobile robots using genetic algorithm and adaptive fuzzy-logic control[J]. Robotics and Autonomous Systems, 2017, 89: 95-109. [百度学术]
康亮, 赵春霞, 郭剑辉. 未知环境下改进的基于RRT算法的移动机器人路径规划[J]. 模式识别与人工智能, 2009, 22(3): 337-343. [百度学术]
Kang L, Zhao C X, Guo J H. Improved path planning based on rapidly-exploring random tree for mobile robot in unknown environment[J]. Pattern Recognition and Artificial Intelligence, 2009, 22(3): 337-343. (in Chinese) [百度学术]
贾李红. 基于GPS的双向搜索路径的研究[D]. 安徽淮南: 安徽理工大学, 2017. [百度学术]
Jia L H. Research on two-way search path based on GPS[D]. Huainan, Anhui: Anhui University of Science & Technology, 2017. (in Chinese) [百度学术]
孙钦鹏, 李猛, 王中华. 基于改进快速扩展随机树算法的移动机器人路径规划[J]. 济南大学学报(自然科学版), 2019, 33(5): 431-438. [百度学术]
Sun Q P, Li M, Wang Z H. Mobile robot path planning based on improved rapidly-exploring random tree algorithm[J]. Journal of University of Jinan (Science and Technology), 2019, 33(5): 431-438. (in Chinese) [百度学术]
Lavalle S M. Rapidly-exploring random trees: a new tool for path planning[R]. Ames, Iowa: Iowa State University, 1998. [百度学术]
Chen W B, Wu X B, Lu Y. An improved path planning method based on artificial potential field for a mobile robot[J]. Cybernetics and Information Technologies, 2015, 15(2): 181-191. [百度学术]
Zhang C. Path planning for robot based on chaotic artificial potential field method[J]. IOP Conference Series: Materials Science and Engineering, 2018, 317: 012056. [百度学术]
潘思宇, 徐向荣. 基于改进RR
Pan S Y, Xu X R. Mobile robot motion planning algorithm based on improved RR
李金良, 舒翰儒, 刘德建, 等. 基于改进RRT路径规划算法[J]. 组合机床与自动化加工技术, 2021(2): 22-24,29. [百度学术]
Li J L, Shu H R, Liu D J, et al. Path planning algorithm based on improved RRT[J]. Modular Machine Tool & Automatic Manufacturing Technique, 2021(2): 22-24,29. (in Chinese) [百度学术]
Saranrittichai P, Niparnan N, Sudsang A. Robust local obstacle avoidance for mobile robot based on dynamic window approach[C]//2013 10th International Conference on Electrical Engineering/Electronics, Computer, Telecommunications and Information Technology,May 15-17, 2013, Krabi, Thailand. IEEE, 2013: 1-4. [百度学术]
Choi B, Kim B, Kim E, et al. A modified dynamic window approach in crowded indoor environment for intelligent transport robot[C]//2012 12th International Conference on Control, Automation and Systems, October 17-21, 2012, Jeju, South Korea. IEEE, 2012: 1007-1009. [百度学术]