基于改进RRT算法的无人机航路规划与跟踪方法研究

马 蓉

(航空工业西安飞行自动控制研究所 飞行器控制一体化技术国家重点实验室，西安 710076)

0 引言

目前，无人机在军用及民用领域的应用呈井喷式增长，可以预见，未来的空域环境趋于复杂化。面对日趋复杂的空域环境，无人机的自主能力等级将极大地影响平台的生存概率及执行任务种类的数目。 例如无人机以较低的高度在城市或者森林中穿梭时，需要具备较强的自主规避能力，航路规划与跟踪方法即为其关键技术之一。

无人机航路规划的目的是根据任务要求、环境特征和飞行平台的动力学约束条件规划出一条有效的航路[1]。目前主流的航路规划方法可以分为三类：以A*为代表[2]的基于几何模型搜索的方法、以人工势场法[3]为代表的虚拟势场方法，以及近几年兴起的基于人工智能的方法包括蚁群算法[4-5]、遗传算法[6]、粒子群算法[7]等。其中A*算法应用较为普及，其在二维平面内可靠性高、运算速度快，应用于无人车与地面机器人效果较好，但是面对三维空间时，该算法的计算量成倍增长，因而无法保证实时性[8]。人工势场的规划方法的实时性较高，但是该算法难以结合运动体的运动学和动力学微分约束，因而无法约束无人机在沿所规划航路快速飞行时的偏差，不能保证无人机不与障碍物发生碰撞[9]。基于人工智能的方法同样存在计算量大、运行时间长的问题[10]。无人机在复杂并且存在动态障碍物的环境中飞行时，对航路规划及跟踪方法具有较高的实时性与可靠性要求，因此亟需一种能够快速规划路径并精确跟踪的方法。

本文的航路规划算法以快速扩展随机树 (Rapidly-exploring Random Tree，RRT)算法[11]为基础，较其他方法在实时性和处理环境不确定性方面具有较强优势。由于算法规划出的航路对于具有较多动力学约束条件的无人机无法直接使用，需要对其进行二次优化，效率较低。本文采取了一种对RRT算法的规划节点进行动力学约束的方法，该方法在航路规划的过程中即对航路点进行动力学约束，无需二次优化，提高了算法的实时性。同时，由于RRT算法是一种全局规划算法，面对动态的任务场景时，需要不停地更新整条航路，不仅增加了计算负荷而且不利于航路跟踪的稳定性。本文设计了局部航路动态优化策略来提高算法效率。此外，为了提高航路跟踪的准确性，在航路跟踪控制律中加入了姿态控制指令，对传统的跟踪控制方法进行了改进。

最后，本文通过自主避障飞行演示验证试验，证明了该算法的有效性。

1 无人机航路规划与跟踪方法设计

1.1 经典RRT算法

RRT算法是一种基于随机采样的树结构搜索算法[11]，以状态空间中给定的起点xinitial出发，通过在状态空间中随机采样引导搜索树生长。当树的节点进入目标区域χgoal时算法结束，然后回溯到根节点即可得到所规划航路。算法流程如算法2所示。

算法2 RRT算法

1.2 基于改进RRT算法的无人机航路规划方法设计

在结合改进的RRT算法进行航路规划时，规划出的航路为每单位步长规划一个航路点，航路点既需位于无碰撞区域，又要满足无人机的动力学约束[12]。本文以固定翼无人机为主要研究对象，对其航路规划算法进行设计。

如果已知转弯时地速的水平分量，则转弯的构造如下

(1)

其中，Rmin为固定翼无人机的最小转弯半径，VH为转弯时地速的水平分量，φmax为最大滚转角。

Δφ=dφ*Tstep

(2)

其中，dφ为偏航角速率，Tstep为规划时间步长。c为pi-1到pi的弦长，由于c值较小，所以c≈l=VH*Tstep。

三维环境中的航路规划，还需考虑无人机的纵向运动轨迹，与横侧向动力学约束条件类似，需对俯仰角变化率进行约束，以免失速[13]。

如图1所示，三维空间中，已知当前位置信息为pi-1，俯仰角和航向角分别为θ0和φ0。

图1 三维空间固定翼无人机动力学约束Fig.1 The scheme of dynamic restriction for fixed-wing UAVs in 3D space

设定固定翼无人机在其动力学约束条件下可飞速度向量有9个，得到pi，表示如下

(3)

(4)

(5)

(6)

(7)

仿真结果如图2所示。

图2 固定翼无人机航路规划仿真结果Fig.2 The simulation result of path planning for fixed-wing UAVs

由图2可以看出，本文算法规划出的航路既能够避开障碍区域又满足固定翼无人机的动力学约束。

1.3 局部航路动态优化策略

RRT航路规划方法是一种全局规划算法，在动态环境中，为了保持实时性，需要每一拍都计算新的航路(图3)，导致航路点不停变化，不利于进行航路跟踪，而且计算机负担过重[14]。

图3 动态环境下的航路重规划Fig.3 Path re-planning in dynamic environment

