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

覃曉蘭，黃美蘭，劉運(yùn)毅

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

0 引言

IMU(inertial measurement unit，慣性測(cè)量單元)是測(cè)量物體三軸運(yùn)動(dòng)加速度與角速度的裝置，其核心組成部件為陀螺儀、磁力計(jì)與加速度計(jì)。常用于衛(wèi)星定位[1]、無(wú)人機(jī)的姿態(tài)控制[2]、自動(dòng)駕駛[3]、解算機(jī)器人的位姿信息[4]以及行人位姿估計(jì)[5]等。在姿態(tài)解算方面，目前主要包括基于互補(bǔ)濾波[6]、卡爾曼濾波及其非線性濾波算法[7]以及深度學(xué)習(xí)[8]等數(shù)據(jù)融合體系。這些體系融合多個(gè)傳感器的信息，通過(guò)不同方式對(duì)陀螺儀與加速度計(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ù)的過(guò)程需要積分運(yùn)算，但僅依靠角速度往往難以解算出準(zhǔn)確的姿態(tài)信息，因此通常需要加速度作為輔助數(shù)據(jù)參與多傳感器的融合算法。但是在加速度計(jì)受到干擾的情況下基于卡爾曼體系的融合算法會(huì)產(chǎn)生誤差積累，導(dǎo)致解算姿態(tài)的效果變差。因此對(duì)于加速度計(jì)易受到干擾的場(chǎng)景中，不可通過(guò)主流的融合算法求解準(zhǔn)確姿態(tài)。而由加速度直接計(jì)算姿態(tài)的方式可避免積分積累誤差問(wèn)題。此外，在資源受限的空間環(huán)境中，如何以輕量級(jí)算法對(duì)加速度進(jìn)行快速校正也十分重要。故對(duì)IMU傳感器中的加速度信息進(jìn)行快速且準(zhǔn)確的校正成了需要解決的問(wèn)題。

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

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

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

1.1 算法基礎(chǔ)

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

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

(1)

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

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

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

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

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

(2)

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

將公式拓展到前m個(gè)樣本，計(jì)算i時(shí)刻與前m個(gè)時(shí)刻的加速度差分：

(3)

式中Δar,i,m表示i時(shí)刻的加速度與前m個(gè)時(shí)刻的加速度差值。

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

(4)

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

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

(5)

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

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

(6)

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

(7)

但是式(7)的未知數(shù)ar,i-k并非需要求解的當(dāng)前時(shí)刻加速度，因此考慮對(duì)等式左邊進(jìn)行簡(jiǎn)化，期望得到僅包括加速度差分與當(dāng)前時(shí)刻加速度的表達(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)前時(shí)刻目標(biāo)加速度的關(guān)系表達(dá)式：

(9)

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

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

(10)

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

(11)

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

g2=x2+y2+z2

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

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

(12)

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

2 實(shí)驗(yàn)及結(jié)果分析

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

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

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

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

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

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

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

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

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

3 結(jié)束語(yǔ)

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

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