謝 敏，趙來定，王召文

(1.南京郵電大學(xué) 通信與信息工程學(xué)院，江蘇 南京210003；2.南京郵電大學(xué) 通信與網(wǎng)絡(luò)技術(shù)國(guó)家地方聯(lián)合工程研究中心，江蘇 南京210003)

0 引言

微機(jī)電系統(tǒng)內(nèi)部集成了動(dòng)態(tài)傳感器、數(shù)字信號(hào)處理模塊、串口通信等模塊，是信息技術(shù)和機(jī)械工程的結(jié)合[1]。目前，微機(jī)電傳感器具有成本低、體積小、功耗低、可靠性高等優(yōu)勢(shì)，廣泛用于高智能化的行業(yè)中。因此，慣性測(cè)量單元(Inertial Measurement Unit，IMU)常采用微機(jī)電傳感器。

慣性測(cè)量單元應(yīng)用廣泛，遠(yuǎn)至國(guó)家軍事防御，近至日常智能設(shè)備。慣性測(cè)量單元可以測(cè)量裝置或載體自身的加速度、角度等狀態(tài)數(shù)據(jù)，一般通過加速度計(jì)等傳感器進(jìn)行測(cè)量。其中，陀螺儀用于測(cè)量設(shè)備自身的旋轉(zhuǎn)運(yùn)動(dòng)。加速度計(jì)用于測(cè)量裝置或載體的受力情況[2]。磁力計(jì)用于確定設(shè)備所處的方位。

陀螺儀易受溫度、摩擦等因素的影響，存在誤差，即零點(diǎn)漂移。加速度計(jì)的實(shí)時(shí)性較差，不能快速、準(zhǔn)確做出響應(yīng)，存在測(cè)量噪聲。在短時(shí)間內(nèi)，加速度計(jì)數(shù)據(jù)可信度較差；相反，陀螺儀的加速度數(shù)據(jù)在短時(shí)間內(nèi)可信度較強(qiáng)，而長(zhǎng)時(shí)間工作后，會(huì)因漂移產(chǎn)生測(cè)量誤差。磁力計(jì)只針對(duì)水平情況下方位角的測(cè)量，且易受周圍環(huán)境影響，可信度單一。因此，需要有效結(jié)合傳感器，提高數(shù)據(jù)可信度。

現(xiàn)有的慣性器件處理算法，利用互補(bǔ)濾波算法修正陀螺儀的值，橢圓擬合解算得到運(yùn)動(dòng)載體的航向角[3]；Mahony互補(bǔ)濾波方法可以提高解算精度[4]；在捷聯(lián)慣導(dǎo)的初始對(duì)準(zhǔn)方面有維納濾波、卡爾曼濾波[5]，在最優(yōu)化算法方面有梯度下降法[6]、Newton法等。其中，梯度下降法可以有效抑制角度噪聲?；パa(bǔ)濾波可以利用系數(shù)結(jié)合多組數(shù)據(jù)?？柭鼮V波可以提高角度和角速度精度，同時(shí)抑制零點(diǎn)漂移。

實(shí)驗(yàn)利用動(dòng)態(tài)三軸搖擺臺(tái)測(cè)試慣性測(cè)量單元的動(dòng)態(tài)特性。實(shí)驗(yàn)證明，將梯度下降算法、互補(bǔ)濾波算法、卡爾曼濾波算法結(jié)合后，對(duì)傳感器數(shù)據(jù)進(jìn)行處理，可以有效提高慣性測(cè)量單元的數(shù)據(jù)精度，使其具有良好的連續(xù)性與穩(wěn)定性。

1 姿態(tài)角計(jì)算

1.1 幾何模型下的姿態(tài)角

