四旋翼无人机的轨迹规划跟踪控制研究

王 琼，郑 晨，姜 昱，吴培利

四旋翼无人机的轨迹规划跟踪控制研究

王 琼1,2，郑 晨1,2，姜 昱1,2，吴培利3

（1 中国电子科技集团第二十研究所，西安 710068；2 陕西省组合与智能导航重点实验室，西安 710068；3 中国兵器工业集团第二一二研究所，西安 710065）

针对四旋翼无人机的实际应用需求，开展轨迹跟踪控制研究。提出了一种基于快速扩展随机树（RRT）算法的Dubins航迹规划方法，同时搭建了四旋翼模型，并设计了轨迹跟踪控制方案对所提出的规划路径进行了轨迹跟踪仿真。该RRT-Dubins算法采用RRT算法对有障碍区域的无人机路径进行有效规划，然后利用Dubins路径对规划出的轨迹进行平滑处理，以形成一条无人机可飞行路径。仿真实验表明，采用所提出的轨迹规划方法及路径可以较好地规避障碍区域，且轨迹平滑更适合无人机飞行，同时验证了所提轨迹跟踪控制方案的有效性。

四旋翼无人机；轨迹规划；轨迹跟踪；快速扩展随机树算法；Dubins路径

0 引言

四旋翼无人机因其具有垂直起降、自主悬停、控制灵活且成本低等优点，在民用乃至军事领域都得到了广泛的应用[1]。而四旋翼无人机作为一种典型的多变量、非线性、强耦合的欠驱动系统，在飞行过程中还容易受到大风等环境天气的干扰，这些对于四旋翼系统的跟踪控制都会产生影响[2-3]。

四旋翼无人机的应用场景广泛，例如快递输送、抢险救灾、事故调查、野生动物摄影和反恐侦查 等[4-5]。而这些大多数的应用场合都期望可以安全稳定地完成飞行任务。传统的四旋翼无人机是由人工遥控操作控制的，但人工操作工作量较大，操作员容易产生视觉疲劳，因此如何让四旋翼无人机自主避障并寻到一条最优或可行的飞行轨迹是任务系统的核心之一[6]。除此之外，持续高精度地按照期望轨迹飞行，亦是完成飞行任务不可或缺的重点，所以能够自动精确地对轨迹进行跟踪控制是四旋翼无人机必须具备的能力[7]。

本文针对四旋翼无人机的实际应用需求，在满足地势限制、自身机动性能和外部威胁等一定约束条件下进行飞行轨迹规划，采用快速扩展随机树（Rapidly-exploring Random Tree，RRT）算法对复杂环境下的无人机进行航迹预规划，同时用Dubins路径对航迹进行优化；然后搭建了轨迹跟踪实验平台，设计了一种位置—姿态控制器，实现对四旋翼无人机的预定轨迹跟踪；最后对规划的航迹路线进行了跟踪飞行仿真，实验结果表明本文所述方法的正确性和有效性。

1 四旋翼无人机运动模型

为建立四旋翼无人机的运动模型，首先假设四旋翼无人机为刚体，且不考虑无人机飞行过程中所受的气动力矩。

图1 四旋翼无人机模型及坐标系定义

根据牛顿定律，四旋翼无人机的动力学方程如式（2）和式（3）所示[8]

2 轨迹规划算法

2.1 RRT算法

1998年，La Valle提出了RRT算法[9]。相比于传统的路径规划算法，该算法具有无需事先对任务进行空间建模的优势，本质是在给定地图中生成一颗随机树来寻找目标点[12]。以探索点的随机性保证了目标点探索的概率完备性，有利于复杂环境下的路径探索，可以解决具有非完整约束的路径规划问题[11]。

RRT算法的伪代码如下所示。

continue

else

end if

break

end if

end while

2.2 Dubins路径

无人机以一定速度转向时，转向半径需要大于在此速度下的极小半径，基于Dubins路径的路径规划方法可以有效解决此问题[14]。

Dubins路径可简单定义为：在最大曲率半径限制下，平面内两个有方向的点间的最短可行路径是CLC路径、CCC路径或它们的子集，其中C代表圆弧段路径，L代表与圆弧段路径相切的直线段路径[15]，如图2所示。

图2 Dubins路径

将数条C和L按照一定的顺序平滑连接起来就可以构成一条由起点到终点的无人机可飞行路径。这些基本曲线段的长度和方向可以根据无人机的转弯半径和路径长度来计算。

Dubins路径的长度可以通过以下三个步骤来计算：

1）计算无人机的转弯半径，并确定可以使用的基本曲线段；

2）根据起点和目标点的位置和方向计算出最短路径的基本曲线段序列；

3）计算每个基本曲线段的长度，并将它们相加得到Dubins路径的总长度。

3 轨迹跟踪控制设计

3.1 总体设计思路

本文采用内外环的控制策略，其中内环对四旋翼姿态角进行控制，而外环对位置进行控制，如 图3所示。

图3 四旋翼无人机控制系统结构图

3.2 位置控制器设计

式中

将式（6）带入式（2）可得

式中

3.3 姿态控制器设计

4 仿真与分析

为了验证本文所提算法的有效性，首先在Matlab/Simulink中搭建了四旋翼无人机的框架模型，如图4所示。

图4 四旋翼无人机系统框架模型

四旋翼无人机规划轨迹起点(25,25,25)，终点(150,140,35)，分别采用RRT算法及RRT-Dubins路径算法在有障碍物的区域进行多次航迹规划，仿真结果如图5和图6所示。

图5 航迹规划对比图

图6 航迹规划对比图

