覃曉蘭，黃美蘭，劉運毅

(廣西大學(xué)計算機(jī)與電子信息學(xué)院，廣西南寧 530004)

0 引言

IMU(inertial measurement unit，慣性測量單元)是測量物體三軸運動加速度與角速度的裝置，其核心組成部件為陀螺儀、磁力計與加速度計。常用于衛(wèi)星定位[1]、無人機(jī)的姿態(tài)控制[2]、自動駕駛[3]、解算機(jī)器人的位姿信息[4]以及行人位姿估計[5]等。在姿態(tài)解算方面，目前主要包括基于互補(bǔ)濾波[6]、卡爾曼濾波及其非線性濾波算法[7]以及深度學(xué)習(xí)[8]等數(shù)據(jù)融合體系。這些體系融合多個傳感器的信息，通過不同方式對陀螺儀與加速度計的數(shù)據(jù)進(jìn)行處理，以得到精準(zhǔn)的姿態(tài)信息。其中，基于卡爾曼濾波體系如擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)[9]、誤差狀態(tài)卡爾曼濾波(error-state Kalman filter，ESKF)[10]等算法為當(dāng)前求解姿態(tài)的主要算法之一。

在姿態(tài)解算算法中，由角速度得到姿態(tài)四元數(shù)的過程需要積分運算，但僅依靠角速度往往難以解算出準(zhǔn)確的姿態(tài)信息，因此通常需要加速度作為輔助數(shù)據(jù)參與多傳感器的融合算法。但是在加速度計受到干擾的情況下基于卡爾曼體系的融合算法會產(chǎn)生誤差積累，導(dǎo)致解算姿態(tài)的效果變差。因此對于加速度計易受到干擾的場景中，不可通過主流的融合算法求解準(zhǔn)確姿態(tài)。而由加速度直接計算姿態(tài)的方式可避免積分積累誤差問題。此外，在資源受限的空間環(huán)境中，如何以輕量級算法對加速度進(jìn)行快速校正也十分重要。故對IMU傳感器中的加速度信息進(jìn)行快速且準(zhǔn)確的校正成了需要解決的問題。

針對IMU中加速度的校正問題，國內(nèi)外也做了較多研究。如喬美英等針對加速度計的噪聲建立線性誤差模型，將其轉(zhuǎn)換為橢球方程且優(yōu)化了去噪?yún)?shù)，最后對姿態(tài)傾角進(jìn)行誤差補(bǔ)償，有效提高了姿態(tài)解算精度[11]。燕斌對礦用隨鉆軌跡測量系統(tǒng)進(jìn)行分析并建立了誤差模型，實現(xiàn)MEMS加速度傳感器的誤差校正[12]。劉旭航等通過建模的方式求解非重力加速度和外部非重力加速度，對加速度計輸出值進(jìn)行修正[13]。W.Youn等通過卡爾曼濾波器對加速度讀數(shù)中的非重力加速度成分進(jìn)行估算，提高了無GPS信號情況下姿態(tài)計算的魯棒性[14]。譚擁等通過角速度對加速度進(jìn)行卡爾曼濾波修正[15]。

通過對干擾成分建模以實現(xiàn)誤差補(bǔ)償?shù)乃惴ㄐ韪鶕?jù)不同情形對干擾分量進(jìn)行分析，而卡爾曼濾波器依賴歷史狀態(tài)且會產(chǎn)生誤差積累。為了避免這些問題，在原有研究的基礎(chǔ)上[16]，本文提出了一種低復(fù)雜度的加速度校正算法，該算法通過角速度融合計算可以得到準(zhǔn)確的加速度差分信息這一特點，根據(jù)此差分校正加速度。在校正加速度時，通過當(dāng)前及前段時刻傳感器讀數(shù)列出加速度與差分信息的方程組，接著解算此方程組與重力方程組成的平面方程組，進(jìn)而解算出加速度。由于該算法未使用主流的卡爾曼濾波體系，因此不存在積累誤差問題。此外，僅需選取短時的角速度與加速度測量值便可快速校正加速度，方便后續(xù)獲得各軸的姿態(tài)角度。

1 基于差分信息的加速度快速校正算法

1.1 算法基礎(chǔ)

加速度與姿態(tài)是密切關(guān)聯(lián)的，通過重力加速度在各個坐標(biāo)軸的分量可以計算其對應(yīng)于三軸的夾角信息。因此只需獲得傳感器坐標(biāo)系下的重力加速度分量便可實現(xiàn)姿態(tài)角度計算。但是IMU中加速度計的讀數(shù)還包含其他分量干擾，故需從加速度計的讀數(shù)中校正加速度。

僅依靠單個傳感器很難實現(xiàn)加速度的校正，因此結(jié)合IMU中陀螺儀的讀數(shù)具有瞬時較為穩(wěn)定的特點，考慮將陀螺儀融合加速度計進(jìn)行加速度的校正。根據(jù)文獻(xiàn)[16]可知，加速度的導(dǎo)數(shù)與角速度之間的關(guān)系如下：

