王 瓊，鄭 晨，姜 昱，吳培利

四旋翼無人機的軌跡規(guī)劃跟蹤控制研究

王 瓊1,2，鄭 晨1,2，姜 昱1,2，吳培利3

（1 中國電子科技集團第二十研究所，西安 710068；2 陜西省組合與智能導航重點實驗室，西安 710068；3 中國兵器工業(yè)集團第二一二研究所，西安 710065）

針對四旋翼無人機的實際應用需求，開展軌跡跟蹤控制研究。提出了一種基于快速擴展隨機樹（RRT）算法的Dubins航跡規(guī)劃方法，同時搭建了四旋翼模型，并設(shè)計了軌跡跟蹤控制方案對所提出的規(guī)劃路徑進行了軌跡跟蹤仿真。該RRT-Dubins算法采用RRT算法對有障礙區(qū)域的無人機路徑進行有效規(guī)劃，然后利用Dubins路徑對規(guī)劃出的軌跡進行平滑處理，以形成一條無人機可飛行路徑。仿真實驗表明，采用所提出的軌跡規(guī)劃方法及路徑可以較好地規(guī)避障礙區(qū)域，且軌跡平滑更適合無人機飛行，同時驗證了所提軌跡跟蹤控制方案的有效性。

四旋翼無人機；軌跡規(guī)劃；軌跡跟蹤；快速擴展隨機樹算法；Dubins路徑

0 引言

四旋翼無人機因其具有垂直起降、自主懸停、控制靈活且成本低等優(yōu)點，在民用乃至軍事領(lǐng)域都得到了廣泛的應用[1]。而四旋翼無人機作為一種典型的多變量、非線性、強耦合的欠驅(qū)動系統(tǒng)，在飛行過程中還容易受到大風等環(huán)境天氣的干擾，這些對于四旋翼系統(tǒng)的跟蹤控制都會產(chǎn)生影響[2-3]。

四旋翼無人機的應用場景廣泛，例如快遞輸送、搶險救災、事故調(diào)查、野生動物攝影和反恐偵查 等[4-5]。而這些大多數(shù)的應用場合都期望可以安全穩(wěn)定地完成飛行任務。傳統(tǒng)的四旋翼無人機是由人工遙控操作控制的，但人工操作工作量較大，操作員容易產(chǎn)生視覺疲勞，因此如何讓四旋翼無人機自主避障并尋到一條最優(yōu)或可行的飛行軌跡是任務系統(tǒng)的核心之一[6]。除此之外，持續(xù)高精度地按照期望軌跡飛行，亦是完成飛行任務不可或缺的重點，所以能夠自動精確地對軌跡進行跟蹤控制是四旋翼無人機必須具備的能力[7]。

本文針對四旋翼無人機的實際應用需求，在滿足地勢限制、自身機動性能和外部威脅等一定約束條件下進行飛行軌跡規(guī)劃，采用快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）算法對復雜環(huán)境下的無人機進行航跡預規(guī)劃，同時用Dubins路徑對航跡進行優(yōu)化；然后搭建了軌跡跟蹤實驗平臺，設(shè)計了一種位置—姿態(tài)控制器，實現(xiàn)對四旋翼無人機的預定軌跡跟蹤；最后對規(guī)劃的航跡路線進行了跟蹤飛行仿真，實驗結(jié)果表明本文所述方法的正確性和有效性。

1 四旋翼無人機運動模型

為建立四旋翼無人機的運動模型，首先假設(shè)四旋翼無人機為剛體，且不考慮無人機飛行過程中所受的氣動力矩。

圖1 四旋翼無人機模型及坐標系定義

根據(jù)牛頓定律，四旋翼無人機的動力學方程如式（2）和式（3）所示[8]

2 軌跡規(guī)劃算法

2.1 RRT算法

1998年，La Valle提出了RRT算法[9]。相比于傳統(tǒng)的路徑規(guī)劃算法，該算法具有無需事先對任務進行空間建模的優(yōu)勢，本質(zhì)是在給定地圖中生成一顆隨機樹來尋找目標點[12]。以探索點的隨機性保證了目標點探索的概率完備性，有利于復雜環(huán)境下的路徑探索，可以解決具有非完整約束的路徑規(guī)劃問題[11]。

RRT算法的偽代碼如下所示。

continue

else

end if

break

end if

end while

2.2 Dubins路徑

無人機以一定速度轉(zhuǎn)向時，轉(zhuǎn)向半徑需要大于在此速度下的極小半徑，基于Dubins路徑的路徑規(guī)劃方法可以有效解決此問題[14]。

Dubins路徑可簡單定義為：在最大曲率半徑限制下，平面內(nèi)兩個有方向的點間的最短可行路徑是CLC路徑、CCC路徑或它們的子集，其中C代表圓弧段路徑，L代表與圓弧段路徑相切的直線段路徑[15]，如圖2所示。

圖2 Dubins路徑

將數(shù)條C和L按照一定的順序平滑連接起來就可以構(gòu)成一條由起點到終點的無人機可飛行路徑。這些基本曲線段的長度和方向可以根據(jù)無人機的轉(zhuǎn)彎半徑和路徑長度來計算。

Dubins路徑的長度可以通過以下三個步驟來計算：

1）計算無人機的轉(zhuǎn)彎半徑，并確定可以使用的基本曲線段；