一般地，加速度計(jì)可以測(cè)到一個(gè)動(dòng)載體平臺(tái)相對(duì)于地球的大地坐標(biāo)系(N系)的運(yùn)動(dòng)狀態(tài)。假設(shè)平臺(tái)(聯(lián)動(dòng)慣性測(cè)量單元)做自由落體運(yùn)動(dòng)，則三軸加速度值均為零。若將加速度計(jì)測(cè)得的加速度值看作矢量R，在載體坐標(biāo)系(B系)中分解成三個(gè)方向矢量R x、R y、R z，即R在xyz軸上的投影，利用三軸矢量可以得到姿態(tài)角中的俯仰角和橫搖角[7]，其幾何關(guān)系如圖1所示。

圖1 加速度計(jì)幾何模型

根據(jù)公式：

求得矢量R在yoz平面上的投影R yz[3]，R x和R yz存在正切關(guān)系，得橫搖角?，同理可得俯仰角θ。

姿態(tài)角中的航向角Ψ，在裝置水平放置時(shí)，可由磁力計(jì)單獨(dú)獲得[4]。磁力計(jì)是利用地球的南北極的微弱磁場(chǎng)作為判斷依據(jù)。地磁場(chǎng)也可看作一個(gè)矢量H地磁，在某一地點(diǎn)，這個(gè)矢量可以被分解為三個(gè)相互垂直的矢量，其中H x、H y與水平面相平行，H z與水平面垂直[8]。若保持磁力計(jì)xoy面和水平面平行，則磁力計(jì)的三軸與這三個(gè)矢量對(duì)應(yīng)，如圖2所示。

圖2 磁力計(jì)分量示意圖

水平方向的H x和H y，矢量和指向磁北。航向角Ψ是當(dāng)前方向和磁北的夾角。裝置水平可得航向角Ψ：

經(jīng)過推導(dǎo)，可以得到一組姿態(tài)角。由三軸加速度計(jì)數(shù)據(jù)可以得到橫搖角、俯仰角，由磁力計(jì)水平分量可以得到航向角，但是，該組姿態(tài)角直接由慣性器件得到，受周圍環(huán)境影響較大，穩(wěn)定性較差。

1.2 基于梯度下降法的姿態(tài)角

四元數(shù)是一種姿態(tài)角的表示方式。四元數(shù)q0、q1、q2、q3，其中q0作為實(shí)部，其余作為虛部[5]，并可由三軸角度表示。

依照四元數(shù)乘法可以得到一個(gè)代表三軸旋轉(zhuǎn)角度的四元數(shù)q。

捷聯(lián)慣導(dǎo)的初始對(duì)準(zhǔn)有兩個(gè)常用坐標(biāo)系[9]：一個(gè)是導(dǎo)航坐標(biāo)系(N系)，導(dǎo)航坐標(biāo)系是導(dǎo)航基準(zhǔn)的坐標(biāo)系；另一個(gè)是載體坐標(biāo)系(B系)，是關(guān)于載體的坐標(biāo)系。

姿態(tài)矩陣生成四元數(shù)，將導(dǎo)航坐標(biāo)系轉(zhuǎn)換到載體坐標(biāo)系。假設(shè)空間矢量在載體系和導(dǎo)航系中的投影分別為r′和r，四元數(shù)在兩個(gè)坐標(biāo)系內(nèi)轉(zhuǎn)換可以通過p′=qpq-1實(shí)現(xiàn)。

梯度下降法定義為標(biāo)量場(chǎng)中某一點(diǎn)上的梯度指向標(biāo)量場(chǎng)增長(zhǎng)最快的方向，梯度的長(zhǎng)度是最大變化率[6]，并沿著最大變化率所在的負(fù)方向?qū)ふ易顑?yōu)解。

通過梯度下降法將姿態(tài)矩陣存在的誤差減至最小。將誤差定義成一個(gè)關(guān)于q的函數(shù)向量f(q)，偏微分處理得到最速下降方向，擴(kuò)展到三軸得到雅克比矩陣J(q)。

下面引入梯度下降法的公式，代入四元數(shù)。

將加速度計(jì)數(shù)據(jù)ax、ay、az與歸一化的重力加速度矢量[0 0 1]之間的差值作為誤差函數(shù)fg(q)，微分得雅克比矩陣J(q)。