(1)

式中：Jf為通過角速度融合計算得到的3個軸加速度導(dǎo)數(shù)組成的矩陣；ax、ay、az為三軸的加速度；wx、wy、wz為陀螺儀3個方向軸的角速度。

文獻(xiàn)[16]證明了融合角速度求解加速度微分信息能有效抵御不同強(qiáng)度噪聲的干擾，得到較為穩(wěn)定準(zhǔn)確的加速度差分信息。因此本文考慮根據(jù)此特點在噪聲干擾情況下解算出目標(biāo)加速度。

1.2 加速度校正算法推導(dǎo)

首先討論加速度差分的關(guān)系，第i時刻與上一時刻的加速度差分計算公式如下：

Δar,i=ar,i-ar,i-1=Jf,i×Δt

(2)

式中：ar,i、ar,i-1為i時刻與i-1時刻的目標(biāo)加速度真值；Jf,i為由角速度參與計算的的i時刻加速度導(dǎo)數(shù)；Δt為傳感器采樣時間間隔，由采樣頻率決定。

將公式拓展到前m個樣本，計算i時刻與前m個時刻的加速度差分：

(3)

式中Δar,i,m表示i時刻的加速度與前m個時刻的加速度差值。

觀察式(3)，可看出共有m+1個加速度真值為未知數(shù)，將每個加速度擴(kuò)寫為3個軸的分量，此時共有3(m+1)個未知數(shù)。再將加速度差分展開為三軸的加速度差分可得3m個方程，因此還需要其他約束方程才能解出目標(biāo)加速度?？紤]到目標(biāo)加速度受重力約束，其約束方程表達(dá)如下：

(4)

式中：arx,i、ary,i、arz,i為3個方向的加速度真值，其組成了向量at,i。

此時對于需求解的m+1個加速度樣本，式(4)可以擴(kuò)寫出m+1個方程。因此，對于由式(3)與式(4)組成的方程組，只要樣本數(shù)大于3，便可通過此方程組解算出樣本的目標(biāo)加速度。下面進(jìn)行方程組的求解，由式(3)可知，目標(biāo)加速度可由上一時刻的目標(biāo)加速度與該時刻的差分相加得到，將其代入到重力約束方程可得：

(5)

對式(5)展開并利用重力約束消除加速度平方部分，可得：

Δarx,i,1arx,i+Δary,i,1ary,i+Δarz,i,1arz,i

(6)

由式(6)得到了當(dāng)前時刻加速度與加速度差分的關(guān)系，而加速度差分可利用角速度計算，因此可認(rèn)為得到了加速度與角速度的關(guān)系。同理，將式(6)擴(kuò)展到其他時刻的加速度差分關(guān)系式中，便能得到其他時刻的加速度差分信息與目標(biāo)加速度之間的關(guān)系：

(7)

但是式(7)的未知數(shù)ar,i-k并非需要求解的當(dāng)前時刻加速度，因此考慮對等式左邊進(jìn)行簡化，期望得到僅包括加速度差分與當(dāng)前時刻加速度的表達(dá)式：

Δarx,i-k,1arx,i-k+Δary,i-k,1ary,i-k+Δarz,i-k,1arz,i-k

=Δarx,i-k,1(arx,i-Δarx,i,k)+Δary,i-k，1·

(ary,i-Δary,i,k)+Δarz,i-k,1(arz,i-Δarz,i,k)

=Δarx,i-k,1arx,i+Δary,i-k,1ary,i+Δarz,i-k,1arz,i-

Δarx,i-k,1Δarx,i,k-Δary,i-k,1Δary,i,k-

Δarz,i-k,1Δarz,i,k

(8)

將式(8)代入式(7)，可得加速度的差分與當(dāng)前時刻目標(biāo)加速度的關(guān)系表達(dá)式：

(9)

從式(9)可看出目標(biāo)加速度與多個樣本的加速度差分存在聯(lián)系。將差分以融合角速度的方式計算，此時僅需當(dāng)前時刻與前m個時刻的角速度便可求出加速度差分，進(jìn)而解算出目標(biāo)加速度。而式(9)的形式可看作某平面方程，其中未知數(shù)arx,i、ary,i、arz,i可看作3個平面在3個軸的坐標(biāo)集合，而差分信息為已知量，可看作平面方程的常系數(shù)參數(shù)與常量。

由一個差分公式可得到一個平面方程，對于m個樣本，根據(jù)式(9)可寫出多個平面方程，此時可通過求解平面方程組的方式解算出目標(biāo)加速度。平面方程組至少有一個解，即公共交點，當(dāng)平面方程組有多個解時，平面方程組相交于某條交線。假設(shè)由式(9)擴(kuò)寫的平面方程有多個解，其中一個交點為(x0，y0，z0)，交線方向為(l，n，p)，則公共交線可表示為

(10)

與此同時，目標(biāo)加速度仍受重力約束關(guān)系的影響，而重力約束方程可看作由三軸目標(biāo)加速度作為參數(shù)組成的球面，表示如下：

(11)

