罗隆福 李冬 钟杭

摘 要：为了达到无人机在电力输电线路环境中能够进行有效的路径规划以及轨迹跟踪的目的，立足于RRT（Rapidly Exploring Random Trees）算法，提出一种在电力输电线路中适用于无人机的路径规划算法.该算法在融合无人机的动力学特性，及RRT扩展的随机性的基础上，通过设计最大航向角以及路径高度的约束，使得规划的路径在高度方向上向目标点渐进逼近，并能克服传统RRT在狭窄空间规划失败的问题.算法在ROS（Robot Operating System）Gazebo环境中进行了仿真试验，验证了该算法在电力输电线路环境下的实用性.

关键词：电力杆塔；无人机；改进RRT；路径规划；ROSGazebo

中图分类号：TM755 文献标志码：A

Abstract：In order to achieve the goal of the effective path planning and trajectory tracking of UAV （Unmanned Aerial Vehicle）， one approach based on the RRT （Rapidly Exploring Random Trees） algorithm proposes a path planning algorithm for UAV in the environment of electric towers. Under the combination of the dynamics characteristics of UAV and the randomness of RRT， the algorithm with constraint of the maximum path angle and path height plans a path which gradually approximates to the target point and overcomes the failure problem of RRT in the narrow space planning. The simulation experiment is established in the ROS （Robot Operating System）Gazebo and verifies the practicability of the algorithm.

Key words：electric towers； aircraft； advanced RRT； path planning； ROSGazebo

電力杆塔作为输电网络的支柱，长期暴露在野外环境中，其塔基、塔身及金具经常出现磨损、腐蚀等问题，对此需要进行常规性的检查.传统的人工巡检工作量大，条件艰苦，特别是对于山区和江河湖流地区巡检存在很大的困难，并且一些巡检项目依靠常规的巡检方法无法实现.无人机具有机动性高，操作性好，成本低的特点，因而在输电线路巡检中应用广泛[1].

在科学研究与工程应用中，无人机越来越得到研究者的重视，随着研究的深入，无人机路径规划的研究也逐渐走向成熟，对于在三维环境中的路径规划遇到的问题，大量的学者提出了解决方案.Lavalle提出的RRT算法[2]，通过在工作空间中以随机采样的方式，生成节点并以此构建搜索树，它不但能够快速获取可行路径，而且在多维空间也能得到很好的应用.Khatib提出的人工势场法[3]（Aritificial Potential Field），其使用一个势场数值函数来描述机器人的运动空间，通过搜索势场的下降方向完成运动规划，依靠障碍物对机器人产生排斥力，目标点对机器人产生吸引力，两个力“推动”机器人向目标点移动.还有学者在路径规划算法的基础上，针对无人机应用的环境特点以及应用场景，提出了一些能满足特定环境要求的算法.清华大学的李达川在RRTConnect算法的基础上[4]，针对无人机在狭小空间的路径规划中存在路径死角从而导致规划失败的问题，采用了随机星型生成器（Randomized Star Builder）和均匀采样的混合采样的策略，该策略能够识别出狭窄空间并且避免路径规划陷入死角，最终得到有效的路径.同样对于上述狭窄空间路径规划问题，栾成志等人基于RRT算法，提出了RPRRT（Random Particles Boosted RRT）算法[5]，该方法的特点在于，搜索树是向着树节点周围均匀分布的粒子中扩展，这样当搜索树扩展到狭窄空间附近时，对该空间中的粒子进行扩展并添加到搜索树中，最终找到可行路径.

在大多数无人机的路径规划研究中，虽然都是在三维的空间中实现，但是所规划的高度都是恒定值，即是在三维的一个平面中开展的研究[6-8].另外RRT算法虽然能在三维环境中规避障碍物，并实现XYZ三轴方向的路径规划，但是由于它自身扩展的随机性，规划出的路径在高度方向上有很强的跳跃性，有时甚至无法让无人机实现轨迹跟踪，并且存在狭窄空间中规划失败的问题.

针对RRT算法在无人机路径规划方面的缺陷，本文在RRT的基础上，提出了一种改进的RRT算法，使其能在电力输电线路中进行轨迹规划.该算法在ROSGazebo中验证，其中无人机物理模型、动力学建模以及位置与姿态控制器都建立在ROSGazebo环境中，并通过在Rviz中构建电力杆塔的物理仿真环境，使得改进算法在规划出路径后并发送给无人机进行轨迹跟踪.并与传统RRT算法做了对比试验，论证了改进的RRT算法在三维空间搜索具有高效性并使无人机在电力杆塔环境中能够对规划的路径进行精确的轨迹跟踪.

1 航向角约束与基于FCL的碰撞检测

1.1 航向角约束

