宋建國，吳鑫磊，謝敏波

(北京工業(yè)大學 數(shù)字社區(qū)教育部工程研究中心和城市軌道交通北京實驗室，北京 100124)

0 引 言

四旋翼飛行器現(xiàn)在廣泛應用于生活的各方各面，包括工業(yè)、農業(yè)、商業(yè)、軍事偵察等應用領域，也是其成為了現(xiàn)在研究的熱點話題。其中四旋翼飛行器的姿態(tài)控制算法以及姿態(tài)融合解算算法是四旋翼飛行器的研究的重中之重，姿態(tài)控制算法將直接影響飛行器的姿態(tài)調整的快速與魯棒性，是飛行器實時平穩(wěn)飛行的核心。

而姿態(tài)融合結算算法則是飛行器姿態(tài)控制的前提，將直接影響飛行控制的可靠性、抗干擾性能以及飛行控制實現(xiàn)的難易程度[1-3]。目前市場上對于飛行器控制主要有兩個方向，其一是基于光纖陀螺儀作為硬件基礎的飛行控制器，其精度較高但是價格極其昂貴難以得到推廣[4-5]。而基于MEMS傳感器的飛行器由于價格相對優(yōu)勢較大得以廣泛推廣，本文則是以后者為基礎進行一系列的研究。

基于航向姿態(tài)參考系統(tǒng)(Attitude and Heading Reference System，AHRS)所提供的基于自身X、Y、Z坐標軸下的橫滾角、航向角、俯仰角等一系列姿態(tài)信息對飛行器自身當前姿態(tài)進行精準的融合分析，并通過濾波處理將姿態(tài)數(shù)據整合到其自身的雙閉環(huán)控制系統(tǒng)中作為姿態(tài)控制輸入初始值，經過雙閉環(huán)控制算法對飛行器姿態(tài)做出快速準確運算，從而實現(xiàn)了穩(wěn)定輸出誤差相對較小的姿態(tài)數(shù)據作為初始值以供雙閉環(huán)控制算法應用，進而達到四旋翼飛行器穩(wěn)定高效控制的要求。

1 四旋翼飛行器模型分析

1.1 物理機體模型

圖1為四旋翼飛行器的物理機體模型，從結構上分析其機體模型分為十字模式和X模式，兩種結構差別不大，但是一般常常采用X模型，方便航拍拓展。

圖1 物理機體模型

俯仰角(pitch)：機體坐標系X軸與水平面的夾角，圍繞X軸旋轉。當X軸的正半軸 位于過坐標原點的水平面之上(抬頭)時，俯仰角為正，否則為負[6]。

偏航角(yaw)：機體坐標系X軸在水平面上投影與地面坐標系Xg軸之間的夾角，圍繞Y軸旋轉。機頭右偏航為正，反之為負。

滾轉角(roll)：機體坐標系Z軸與通過機體Z軸的鉛垂面間的夾角，圍繞Z軸旋轉。 機體向右滾為正，反之為負。

四旋翼飛行器的某一時刻的姿態(tài)可以用歐拉角來表示[7]。其中當機體懸停于空中時，4個旋翼所產生的升力與飛行器自身的重力相等，而四旋翼中的M1與M3電機順時針旋轉，M2與M4電機逆時針旋轉，各個旋翼的轉動慣量相等，從而可以很好的抵消其自身的的自旋，從而使得飛行器更加方便控制[8-9]。

該飛行器采用9軸傳感器MPU9250來對四旋翼飛行器的角速度以及加速度進行采集，并通過數(shù)據融合濾波對原始數(shù)據進行處理，使得其作為飛行器控制輸入計算時更加準確，以便于實現(xiàn)飛行器更加穩(wěn)定有效的響應控制。

1.2 飛行器的雙閉環(huán)控制模型

四旋翼飛行器簡化的雙閉環(huán)控制框圖如圖2所示，該結構運用了9軸傳感器MPU9250來獲取原始數(shù)據，經過一系列的濾波校正與融合來解算飛行器的實時姿態(tài)，即飛行器的實時角度與角速度。

圖2 雙閉環(huán)控制框圖