下一步求解公共交線在重力約束下的表達(dá)形式，將交線方程代入式(11)中并展開可得：

g2=x2+y2+z2

=(x0+lt)2+(y0+nt)2+(z0+pt)2

=(l2+n2+p2)t2+2(x0l+y0n+z0p)t+

(12)

式(12)是一個一元二次方程的表達(dá)式，通過求根公式求解兩個根即可求解出未知數(shù)。此時t有2個解，分別將其代入公共交線方程可求解出2組目標(biāo)加速度，最后再篩選出一個最優(yōu)解即可。至此得到了解算目標(biāo)加速度的方法，僅需陀螺儀測量值參與計算加速度差分，再根據(jù)此信息求解由式(9)展開式與重力約束方程組成的方程組。在解算方程組時，根據(jù)最小二乘法獲得公共交線的方向與交點坐標(biāo)，再根據(jù)式(12)計算方程的2個解并將解代入公共交線方程獲得2組重力加速度，最后通過選擇與平滑濾波后的加速度測量值最接近的一組解作為最優(yōu)的目標(biāo)加速度。

2 實驗及結(jié)果分析

為了驗證本文提出的加速度校正算法的性能，選取經(jīng)典的EuRoC數(shù)據(jù)集進(jìn)行實驗，該數(shù)據(jù)集由搭載在微型飛行器上的IMU測量得到，采樣頻率為200 Hz，包括了狀態(tài)測量值以及真值信息[17]。程序運行環(huán)境為MATLAB2020a，選取該數(shù)據(jù)集的多個序列進(jìn)行加速度的求解，其中某一序列V1_01_easy的x軸的某段時刻采樣點求解結(jié)果如圖1所示。

圖1 V1_01_easy序列解算的x軸加速度(部分)

圖1展示了加速度計測量值、參考真值加速度、本文算法求解的x軸加速度。由圖1可看出本文算法計算的目標(biāo)加速度可以在干擾較大的環(huán)境中解算出與加速度真值較為貼近的加速度，說明了所提算法可以獲得較準(zhǔn)確的加速度。

為了進(jìn)一步探討本文提出的算法的性能，將其與Matlab的imufilter模型比較，該算法通過融合卡爾曼濾波器對狀態(tài)誤差進(jìn)行建模以求解六軸狀態(tài)。對EuRoC數(shù)據(jù)集的多個序列分別用2種算法求解加速度，對比不同序列的RMSE(root mean squard error，均方根誤差)，最終結(jié)果如表1所示。

表1 經(jīng)不同算法處理后不同序列的RMSE

表1進(jìn)一步證明了本文提出的加速度求解算法能有效地求解出目標(biāo)加速度分量，與基于卡爾曼濾波的融合算法的加速度校正效果近似，均能恢復(fù)出較準(zhǔn)確的加速度。

在求解目標(biāo)加速度時，選取的樣本為一小段數(shù)據(jù)。實際情況中加速度計采樣頻率較高，因此選取較短時間的樣本點即可完成目標(biāo)加速度的計算，避免了融合算法中依賴歷史狀態(tài)并隨著時間增加導(dǎo)致的誤差積累問題。同時觀察整個求解過程可發(fā)現(xiàn)大部分是較簡單的加法與乘法運算，避免了卡爾曼濾波融合算法中如更新后驗錯誤等步驟所需的三角函數(shù)等運算，可有效提高求解速度。為對比2種算法的運行時間，將EuRoC數(shù)據(jù)集的每個序列分別運行100次統(tǒng)計運行時長，結(jié)果如表2所示。

表2 EuRoC數(shù)據(jù)集中不同序列運行時間結(jié)果 s

表2說明了數(shù)據(jù)集中不同序列利用不同算法計算加速度的運行時間均值與方差。由表2可知運行速度較基于卡爾曼濾波的融合算法提高了10倍，證明了本文提出的求解目標(biāo)加速度的算法可快速求解出較準(zhǔn)確的目標(biāo)加速度。

3 結(jié)束語

本文提出了一種快速校正加速度的算法，利用融合角速度的方式得到較準(zhǔn)確的加速度差分信息，并根據(jù)加速度差分的定義推導(dǎo)出以目標(biāo)加速度為未知量的平面方程組。然后，根據(jù)部分?jǐn)?shù)據(jù)解算該方程組，便可實現(xiàn)加速度的校正。算法的優(yōu)點在于可以快速計算出IMU的加速度信息，且不需要依賴歷史狀態(tài)，僅需小段數(shù)據(jù)便能計算出較準(zhǔn)確的加速度。同時，不同于基于卡爾曼濾波融合算法，極大避免了積累誤差的問題。

經(jīng)過實驗測試，所提出的算法可以在保持與基于卡爾曼濾波的融合算法近似的校正性能基礎(chǔ)上，運行速度提高10倍。在后續(xù)工作中，計劃深入研究提升校正精度，以期將其運用于加速度計受到干擾而無法作為輔助信息與角速度進(jìn)行融合計算姿態(tài)的場景中。