已知誤差函數(shù)及其雅克比矩陣，可以利用梯度公式求出誤差函數(shù)的梯度▽fg(q)：

將該梯度代入梯度下降的公式，其中步長(zhǎng)μt與實(shí)際運(yùn)動(dòng)時(shí)的角速度和采樣時(shí)間有關(guān)。這樣就獲得了姿態(tài)四元數(shù)q▽，t。

1.3 四元數(shù)微分方程

姿態(tài)四元數(shù)q是時(shí)間的變量，自變量為t時(shí)刻角度θt，由陀螺儀積分得到，因變量為四元數(shù)q t。u為三軸矢量i、j、k歸一化后的矢量，w t為角速度矢量[10]。

對(duì)q t求導(dǎo)得到另一組姿態(tài)四元數(shù)q w，t：

2 互補(bǔ)濾波和卡爾曼濾波

通過加速度計(jì)和磁力計(jì)數(shù)據(jù)所得姿態(tài)角，僅反映出慣性測(cè)量單元當(dāng)前狀態(tài)，受到環(huán)境噪聲影響大，數(shù)據(jù)容易出現(xiàn)野值。雖然基于梯度下降法的數(shù)據(jù)和基于微分方程的數(shù)據(jù)在一定程度上可以抑制測(cè)量噪聲，但是梯度下降法具有“碗底”效應(yīng)，步長(zhǎng)過大時(shí)不能快速收斂。因此，需要將梯度下降法數(shù)據(jù)和微分方程數(shù)據(jù)相結(jié)合[11]。

2.1 微積分的互補(bǔ)濾波

將 姿態(tài) 四 元 數(shù)qΔ，t[12-13]與 姿 態(tài) 四 元 數(shù)q w，t互 補(bǔ)結(jié)合，得到q t：

基于四元數(shù)的微分方程適合用于物體高速運(yùn)動(dòng)狀態(tài)，梯度下降法適合用于低速運(yùn)動(dòng)。因此，高速運(yùn)動(dòng)下α取值要小一些，低速運(yùn)動(dòng)下α要大一些。從收斂速度的角度來看，當(dāng)梯度下降的收斂速度等于微分方程的收斂速度時(shí)，可得最優(yōu)解。設(shè)四元數(shù)微分方程的收斂速度為β。當(dāng)α近似為0時(shí)適合對(duì)動(dòng)態(tài)物體的測(cè)量。

式(20)中q▽，t-1相比于μt▽fg(q)所占比重較小，可忽略不計(jì)，q▽，t得到近似表達(dá)，并求得q t。 根據(jù)姿態(tài)四元數(shù)q t和姿態(tài)矩陣得到姿態(tài)角?t、θt和Ψt。

2.2 基于PI的姿態(tài)角互補(bǔ)濾波

對(duì)于梯度下降法，經(jīng)過仿真后發(fā)現(xiàn)，若梯度下降的步長(zhǎng)μ過大，則尋至最優(yōu)解時(shí)間較長(zhǎng)，就像“碗底”一樣。步長(zhǎng)過大會(huì)越過最優(yōu)值，在最優(yōu)值附近持續(xù)振蕩，如圖3所示；步長(zhǎng)太小，到達(dá)“碗底”的時(shí)間就會(huì)變長(zhǎng)。

圖3 梯度下降法“碗底”示意圖

因?yàn)榇蟛介L(zhǎng)會(huì)引起振蕩，使數(shù)據(jù)產(chǎn)生毛刺，所以在實(shí)驗(yàn)過程中采用的步長(zhǎng)較小。

