基于航跡規(guī)劃的四旋翼飛行器軌跡跟蹤控制

丁 力， 柴華偉， 李興成

(1.江蘇理工學(xué)院機(jī)械工程學(xué)院，江蘇 常州 213001； 2.香港理工大學(xué)工業(yè)及系統(tǒng)工程學(xué)系，香港 999077)

0 引言

隨著機(jī)器人技術(shù)、控制技術(shù)、通信技術(shù)的發(fā)展，四旋翼被廣泛應(yīng)用于生產(chǎn)生活，如建筑現(xiàn)場測繪與管理、橋梁架線、電力巡線、地理建模等[1-2]。當(dāng)四旋翼在城市中自主飛行時(shí)，航跡規(guī)劃與飛行控制是必不可少的環(huán)節(jié)。然而與其他空中機(jī)器人一樣，四旋翼具有強(qiáng)耦合、欠驅(qū)動(dòng)、非線性的多輸入多輸出特性，要實(shí)現(xiàn)能夠避障、高穩(wěn)定性的飛行作業(yè)任務(wù)具有一定的難度。因此，開發(fā)高效率的航跡規(guī)劃算法和高精度的飛行控制策略是工業(yè)領(lǐng)域和學(xué)術(shù)領(lǐng)域共同關(guān)注的熱點(diǎn)。

對于航跡規(guī)劃，很多學(xué)者僅簡單地將無人機(jī)看作是一個(gè)質(zhì)點(diǎn)，并采用智能算法對其進(jìn)行航跡規(guī)劃。例如，WANG等[3]提出一種改進(jìn)的BA算法來實(shí)現(xiàn)無人機(jī)的三維航跡規(guī)劃，并在仿真環(huán)境下獲得了平滑可飛的航跡；在處理編隊(duì)無人機(jī)三維航跡規(guī)劃時(shí)，YANG等[4]采用自適應(yīng)調(diào)整策略來改進(jìn)PSO算法以避免陷入局部最優(yōu)值，有效實(shí)現(xiàn)了無人機(jī)在三維空間中的編隊(duì)飛行；SHORAKAEI等[5]提出一種并行GA算法來規(guī)劃無人機(jī)的二維航跡，改進(jìn)算法可處理障礙物突變問題。但上述航跡規(guī)劃過程忽略了無人機(jī)的飛行力學(xué)特性，像四旋翼的姿態(tài)角對陣風(fēng)干擾、機(jī)械振動(dòng)等極為敏感，微小的擾動(dòng)就可能會(huì)導(dǎo)致墜機(jī)。因此，四旋翼的航跡規(guī)劃必須考慮其飛行力學(xué)特性。

對于飛行控制策略的設(shè)計(jì)，常見的方法有LQR控制[6]、PID控制[7]、H∞控制[8]等。近年來，學(xué)者們發(fā)現(xiàn)在穩(wěn)定位姿控制的基礎(chǔ)上，當(dāng)四旋翼受到外界擾動(dòng)時(shí)，其動(dòng)力學(xué)模型也會(huì)發(fā)生改變。為此，所設(shè)計(jì)的控制算法必須能有效抑制未知擾動(dòng)，并且具有一定的魯棒性。例如:文獻(xiàn)[9]提出了一種非線性自適應(yīng)狀態(tài)反饋控制器來補(bǔ)償未知擾動(dòng)，實(shí)現(xiàn)了四旋翼對“8”字型軌跡的跟蹤；文獻(xiàn)[10]利用擴(kuò)張觀測器實(shí)現(xiàn)對系統(tǒng)狀態(tài)及集總干擾的準(zhǔn)確估計(jì)，并設(shè)計(jì)級聯(lián)式比例控制器保證四旋翼在低速條件下的穩(wěn)定飛行。