RRT树在空间中是随机扩展的，因而存在父节点与子节点相对位置的不确定性，考虑到无人机运动学约束，为了保证规划的路径能让无人机可以有效的跟踪，需要设计路径的转角阈值，由于是在3D的环境中规划，我们考虑三个相邻节点的情况 .

2 改进算法的搜索策略

2.1 传统RRT算法

传统的RRT算法的RRT树扩展过程如图3所示，图中的起始点为树（T）的根节点，在扩展的过程中，算法在空间中随机选取随机点，然后在RRT树节点中找到一个距离随机点最近的点并确定两个点的方向即搜索方向，最后根据搜索步长在最近点的基础上沿着搜索方向扩展新节点并添加到树中，完成上述步骤后，开始进行循环搜索过程.这样可以完成对整个空间的搜索[2].

2.2 改进算法搜索思想

2.2.1 局部搜索思想

改进RRT的基本思想，不同于传统RRT在三维空间中扩展树的过程是在XYZ三轴方向随机进行，而是在XY方向上保持了扩展的随机性，在Z轴方向上加以约束，使得每次的扩展是在XY（Z+ΔZ）的空间中进行的.通过对Z轴的分割，使得在全局三维空间的扩展过程是逐步的通过搜索局部三维空间实现的.这样保留了RRT算法在XY轴方向上的随机扩展性，同时使得在Z轴方向上具有渐进性.

改进RRT对空间搜索中，首先，由于搜索高度上存在约束，全局三维空间被细分为数个局部三维空间，由此使得在全局三维空间中的障碍物被细分后，在局部搜索中也表现为局部障碍物，同时，存在于全局三维空间中的狭窄空间在局部三维空间对路径规划的影响被削弱，甚至不产生影响.再则，局部空间XYΔZ搜索中，对空间的扩展可视为算法在平面中进行，而保留了RRT在二维平面搜索随机性的改进算法，由于维度的降低，搜索过程复杂度降低. 最后，搜索过程中，在局部空间XY（Z+ΔZ）中的空间扩展生成的RRTk+1树是基于在XYZ扩展后生成的RRTk树，从而对于狭窄空间造成的扩展阻碍，影响更加弱小.搜索过程如图4所示.

2.2.2 改进算法搜索步骤

算法1为改进的RRT算法的完整步骤.首先确定起始点和目标点并添加到树中，然后通过随机点生成函数在特定高度平面生成随机点，在树节点中搜索距离随机点的最近点，并扩展新节点，经过FCL碰撞检测判断新节点是否可行，可行则添加到树，否则丢弃.当在特定高度平面扩展一定次数时，进行高度平面的调整，上升或者下降取决于整个空间的上下界约束.当新节点在目标点的指定邻域时，则判断路径规划完成，生成路径.

算法2为扩展新节点的过程.由于传统RRT只能对空间进行简单的搜索，为了保证搜索的可控性，在算法中引入了航向角约束以及基于FCL的碰撞检测.该部分算法中，在空间中取得随机点Qrand后，在搜索树上取离随机点的最近点Qnearest，在最近点沿着随机点的方向扩展新节点Qnew，并对新节点进行碰撞检测，判断其是否添加到树中.另外新节点是否可取也是搜索空间上升或下降的依据，当新节点在目标点的邻域中时，则路径规划完成，并返回最终路径.

算法3为路径规划中空间高度调整的接口.在新节点的生成过程中，通过判断其是否添加到树节点，并得到两个阈值newnodeT， newnodeF，其中newnodeT即新节点可行的优先级更高，当其达到阈值时，可以进行规划空间高度调整，另外，当newnodeF即新节点不可行点达到k倍阈值时，也可以进行高度调整，防止路径规划因环境障碍物过多而陷入停滞，路径规划空间的上界和下界作为规划空间约束，保证当规划高度达到上界时，高度向下调整，当规划高度达到下界时，高度向上调整.

3 仿真试验与分析

3.1 试验物理环境分析

在输电线路中，电压等级一般分为1 000、750、500、220、110 kV，在不同输电电压等级下所采用的电力杆塔的类型也不同.在本文中，采用220 kV线路JG3干字型耐张塔为例开展试验，其塔型图及相应参数如图5所示[13].

电力杆塔巡检的对象主要包括地基与基面、杆塔本体、线路连接件等.巡检中根据线路运行情况、巡检要求，选择不同的检测设备.具体的巡检对象、内容及设备如表2所示[14].

