基于自抗擾的多旋翼全姿態(tài)矢量控制方法

羅寓文, 朱玉玉

(西南科技大學 信息工程學院,四川 綿陽 621010)

0 引 言

由多旋翼為代表的小型無人機(unmanned aerial vehicle,UAV)在機器人領域內(nèi)得到了大家的廣泛關注。在傳統(tǒng)的捷聯(lián)慣導系統(tǒng)導航控制中,絕大多數(shù)的多旋翼姿態(tài)控制還是以歐拉角作為姿態(tài)環(huán)的被控對象分別控制偏航角、俯仰角、橫滾角三個自由度。這種方法不可避免地忽略了歐拉角表示剛體旋轉的奇異點也就是萬向節(jié)死鎖問題[1],由于歐拉角的萬向節(jié)死鎖問題,無人機在處于機頭向上垂直姿態(tài)時其真實值沒有明確的物理意義可言,對于歐拉角萬向節(jié)死鎖問題,文獻[2～4]提出了基于四元數(shù)反饋的姿態(tài)控制方法,文獻[5～8]提出了基于李群李代數(shù)的姿態(tài)控制方法。

本文將提出了一種基于方向余弦矩陣(DCM)姿態(tài)矢量的控制方法,避免使用歐拉角出現(xiàn)奇異值問題,實現(xiàn)了飛行器的全姿態(tài)控制,并在此基礎上將無人機姿態(tài)內(nèi)環(huán)角速度控制算法進一步改進為自抗擾控制(auto-disturbance rejection control,ADRC)技術[9～12],使用自抗擾控制技術中的微分跟蹤器TD合理安排期望值過渡，避免了因控制器期望值發(fā)生大階躍信號而引起控制器系統(tǒng)超調(diào),擴張觀測器ESO估計被控對象狀態(tài)量與系統(tǒng)擾動。

1 全姿態(tài)矢量控制

對于姿態(tài)的表示,可以通過歐拉角、四元數(shù)、方向余弦矩陣DCM表示[13,14],而在姿態(tài)解算中方向余弦矩陣通常習慣用四元數(shù)來表示。本文提出的采用定義在北西天世界坐標系下的方向余弦矩陣DCM中的姿態(tài)矢量來實現(xiàn)飛行器全姿態(tài)控制。

1.1 表示出期望方向余弦矩陣

對于飛行器的姿態(tài)控制三個通道,期望值可以用歐拉角表示分別是Yaw，Pitch，Roll,同樣的期望值也可以用方向余弦矩陣DCM1表示,公式如下

(1)

(2)

1.2 通過四元數(shù)表示出實際姿態(tài)方向余弦矩陣

在姿態(tài)解算中可以得到當前姿態(tài)四元數(shù)為q=(q0,q1,q2,q3)T,用四元數(shù)通過式(3)轉換為當前姿態(tài)的方向余弦矩陣DCM2[15],世界坐標系為北西天,公式如下

(3)

(4)

1.3 計算姿態(tài)外環(huán)控制器期望與反饋的誤差

通過向量叉積的方式表示出三個姿態(tài)通道上的姿態(tài)誤差ex,ey,ez公式如下

ex=xx+yx+zx,ey=xy+yy+zy,ez=xz+yz+zz

(5)

(6)

(7)

(8)

1.4 姿態(tài)外環(huán)控制率設計

對于多旋翼姿態(tài)外環(huán)來說,是一個一階運動學方程

(9)

式中φ,θ,ψ為姿態(tài)三個通道,ωx,ωy,ωz為姿態(tài)對應的角速度三個通道,該公式為一個運動學模型,沒有任何干擾,P控制器可以控制好系了,所以,姿態(tài)外環(huán)的控制率可以設計為

(10)

2 自抗擾控制技術的實現(xiàn)

本文把自抗擾控制技術中的微分跟蹤器TD和擴張狀態(tài)觀測器ESO應用在多旋翼姿態(tài)控制的角速度環(huán)中。

2.1 微分跟蹤器TD安排期望角速度值過渡

(11)

式中fh為微分跟蹤器中間計算變量,fhan函數(shù)為微分跟蹤器特有計算公式,r為可調(diào)參數(shù)跟蹤速度,h為可調(diào)參數(shù)步長。

2.2 擴張狀態(tài)觀測器估計被控對象狀態(tài)量與系統(tǒng)干擾

(12)

式中e為擴張狀態(tài)觀測器(ESO)中間計算變量,Z1為角速度狀態(tài)估計量,Z2為角加速度狀態(tài)估計量,Z3為系統(tǒng)擾動,β01,β02,β03為可調(diào)參數(shù),b為可調(diào)參數(shù)補償因子,h為可調(diào)參數(shù)步長。

2.3 姿態(tài)內(nèi)環(huán)控制率設計

對于多旋翼姿態(tài)內(nèi)環(huán)角速度環(huán)來說,是一個包含擾動的動力學方程

(13)

(14)

式中Kpω為P控制器可調(diào)參數(shù),Kdω為D控制器可調(diào)參數(shù)。姿態(tài)內(nèi)環(huán)控制完整的控制框圖如圖1所示。

圖1 完整的姿態(tài)內(nèi)環(huán)控制框圖

通過以上理論分析可以得出,此方法選取了姿態(tài)方向余弦矩陣(DCM)中的向量信息進行姿態(tài)控制,避免了使用歐拉角Yaw，Pitch，Roll進行姿態(tài)控制算法設計時存在算法奇異問題,真正從本質上解決了歐拉角萬向節(jié)死鎖問題。

3 實驗結果分析

實驗平臺是開源PCB小型多旋翼無人機平臺,其實物圖片如圖2。參數(shù)設置：起飛重量為38 g,單電機推力為15 g，電機個數(shù)為4個，機架軸距為130 mm，槳葉大小為46 mm。

圖2 飛行器實物

在這個實驗平臺上,測量了一些飛行器重要的參數(shù),如：單電機最大推力、電機推力與被控對象之前的關系等,來幫助整定控制器的參數(shù),還在室內(nèi)的環(huán)境中進行了真實的飛行實驗測試,驗證了此算法的工程應用,圖3為飛行器在懸停狀態(tài)下期望姿態(tài)與真實姿態(tài)之間的誤差,可見最大橫滾姿態(tài)和偏航姿態(tài)的誤差都低于0.6°,而這個姿態(tài)誤差在通常情況下是幾乎無法用肉眼觀察出來的。

圖3 飛行中的姿態(tài)角度誤差

4 結 論

本文研究的飛行器自抗擾全姿態(tài)矢量控制通過實物驗證表明了此算法具有正確性和可靠性。通過大量的飛行測試與驗證,本文所研究的新型姿態(tài)控制器能夠實現(xiàn)。原則上只要飛行器飛行空間足夠大,可以保證飛行器隨手拋向空中時無論飛行器處于什么姿態(tài),在空中解鎖飛行器都可以回到水平正常飛行姿態(tài),可以很好地完成復雜的多旋翼高機動飛行,并且從根本上解決了姿態(tài)控制使用歐拉角存在奇異值的問題。在保證控制算法具備可實施性的同時,又保證了控制算法的控制性能達到相應的指標要求。最后本文研究的飛行器控制算法性能視頻同時被美國俄亥俄州克利夫蘭州立大學(CSU)先進控制技術中心主任高志強教授收錄于自抗擾控制技術網(wǎng)上頻道。