本文另辟蹊徑，針對四旋翼的航跡規(guī)劃與飛行控制問題，提出了一種改進(jìn)人工蜂群(IABC)算法來實(shí)現(xiàn)四旋翼在三維空間中的航跡規(guī)劃，并引入LADRC(Linear Active Disturbance Rejection Control)控制策略來設(shè)計(jì)其位姿鎮(zhèn)定控制器。通過IABC算法可獲得最優(yōu)或次優(yōu)的飛行航跡，若讓四旋翼簡單地跟蹤這些離散航點(diǎn)，則整個(gè)飛行過程可被視作多個(gè)階躍信號的跟蹤。根據(jù)飛行力學(xué)理論可知，這樣的跟蹤方式并不能滿足四旋翼飛行力學(xué)特性的要求(例如相鄰兩航點(diǎn)處的速度不連續(xù))，從而會(huì)降低四旋翼的飛行品質(zhì)甚至引起其失穩(wěn)。因此，本文還引入高階多項(xiàng)式函數(shù)來平滑IABC算法所獲得的初始航跡。最后，通過一個(gè)仿真算例驗(yàn)證了本文所提方法的有效性。

1 航跡規(guī)劃問題描述

1.1 四旋翼的動(dòng)力學(xué)模型

在四旋翼的三維航跡規(guī)劃中，往往存在轉(zhuǎn)向、加速、低速巡航等飛行狀態(tài)，故不能忽視飛行動(dòng)力學(xué)特性。而在飛行力學(xué)層面，可將四旋翼視作沿法向驅(qū)動(dòng)力自由運(yùn)動(dòng)的單剛體模型，如圖1所示。

圖1 四旋翼的結(jié)構(gòu)示意圖Fig.1 Schematic diagram of a quadrotor

根據(jù)牛頓-歐拉法可以推導(dǎo)其力學(xué)方程為

(1)

(2)

1.2 障礙物建模

對于航跡規(guī)劃中障礙物的建模，現(xiàn)有的報(bào)道大多將其簡化成一個(gè)圓柱體，這樣的處理一定程度上可降低規(guī)劃算法的計(jì)算量。四旋翼在城市間的航跡規(guī)劃，考慮到建筑物大多為長方體，本文引入長方體作為障礙物模型。對于航點(diǎn)P，若它在長方體內(nèi)或表面上，則判斷其沒有避障，具體的數(shù)學(xué)實(shí)現(xiàn)過程如下：

2) 計(jì)算各頂點(diǎn)與航點(diǎn)P組成矩陣的行列式值，即

(3)

3) 若矩陣D1～D5的行列式符號均相同(同為正或同為負(fù))，則判斷航點(diǎn)P在長方體障礙物內(nèi)或表面上，不避障,否則，航點(diǎn)P避障。

另外，為進(jìn)一步保證四旋翼的飛行安全，還需設(shè)置一個(gè)安全飛行距離l，故可將長方體障礙物的邊界設(shè)為

(4)

1.3 航跡代價(jià)的建模

通常，最優(yōu)的飛行航跡由航跡代價(jià)函數(shù)決定。在四旋翼飛行過程中主要考慮航程與油耗兩個(gè)因素。本文利用Lagrange方程對航跡代價(jià)進(jìn)行建模[13]，即

(5)

式中：CCost為航跡代價(jià)函數(shù);C1為航程代價(jià)函數(shù);C2為油耗代價(jià)函數(shù);w1與w2為權(quán)重因子，本文均取為0.5;T1為時(shí)間上限。

2 航跡規(guī)劃算法

2.1 人工蜂群算法

ABC算法的詳細(xì)步驟如下所述。

1) 算法初始化。在解空間內(nèi)，對于當(dāng)前待搜索航點(diǎn)位置可通過式(6)隨機(jī)產(chǎn)生xi j(i=1,2,…,N;j=1,2,…,D)組解，即隨機(jī)蜜源組。

(6)

