庄佳园，张 磊，孙寒冰，苏玉民

(哈尔滨工程大学 水下机器人技术重点实验室，15000哈尔滨)

水面无人艇(Unmanned Surface Vehicle，USV)简称无人艇，是一种具有自主规划、自主航行能力，并可自主完成环境感知、目标探测及战术攻击等任务的小型水面船舶.其中，以色列“Protector”和美国“Spartan”无人艇引领着当今世界无人艇的发展方向，其他国家进行了无人艇的研究，如意大利的“Charlie”［1］、英 国 的“Springer”［2］和葡萄牙的“Delfim”等.文献［3-5］总结了USV的发展历史及现状.USV的路径规划方法按对环境信息已知程度不同可分为两类:环境信息完全已知的全局路径规划;环境信息完全未知或部分未知，通过传感器实时地对USV的当前工作环境进行探测，以获取障碍物的位置和尺寸等信息的局部路径规划［6］.当USV按电子海图等已知环境信息规划的全局路径航行时，同时需要根据航海雷达等当前传感器感知的局部环境进行动态局部路径规划［7］.

本文针对USV航速快、机动性强、对局部路径规划算法实时性要求高等特点，改进了快速扩展随机树(RRT)算法，以当前雷达图像为环境模型完成了USV动态局部路径规划，通过对规划航线的优化处理，在保证生成航线安全的同时，使优化后航线相对航程最短且光顺可行.

1 路径规划算法的描述与实现

1.1 经典的RRT算法

快速扩展随机树(rapidly-exploring random tree，RRT)算法［8］由Lavalle首次提出.用树结构代替有向图结构，可以在给定控制率的条件下，解决高维多自由度机器人的复杂约束下的运动规划问题，适用于包含几何和动力学约束的路径规划.

RRT算法的主要思想是逐步、快速地降低一个随机选择的节点与树之间的距离，直至满足预期要求.目标是搜索到一条从起点xinit到终点xgoal的可行路径，基本的RRT构建过程如图1所示.

图1 基本的RRT构建过程

在航海雷达图像范围内，采用经典RRT算法分别扩展100、500、1000、2000步的效果见图2.

图2 RRT算法扩展过程

可以看出，RRT算法每次扩展倾向于探索未知部分，主体枝干会迅速扩散到空间的4个顶点，同时主干的分叉又会深入到其他局部区域，这种平衡的扩展方式是RRT算法具有快速性的主要原因［9］.

1.2 RRT算法的改进

RRT算法在完成对未知环境探索并完成规划的同时，存在以下问题［10］:在全局空间内均匀搜索，导致算法无谓耗费较大;先全局搜索构建随机树，然后一次性规划路径，实时应用性较差;路径的搜索树由随机采样点生成，缺乏可重复性，导致规划路径不是最优路径.

针对以上基本RRT算法存在的问题，以提高算法的效率和性能为目标，出现了一些RRT算法的改进算法.Nik等［11］将粒子滤波引入到RRT算法中，提高了随机树扩展的自适应性.Aldahak等［12］提出了KD树概念，提高了搜索效率.Yershova等［13］加入了扩展反馈信息用以抑制扩展点范围，Jaillet等［14］在此基础上增加了动态调整信息.Burns等［15］提出了以预测模型为基础的动态RRT算法，减少了规划时间.康亮等［16］将RRT算法与基于滚动窗口的路径规划相结合，以增强算法探索未知空间的能力.宋金泽等［17］将非完整约束条件与双向多步RRT算法相结合，在提高搜索效率的同时保证了路径的可行性.彭辉等［18］在无人机区域目标搜索中改进了RRT算法，提高了搜索效率.

综合以上优化算法，针对USV的运动特点，本文在两个方面改进RRT算法:

1)改进生长点的选择，限制陷入局部区域节点附近一定范围内的节点被选为生长点.假设，当前树T中含有n个节点，T={xi}，i=1，2，…，n．以xi为生长点，对应的探索点xnew与障碍物发生碰撞，称xi探索失败;若未与障碍物发生碰撞，称xi探索成功．记fi对节点xi探索失败的次数，即节点xi探索失败一次，则fi=fi+1;如果该节点探索成功，则重置fi为1．探索失败节点xi与其余节点xj(xj∈T)的距离为rij=xj-xi，定义δj为节点xj的抑制因子:

式中:ε为探索步长，文中取USV在最大航速下的最小直航距离.树中节点xj与xrand间的距离为Dj=xj-xrand，则节点xj的权值wj为

权值更新后，选取树中权值最大的节点作为树的生长点.

2)改进探索点的选择，以USV最大转角θmax为约束条件限制探索点的范围，引入距离启发信息，使得规划出来的航迹接近最优搜索航迹．在未搜索区域产生m个随机点，k=1，2，…，m，以为目标点，分别按上述生长点改进方法计算探索点为使规划后航迹满足USV的可航性约束，需要根据当前位置和到的航向改变量θk，对于超出USV可达范围内的随机点xkrand，以巡航速度下最大转角θmax为限制条件来计算