2）根據(jù)起點和目標點的位置和方向計算出最短路徑的基本曲線段序列；

3）計算每個基本曲線段的長度，并將它們相加得到Dubins路徑的總長度。

3 軌跡跟蹤控制設(shè)計

3.1 總體設(shè)計思路

本文采用內(nèi)外環(huán)的控制策略，其中內(nèi)環(huán)對四旋翼姿態(tài)角進行控制，而外環(huán)對位置進行控制，如 圖3所示。

圖3 四旋翼無人機控制系統(tǒng)結(jié)構(gòu)圖

3.2 位置控制器設(shè)計

式中

將式（6）帶入式（2）可得

式中

3.3 姿態(tài)控制器設(shè)計

4 仿真與分析

為了驗證本文所提算法的有效性，首先在Matlab/Simulink中搭建了四旋翼無人機的框架模型，如圖4所示。

圖4 四旋翼無人機系統(tǒng)框架模型

四旋翼無人機規(guī)劃軌跡起點(25,25,25)，終點(150,140,35)，分別采用RRT算法及RRT-Dubins路徑算法在有障礙物的區(qū)域進行多次航跡規(guī)劃，仿真結(jié)果如圖5和圖6所示。

圖5 航跡規(guī)劃對比圖

圖6 航跡規(guī)劃對比圖

從圖5和圖6可以看出，兩組仿真實驗中，RRT算法和RRT-Dubins路徑都可以在有障礙的區(qū)域內(nèi)完成四旋翼無人機的軌跡規(guī)劃，但是RRT-Dubins路徑較未采用Dubins路徑規(guī)劃出的軌跡更為平滑，更加適合用于無人機的飛行路徑規(guī)劃。

下面采用位置—姿態(tài)控制器對規(guī)劃的軌跡進行跟蹤控制仿真，并采用Matlab/Simulink中的無人機仿真動畫模塊UAV Animation進行跟蹤曲線顯示，如圖7所示。

圖7 UAV Animation跟蹤模塊

仿真中四旋翼無人機從原點起飛，對本文所述RRT-Dubins路徑規(guī)劃軌跡進行飛行跟蹤，如圖8所示，無人機跟蹤軌跡及跟蹤誤差如圖9和圖10所示。

圖8 規(guī)劃軌跡示意圖

圖9 四旋翼無人機跟蹤飛行軌跡

圖10 四旋翼跟蹤誤差

仿真中無人機初始位置誤差為(25,25,25)，軌跡跟蹤過程中，無人機從地面起飛在高度目標被跟蹤到之后基本沒有產(chǎn)生變化；而水平方向由于目標軌跡變化較快，在每次機動飛行產(chǎn)生變化的時候，跟蹤誤差都會有一些小幅度的變化，當最后四旋翼無人機懸停在目標點附近時，跟蹤誤差基本趨近于0，說明本方法具有較好的跟蹤控制效果。

5 結(jié)語

本文針對四旋翼無人機的實際應用需求，開展軌跡跟蹤控制研究。首先根據(jù)四旋翼無人機動力學方程搭建了可靠的四旋翼系統(tǒng)模型；然后根據(jù)無人機飛行避障及自身機動性能需求，提出一種RRT-Dubins路徑規(guī)劃方法，以規(guī)劃一條軌跡平滑可滿足無人機飛行需求的路徑；最后完成了軌跡跟蹤控制方案設(shè)計，對所提出的RRT-Dubins路徑進行了軌跡跟蹤仿真。該算法仿真實驗表明了本文設(shè)計的位置—姿態(tài)控制器的可行性，同時采用本文RRT-Dubins算法規(guī)劃的路徑可以較好地規(guī)避障礙區(qū)域，而且較未采用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] 張?zhí)旌?，白金? 旋翼式無人機的發(fā)展和趨勢[J]. 人工智能與機器人研究，2013，2（1）：16-23.

[6] 郭毅. 四旋翼飛行器飛行路徑規(guī)劃及控制方法研究[D]. 贛州：江西理工大學，2019.

[7] 王春陽，姜明瑞，史紅偉. 四旋翼無人機軌跡跟蹤控制系統(tǒng)設(shè)計[J]. 電光與控制，2019（3）：5.

[8] 左宗玉. 四旋翼無人飛行器自適應軌跡跟蹤控制[C]//中國自動化學會控制理論專業(yè)委員會B卷. 2011.

[9] 簡康. 無人機航跡規(guī)劃算法研究[D]. 西安：西安電子科技大學，2014.

[10] 魏濤. 無人機路徑規(guī)劃算法研究[D]. 重慶：重慶郵電大學，2020.

[11] 張藝巍，譚建豪，王耀南. 3維復雜山地環(huán)境下旋翼無人飛行器高時效航跡規(guī)劃策略[J]. 機器人，2016，38（6）：11.

[12] 任鵬博，董澤華. 基于RRT算法的無人機路徑規(guī)劃應用研究[J]. 現(xiàn)代導航，2022（1）：13.

[13] 田曉亮. 無人機航跡規(guī)劃方法研究[D]. 西安：西安電子科技大學，2014.

[14] 劉樂柱，肖長詩，文元橋. 基于Dubins路徑的無人艇運動規(guī)劃算法[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—），陜西西安人，博士，工程師，主要研究方向為無人機導航算法。