之所以選用雙閉環(huán)PID對四旋翼飛行器的飛行姿態(tài)進行控制，是因為傳統(tǒng)的角度單環(huán)PID控制算法僅僅考慮了飛行器的飛行角度，這樣飛行器在飛行過程中的抗擾動能力很差，而本文采用了雙閉環(huán)的PID控制算法，這在很大程度上增加了飛行的穩(wěn)定性，有效的提高了飛行器的控制品質，所謂的雙閉環(huán)控制即使得角度環(huán)作為外環(huán)，角速度環(huán)作為內環(huán)，這使得該控制方法可以控制更多的飛行變量，從而飛行器在外界惡劣環(huán)境下的適應能力變得更強。雙閉環(huán)PID控制的程序流程圖如圖3所示。

圖3 程序流程圖

其中主控制器對PID各個控制參數(shù)進行初始化，并對姿態(tài)傳感器進行自檢與零漂校正，然后以一定周期對傳感器姿態(tài)原始數(shù)據進行采集并給定初始油門，然后經過濾波運算將角度信息送給外環(huán)角度環(huán)輸入并與遙控端的給定值進行做差處理，然后再將外環(huán)輸出值與角速度值進行做差處理作為角速度內環(huán)輸入，從而內環(huán)輸出PWM波占空比控制量以方便控制各個電機的轉速進而來控制飛行器的實時姿態(tài)。在其中還增加了無人機觸地判斷算法來及時判斷飛機是否準確降落，以及擴展卡爾曼濾波算法對數(shù)據姿態(tài)進行融合解算更新。從而可以準確進行一次飛行器姿態(tài)的更新控制。

2 飛行器的濾波融合算法

2.1 通過加速計與磁力計進行數(shù)據解析計算求出歐拉角

本文先通過三軸加速度計來計算出飛行器的橫滾角γ和俯仰角θ，先使得飛行器初始位置處于水平靜置狀態(tài)，并假設當?shù)氐闹亓铀俚膅為1g，此時

四旋翼飛行器所固聯(lián)的載體坐標系b系的三軸加速度ax,ay,az為重力加速的g在其載體坐標系上的分量[10-13]。

(1)

(2)

(3)

(4)

由式(1)，式(2)，式(3)可以推出橫滾角γ和俯仰角θ的計算公式：

(5)

(6)

(7)

最后經過計算可以得到：

(8)

歐拉角與四元數(shù)之間可以進行互相轉換，在進行飛行器姿態(tài)解算中本文都運用了四元數(shù)進行算法運算，因為四元數(shù)在運算期間更加方便快捷，這節(jié)省了大量的CPU資源以方便處理其他事件，最終再將融合出的四元數(shù)數(shù)據轉換為歐拉角來進行串級PID控制。

2.2 擴展卡爾曼濾波算法在飛行器的應用

在進行數(shù)據解算處理時需要對周圍磁場的干擾以及陀螺儀經過長時間的積分后所累加的積分誤差進行處理與校正[14-15]。校正時通過加速度計的加速度數(shù)據以及磁力計的采集數(shù)據通過四維拓展卡爾曼濾波對數(shù)據進行觀測更新，從而使得數(shù)據變得更加真實有效。

因為在進行飛行器姿態(tài)實時解算時運用歐拉角會涉及到大量的三角函數(shù)運算，為了保證更新速度以及保持良好的實時性，在進行時間更新與觀測更新之前，先把歐拉角轉換為四元數(shù)。其轉換關系如:

(9)

由上面計算所得到的初始歐拉角數(shù)據經過式(9)轉換后就得到了初始的四元數(shù)，然后在運用擴展卡爾曼濾波對狀態(tài)進行時間更新和觀測更新。

時間更新：

四旋翼飛行器控制系統(tǒng)的自身姿態(tài)的狀態(tài)方程如:

(10)

(11)

協(xié)方差矩陣Pk預測如：

Pk=APk-1A-T+W

(12)

式中,W為飛行器的過程激勵噪聲協(xié)方差。由以上各式我們就可以得到飛行器在飛行過程中隨著時間變化飛行器在各個時間段的飛行姿態(tài)，即飛行器的狀態(tài)變量Q。觀測更新：