从图5和图6可以看出，两组仿真实验中，RRT算法和RRT-Dubins路径都可以在有障碍的区域内完成四旋翼无人机的轨迹规划，但是RRT-Dubins路径较未采用Dubins路径规划出的轨迹更为平滑，更加适合用于无人机的飞行路径规划。

下面采用位置—姿态控制器对规划的轨迹进行跟踪控制仿真，并采用Matlab/Simulink中的无人机仿真动画模块UAV Animation进行跟踪曲线显示，如图7所示。

图7 UAV Animation跟踪模块

仿真中四旋翼无人机从原点起飞，对本文所述RRT-Dubins路径规划轨迹进行飞行跟踪，如图8所示，无人机跟踪轨迹及跟踪误差如图9和图10所示。

图8 规划轨迹示意图

图9 四旋翼无人机跟踪飞行轨迹

图10 四旋翼跟踪误差

仿真中无人机初始位置误差为(25,25,25)，轨迹跟踪过程中，无人机从地面起飞在高度目标被跟踪到之后基本没有产生变化；而水平方向由于目标轨迹变化较快，在每次机动飞行产生变化的时候，跟踪误差都会有一些小幅度的变化，当最后四旋翼无人机悬停在目标点附近时，跟踪误差基本趋近于0，说明本方法具有较好的跟踪控制效果。

5 结语

本文针对四旋翼无人机的实际应用需求，开展轨迹跟踪控制研究。首先根据四旋翼无人机动力学方程搭建了可靠的四旋翼系统模型；然后根据无人机飞行避障及自身机动性能需求，提出一种RRT-Dubins路径规划方法，以规划一条轨迹平滑可满足无人机飞行需求的路径；最后完成了轨迹跟踪控制方案设计，对所提出的RRT-Dubins路径进行了轨迹跟踪仿真。该算法仿真实验表明了本文设计的位置—姿态控制器的可行性，同时采用本文RRT-Dubins算法规划的路径可以较好地规避障碍区域，而且较未采用Dubins路径的算法，轨迹更平滑，更适合无人机的飞行路径。

[1] 王成，杨杰，姚辉，等. 四旋翼无人机飞行控制算法综述[J]. 电光与控制，2018，25（12）：53-58.

[2] Liu Hao，Xi Jianxiang，Zhong Yisheng. Robust motion control of quadrotors[J]. Journal of the Franklin Institute，2014，351（12）：5494-5510.

[3] 杨柳，刘金琨. 基于干扰观测器的四旋翼无人机轨迹跟踪鲁棒控制[J]. 飞行力学，2015（4）：6.

[4] Kendoul F. Survey of advances in guidance, navigation，and control of unmanned rotorcraft systems[J]. Journal of Field Robotics，2012，29（2）：315-378.

[5] 张天航，白金平. 旋翼式无人机的发展和趋势[J]. 人工智能与机器人研究，2013，2（1）：16-23.

[6] 郭毅. 四旋翼飞行器飞行路径规划及控制方法研究[D]. 赣州：江西理工大学，2019.

[7] 王春阳，姜明瑞，史红伟. 四旋翼无人机轨迹跟踪控制系统设计[J]. 电光与控制，2019（3）：5.

[8] 左宗玉. 四旋翼无人飞行器自适应轨迹跟踪控制[C]//中国自动化学会控制理论专业委员会B卷. 2011.

[9] 简康. 无人机航迹规划算法研究[D]. 西安：西安电子科技大学，2014.

[10] 魏涛. 无人机路径规划算法研究[D]. 重庆：重庆邮电大学，2020.

[11] 张艺巍，谭建豪，王耀南. 3维复杂山地环境下旋翼无人飞行器高时效航迹规划策略[J]. 机器人，2016，38（6）：11.

[12] 任鹏博，董泽华. 基于RRT算法的无人机路径规划应用研究[J]. 现代导航，2022（1）：13.

[13] 田晓亮. 无人机航迹规划方法研究[D]. 西安：西安电子科技大学，2014.

[14] 刘乐柱，肖长诗，文元桥. 基于Dubins路径的无人艇运动规划算法[J]. 计算机应用，2017，37（7）：5.

[15] Tsourdos A，White B，Shanmugavel M. Cooperative path planning of unmanned aerial vehicles[M]. Hoboken：Wiley，2010.

Research on Trajectory Planning and Tracking Control of Quadrotor Aircraft

WANG Qiong, ZHENG Chen, JIANG Yu, WU Peili

Aiming at the practical application requirements of quadrotor Unmanned Aerial Vehicle (UAV), the trajectory tracking control research is carried out. A Dubins trajectory planning method based on Rapidly-exploring Random Tree (RRT) algorithm is proposed, and a quadrotor model is built. A trajectory tracking control method is designed to perform trajectory tracking simulation on the proposed planned path. The RRT-Dubins algorithm uses the RRT algorithm to effectively plan the path of UAV in obstacle areas, and then uses the Dubins path to smooth the planned trajectory to form a flight path for UAVs. Simulation experiments show that the trajectory planning method and path proposed in the paper can better avoid obstacle areas, and the trajectory smoothing is more suitable for UAV flight. At the same time, the effectiveness of the proposed trajectory tracking control method is verified.

Quadrotor Unmanned Aerial Vehicle; Trajectory Planning; Trajectory Tracking; Rapidly-exploring Random Tree Algorithm; Dubins Path

V279

A

1674-7976-(2023)-05-333-06

2023-03-31。

王琼（1989.08—），陕西西安人，博士，工程师，主要研究方向为无人机导航算法。