为了使航路保持稳定以及减少计算负荷，本文采取了局部航路动态优化策略，即首先检查上一拍规划出的航路是否依旧可行，计算每个航路点是否处于安全区域。如果可行，保留原有航路并继续跟踪；如果有航点处于碰撞区，即重规划该航路点及其之后的航点并更新航路。如未能重规划成功，说明该航点脱离上一个航点的规划约束范围，追溯回之前的航点进行重规划，直至规划出新的可行航路。

1.4 航路跟踪控制律设计

为实现快速准确的航路跟踪，本文设计了航路跟踪控制律模块[15]。航路规划算法规划出的每个航路点包含信息有[position_nyaw_nroll_n],其中position_n为航路点位置信息，yaw_n为航向角信息，roll_n为滚转角指令信息，如图4所示。

图4 航路跟踪控制律结构Fig.4 Trajectory tracking control law

由试验可知，在无规划的滚转角指令输入时，直接由侧向偏离的控制律跟踪航路点的坐标位置不能很好地跟踪航路。

如图5(a)所示，红色轨迹为规划航路点，蓝色轨迹为实际飞行航路。其原因是侧向纠偏控制律需要在误差已产生的情况下才能进行纠偏，所以跟踪航路存在一定延迟。增加了滚转角指令输入后，跟踪控制律能够较好地跟踪规划航路。由试验数据，如图5(b)可知，其侧偏距跟踪误差可以保持在2.5m以内，能够满足本试验中对小型固定翼无人机避障的安全性要求。

(a)无滚转指令输入的跟踪结果

(b)加入滚转指令输入的跟踪结果图5 航路跟踪仿真结果Fig.5 Simulation results of trajectory tracking

2 基于两种无人机平台的试飞验证

2.1 试飞平台搭建

为了验证本文提出的航路规划方法对不同的无人机平台及环境均具有良好的适用性。本文搭建了2款小型无人机验证平台，分别为固定翼和四旋翼，如图6所示。

(a)小型固定翼无人机平台

(b)旋翼无人机平台图6 试飞验证平台Fig.6 Platforms for flight tests

2款无人机平台均配置了用于环境感知的传感器。其中固定翼无人机配置了Hokuyo激光雷达传感器，探测距离为30m，精度为0.03m；小型旋翼无人机配置了同款激光雷达及单目视觉传感器，单目视觉传感器为激光雷达获得的环境信息进行补充，主要为环境中的纹理信息。利用环境感知传感器，无人机平台在飞行的过程中，可获得环境中的障碍物信息并进行环境建模，为无人机实现自主避障奠定基础。

2.2 试飞结果与分析

基于固定翼平台的自主避障飞行试验中，2m翼展小型固定翼无人机搭载激光雷达全自动飞行，飞行速度为8m/s，如图7所示。

图7 基于固定翼无人机的自主避障视频截图Fig.7 Obstacle avoidance by fixed-wing UAV

固定翼无人机自动起飞后盘旋一周对准跑道，以4m相对高度低空通场，依靠机载激光雷达感知跑道上树立的2个10m高充气立柱，先后完成2次在线运动规划与航路跟踪，成功绕开障碍物。

由图8可以看出，航路跟踪侧偏距小于2.5m，满足精度要求。

图8 航迹跟踪侧偏距Fig.8 Lateral deviation of trajectory tracking

基于四旋翼平台的自主避障飞行试验中，旋翼无人机巡航速度2m/s，需要在树林中执行任务，对无人机的自主避障性能具有更高要求，如图9所示。无人机在飞行至规定的目标点的过程中，顺利完成自主避障，跟踪误差不超过0.3m，满足小型旋翼无人机自主避障需求。

图9 基于四旋翼平台的自主避障视频截图Fig.9 Obstacle avoidance by quadrotors UAV

本文根据不同步长，对算法的规划用时进行了测试，每个步长下测试10次，取平均用时和最长用时。 由表1可知，规划时间随着规划步长减少，但是由于规划步长过大会导致规划出的航路点越过障碍物，因此，需要根据任务场景中障碍物的具体情况，设定合适的规划步长。

表1 规划时间步长与规划时间Tab.1 Planning time step and planning time

对于固定翼无人机平台，规划时长应小于无人机到达下一个航路点的时长，否则即使航路规划成功也无法进行航路跟踪。演示验证中，选取了0.3s规划时间步长，最长规划用时符合规划时长要求，满足自主航路规划的实时性要求。

3 结论

本文针对无人机复杂任务场景中自主航路规划能力的需求，对经典RRT算法进行了改进。对规划航路点进行了无人机飞行动力学约束，无需二次优化航路，提高了算法在三维空间航路规划的实时性。设计了局部航路动态优化策略，减少了算法在动态场景中的计算负荷，并且使航路点趋于稳定。针对传统的航路跟踪控制律避障时跟踪误差较大的问题，通过将规划算法得出的姿态指令引入姿态控制回路的方式，提高了航路跟踪控制精度。利用固定翼和旋翼两种无人机平台在多种任务场景下的试飞验证，证明了本文提出的自主航路规划方法在工程应用中的可行性与有效性，说明了该航路规划方法能够有效提高无人机的自主能力。

由于无人机在自主规避的过程中，需要准确的环境模型，本文目前采用的环境模型较为简单，不适用于更复杂的环境。在后续的研究中，将会侧重于环境感知与建模方面的研究，以提升无人机自主规避技术在复杂环境中应用的效果。