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

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

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

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

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

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

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

0 引言

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

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

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

1 四旋翼無人機(jī)運(yùn)動模型

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

圖1 四旋翼無人機(jī)模型及坐標(biāo)系定義

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

2 軌跡規(guī)劃算法

2.1 RRT算法

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

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

continue

else

end if

break

end if

end while

2.2 Dubins路徑

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

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

圖2 Dubins路徑

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

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

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

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

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

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

3.1 總體設(shè)計思路

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

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

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

式中

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

式中

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

4 仿真與分析

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

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

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

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

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

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

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

圖7 UAV Animation跟蹤模塊

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

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

圖9 四旋翼無人機(jī)跟蹤飛行軌跡

圖10 四旋翼跟蹤誤差

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

5 結(jié)語

本文針對四旋翼無人機(jī)的實際應(yīng)用需求，開展軌跡跟蹤控制研究。首先根據(jù)四旋翼無人機(jī)動力學(xué)方程搭建了可靠的四旋翼系統(tǒng)模型；然后根據(jù)無人機(jī)飛行避障及自身機(jī)動性能需求，提出一種RRT-Dubins路徑規(guī)劃方法，以規(guī)劃一條軌跡平滑可滿足無人機(jī)飛行需求的路徑；最后完成了軌跡跟蹤控制方案設(shè)計，對所提出的RRT-Dubins路徑進(jìn)行了軌跡跟蹤仿真。該算法仿真實驗表明了本文設(shè)計的位置—姿態(tài)控制器的可行性，同時采用本文RRT-Dubins算法規(guī)劃的路徑可以較好地規(guī)避障礙區(qū)域，而且較未采用Dubins路徑的算法，軌跡更平滑，更適合無人機(jī)的飛行路徑。

[1] 王成，楊杰，姚輝，等. 四旋翼無人機(jī)飛行控制算法綜述[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ī)軌跡跟蹤魯棒控制[J]. 飛行力學(xué)，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í)旌?，白金? 旋翼式無人機(jī)的發(fā)展和趨勢[J]. 人工智能與機(jī)器人研究，2013，2（1）：16-23.

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

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

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

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

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

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

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

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

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