试验使用计算机的处理器为Intel CoreTM i54460 CPU@ 3.20 GHz×4，内存为7.7 GiB.试验包括两个部分.一为相应算法在自由空间中无目标点的扩展过程，二为相应算法在电力杆塔环境下的无人机路径规划对比试验.仿真试验都在ROSGazebo环境中进行，试验一主要表现两种算法对空间的搜索过程，试验二表现任务环境中算法的应用性.对于试验二，无人机的动力学建模、路径规划以及轨迹跟踪控制在ROSGazebo环境中进行，电力杆塔环境模型建立在Rviz环境中.试验中，在Rviz环境中进行路径规划后，把生成的路径点发送到Gazebo中的无人机，并通过建立的动力学模型以及位置与姿态控制器进行轨迹跟踪，从而完成巡检任务.

3.2 自由空间无目标点算法扩展过程

本仿真试验的环境为建立在Rviz中长30 m，宽30 m，高6 m的三维空间，并且没有障碍物和目标点，没有引入无人机物理模型、动力学模型和控制器.设立扩展树的起点坐标为（15，15，0.2），生长步长为0.3 m，算法扩展终止节点数为2 000.

传统RRT扩展过程如图6所示，由于其扩展过

程的随机性，其树的生长十分杂乱，尤其是在Z轴高度方向上表现为高低起伏，呈跳跃状.这样规划出的路径并不优化，且不符合无人机的运动.

改进RRT的扩展过程如图7所示，相比传统RRT的扩展过程，其保留了在XY平面上扩展的随机性，有利于向空间进行搜索，而在Z轴高度上加以约束后，使得生长过程为有规律的沿着Z轴进行，规划的路径也更加优化，符合无人机的运动特性.

3.3 作业环境下路径规划与无人机轨迹跟踪

作业环境为建立在Rviz中的两个电力杆塔，作业要求规划出一条从起始点到目标点的有效轨迹，起始點为（x，y，z），目标点为（20，18，14），该点位于杆塔的中上部，在此位置使用相机拍摄并检查塔身是否倾斜、变形、锈蚀以及桁架链接处是否断裂等.试验的空间约束为（0，0，0）至（36，36，24）的坐标范围.为保证安全，要求无人机不能穿过杆塔，则路径规划轨迹也不能穿过电力杆塔.

算法中相应参数如最大航向角γ、评估函数f（n，t）即公式（2）中α、β、λ以及Δhight、k取值分别如表3所示.

3.3.1 作业环境下路径规划

设立起始点为（22，2，0.2），且位于空旷地带，起始点至目标点不存在狭窄空间，改进RRT与传统RRT在此条件下开展试验，对比两种算法在高度扩展上的优劣性.

图8为传统RRT路径规划过程，根据规划轨迹及其在YZ面投影轨迹看出，该路径在高度方向表现为跳跃性，且整个路径十分曲折并且跨过目标点，产生了冗余的路径，并且效率不高，并不利于无人机运动.

而改进的RRT算法其规划的路径在高度方向上是渐进上升的，所以最终的路径是逐渐逼近目标点，没有冗余路径且非常的优化.图8、图9和图10中左下角标示X、Y的箭头分别表示惯性坐标系下XY轴方向，未标示的向上的方向表示Z轴方向.

3.3.2 作业环境中狭窄空间下路径规划与轨迹跟踪

在作业环境中，存在由杆塔与边界约束所构成的两段狭窄空间，其坐标所属段分别为（0，0，0）至（20，11.5，12），（0，0，0）至（11.5，20，12）.设立起始点（0.3，0.3，0.2），从此起始点到目标点都要经过两段狭窄空间.试验中传统RRT算法路径规划无法生成可行路径，对比起始点设立在（22，2，0.2）时，传统RRT算法能够进行路径规划，可判断出传统RRT在狭窄空间中无法搜索到可行路径.

无人机在作业环境中跟踪改进RRT算法的规划路径过程，以及生成的航迹如图10所示，图中向右斜上的曲线为路径规划轨迹，在XY、YZ面的曲线为规划轨迹分别在XY、YZ平面的投影，树结构曲线为可视化的RRT树结构，标示跟踪航迹的曲线为无人机跟踪规划路径时产生的航迹.

改进RRT树结构的生长过程显示了算法在空间扩展中的渐进性，生成的轨迹也更平滑.并且，该树结构在狭窄空间的部分较为稀疏，表明狭窄空间对算法的扩展有一定的阻碍作用，但仍能搜索到可行路径，并被无人机跟踪，到达目标点开展作业.

改进RRT算法规划的路径具有可跟踪性由图11体现，图11为无人机轨迹跟踪时实际航迹变化过程以及改进RRT算法输出的规划路径的变化过程.虚线点为改进RRT路径规划轨迹，实线为无人机输出的跟踪轨迹，图中标示X、Y、Z的曲线分别表示在惯性坐标系下XYZ轴方向的分轨迹.从图中看出，无人机在接收到路径信息后，起飞中存在位置漂移，但在无人机稳定后，其实际轨迹能够准确跟踪算法输出轨迹，并最终达到目标位置.

4 结 论

