2020, Vol. 35
2. 北京工业大学 计算智能与智能系统北京市重点实验室，北京 100124
2. Beijing Key Laboratory of Computational Intelligence and Intelligent System, Beijing University of Technology, Beijing 100124, China
路径规划是移动机器人导航的关键问题之一[1], 未知环境下的路径规划更是该领域研究的热点和难点.目前, 移动机器人的路径规划算法主要分为两类:基于地图的环境已知的全局路径规划和基于传感器的环境未知的局部路径规划[2-3].
全局路径规划算法简单, 能够根据环境地图搜索出一条最优或者近似最优的无碰撞路径, A*、D*、栅格法、拓扑法[4]和RRT算法[5-6]等都是常用的全局路径规划算法.RRT算法是近十几年得到广泛发展与应用的基于采样的运动规划算法, 由LaValle[7]于1998年提出.相比于其他全局路径规划算法, RRT具有独特优势:1)对状态空间中的采样点进行碰撞检测, 避免对环境的建模; 2)随机树扩展得速度快, 搜索效率高; 3)适合解决动态、多障碍物环境下的路径规划问题; 4)可应用于高维环境下的路径规划.以上所述正是RRT算法得以快速发展和应用的原因.学者们针对RRT存在的不足, 提出改进算法.Zhu等[8-11]采用偏向目标搜索的策略, 使搜索树朝目标点方向生长; LaVall等[12-13]利用双向搜索的RRT-Connect算法进一步缩短搜索时间; Sertac等[14-16]使用渐近最优的RRT*算法解决路径规划问题.随着研究的不断深入, RRT与其他算法融合的情况逐渐增多, 冯来春[17]将RRT与A*融合, 结合RRT搜索速度快的特点, 将其用于无人驾驶的车辆运动; Amiryan等[18]将RRT与人工势场法组合, 简化重新规划任务; 康亮等[19-20]结合RRT与滚动窗口法, 实现了机器人对未知环境的探索.全局路径规划算法的缺点是无法解决机器人在未知和动态环境下的路径规划问题.
相比之下, 局部路径规划可以很好地解决未知和动态环境下的路径规划问题, 在实际导航中应用性更强.局部路径规划算法包括:人工势场法[21]、模糊逻辑法等传统算法; 神经网络、遗传算法、粒子群等智能算法[22]; 以及基于预测控制理论的滚动窗口法等.局部路径规划的不足在于通常得不到最优路径, 且易陷入局部极小点.
目前, 对于已知环境下路径规划的研究已相对成熟, 而机器人在未知环境下的路径规划问题有很大的探索空间, 本文探讨的便是移动机器人未知环境下的路径规划问题.当环境未知时, 机器人只能获得传感器扫描范围内的信息, 因此只能在该范围内探索一条最优路径, 这条路径的终点是整个环境的一个子目标点.白金柯等[23]运用了新的滚动窗口子目标确定方法, 有效解决了滚动窗口局部路径规划中存在的局部极小值问题.朱毅等[24]通过实时设置子目标的方式改进沿墙走算法, 引导机器人沿某障碍物边缘运动的同时躲避其他障碍物.
综合已有研究成果, 本文提出基于子目标搜索的机器人目标导向RRT路径规划算法.一方面, 针对传统RRT算法固有的盲目搜索问题, 提出目标导向RRT路径规划算法, 这一改进能够极大地提高机器人的搜索效率; 另一方面, 为了使机器人在首次探索未知环境时也能顺利抵达目标点, 提出3种不同情况下的子目标搜索策略, 包括无障碍环境下的直达策略、扫到边界点时的最短距离策略和扫不到边界点时的后退策略, 这3种策略使机器人能够完成对未知环境的探索, 而且具备逃离局部极小点的能力.
1 基本RRT算法 1.1 RRT算法原理RRT算法是近十几年得到广泛发展与应用的基于采样的运动规划算法, 其原理是:将一个起始点作为根节点, 通过随机采样节点、增加新节点的方式, 快速地生成一个随机扩展树, 当随机树中的所有节点包含了目标点或进入了目标区域, 便可以在随机树中找到一条由节点组成的从起始点到目标点的路径.随机树扩展过程如图 1所示.
|
图 1 随机树扩展流程 |
新节点生成过程是RRT算法中至关重要的一步.从起始点xinit出发, 创建随机树T, 随机树最初只由一个起始节点组成, 后续会逐步将生成的一系列新节点xnew添加到随机树中.
新节点的生成过程是:空间中随机生成一个采样节点xrand, 在随机树已有的节点中找到与采样节点距离最近的节点xnear, 若采样节点xrand为有效节点(节点不在障碍物内视为有效), 则最近节点xnear与采样节点xrand的连线方向即为新节点xnew所在方向, 在该方向上取一个步长ρ, 便可以获得新节点xnew的位姿如下:
|
(1) |
|
(2) |
其中:nearNode.posX为最近节点的横坐标, nearNode.posY为最近节点的纵坐标; newNode.posX为新节点的横坐标, newNode.posY为新节点的纵坐标; θ为最近节点、采样节点的连线与x轴的夹角; ρ为步长, 本文取ρ = 3.新节点生成过程如图 2(a)所示.
|
图 2 RRT新节点生成过程 |
从目标点依次向前寻找父节点, 直到到达起始点, 可以获得L条通路.计算每一条通路的长度, 选择长度最短的一条作为最终规划的路径.RRT算法的伪代码如下所示:
RRT algorithm
T.init
for i = 1 to n
if Xrand in environment and out obstacle
Xnewx = Xrandx + ρcosθ
Xnewy= Xrandy+ ρsinθ
T.add(Xnew)
if Xnew = Xgoal
end if
end if
end for
传统RRT算法存在两点不足:1)RRT采样点随机性大导致搜索具有盲目性, 这将造成搜索时间的延长和存储空间的浪费; 2)该算法只适用于环境已知的全局路径规划, 无法在未知环境下直接应用.下面对以上两点不足加以改进.
2 目标导向RRT算法针对RRT采样点随机性大导致的盲目搜索问题, 提出目标导向RRT路径规划算法.核心思想是将目标导向函数加入RRT算法, 模拟人工势场法中目标点对研究对象有一个吸引力, 从而引导随机树朝着目标方向生长.改进后, 新节点xnew的位姿不再单纯地由采样节点xrand决定, 而是由采样节点xrand和目标节点xgoal共同决定; 其位姿也不再单纯地由步长ρ决定, 而是由步长ρ和引力系数k共同决定.改进后新节点位姿方程为
|
(3) |
|
(4) |
其中:nearNode.posX为最近节点的横坐标, nearNode.posY为最近节点的纵坐标; newNode.posX为新节点的横坐标, newNode.posY为新节点的纵坐标; θ为最近节点和采样节点的连线与x轴的夹角; α为最近节点、目标节点的连线与x轴的夹角; ρ为步长, 本文取ρ = 3;k为引力系数, 为计算简便, 此处暂不考虑引力系数随离目标点远近而发生变化, 经反复实验观察可知, 选取k = 0.1时, 在搜索效率上达到更优, 因此k值取0.1.目标导向RRT算法的新节点生成过程如图 2(b)所示.
按照此方法获得的每一个新节点都具有目标导向性.目标导向RRT的路径生成过程与传统RRT的路径生成过程相同, 均由目标点依次向前寻找父节点, 直到起始点为止, 获得L条通路, 计算每一条通路的长度, 选择长度最短的一条作为最终路径.目标导向RRT算法的伪代码如下所示:
Goal Guide RRT algorithm
T.init
for i = 1 to n
if Xrand in environment and out obstacle
Xnewx = Xrandx + ρcosθ+kcosα
Xnewy = Xrandy + ρsinθ+ksinα
T.add(Xnew)
if Xnew = Xgoal
end if
end if
end for
3 子目标搜索策略RRT和加入目标导向的RRT都只能用于全局路径规划, 当环境未知时, 机器人只能获取传感器探测范围内的信息, 而传感器探测范围外的信息无法获得.为了使机器人在首次探索未知环境时也能顺利抵达目标点, 本文提出基于目标导向RRT的子目标搜索算法.根据搜索范围内可视的障碍物状态不同, 将分为以下3种情景加以讨论:1)扫描区域无障碍物或朝向目标的方向无障碍物; 2)扫描区域朝向目标方向有障碍物, 且能扫到障碍物边界点; 3)扫描区域朝向目标方向有障碍物, 但扫不到障碍物边界点, 只能扫到障碍物的边.
3.1 无障碍无障碍分为两种情况:1)传感器扫描区域内检测不到障碍物; 2)传感器扫描区域内检测到了障碍物, 但朝向目标的方向无障碍物.如果满足上述两种情况, 则机器人采取的策略是径直朝目标方向运动.子目标点的位置如图 3(a)和图 3(b)中的A、B点所示, 即设定当前位置与目标点的连线与扫描边界的交点为子目标点, 这样可以保证机器人以最快的速度趋近最终目标.
|
图 3 子目标搜索策略 |
如果传感器扫描区域指向目标方向有障碍物, 且能扫到障碍物边界点, 则选取其中一个边界点为子目标点, 如图 3(c)的C点所示.之所以选择C点, 是利用了启发式函数的思想.计算当前位置到预设子目标的距离与预设子目标到终点距离之和, 令所求和最小的预设子目标点作为子目标.由此可见, 过C点的两条线段之和最小, 设置C点为子目标点min{h(xi)+g(xi)}, i∈(1, n).
3.3 扫描不到边界点如果传感器扫描区域指向目标方向有障碍物, 且只扫描到障碍物的边时, 则采取朝远离障碍物方向退一步的原则, 子目标点的位置如图 3(d)和图 3(e)的D、E点所示.具体步骤如下.
step1:确定扫描边界线与障碍物的交点, 以图 3(d)为例, 交点有4个.
step2:求各个交点与机器人连线的总的角平分线, 仍以图 3(d)为例, 先求出交点1与交点2的角平分线, 再求出交点3与交点4的角平分线, 最后对两条角平分线再求平分线.
step3:沿远离障碍物角平分线方向退一步λ.
如何得知是靠近障碍物方向还是远离障碍物方向, 本文拟采用激光传感器探测距离.设激光传感器扫描半径为R, 则传感器读数连续为R的方向即为远离障碍物的方向.图 3(e)的规划策略与图 3(d)同理.
当机器人沿远离障碍物方向后退一定距离, 可以扫描到障碍物的边界点时, 切换到第2种情况; 当机器人利用边界点绕过障碍物, 前方不再有障碍时, 切换到第1种情况.
4 仿真实验为了验证所提出算法的有效性, 在ROS系统下设计多组对比实验.设定黑色外框为仿真实验场地(仿真场地大小100×100), 框内的几何图形代表障碍物, 几何图形外部区域为可达区域, 设置激光传感器搜索半径为40, A点为起始点, B点为目标点, 细短线表示随机树分支, 连接起始点和目标点的粗线表示最终生成的路径.仿真实验忽略机器人自身大小, 认为环境是经过膨化处理之后的.
4.1 RRT与目标导向RRT对比在环境已知的情况下, 设计相同的起始点、终点和障碍物, RRT算法和目标导向RRT算法的比较结果如图 4和表 1所示.
|
图 4 RRT与目标导向RRT对比仿真实验 |
|
表 1 RRT与目标导向RRT性能对比 |
由实验结果可见:加入目标导向后的目标导向RRT算法冗余分支大大减少, 由896条减少到227条; 路径分支数也减少, 由223条减少到163条, 表明路径的曲折度有所降低; 耗费时间由25s缩减到10s, 搜索效率明显高于传统的RRT路径规划算法.
4.2 基于3种子目标搜索策略的目标导向RRT路径规划在环境未知的情况下, 机器人只能获取传感器探测范围内的信息, 为了使机器人有效探索环境并顺利到达指定目标点, 需要设定合理的子目标.实验中, 采用本文设计的3种不同条件下的子目标搜索策略对机器人进行目标导向RRT路径规划, 如图 5所示.
|
图 5 子目标搜索仿真实验 |
图 5(a)为无障碍情况, 机器人径直朝目标方向移动; 图 5(b)为扫描到边界点的情况, 机器人借助边界点, 找到被遮挡的目标; 图 5(c)为扫描不到边界点, 只能扫描到障碍物边的情况, 机器人可以顺利逃离环形区域, 抵达目标.实验结果表明, 在上述3种情况下, 机器人均能借助子目标顺利到达目标点, 完成了未知环境下的路径搜索过程.尤其是图 5(c)中, 机器人能够摆脱局部极小环境, 克服了人工势场等其他算法中易出现的局部极小点问题.
5 结论机器人在未知动态环境下的路径规划是移动机器人导航领域的一个关键问题.鉴于此, 本文提出了基于子目标搜索策略的目标导向RRT路径规划算法.在传统RRT算法的基础上, 引入目标导向函数, 形成目标导向RRT路径规划算法, 减少冗余搜索, 提高了路径规划效率.在目标导向RRT算法的基础上, 提出了3种不同情况下的子目标搜索策略, 包括无障碍环境下的直达策略、扫到边界点时的最短距离策略和扫不到边界点时的后退策略.这3种策略使机器人能够完成对未知环境的探索, 并具有逃离局部极小点的能力.通过仿真实验对所提出算法进行了验证, 并与其他算法对比.实验结果表明, 所提出算法不仅能够使机器人在未知环境下完成探索, 顺利到达指定目标, 而且能够解决局部路径规划中容易遇到的局部极小问题.
RRT及其改进算法还具备的一个优点是在高维度路径规划场景中的应用, 如机械臂的轨迹运动、无人机航迹规划以及爬坡机器人的导航等.本文主要针对移动机器人在二维环境中的路径规划进行了研究, 下面的工作将针对动态、高维度环境下的局部路径规划问题进行更深入的研究.
| [1] |
冯楠.自主移动机器人路径规划的RRT算法研究[D].大连: 大连理工大学计算机科学与技术学院, 2014. (Feng N. Research on RRT path planning algorithms for autonomous mobile robots[D]. Dalian: School of Computer Science and Technology, Dalian University of Technology, 2014.) http://cdmd.cnki.com.cn/Article/CDMD-10141-1015573387.htm |
| [2] |
Mac T T, Copot C, Tran D T, et al. Heuristic approaches in robot path planning: A survey[J]. Robotics and Autonomous Systems, 2016, 86: 13-28. DOI:10.1016/j.robot.2016.08.001 |
| [3] |
Raja P, Pugazhenthi S. Optimal path planning of mobile robots: A review[J]. International Journal of Physical Sciences, 2012, 7(9): 1314-1320. |
| [4] |
张捍东, 郑睿, 岑豫皖. 移动机器人路径规划技术的现状与展望[J]. 系统仿真学报, 2005, 17(2): 439-443. (Zhang H D, Zheng R, Cen Y W. Current situation and prospect of mobile robot path planning technology[J]. Journal of System Simulation, 2005, 17(2): 439-443.) |
| [5] |
Kothari M, Postlethwaite I. Aprobabilistcally robust path planning algrithm for UAVs using rapidly-exploring random trees[J]. Journal of Intelligent & Robotic Systems, 2013, 71(2): 231-253. |
| [6] |
Zou Y P, Guo J, Zhang R L, et al. A SRT-based path planning algorithm in unknown complex environment[C]. Chinese Control and Decision Conference. Changsha: IEEE, 2014: 3857-3862.
|
| [7] |
LaValle S M. Rapidly-exploring random trees: A new tool for path planning[R]. Ames: Computer Science Department, Iowa State University, 1998.
|
| [8] |
朱大奇, 颜明重. 移动机器人路径规划技术综述[J]. 控制与决策, 2010, 25(7): 961-967. (Zhu D Q, Yan M C. A survey of mobile robot path planning technology[J]. Control and Decision, 2010, 25(7): 961-967.) |
| [9] |
杜明博, 梅涛, 陈佳佳, 等. 复杂环境下基于RRT的智能车辆运动规划算法[J]. 机器人, 2015, 37(4): 443-450. (Du M B, Mei T, Chen J J, et al. RRT-based intelligent vehicle motion planning in complex environment[J]. Robot, 2015, 37(4): 443-450.) |
| [10] |
郝利波, 侯媛彬. 基于一种改进RRT算法的足球机器人路径规划[J]. 西安科技大学学报, 2011, 31(1): 81-85. (Hao L B, Hou Y B. Path planning of soccer robot based on an improved RRT algorithm[J]. Journal of Xi'an University of Science and Technology, 2011, 31(1): 81-85.) |
| [11] |
刘成菊, 韩俊强, 安康. 基于改进RRT算法的RoboCup机器人动态路径规划[J]. 机器人, 2017, 39(1): 8-15. (Liu C J, Han J Q, An K. RoboCup robot dynamic path planning based on improved RRT algorithms[J]. Robot, 2017, 39(1): 8-15.) |
| [12] |
LaValle S M, Kuffner J J. Rapidly-exploring random trees: Progress and prospects[J]. Algorithmic and Computational Robotics: New Directions, 2001(5): 293-308. |
| [13] |
莫栋成, 刘国栋. 改进的RRT-connect双足机器人路径规划算法[J]. 计算机应用, 2013, 33(8): 2289-2292. (Mo D C, Liu G D. Improved RRT-connect biped robot path planning algorithms[J]. Computer Application, 2013, 33(8): 2289-2292.) |
| [14] |
Sertac K, Emilio F. Incremental sampling-based algorithms for optimal motion planning[R]. Cambridge, MA: Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, 2010.
|
| [15] |
潘思宇, 徐向荣. 基于改进RRT*的移动机器人运动规划算法[J]. 山西大学学报, 2017, 40(2): 244-254. (Pan S Y, Xu X R. Motion planning algorithm of mobile robot based on improved RRT*[J]. Journal of Shanxi University, 2017, 40(2): 244-254.) |
| [16] |
Islam Fahad, Nasir Jauwairia, Malik Usman, et al. RRT*-smart: Rapid convergence implementation of RRT* towards optimal solution[C]. Proceedings of IEEE International Conference on Mechatronics and Automation. Chengdu: ICMA, 2012: 1651-1656.
|
| [17] |
冯春来.基于引导域的参数化RRT无人驾驶车辆运动规划算法研究[D].合肥: 中国科学技术大学信息科学技术学院, 2017. (Feng C L. Research on parametric RRT vehicle motion planning algorithms based on guidance domain[D]. Hefei: School of Information Science and Technology, China University of Science and Technology, 2017.) http://cdmd.cnki.com.cn/Article/CDMD-10358-1017194508.htm |
| [18] |
Amiryan Javad, Jamzad Mansour. Adaptive motion planning with artificial potential fields using a prior path[C]. Robotics and Mechatronics (ICROM). Tehran: IEEE, 2015: 731-736.
|
| [19] |
康亮, 赵春霞, 郭剑辉. 未知环境下改进的基于RRT算法的移动机器人路径规划[J]. 模式识别与人工智能, 2009, 22(3): 337-343. (Kang L, Zhao C X, Guo J H. Modified RRT-based path planning for mobile robots in unknown environments[J]. Pattern Recognition and Artificial Intelligence, 2009, 22(3): 337-343.) |
| [20] |
张捍东, 陈阳, 吴玉秀. 未知环境下移动机器人实时路径规划[J]. 计算机工程与应用, 2018, 54(19): 140-146. (Zhang H D, Chen Y, Wu Y X. Real-time path planning for mobile robots in unknown environments[J]. Computer Engineering and Applications, 2018, 54(19): 140-146.) |
| [21] |
Gilbert E G, Johnson D W. Distance functions and their application to robot path planning in the presence of obstacles[J]. IEEE Journal of Robotics and Automation, 1985, 1(1): 21-30. |
| [22] |
王晓燕, 杨乐, 张宇, 等. 基于改进势场蚁群算法的机器人路径[J]. 控制与决策, 2018, 33(10): 1775-1781. (Wang X Y, Yang L, Zhang Y, et al. Robot path based on improved potential field ant colony algorithms[J]. Control and Decision, 2018, 33(10): 1775-1781.) |
| [23] |
白金柯, 陈立家, 金何, 等. 动态未知环境下一种新的机器人路径规划方法[J]. 传感器与微系统, 2011, 30(10): 33-36. (Bai J K, Chen L J, Jin H, et al. A new robot path planning method in dynamic unknown environment[J]. Sensors and Microsystems, 2011, 30(10): 33-36.) |
| [24] |
朱毅, 张涛, 程农, 等. 动态环境下基于子目标的移动机器人路径规划方法[J]. 系统仿真学报, 2010, 22(1): 254-257. (Zhu Y, Zhang T, Cheng N, et al. Subgoal-based path planning for mobile robots in dynamic environments[J]. Journal of Systems Simulation, 2010, 22(1): 254-257.) |