图3 改进的RRT节点扩展

根据上述策略，基于改进RRT算法的局部路径规划算法描述如图4所示:

步骤1 初始化生长树T，以初始点xinit为生长树的根节点x1;

步骤2 在未搜索区域产生m个随机点，k=1，2，…，m，作为树的探索方向点;

步骤5 选择最优节点xnew，按式(4)计算Jk，选择min{Jk}对应的探索点为最优节点加入生长树;

步骤6 更新生长树T，若xnew未与任何障碍物发生碰撞，则xnew加入生长树T中，新的生长树更新为T=T+xnew，否则放弃xnew，T不变，返回步骤2;

步骤7 判断是否到达目标点xgoal，如果xgoal-xnew＜τ(τ为目标点的范围阈值)，则认为到达目标点xgoal，否则返回步骤2;

步骤8 从目标点xgoal回溯到初始点xinit，返回路径．

图4 改进RRT算法流程图

由以上算法过程和图4可以看出:通过对RRT算法生长点选取的改进，在选取生长点时不仅考虑随机方向点和树节点之间的距离，同时加入衡量节点探索失败次数的抑制因子，实现自适应调整树中节点的生长权值，使树朝着最有利的方向生长;通过对探索点选取的改进，以最大转角限制探索方向，使规划航迹趋于实用，以距目标点的距离为启发因子，削弱新增节点的随机性，使得规划航迹接近最优航迹.

2 试验结果及分析

2010年～2012年，某型USV在中国进行了多次外场试验，完成了自主航行和无人自主避碰试验，试验结果验证了本文算法的有效性.

选取3种自主避碰试验中的典型实际雷达图像，如图5所示.

图5 原始雷达图像

试验1 港口内自主出港试验，起始点为USV当前位置，目标点为港口外一点.

试验2 湖泊内一个障碍物规避试验，起始点为USV当前位置，目标点为障碍物后一点.

试验3 海上两个障碍物规避试验，起始点为USV当前位置，目标点为右前方两个障碍物中间一点.

为验证改进RRT算法有效性，以经过处理的二值化雷达图像为环境模型［19］，采用经典RRT算法和本文改进算法进行航迹搜索，USV航速20节(搜索步长为10 m)，最大转弯角为60°，试验结果如图6～8所示.

图6 试验1规划航线

图7 试验2规划航线

图8 试验3规划航线

表1可以看出，经典RRT算法和改进RRT算法均可以生成一条由起始点到目标点的安全航线，通过对该航线中航点的跟踪，即可使USV达到自主航行和避碰的目的.但从搜索效率来看，改进RRT算法对规划路径的搜索方向性更明确，树中的无用节点大幅减少，同时对陷入局部区域中的节点具有较好的限制.如试验1中在第一次搜索不成功的情况下，能快速调整生长点并完成路径搜索.从规划时间来看，由于经典RRT算法在整个模型空间内均匀搜索可行路线，因此搜索节点较多，规划时间较长，不能满足USV的实时性要求.综上，本文对探索点和生长点的选择的改进，可以提高算法的效率，满足USV嵌入式系统的实时性要求.

表1 3种试验规划结果比较

3 规划路径的优化处理

文中提出的改进RRT算法虽然可以完成可行路径的搜索，但规划航线中都存在多余航点，使得规划出来的路径并不理想.多余航点是指那些去除后不会影响航线有效性和安全性的航点.如果将规划结果直接作为USV航行路径，多余航点造成的阶梯形和锯齿形线段不利于USV的运动控制，因此需要对规划路径进行优化处理，仅保留转向点，以适应USV的实际航行路线.本文采用二分查找法逐次判定线段安全性，对航点序列进行多余航点去除，过程如下［20］:在规划出的路径中依次取出连续的航点pi、pi+1、pi+2，若pi与pi+2之间的连线不与障碍物发生碰撞，则去除pi+1;继续判断pi与pi+2之后的航点连线，若其连线不与障碍物发生碰撞，则删去pi+2;依次类推，直到pi与后面的某航点的连接线与障碍物发生碰撞，则在该航点后重新取出连续的3个航点并作为pi、pi+1、pi+2;重复上述过程，直至取完全部航点．

称经过多余航点去除后的规划路径为关键路径.由于仅保留了少量必要航点(关键点)，使得关键路径上存在许多拐角，USV到达关键路径上关键点时要保持一定的速度，但在该关键点处艏向角要发生变化.由于USV在航行过程中惯性较大，经常会冲出关键点，艏向控制也需要一定的时间达到稳定，会造成USV偏离原规划航线，对运动控制尤其是航迹跟踪有不利影响，因此需要对关键路径的拐点进行平滑处理.本文采用的处理方式为基于试验数据的曲线拟合，在分析总结USV大量回转试验数据的基础上，统计USV在不同航速下的战术回转直径，以每个拐点处的转艏角度对应的圆弧代替原路径中的拐点，达到对规划路径进行平滑处理的目的，使处理后的航行路径更接近于实际航行情况.本文以运动控制限定的最大舵角(10°)对应的回转试验数据作为拟合曲线.

