四旋翼姿態(tài)解算算法的對比與研究

王曉初，盧 琛

（廣東工業(yè)大學(xué) 機(jī)電工程學(xué)院，廣州 510006）

0 引言

在捷聯(lián)式慣性導(dǎo)航中，飛行器的姿態(tài)是機(jī)體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系決定的，而描述這種坐標(biāo)系的關(guān)系可以分為三類，有三參數(shù)法、四參數(shù)法和九參數(shù)法。其中三參數(shù)法也叫歐拉角法，四參數(shù)法有四元數(shù)，九參數(shù)法為方向余弦[4]。

利用慣性元件，對飛行器的姿態(tài)的解算，是捷聯(lián)式慣性導(dǎo)航的關(guān)鍵。文獻(xiàn)[6]從信號的頻域出發(fā)，給出了基于互補(bǔ)濾波器的解算算法。文獻(xiàn)[2]結(jié)合卡爾曼濾波器，給出了基于狀態(tài)空間的最優(yōu)解算法。文獻(xiàn)[7]采用梯度下降為修正準(zhǔn)則，給出了偏差沿梯度負(fù)方向修正算法。文獻(xiàn)[5]則結(jié)合梯度下降與互補(bǔ)濾波器，吸收了各自的優(yōu)點。本文針對這幾種常見的姿態(tài)解算算法，從設(shè)計準(zhǔn)則與實際應(yīng)用出發(fā)，對比分析了各自算法的特點與相似之處。

1 姿態(tài)的更新算法

姿態(tài)的更新常采用如下流程，將角速度向量[ω]帶入四元數(shù)微分方程并求解，即可更新四元數(shù)[q]。

圖1 姿態(tài)解算流程

四元數(shù)微分方程為：

其中，四元數(shù)q定義為：q=q0+q1i+q2j+q3k。

寫成矩陣形式為：

用角增量算法求解微分方程迭代格式為：

其中：

將cos和sin泰勒展開，取一階近似有：

采用式(2)即可更新四元數(shù)，但是直接使用四元數(shù)不利于PID控制，還需利用方向余弦矩陣[C]將四元數(shù)轉(zhuǎn)變?yōu)闅W拉角。

方向余弦矩陣也稱作坐標(biāo)變換矩陣，是導(dǎo)航坐標(biāo)系[Xn]到運(yùn)載體坐標(biāo)系[Xb]的變換矩陣，有并且方向余弦有兩種表示方法，分別為為四元數(shù)表出和歐拉角表出，并且兩種表出矩陣在同一坐標(biāo)系下是等價的[1]。

四元數(shù)表示方向余弦為[1]：

另外，歐拉角表出方向余弦為[1]：

其中c和s分別代表cos和sin，?、θ和φ分別代表歐拉角中的航向角，俯仰角和橫滾角[1]。如果已知方向余弦，可以由下列公式求得對應(yīng)的歐拉角：

如圖1所示，式（2）給出了四元數(shù)法的更新迭代格式，更新四元數(shù)后即可求得方向余弦，由式（3）～式（5）便可求得到更新之后的歐拉角。

但是整個過程是建立在對角速度的積分，由于字長丟失精度會導(dǎo)致積分誤差，且誤差會隨著時間的延長而加大。所以需要組合加速度傳感器來修正姿態(tài)矩陣，使誤差項逐步減小，最終消除。常用的組合算法有互補(bǔ)濾波算法、梯度下降算法和卡爾曼濾波算法等。

2 常見組合算法的特點

姿態(tài)解算算法可以用圖2表示，其中由式（2）更新的姿態(tài)為qg，它是整個姿態(tài)估計q的基礎(chǔ)。

圖2 姿態(tài)解算算法流程

因為積分誤差的存在，所以需結(jié)合加速度計的輸出qc構(gòu)成修正偏差ε(q)，然后以不同的設(shè)計準(zhǔn)則構(gòu)成算法修正qg，即可得到在特定評判標(biāo)準(zhǔn)下的最優(yōu)估計姿態(tài)q。

2.1 以高通和低通濾波器為修正算法

陀螺儀動態(tài)響應(yīng)特性好，但會產(chǎn)生積分誤差。加速度傳感器測量姿態(tài)沒有積分誤差，但是動態(tài)響應(yīng)較差。它們在頻域上的特性形成互補(bǔ)，從而提高測量精度和系統(tǒng)的動態(tài)性能[3]。

如圖3所示[6]，表示互補(bǔ)濾波器的輸出姿態(tài)矩陣，代表載體坐標(biāo)下的測量加速度單位向量，代表在慣性坐標(biāo)系下的重力場向量，設(shè)。

圖3 互補(bǔ)濾波器模型

其中誤差向量定義為：