2) 收益度評價(jià)。通過式(7)來計(jì)算每個(gè)蜜源的收益度值，也就是每組航跡代價(jià)的評價(jià)值。進(jìn)而，保留前N/2高評價(jià)值的航跡。

Fi=1/(1+CCosti)

>。

(7)

(8)

(9)

式中：r∈[1,2,…,N/2](r≠i)；Φi j為[-1,1]間的隨機(jī)數(shù)；ω∈[ωmin,ωmax]為權(quán)重因子；T2∈[1,Tmax]為迭代次數(shù)。

4) 貪婪選擇階段。根據(jù)貪婪選擇方式保留航跡代價(jià)較小的解，促使算法朝最優(yōu)方向進(jìn)化。

(10)

5) 跟隨蜂搜索階段。在此階段，跟隨蜂通過概率值選擇雇傭蜂進(jìn)行跟蹤搜索。傳統(tǒng)ABC算法中跟隨蜂的選擇概率與收益度值呈正相關(guān)關(guān)系，多次迭代后，會(huì)過濾掉較差的解，但這些解同樣也包含著有用信息。為了保持種群多樣性，本文采用如下概率選擇方式，即

(11)

6) 偵察蜂搜索階段。當(dāng)更新次數(shù)超過某個(gè)閾值時(shí)，若當(dāng)前蜜源不是最優(yōu)解，且搜索空間中存在最優(yōu)解，則放棄該蜜源，與之對應(yīng)的雇傭蜂也轉(zhuǎn)變成偵察蜂重新去尋找蜜源。為了提高偵察蜂對數(shù)據(jù)的挖掘能力以及避免ABC算法陷入局部最優(yōu)值，本文引入立方混沌映射算子來改進(jìn)其性能，如式(12)所示。這里需指出，文獻(xiàn)[14]通過縝密的數(shù)學(xué)推理證明了立方混沌映射算子比常見的Logistic混沌映射算子具有更好的均勻遍歷性與更快的收斂速度。

y(n+1)=4y(n)3-3y(n)

(12)

式中,y(n)∈[0,1]，n=1,2,…,M,M為偵察蜂的數(shù)量。將生成的混沌序列映射到偵察蜂搜索到的蜜源空間內(nèi)，實(shí)現(xiàn)對新蜜源的混沌搜索，具體過程可參考文獻(xiàn)[14]，這里不再贅述。

7) 最優(yōu)值記錄。當(dāng)?shù)螖?shù)達(dá)到最大值時(shí)，算法輸出最優(yōu)的航跡點(diǎn)序列以及迭代曲線。

2.2 航跡平滑

對于IABC算法優(yōu)化而來的最優(yōu)或次優(yōu)飛行航跡，學(xué)者們都希望能以最短的時(shí)間完成全部的參考航跡跟蹤，同時(shí)又能利于四旋翼姿態(tài)的控制。本文先用高階多項(xiàng)式函數(shù)對IABC算法求得航點(diǎn)進(jìn)行插值處理，得到一條光滑可飛的新航跡,進(jìn)而引入速度項(xiàng)和加速度項(xiàng)建立線性規(guī)劃模型，計(jì)算出最優(yōu)速度序列，使得四旋翼飛行時(shí)間最優(yōu)。

為了獲得兩航點(diǎn)Po與Pf之間的光滑曲線，采用高階多項(xiàng)式函數(shù)(五階多項(xiàng)式)進(jìn)行插補(bǔ)計(jì)算，即