本文在传统RRT算法的基础上，提出一种改进的RRT算法，该算法针对传统RRT算法在三维空间中搜索的随机性导致产生的路径在高度上具有很强的跳跃性，并且在狭窄空间中规划失败的问题，通过继承RRT在XY平面搜索的随机性，而在Z轴高度上对搜索过程进行约束，使得路径最终逐渐逼近目标点，并且该算法生成的路径能够穿过狭窄空间.同时，本文也考虑了无人机的运动學约束，而对路径规划进行了航向角约束，保证路径的可跟踪性.改进算法的应用环境为电力杆塔巡检，因而环境中的电力杆塔被视为障碍物，同时为保证安全性，要求无人机不能穿过电力杆塔，则通过引入FCL碰撞检测算法进行本文中的碰撞检测.改进算法在ROSGazebo环境中进行了仿真试验，试验结果表明该算法能够在电力杆塔环境中规划可行路径，并且无人机能够进行轨迹跟踪.

参考文献

[1] 吕明，盛戈皞，张卫东.无人飞行器巡检输电线路的杆塔和导线跟踪算法[J].电力系统自动化，2012，36（9）：92-97.

LV M，SHENG G H， ZHANG W D. A tower and line tracking algorithm for power transmission line inspection based on unmanned aerial vehicles[J].Automation of Electric Power Systems，2012，36（9）：92-97.（In Chinese）

[2] LAVALLE S. Rapidlyexploring random trees： a new tool for path planning[R]. Computer Science Dept， Iowa State University，1998.

[3] KHATIB Q. Realtime obstacle avoidance for manipulators and mobile robots[J].The International Journal of Robotics Research，1986，5（1）：90-98.

[4] LI D C， LI Q，CHEN N. Extended RRTbased path planning for flying robots in complex 3D environments with narrow passages[C]//2012 IEEE International Conference on Automation Science and Engineering. Seoul， Korea，2012：1173-1178.

[5] LUAN C Z， FANG Z. Random particles boosted RRT for complicated 3D environments with narrow passages[C]//2016 12th World Congress on Intelligent Control and Automation. Guilin， China，2016：3271-3277.

[6] AN B， KIM J， PARK F C. An adaptive stepsize RRT planning algorithm for openchain robots[J].IEEE Robotics and Automation Letters，2018，3（1）：312-319.

[7] YU L J， WEI Z H， WANG Z G. Path optimization of AUV based on smoothRRT algorithm[C]//Proceedings of 2017 IEEE International Conference on Mechatronics and Automation. Takamatsu， Japan，2017：1498-1502.

[8] 池毓濟.架空电力线巡检用四旋翼飞行器设计和研究[D].青岛：山东科技大学，2015：48-53.

CHI Y J. Designed and research of aquadcopter for the inspection of over heard power line[D].Shandong University of Science and Technology，2015：48-53.（In Chinese）

[9] 林娜，张亚伦.自适应RRT无人机航路规划算法研究与仿真[J].计算机仿真，2015，32（1）：73-77.

LIN N，ZHANG Y L. Research and simulation on adaptive RRT algorithm for UVAs path planning[J].Computer Simulation，2015，32（1）：73-77.（In Chinese）

[10]尹高扬，周绍磊，吴青坡.基于改进RRT算法的无人机航迹规划[J].电子学报，2017，45（7）：1764-1769.

YIN G Y， ZHOU S L， WU Q P. An improved RRT algorithm for UAV path planning[J].Acta Electronica Sinica，2017，45（7）：1764-1769.（In Chinese）

[11]王兴博，柳长安，周宏.电力线路巡检飞行机器人的轨迹平滑研究[J].华北电力大学学报，2006，33（4）：39-42.

WANG X B， LIU C A， ZHOU H. Study on smooth trajectory for flying robot for overhead powerline inspection[J].Journal of North China Electric Power University，2006，33（4）：39-42.（In Chinese）

[12]PAN J， CHITTA S， MANOCHA D.FCL： A general purpose library for collision and proximity queries[C]//2012 IEEE International Conference on Robotics and Automation（ICRA）.Saint Paul， Minnseota：2012 ， 20 （10） ：3859-3866.

[13]山西电力公司.输电线路塔型手册[M].北京：中国电力出版社，2009：176-177.

Shanxi Electric Power Company. Manual of tower for power transmission[M].Beijing： China Electric Power Press，2009：176-177. （In Chinese）

[14]彭向阳，陈驰，饶章权.大型无人机电力线路巡检作业及智能诊断技术[M].北京：中国电力出版社，2015：58-59.

PENG X Y， CHEN C，RAO Z Q. Operating of power line inspection and intelligent diagnosis technology of large UAV[M].Beijing： China Electric Power Press，2015：58-59. （In Chinese）