磁力計(jì)和加速度計(jì)所測(cè)得角度沒有累積誤差，但動(dòng)態(tài)響應(yīng)較差[14]。利用互補(bǔ)濾波器融合三種傳感器的數(shù)據(jù)，提高測(cè)量精度和系統(tǒng)的動(dòng)態(tài)性能?；パa(bǔ)濾波法需要用到加速度計(jì)數(shù)據(jù)ax、ay、az，陀螺儀數(shù)據(jù)wx、wy、wz，磁力計(jì) 數(shù)據(jù)mx、my、mz。 互補(bǔ)濾波器 公式如下，其中C0(s)為加速度計(jì)和磁力計(jì)觀測(cè)到的姿態(tài)矩陣，Cw(s)為陀螺儀測(cè)量數(shù)據(jù)計(jì)算得到的姿態(tài)矩陣。

互補(bǔ)濾波器傳遞函數(shù)：

姿態(tài)矩陣：

通過與梯度下降法相同的方法得到重力分量Vx、Vy、Vz及ax、ay、az，誤差ex、ey、ez可以表示為兩者叉乘：

角速度更新四元數(shù)，并轉(zhuǎn)化為姿態(tài)角：

2.3 姿態(tài)角之間的互補(bǔ)

基于幾何的姿態(tài)角?、θ、Ψ，基于梯度下降法的姿 態(tài) 角?t、θt、Ψt和 基 于 互 補(bǔ) 濾 波 的 姿 態(tài) 角?n、θn、Ψn，這三組不同的姿態(tài)角進(jìn)行姿態(tài)角之間的互補(bǔ)，其中濾波系數(shù)k是根據(jù)加速度計(jì)數(shù)據(jù)ax、ay、az所得[11]：

當(dāng)載體處于低速運(yùn)動(dòng)時(shí)，梯度下降法通過小步長(zhǎng)迭代能夠快速收斂；當(dāng)載體處于高速運(yùn)動(dòng)時(shí)，PI互補(bǔ)濾波跟蹤性能更好。幾何的姿態(tài)角作為參照標(biāo)準(zhǔn)，可以防止低速所帶來的震動(dòng)以及超速運(yùn)動(dòng)。

2.4 卡爾曼濾波

梯度下降法和PI互補(bǔ)濾波解決了加速度計(jì)和陀螺儀的部分噪聲和收斂問題，但陀螺儀因?yàn)橹亓Υ嬖诹泓c(diǎn)漂移，漂移角度經(jīng)過較長(zhǎng)時(shí)間的累計(jì)，會(huì)使測(cè)量出現(xiàn)偏差。

卡爾曼濾波是一種基于無偏最小方差遞推準(zhǔn)則的濾波算法[16]，它既適用于平穩(wěn)隨機(jī)過程，也適用于非平穩(wěn)過程。其核心是通過建立狀態(tài)模型和測(cè)量模型，利用上一狀態(tài)的估計(jì)值和這一狀態(tài)的測(cè)量值得到狀態(tài)轉(zhuǎn)移方程，并更新狀態(tài)模型。相比于需要知曉所有狀態(tài)的維納濾波，卡爾曼濾波更適合于傳感器輸出濾波處理。

卡爾曼濾波第一個(gè)公式給出狀態(tài)方程、觀測(cè)方程：

其中X k為k時(shí)刻的系統(tǒng)狀態(tài)矩陣；A為狀態(tài)轉(zhuǎn)移矩陣；u k為k時(shí)刻的角速度矢量；B為輸入加權(quán)矩陣；w k為系統(tǒng)噪聲，為零均值白噪聲過程。以t時(shí)刻俯仰角θt，陀螺儀輸出wt，零漂Biast，采樣周期Δt為例，對(duì)狀態(tài)更新方程進(jìn)行二維矩陣描述。

卡爾曼濾波的第二個(gè)公式計(jì)算的是系統(tǒng)的協(xié)方差矩陣：

其中需要給出兩個(gè)值Qaccl、Qgyro，分別為加速度計(jì)所得到的角度噪聲、陀螺儀的角速度噪聲。Q為矩陣[θtBiast]T的協(xié)方差矩陣：

設(shè)矩陣P k-1=[a b；c d]，A已知，根據(jù)方程可得：

第三個(gè)公式通過協(xié)方差矩陣計(jì)算卡爾曼增益：