P(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(13)

其約束條件為

上述約束條件可以確定兩航點(diǎn)間的速度和加速度，并保證航點(diǎn)間的速度是連續(xù)的。

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

LADRC控制理論是高志強(qiáng)教授在韓京清教授提出的自抗擾控制基礎(chǔ)上改進(jìn)而來的[15]。與自抗擾控制相比，LADRC的擴(kuò)張觀測器與控制器均是線性的，并且不使用微分跟蹤器，結(jié)構(gòu)更為簡單。另外，LADRC可將控制器參數(shù)降到3個(gè)，即觀測器ωo、控制器帶寬ωc及增益b0。觀察四旋翼的動(dòng)力學(xué)模型(式(2))可知，位置與姿態(tài)的狀態(tài)變量分別與控制輸入呈二階導(dǎo)數(shù)關(guān)系，故可以設(shè)計(jì)6個(gè)二階LADRC控制器分別鎮(zhèn)定四旋翼的位置環(huán)與姿態(tài)環(huán)。

3.1 二階LADRC控制原理

不失一般性的，對于一個(gè)二階被控對象

(14)

式中：x為系統(tǒng)的狀態(tài)變量；u為系統(tǒng)的輸入；y為系統(tǒng)的輸出量；w為系統(tǒng)的擾動(dòng)量。

(15)

式中：b0為b(t)的估計(jì)值。因此，有

(16)

(17)

采用線性擴(kuò)張觀測器對式(17)進(jìn)行觀測，則有

(18)

(19)

3.2 四旋翼軌跡跟蹤控制框圖

根據(jù)時(shí)間尺度原理，可將四旋翼的動(dòng)力學(xué)模型分為位置與姿態(tài)兩個(gè)控制環(huán)，并分別設(shè)計(jì)二階LADRC控制器，如圖2所示。其中，(Xr,Yr,Zr)為參考軌跡，(φr,θr,ψr)為控制過程中產(chǎn)生的中間姿態(tài)參考值。

圖2 四旋翼位姿控制框圖Fig.2 Position and attitude control for a quadrotor

4 仿真分析

本章通過一個(gè)仿真算例來驗(yàn)證所提基于IABC算法的四旋翼三維航跡規(guī)劃以及基于LADRC的軌跡跟蹤控制的有效性。設(shè)置仿真環(huán)境為：飛行空間為12 m×8 m×10 m ，環(huán)境中存在4個(gè)長方體障礙物；飛行的起始位置為(1 m,-4 m,1 m)，目標(biāo)位置為(8 m,7 m,5 m)，安全飛行距離l=0.1 m；飛行要求為從起始點(diǎn)位置到目標(biāo)位置的航程短且避障、飛行耗時(shí)少；四旋翼的質(zhì)量m=0.2 kg，臂長為0.1 m，轉(zhuǎn)動(dòng)慣量為Ixx=Iyy=2×10-4kg·m2，Izz=3.7×10-4kg·m2；偏航角設(shè)為ψr=0 rad。

整個(gè)仿真過程分為兩步：1) 用尋優(yōu)算法找出一條航跡代價(jià)值最小的航跡，并對其進(jìn)行平滑處理；2) 將處理后的平滑航跡作為飛行控制器的參考軌跡，通過LADRC控制器實(shí)現(xiàn)對參考軌跡的跟蹤。設(shè)置IABC算法的仿真條件為：N=30，llimit=5。同時(shí)，分別引入ABC算法和GA算法來比較IABC算法的性能。ABC算法的條件與IABC算法相同，GA算法的仿真條件為：抗體種群數(shù)為30，交叉因子與變異因子分別為0.8與0.2。考慮到航跡規(guī)劃的時(shí)效性，設(shè)置3種算法的迭代總次數(shù)為20。另外，每種算法均重復(fù)10次，記錄下每次的規(guī)劃效果，并取其中最優(yōu)結(jié)果作為參考。

航跡規(guī)劃的效果如圖3和圖4所示。從圖中可以看出，3種算法均能為四旋翼規(guī)劃出一條安全避障的飛行軌跡，但這些軌跡不可飛；高階多項(xiàng)式函數(shù)對規(guī)劃的軌跡進(jìn)行了插值處理，使得軌跡平滑可飛。相比而言，IABC算法規(guī)劃的效果最好。這表明IABC算法對數(shù)據(jù)的開采能力更強(qiáng)。圖5給出了3種算法迭代曲線，可以看出,IABC算法的收斂速度明顯高于ABC算法與GA算法，且能夠挖掘到更好的解。這表明改進(jìn)后的ABC算法無論在收斂速度上還是數(shù)據(jù)挖掘上都有很大的性能提升。

