路径规划的目标是在有障碍物的工作环境中，规划出一条连接起点和目标点，且满足机器人自身约束条件的最优或次优避障路径[1]．目前常见的路径规划方法主要有A*算法、人工势场法、神经网络和快速扩展随机树算法RRT[1-2]．其中，RRT算法具有灵活强大的搜索能力，能用于各种复杂环境下的路径规划[3]，然而却没有考虑路径花费，得到的路径并不是最优的．实际上最优路径规划算法对于机器人的实际应用尤为重要[4]．针对RRT算法的不足，文献[5]提出了具有渐进最优性(路径最短)的RRT*算法，该算法考虑了路径代价，并加入了父节点择优选择和邻节点回溯重构的策略，能够收敛到全局最优解，现已发展为一种重要的最优路径规划算法．为进一步提高RRT*算法性能，文献[6]提出RRT*-smart算法，通过启发式采样实现智能采样搜索，加快了收敛速度，但自适应性差．文献[7]提出Informed-RRT*算法，通过生成椭圆形采样子集来限制节点采样范围，提高最优路径收敛速度，但是整体时间成本大．文献[8]结合飞行机器人运动性能约束和采样优化方法，提出一种改进的RRT*FN算法，能实现更快的收敛．文献[9]提出同伦变体算法HARRT*，实现了人机交互规划器，但是计算效率和路径质量不能同时取得最优．文献[10]提出了快速扩展学习树算法RLT*，能够降低计算成本，实现快速收敛，但是计算复杂．文献[11]提出了一种学习人类意图的路径规划方法，实现了比RLT*更好的效果．面对狭窄通道等复杂受限的特殊环境，RRT*算法仍然存在内存占用大、规划效率低等问题[12]；同时，也有必要对搜索路径进一步平滑优化[13]．本研究提出了一种基于目标约束采样和目标偏置扩展的改进RRT*算法．1 改进的RRT*算法1.1　特征描述移动机器人路径规划问题是已知机器人的运动学约束(如最大转弯半径)、起始状态、目标状态及带障碍物的环境信息，规划出一条从起点到终点、满足约束且无碰撞的可行路径，并且尽可能高效完成作业任务．该问题的特征可以概括为：a. 特殊环境经常是含有狭窄通道等受限的空间环境，很容易导致规划连续低效；b. 须尽可能优化路径，如规划时间更短、路径更短，以提高机器人工作效率；c. 规划出的路径曲线只有满足运动学约束，机器人的移动才是平稳可行的．为了让路径规划更好地匹配和改善上述特征，提出了一种基于目标约束采样和目标偏置扩展的改进RRT*算法(goal-bias constrained sampling and goal-biased extending RRT*，GCSE-RRT*)，具体思路为：针对特征a，在采样环节中以合适的偏置概率来平衡目标采样和随机采样的比重，从而兼顾特殊环境下路径搜索的灵活性和高效性；针对特征b，在新节点生成中使扩展方向同时由目标点和采样点共同决定，以降低扩展的盲目性并加快扩展速度，从而尽可能缩短规划时间和路径长度；针对特征c，采用B样条曲线对规划出的路径进行平滑处理，使得曲线光滑连续且无过大折角，以满足约束，最终保证机器人在路径跟随过程中平稳可行．1.2　目标约束采样为提高采样的目标导向性，结合目标偏置和位置约束两个策略，同时借鉴启发式思想[4]先设置一个目标偏置概率pbias(设置为0.1)，然后按照均匀概率随机产生一个概率值p，若该随机概率小于目标偏置概率，则将目标点xgoal作为采样点xrand，否则就在自由空间中随机产生一个采样点，目标偏置采样公式为xrand=xgoal    (ppbias);R    (p≥pbias),式中R为随机采样函数．若随机生成采样点，则须对采样点进行位置约束．约束思想是：每随机生成一个采样点，即对其位置进行判断，判断条件为该采样点在X方向或Y方向比前一个采样点更接近目标点，若条件满足则认为采样成功；否则继续循环采样，直到满足条件为止．将起点设置为第一个参考采样点，之后不断更新，采样约束示意图如图1所示．10.13245/j.hust.210101.F001图1采样约束示意图1.3　目标偏置扩展为克服新节点扩展的盲目性，借鉴人工势场中引力场的思想，通过给采样点方向和目标点方向分配不同权重，使得新点的扩展方向不再单纯由采样点决定，而是由采样点和目标点共同决定，这样每一次扩展都能更接近目标点，从而加快搜索速度．目标偏置扩展的示意图如图2所示．10.13245/j.hust.210101.F002图2目标偏置扩展示意图新节点偏向目标的生成公式为xnew=xnearest+δ(wgngoal+(1-wg)nrand)，式中：δ为扩展步长；xnearest为距离采样点最近的邻近节点；wg为目标点方向权重；ngoal为目标点方向单位矢量，nrand为采样点方向单位矢量，ngoal=xgoal-xnearest||xgoal-xnearest||,nrand=xrand-xnearest||xrand-xnearest||．1.4　路径平滑处理B样条曲线能够对路径进行局部修改而不影响整条路径的基本形状特征，被广泛应用于路径平滑和轨迹平滑中[14]，因此采用3次B样条曲线来对算法规划的路径进行平滑处理．在Matlab中基于B样条曲线编写路径平滑函数，以GCSE-RRT*算法为例，在一个有障碍物的简单环境中规划出最优路径并经过3次B样条平滑处理，平滑前后对比如图3所示．GCSE-RRT*算法的详细流程图如图4所示．10.13245/j.hust.210101.F003图3路径平滑前后对比10.13245/j.hust.210101.F004图4GCSE-RRT*算法实现流程图图3中：S为路径起点；G为路径目标点；绿色线条为最终扩展树；蓝色线条为平滑处理前的初始路径；红色线条为经过平滑处理后的最终路径．可以看出：原始路径在经过3次B样条平滑处理后，变得比较光滑，折角也被消除了，且总体形状走势几乎不变．代码结构主要包括目标约束采样函数、目标偏置扩展函数、路径连接函数和路径平滑函数四个部分．该算法能提高采样点的目标导向性，加快扩展搜索速度，并保证最终路径可行．2 Matlab实验验证为验证改进算法GCSE-RRT*性能优势，设计了4种特殊环境进行仿真和分析．实验中，考虑到移动机器人自身尺寸，对每种环境的障碍物进行膨化处理，并将机器人看作一个质点处理．仿真实验平台及配置为：Matlab R2018b，64 bit Windows 10，处理器Intel(R) Core(TM) i5-4200H，CPU主频2.8 GHz，内存8 GiB．2.1　仿真实验所有环境的地图规模均为[100，100]，路径规划起点S设置为[5，5]，目标点G设置为[95，95]．由于RRT*算法具有随机性，因此在每个环境中，对RRT*和GCSE-RRT*算法分别执行1 000次重复试验，最后给出平滑后的路径，并统计各个指标的平均值．4种环境下的规划结果如图5~8所示．10.13245/j.hust.210101.F005图5环境1规划结果10.13245/j.hust.210101.F006图6环境2规划结果10.13245/j.hust.210101.F007图7环境3规划结果10.13245/j.hust.210101.F008图8环境4规划结果图中：红色线条为经过平滑处理后的最优路径；绿色线条为扩展树的分支．综合4种环境的规划结果可知：相比RRT*算法，GCSE-RRT*算法的路径搜索更有目的性和方向性，大大减少了无效的扩展搜索，且得到的最优路径更加平滑，更加符合机器人的运动平稳性需求．2.2　内存占用分析对于RRT*算法，有效节点数越多，扩展节点数越少，节点的利用率越高，算法消耗的内存就越少[3]，因此以路径搜索后的节点利用率来衡量算法的内存占用多少．4种环境下路径搜索的节点使用情况如表1所示．10.13245/j.hust.210101.T001表14种环境下路径搜索的节点使用情况环境算法扩展节点数有效节点数节点利用率/%环境1(稠密障碍)RRT*1972010GCSE-RRT*982122环境2(狭窄通道)RRT*1891910GCSE-RRT*992121环境3(凹坑凸台)RRT*2072110GCSE-RRT*962324环境4(简单迷宫)RRT*259218GCSE-RRT*1472316由表1可知：4种环境下RRT*算法平均扩展节点有213个，而GCSE-RRT*算法有110个，减少了48%；RRT*算法平均平均有效节点有20个，GCSE-RRT*有22个，增加了2个；RRT*算法平均节点利用率为10%，而GCSE-RRT*为21%．综上，GCSE-RRT*算法的节点利用率是RRT*的2倍左右，节点利用率更高，占用的内存更少．2.3　规划效率分析一般而言，如果算法搜索到成功路径的迭代次数越少，运行时间越短，那么规划路径的效率就越高，因此通过算法平均迭代次数和平均运行时间来衡量规划效率的高低．4种环境下运行算法的平均迭代次数和平均运行时间分别如表2所示．10.13245/j.hust.210101.T002表24种环境下算法的平均迭代次数和平均运行时间环境算法平均迭代次数平均运行时间/s环境1(稠密障碍)RRT*4750.35GCSE-RRT*2790.19环境2(狭窄通道)RRT*4250.18GCSE-RRT*2830.10环境3(凹坑凸台)RRT*3720.16GCSE-RRT*3190.09环境4(简单迷宫)RRT*4560.29GCSE-RRT*3480.16由表2可知：RRT*算法在4种环境下的平均迭代次数为432次，而GCSE-RRT*算法为307次，降低了29%；同时，RRT*算法在4种环境下的平均运行时间为0.25 s，而GCSE-RRT*为0.14 s，缩短了44%．因此，相比RRT*算法，GCSE-RRT*算法的规划效率更高，能更快地规划出可行路径．2.4　路径优化效果分析RRT*算法本身具备渐进最优性，能优化路径长度，因此以平均路径长度为主，路径平滑效果为辅，来综合衡量算法对路径的优化效果．2种算法在4种环境下所规划路径的平均长度如表3所示．10.13245/j.hust.210101.T003表32种算法在4种环境下规划的平均路径长度环境算法平均路径长度/mm环境1(稠密障碍)RRT*157.50GCSE-RRT*153.29环境2(狭窄通道)RRT*153.52GCSE-RRT*150.28环境3(凹坑凸台)RRT*172.46GCSE-RRT*169.73环境4(简单迷宫)RRT*169.28GCSE-RRT*166.98由表3可知：RRT*算法在4种环境下所得路径的平均长度为163.19 mm，而GCSE-RRT*算法为160.07 mm，缩短了3.12 mm．结合表1~3与图5~8可看出：GCSE-RRT*得到的路径总体上比RRT*更加平滑，因此GCSE-RRT*算法所规划的路径更短更平滑，路径优化的效果更好．3 V-REP实验验证虚拟机器人实验平台(V-REP)是机器人仿真器里的“瑞士军刀”，在物理环境仿真上很有优势，非常适合用作路径规划的仿真平台．为验证GCSE-RRT*算法为真实移动机器人规划路径的可行性和有效性，以双轮差分驱动机器人Pioneer P3dx为实验对象，在V-REP中进行了3D路径规划实验．首先，在V-REP中构建出一个含有狭窄通道和内凹空间的迷宫环境，规模区域是5 m×5 m，原点在正中心．然后，从模型库导入Pioneer P3dx机器人模型，设置其初始点为S(-2.0，-1.7，0.1)m，目标点为G(1.1，1.8，0.1)m，双轮的最大转弯半径为0.3 m，并添加碰撞检测模块．接着，基于Lua脚本语言分别应用RRT*和GCSE-RRT*算法规划出路径．最后，对路径可视化，并发送指令给Pioneer P3dx去跟随路径．最终规划的路径分别如图9和10所示．10.13245/j.hust.210101.F009图9RRT*算法规划路径10.13245/j.hust.210101.F010图10GCSE-RRT*算法规划路径两种算法路径规划的实验结果总结为表4所示．10.13245/j.hust.210101.T004表4V-REP仿真结果算法规划时间/s仿真时间/s路径长度/mRRT*GCSE-RRT*12.6218.747.647.1413.057.41结合图9，10和表4可知：相比RRT*算法，GCSE-RRT*算法规划路径的时间缩短了43.4%；同时路径更短，故对应路径的跟随时间更短，从而使整体仿真时间缩短了30%左右．两种算法都能为Pioneer P3dx机器人规划出可行的路径，GCSE-算法的规划效率更高，所得路径更优．4 结语针对RRT*算法在特殊环境中规划路径存在的问题，提出一种基于目标约束采样和目标偏置扩展的改进算法GCSE-RRT*．首先将均匀采样改为目标偏置的有约束采样，使采样的目标导向性比现有算法更强；然后摒弃了已有算法单纯朝着采样点扩展的思想，通过分配不同权重的方式使得新节点的扩展方向同时由采样点和目标点共同决定；最后采用三次B样条曲线对改进算法得到的路径进行了平滑处理．相比于RRT*算法，GCSE-RRT*算法的优越性在于：节点利用率提高了1倍多，内存占用更少；迭代次数降低了29%，运行时间缩短了44%，规划效率更高；最终路径更短更平滑，优化效果更好．基于V-REP的三维仿真实验表明：相比RRT*算法，GCSE-RRT*算法能更高效地规划出可行路径，所得路径更优．