其中P k和H已知，R為觀測(cè)噪聲矩陣。在慣性測(cè)量單元系統(tǒng)中有兩種狀態(tài)變化量：角度θt和零漂Biast，分別對(duì)應(yīng)各自的卡爾曼增益，設(shè)為K k=[k0k1]。

得到[k0k1]后，通過第四個(gè)公式對(duì)狀態(tài)矩陣X k進(jìn)行修正。

寫成矩陣形式如式(49)所示，其中Z k=θaccl，是加速度計(jì)所得到的角度值，作為觀測(cè)輸入。

零點(diǎn)漂移值可以對(duì)陀螺儀角速度修正：

最后一個(gè)公式對(duì)協(xié)方差矩陣進(jìn)行更新：

至此，即為卡爾曼濾波的五大方程[17]。輸入?yún)?shù)為加速度計(jì)所得角度值和陀螺儀的角速度值，以及角度、角速度噪聲值，輸出為修正后的角度和角速度。

3 算法總結(jié)

上文中算法的數(shù)據(jù)處理流程可以分為3個(gè)部分：(1)根據(jù)幾何模型計(jì)算出姿態(tài)角，即從器件寄存器直接計(jì)算得到；(2)利用加速度計(jì)和陀螺儀數(shù)據(jù)分別通過梯度下降法和PI互補(bǔ)濾波法得出姿態(tài)角，兩組姿態(tài)角再進(jìn)行互補(bǔ)計(jì)算；(3)將第二步得到的姿態(tài)角和角速度通過卡爾曼濾波修正，得到最終三軸的角度和角速度?？柭鼮V波流程圖如圖4所示。

圖4 卡爾曼算法流程圖

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證該姿態(tài)融合算法，本次實(shí)驗(yàn)采用了STM32F405RGT6作為芯片平臺(tái)，ADIS16460作為六軸傳感器模塊，HMC5883_L作為磁力計(jì)模塊進(jìn)行實(shí)驗(yàn)，通過串口將數(shù)據(jù)反饋給上位機(jī)。其中，對(duì)傳感器初始化做出了改進(jìn)：通過對(duì)ADIS16460的DEC_RATE將原先的采樣率從512 S/s提升至2 048 S/s，增強(qiáng)了慣導(dǎo)輸出的連續(xù)性。ADIS16460的FLTR_CTRL寄存器可以提供對(duì)數(shù)字低通濾波器的控制。此濾波器包含兩個(gè)級(jí)聯(lián)均值濾波器，它們提供Bartlett窗口、FIR濾波器響應(yīng)。為了保證數(shù)據(jù)輸出的實(shí)時(shí)性，將其濾波器抽頭數(shù)從64降至16。原先數(shù)據(jù)輸出是通過外部中斷結(jié)束后輸出，現(xiàn)改進(jìn)為2 ms的計(jì)時(shí)器中斷，一方面是為了數(shù)據(jù)的連續(xù)性，另一方面固定的間隔時(shí)間數(shù)據(jù)可以保證數(shù)據(jù)的實(shí)時(shí)性。

4.1 靜態(tài)實(shí)驗(yàn)分析

實(shí)驗(yàn)裝置分為兩套：第一套是原始算法，由慣性器件直接輸出數(shù)據(jù)的慣性測(cè)量單元；第二套是經(jīng)過姿態(tài)融合濾波算法處理的慣性測(cè)量單元。將兩套實(shí)驗(yàn)裝置靜置于桌面，以俯仰軸的輸出數(shù)據(jù)為例，進(jìn)行對(duì)零點(diǎn)漂移的驗(yàn)證。

從圖5中虛線可以看出，未經(jīng)過融合濾波算法的俯仰角在靜止時(shí)總是朝著負(fù)角度方向呈現(xiàn)增加趨勢(shì)，最終會(huì)偏離真實(shí)值。從圖中實(shí)線可以看出，經(jīng)過融合濾波算法的俯仰角在靜止時(shí)的角度總是圍繞著一個(gè)數(shù)值上下小幅度變化，不會(huì)出現(xiàn)偏移。因此，姿態(tài)融合濾波算法能夠有效地抑制角度偏移。