圖3 3D航跡規(guī)劃結(jié)果Fig.3 3D path planning result

圖4 XY平面內(nèi)的規(guī)劃效果Fig.4 Result of path planning in XY plane

進(jìn)一步地，表1給出了3種算法的尋優(yōu)性能指標(biāo)對比。從表中可以看出，IABC算法規(guī)劃的航跡長度是最短的，分別比ABC算法、GA算法縮短了0.9%和3.18%。由于IABC算法中引入了自適應(yīng)策略、新的概率選擇方式及立方混沌搜索算子，其優(yōu)化效率不及ABC算法，且優(yōu)化成功率也低于后者。但總體來講，80%的成功率還是說明IABC算法具有較高的可靠性。

表1 3種算法的性能指標(biāo)對比

表2給出了3種航跡平滑后的性能指標(biāo)對比。從表2中可以看出，經(jīng)平滑后IABC-1獲得的航跡長度還是最短的，飛行時(shí)間也最少，并且能夠有效避障。

表2 3種航跡平滑后的性能指標(biāo)對比

將平滑處理的航跡IABC-1作為四旋翼的參考軌跡，并在測量端添加幅值為0.05的隨機(jī)擾動(dòng)。在二階LADRC控制器的作用下，四旋翼的三維軌跡跟蹤效果如圖6和圖7所示。從圖中可以看出，位置控制器基本上可以跟蹤參考軌跡。由于擾動(dòng)的存在，前期的跟蹤超調(diào)比較大，但是LADRC還是可以迅速地抑制擾動(dòng)，減小跟蹤誤差。

圖6 三維軌跡跟蹤結(jié)果Fig.6 Result of 3D trajectory tracking

圖8和圖9給出了俯仰角和橫滾角的響應(yīng)曲線，姿態(tài)角的參考值由位置環(huán)給出。從圖中可以看出，俯仰角和橫滾角均在1 s左右跟蹤上參考值，可以滿足四旋翼實(shí)際操控的需求。另外，兩個(gè)姿態(tài)角的穩(wěn)態(tài)誤差均能夠保持在±0.02 rad內(nèi)。這表明LADRC控制器魯棒性強(qiáng)、響應(yīng)速度快及控制精度高。

圖7 XY平面內(nèi)跟蹤軌跡Fig.7 Tracking trajectory in XY plane

圖8 俯仰角的響應(yīng)Fig.8 Response of pitch angle

圖9 橫滾角的響應(yīng)Fig.9 Response of roll angle

5 結(jié)論

提出了一種改進(jìn)的人工蜂群算法來實(shí)現(xiàn)四旋翼的航跡規(guī)劃。算法中，在雇傭蜂搜索階段引入自適應(yīng)策略、跟隨蜂搜索階段引入一個(gè)新的概率選擇方式、偵察蜂搜索階段引入立方映射混沌算子來提高收斂速度，避免陷入局部最優(yōu)值。

根據(jù)四旋翼動(dòng)力學(xué)特性，設(shè)計(jì)位置環(huán)與姿態(tài)環(huán)二階LADRC控制器。以高階多項(xiàng)式函數(shù)平滑后的航跡為參考軌跡，實(shí)現(xiàn)高精度的抗干擾軌跡跟蹤控制。

仿真結(jié)果表明，IABC算法的迭代速率、規(guī)劃結(jié)果均優(yōu)于ABC算法與GA算法，改進(jìn)后的ABC算法具有更強(qiáng)的數(shù)據(jù)挖掘能力與魯棒性；所設(shè)計(jì)的二階LADRC控制器能夠有效抑制擾動(dòng)，使四旋翼快速跟蹤上參考軌跡。