則整個互補(bǔ)濾波器的估計輸出為：

如圖3所示，令ξ1為加速度計測量噪聲，為陀螺儀測量噪聲，則噪聲傳遞函數(shù)為：

式（8）和式（9）可以用來設(shè)計k的取值，從而確定互補(bǔ)濾波器的截止頻率在大于fτ的高頻段，陀螺儀對解算起主要作用，反之，加速度對解算起主要作用[3]。

2.2 以偏差函數(shù)梯度負(fù)方向為修正準(zhǔn)則

使用式（6）為目標(biāo)函數(shù)，則有梯度：

這樣就可以通過下列公式進(jìn)行修正：

式（10）、式（11）即為梯度下降迭代格式，其中步長λ用來限制偏差收斂速度，但過大的步長可能會導(dǎo)致估計不準(zhǔn)確[5]。

2.3 以卡爾曼濾波器為基礎(chǔ)的修正算法

卡爾曼濾波器以狀態(tài)空間為基礎(chǔ)，把信號過程看做在白噪聲作用下的線性輸出[5]，

算法表述為：最優(yōu)值=預(yù)測值+kg.修正量，其中kg為動態(tài)調(diào)整增益。

考慮如下線性系統(tǒng)離散模型：

其中X(k)是系統(tǒng)的狀態(tài)向量，A是狀態(tài)轉(zhuǎn)移矩陣，B是控制輸入矩陣，u(k-1)是控制輸入向量，Q是過程噪聲協(xié)方差矩陣，Z(k)是系統(tǒng)的觀測向量，H是觀測向量，R是觀測噪聲協(xié)方差矩陣。

使用陀螺儀數(shù)據(jù)qg和ε(q)建立時間傳遞模型如下：

取：

可以得到狀態(tài)矩陣方程[2]：

則卡爾曼濾波算法實現(xiàn)步驟為：

根據(jù)時間傳遞方程（12）預(yù)測k時刻的輸出：

其中X(k- 1 |k- 1 )為上一時刻的最優(yōu)輸出。

計算k時刻預(yù)測輸出協(xié)方差矩陣：

更新k時刻修正增益矩陣：

根據(jù)觀測方程（13）修正預(yù)測矩陣：

更新最優(yōu)預(yù)測輸出的協(xié)方差矩陣：

3 實驗分析

互補(bǔ)濾波最為簡單，基于高通與低通濾波器，構(gòu)成設(shè)計準(zhǔn)則來修正姿態(tài)；梯度下降則以偏差函數(shù)的負(fù)梯度為校正準(zhǔn)則，使其以最快下降方向進(jìn)行校正；卡爾曼濾波以狀態(tài)空間為基礎(chǔ)，從時間傳遞方程和測量方程出發(fā)，得出考慮各種隨機(jī)誤差在內(nèi)的最佳線性估計，如圖4所示，為實時角度采樣輸出。

圖4 三種算法輸出對比

從圖中可以看到，當(dāng)姿態(tài)在迅速變化時，三種算法的估計輸出幾乎重疊在一起，此時陀螺儀對姿態(tài)更新起主導(dǎo)作用，加速度的修正效果可以忽略。在姿態(tài)變化較為平緩階段，三種算法對姿態(tài)修正的差異才凸顯出來，這也體現(xiàn)了在不同修正準(zhǔn)則下的輸出差異，但最終估計輸出都會收斂于某一穩(wěn)定值。

[1]高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)[M].北京:清華大學(xué)出版社,2012.

[2]王帥,魏國.卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應(yīng)用[J].兵工自動化,2011,01:73-74,80.

[3]梁延德,程敏,何福本,李航.基于互補(bǔ)濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,11:56-58,61.

[4]胡慶.基于STM32單片機(jī)的無人機(jī)飛行控制系統(tǒng)設(shè)計[D].南京航空航天大學(xué),2012.

[5]譚廣超.四旋翼飛行器姿態(tài)控制系統(tǒng)的設(shè)計與實現(xiàn)[D].大連理工大學(xué),2013.

[6]Bachmann, E.R., et al.“Orientation tracking for humans and robots using inertial sensors.” Computational Intelligence in Robotics and Automation, 1999.CIRA’99.Proceedings.1999 IEEE International Symposium on.IEEE,1999.

[7]Madgwick,Sebastian OH,Andrew JL Harrison, and Ravi Vaidyanathan.“Estimation of IMU and MARG orientation using a gradient descent algorithm.”Rehabilitation Robotics (ICORR), 2011 IEEE International Conference on.IEEE, 2011.

[8]Fresk, Emil,and George Nikolakopoulos.“Full quaternion based attitude control for a quadrotor.” Control Conference (ECC),2013 European.IEEE,2013.