飛行器的觀測更新是由加速度計與磁力計所采集到數(shù)據和本體坐標系中的重力加速度與地磁場的參考矢量變換到導航坐標系下的數(shù)據進行比較，從而對飛行器的姿態(tài)進行觀測更新[16]。而從飛行器自身坐標系到導航坐標系的轉移矩陣如式(13)所示：

(13)

由此可知重力加速度與地磁場的參考矢量變換到導航坐標系下的計算公式為

(14)

(15)

式中,Gb，Mb分別為飛行器的重力加速度參考矢量和磁場參考矢量?？柭鲆婢仃嘖k=Pk-1HT(HPk-1HT+R)-1，其中R為三維觀測噪聲協(xié)方差矩陣，H為Gn和Mn對Q求偏導的雅可比矩陣，由此可以得出重力加速度和磁場的觀測更新公式為

(16)

(17)

在式(16)和式(17)中Zk分別代表重力加速度觀測更新和磁場觀測更新時各傳感器所測量得到的三維矢量Zk=[axayaz]T和Zk=[MxMyMz]T。而在觀測更新下的協(xié)方差更新如：

Pk=(1-KkH)Pk-1

(18)

數(shù)據融合濾波程序流程圖如圖4所示。

圖4 數(shù)據融合濾波程序流程圖

3 仿真實驗

在Matlab/Simulik仿真環(huán)境中進行實驗分析，在給定一個初始期望的橫滾角度后對比的橫滾角控制輸出曲線圖如圖5、圖6所示，同時對直接采集的原始數(shù)據與經過卡爾曼濾波融合處理后的數(shù)據分別應用到四旋翼飛行器的雙閉環(huán)控制系統(tǒng)中去，然后再對飛行器從靜態(tài)到運動狀態(tài)再回到初始狀態(tài)的姿態(tài)角進行對比，其對比曲線圖如圖7、圖8、圖9所示。

圖5 傳統(tǒng)PID控制曲線

圖6 雙閉環(huán)PID控制曲線

在圖5與圖6的對比中我們可以明顯看出運用雙閉環(huán)控制之后所得到的閉環(huán)控制系統(tǒng)的輸出曲線從一個給定值再到一個穩(wěn)定值的調節(jié)曲線更加平滑，控制效果更加穩(wěn)定，震蕩性更小，從圖中可以明顯看出雙閉環(huán)PID控制系統(tǒng)比單環(huán)控制系統(tǒng)的響應速度快了將近4倍，相對比而言雙閉環(huán)控制系統(tǒng)在控制四旋翼飛行器的過程中所體現(xiàn)的優(yōu)勢更加明顯。

圖7 橫滾角對比曲線

圖8 俯仰角對比曲線

圖9 航向角對比曲線

在圖7、圖8、圖9中的橫滾角、俯仰角、航向角在純陀螺儀作用下隨著時間的增長姿態(tài)角再次回到靜止狀態(tài)下后會產生漂移誤差，然而在運用了數(shù)據融合濾波之后姿態(tài)角的變化曲線如圖中AHRS線所示，姿態(tài)角的誤差得到了有效改善，使得飛行器在運動過程所產生的動態(tài)誤差得以修正，從而使得飛行器的控制更加準確高效。

4 結 論

四旋翼飛行器在雙閉環(huán)系統(tǒng)下能夠實時有效的保證飛行器的正常飛行，并且相對于單一的角度環(huán)而言，該雙閉環(huán)控制系統(tǒng)的抗干擾能力以及魯棒性都有實質性的提高。而在雙閉環(huán)系統(tǒng)的基礎上又增添了擴展卡爾曼濾波，這種姿態(tài)調整復合控制算法使得控制系統(tǒng)最終輸出的控制量更加貼合真實值，準確性和可靠性相對于單一的雙閉環(huán)控制又有了進一步提高。本文將雙閉環(huán)控制系統(tǒng)與擴展卡爾曼濾波算法進行了互補融合運用，最終將這種復合控制方法運用到了飛行器的姿態(tài)調整控制中，最終使得飛行器在進行前后運動、懸停運動以及偏航運動時比普通的飛行器控制方式更加的穩(wěn)定高效。