丁 力， 柴華偉， 李興成

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

0 引言

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

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

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

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

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

1.1 四旋翼的動力學模型

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

圖1 四旋翼的結構示意圖Fig.1 Schematic diagram of a quadrotor

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

(1)

(2)

1.2 障礙物建模

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

2) 計算各頂點與航點P組成矩陣的行列式值，即

(3)

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

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

(4)

1.3 航跡代價的建模

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

(5)

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

2 航跡規(guī)劃算法

2.1 人工蜂群算法

ABC算法的詳細步驟如下所述。

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

(6)

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

Fi=1/(1+CCosti)

>。

(7)

(8)

(9)

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

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

(10)

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

(11)

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

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

(12)

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

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

2.2 航跡平滑

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

為了獲得兩航點Po與Pf之間的光滑曲線，采用高階多項式函數(shù)(五階多項式)進行插補計算，即

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

(13)

其約束條件為

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

3 軌跡跟蹤控制器設計

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

3.1 二階LADRC控制原理

不失一般性的，對于一個二階被控對象

(14)

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

(15)

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

(16)

(17)

采用線性擴張觀測器對式(17)進行觀測，則有

(18)

(19)

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

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

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

4 仿真分析

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

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

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

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

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

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

表1 3種算法的性能指標對比

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

表2 3種航跡平滑后的性能指標對比

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

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

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

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

圖8 俯仰角的響應Fig.8 Response of pitch angle

圖9 橫滾角的響應Fig.9 Response of roll angle

5 結論

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

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

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