经过优化处理后的规划航线见图9，优化前后结果比较见表2.

图9 优化后规划航线

表2 优化前后规划结果比较

由表2可以看出:本文采用的优化方法可以有效地去除原规划路径中多余的航点，减少规划后的航行距离;以USV实际回转试验数据光顺处理后的路径更加容易满足USV运动控制系统要求，具有可跟踪性.

4 结 论

1)改进了经典RRT算法探索点和生长点的选择方式，在保证树生长方向有利的同时削弱新增节点的随机性，使其更适用于USV的局部路径规划.

2)以海上和湖上实际雷达图像为环境模型，完成了局部路径规划试验，并与经典RRT算法进行对比.本文的改进方法可以较好地提高搜素效率，同时对陷入局部区域的节点有一定的抑制作用.

3)通过对规划航线的优化和基于回转试验数据的平滑处理，去除了规划路径中的多余航点，减少了规划距离，同时光顺后的路径可更好地适应USV控制系统的要求.

［1］CACCIA M，BIBULI M，BONO R，et al.Basic navigation，guidance and control of an unmanned surface vehicle［J］.Autonomous Robots，2008，25(4):349-365.

［2］XU T，CHUDLEY J，SUTTON R.Soft computing design of a multisensory data fusion system for unmanned surface vehicle navigation［C］//Proceedings of the 7th IFAC Conference on Maneuvering and Control of Marine Craft.Lisbon:IFAC，2006:124-156.

［3］YAN Rujian，PANG Shuo，SUN Hanbing，et al.Development and missions of unmanned surface vehicle［J］.Journal of Marine Science and Application，2010，9(4):451-457.

［4］MANLEY J E.Autonomous surface vessels，15 years of development［C］//Proceedings of Oceans 2008 MTS/IEEE Quebec Conference and Exhibition.Quebec:IEEE，2008:1-4.

［5］VEERS J，BERTRAM V.Development of the USV multi-mission surface vehicle III［C］//Proceedings of 5th Int Conference Computer and IT Application in the Maritime Industries(COMPIT).Leiden:COMPIT，2006:345-355.

［6］马仁利，关正西.路径规划技术的现状与发展综述［J］.现代机械，2008(3):22-27.

［7］CAMPBELL S，NAEEM W，IRWIN G W.A review on improving the autonomy of unmanned surface vehicles through intelligent collsion avoidance manoeuvres［J］.Annual Reviews in Control，2012，36(9):167.283.

［8］LAVALLE S.Rapidly-exploring random trees:A new tool for path planning［D］.Iowa:Iowa State University，1998.

［9］王维.虚拟人运动规划与运动合成关键技术研究［D］.长沙:国防科技大学，2011.

［10］康亮.自主移动机器人运动规划的若干算法研究［D］.南京:南京理工大学，2009.

［11］NIK A M，REID S.Particle RRT for path planning with uncertainty［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:1617-1624.

［12］ALDAHAK A，ELNAGAR A.Practical pursuit-evasion algorithm:detection and tracking［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:343-348.

［13］YERSHOVA A，JAILLET L，SIMÉON T，et al.Dynamic domain RRTs:Efficient exploration by controlling the sampling domain［C］//Proceedings of IEEE International Conference on Robotics and Automation.Barcelona:IEEE，2005:3856-3861.

［14］JAILLET L，YERSHOVA A，LAVALLE S，et al.Adaptive tuning of the sampling domain for dynamicdomain RRTs ［C］//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems.Edmonton:IEEE，2005:2851-2856.

［15］BURNS B，BROCK O.Single-query motion planning with utility guided random trees［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:3307-3312.

［16］康亮，赵春霞，郭剑辉.基于模糊滚动RRT算法的移动机器人路径规划［J］.南京理工大学学报:自然科学版，2010，34(5):642-648

［17］宋金泽，戴斌，单恩忠，等.一种改进的RRT路径规划算法［J］.电子学报，2010，32(2):225-228.

［18］彭辉，王林，沈林成.区域目标搜索中基于改进RRT的UAV实时航迹规划［J］.国防科技大学学报，2009，31(5):86-91.

［19］庄佳园，徐玉如，万磊，等.基于雷达图像的水面无人艇目标检测技术［J］.哈尔滨工程大学学报，2012，33(2):29-135.

［20］庄佳园，苏玉民，廖煜雷，等.基于航海雷达的水面无人艇局部路径规划［J］.上海交通大学学报，2012，46(9):1371-1375.