丁 力， 柴華偉， 李興成

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

0 引言

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

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

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

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

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

1.1 四旋翼的動力學模型

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

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

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

(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避障。

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

(4)

1.3 航跡代價的建模

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

(5)

式中：CCost為航跡代價函數(shù);C1為航程代價函數(shù);C2為油耗代價函數(shù);w1與w2為權(quán)重因子，本文均取為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]為權(quán)重因子；T2∈[1,Tmax]為迭代次數(shù)。

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

(10)

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

(11)

6) 偵察蜂搜索階段。當更新次數(shù)超過某個閾值時，若當前蜜源不是最優(yōu)解，且搜索空間中存在最優(yōu)解，則放棄該蜜源，與之對應(yīng)的雇傭蜂也轉(zhuǎn)變成偵察蜂重新去尋找蜜源。為了提高偵察蜂對數(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 軌跡跟蹤控制器設(shè)計

LADRC控制理論是高志強教授在韓京清教授提出的自抗擾控制基礎(chǔ)上改進而來的[15]。與自抗擾控制相比，LADRC的擴張觀測器與控制器均是線性的，并且不使用微分跟蹤器，結(jié)構(gòu)更為簡單。另外，LADRC可將控制器參數(shù)降到3個，即觀測器ωo、控制器帶寬ωc及增益b0。觀察四旋翼的動力學模型(式(2))可知，位置與姿態(tài)的狀態(tài)變量分別與控制輸入呈二階導(dǎo)數(shù)關(guān)系，故可以設(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)，并分別設(shè)計二階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的軌跡跟蹤控制的有效性。設(shè)置仿真環(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；飛行要求為從起始點位置到目標位置的航程短且避障、飛行耗時少；四旋翼的質(zhì)量m=0.2 kg，臂長為0.1 m，轉(zhuǎn)動慣量為Ixx=Iyy=2×10-4kg·m2，Izz=3.7×10-4kg·m2；偏航角設(shè)為ψr=0 rad。

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

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

進一步地，表1給出了3種算法的尋優(yōu)性能指標對比。從表中可以看出，IABC算法規(guī)劃的航跡長度是最短的，分別比ABC算法、GA算法縮短了0.9%和3.18%。由于IABC算法中引入了自適應(yīng)策略、新的概率選擇方式及立方混沌搜索算子，其優(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 三維軌跡跟蹤結(jié)果Fig.6 Result of 3D trajectory tracking

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

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

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

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