圖5 靜止俯仰角度

4.2 動(dòng)態(tài)實(shí)驗(yàn)驗(yàn)證

將慣性測(cè)量單元放置在搖擺臺(tái)的重心位置，并設(shè)置搖擺臺(tái)運(yùn)動(dòng)方式為正弦運(yùn)動(dòng)，其幅度為20°，運(yùn)動(dòng)周期為8 s。共進(jìn)行3次試驗(yàn)，3次試驗(yàn)分別包括橫搖、俯仰和方位。實(shí)驗(yàn)結(jié)果如圖6～圖10所示。

圖10 動(dòng)態(tài)方位角度

在動(dòng)態(tài)實(shí)驗(yàn)中，未進(jìn)行過濾波算法修正的橫搖、俯仰和方位可以總結(jié)出以下三個(gè)問題：

(1)從圖6動(dòng)態(tài)俯仰角度可以看出，原始算法數(shù)據(jù)存在著滯后的問題，在前四分之一周期內(nèi)延時(shí)了約1 000個(gè)采樣點(diǎn)，并隨時(shí)間逐漸增多；

圖6 動(dòng)態(tài)俯仰角度

(2)原始算法下俯仰角度區(qū)間會(huì)隨時(shí)間出現(xiàn)角度偏移，不能始終保持在正負(fù)20°之間；

(3)從圖9動(dòng)態(tài)橫搖局部角度可以看出，原始數(shù)據(jù)在角度變化過程中呈現(xiàn)不連續(xù)的階梯狀、斷崖式的變化。

圖7 動(dòng)態(tài)俯仰局部角度

圖8 動(dòng)態(tài)橫搖角度

圖9 動(dòng)態(tài)橫搖局部角度

經(jīng)過姿態(tài)融合濾波算法的修正后，解決了俯仰軸的滯后問題，不會(huì)出現(xiàn)相位偏移現(xiàn)象；減小了零點(diǎn)漂移帶來的時(shí)間偏移，俯仰角度總是保持在正負(fù)20°之間；平滑了輸出角度，使其在局部呈現(xiàn)為一條光滑曲線。因此，經(jīng)過姿態(tài)融合濾波算法的慣導(dǎo)裝置在動(dòng)態(tài)特性的追蹤和穩(wěn)定性上優(yōu)于原始程序。

5 結(jié)論

本文介紹了一種基于梯度下降法、互補(bǔ)濾波法和卡爾曼濾波的姿態(tài)融合算法。首先，從原理上分析了梯度下降法和四元數(shù)微分法的優(yōu)勢(shì)；其次，介紹了互補(bǔ)濾波法和卡爾曼濾波算法對(duì)姿態(tài)數(shù)據(jù)進(jìn)行結(jié)合的過程；最后，通過動(dòng)態(tài)分析實(shí)驗(yàn)驗(yàn)證了姿態(tài)融合算法的可行性。

實(shí)驗(yàn)結(jié)果證明，基于梯度下降法、互補(bǔ)濾波法和卡爾曼濾波的姿態(tài)融合算法能夠成功地抑制加速度計(jì)的測(cè)量噪聲，并減小陀螺儀的零點(diǎn)漂移所帶來的影響。相比于傳統(tǒng)算法輸出數(shù)據(jù)的滯后、不連續(xù)，基于姿態(tài)融合算法的數(shù)據(jù)實(shí)時(shí)性強(qiáng)，角度曲線連續(xù)且平滑，能夠有效、準(zhǔn)確地描述運(yùn)動(dòng)中物體的姿態(tài)。同時(shí)，在硬件上調(diào)整了濾波器的參數(shù)，提高了數(shù)據(jù)輸出速率。因此，基于該姿態(tài)融合算法的慣性測(cè)量單元具有良好的穩(wěn)定性與可靠性，在工程應(yīng)用中有一定的參考價(jià)值。