基于Jerk約束的加速度校正算法

劉運毅，黃美蘭

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

0 引言

慣性傳感器又稱為IMU傳感器，包括三軸的陀螺儀與加速度計，其被廣泛應用于行人室內(nèi)位置估計[1]、無人機姿態(tài)估計[2]等領域。目前，經(jīng)典的姿態(tài)估計算法可分為2類：一類是基于陀螺儀數(shù)據(jù)的算法；另一類是基于加速度計與磁強計的融合算法。

對于前者，由于陀螺儀自身的漂移，在長時間通過角速度積分求取姿態(tài)的過程中，難免會產(chǎn)生誤差積累，導致姿態(tài)解算不準確。為了提高姿態(tài)估計精度，此類算法常利用加速度計數(shù)據(jù)聯(lián)合卡爾曼濾波來校正姿態(tài)角[3-6]。雖然各類卡爾曼濾波能在一定程度上緩解誤差積累問題，但使算法實現(xiàn)變得更為復雜。此外，神經(jīng)網(wǎng)絡[7]等方法也被用于校正陀螺儀的誤差。但是，這類算法在實際上并沒有消除積分誤差積累，且計算精度在一定程度上受到有用加速度分量的準確性的影響。對于另一類算法，加速度計易受到噪聲以及機體運動等因素的干擾，需對由此帶來的非重力加速度進行抑制處理，以獲得較為精準的、有用的重力加速度成分[8-9]。簡言之，這兩類算法都需從加速度計讀數(shù)中分離出準確的重力加速度。

為了獲得準確的加速度，文獻[10]聯(lián)合卡爾曼濾波與全球定位系統(tǒng)來構(gòu)建加速度修正模型，以消除非重力加速度的影響。此外，基于導航濾波[11]、互補濾波[12]等多種用于消除加速度計的外部非重力加速度以及噪聲干擾的算法被提出，以確保加速度具有高度容錯性。但是，這些算法的濾波處理過程會使算法復雜度顯著增加。

為了快速獲得目標加速度，本文提出一種新的Jerk(加加速度)信息提取方法，并在此法的約束下對加速度信號進行校正。此時，僅利用當前的陀螺儀與加速度計讀數(shù)即可獲得較準確的加速度信號，而不存在積分誤差積累。

1 Jerk提取模型

1.1 基本IMU數(shù)學模型

IMU的姿態(tài)可以用四元數(shù)q表示，四元數(shù)微分方程[13]及其矩陣展開形式可以表示如下：

(1)

(2)

式中：w為陀螺儀輸出的三軸角速度，w=[wx,wy,wz]T；q為姿態(tài)四元數(shù),q=[qw,qx,qy,qz]T。

對于式(1)，通常使用Runge-Kutta積分迭代求解姿態(tài)四元數(shù)q，再利用卡爾曼濾波融合加速度計的輸出進行姿態(tài)校正。

然而，加速度計會受到機體運動以及外界干擾因素的影響，導致其讀數(shù)不僅表現(xiàn)為重力加速度成分。假定IMU系統(tǒng)處于靜止狀態(tài)，即無線運動加速度時，加速度計輸出的加速度與姿態(tài)四元數(shù)在旋轉(zhuǎn)矩陣的映射下，有如下關(guān)系：

(3)

式中：a為加速度計輸出的三軸加速度,a=[ax,ay,az]T；g為重力加速度。

根據(jù)式(2)和式(3)，可知陀螺儀、加速度計與姿態(tài)間的關(guān)系，根據(jù)二者互相約束關(guān)系，可構(gòu)建卡爾曼濾波姿態(tài)融合系統(tǒng)。

基于四元數(shù)的卡爾曼濾波姿態(tài)融合算法得到了廣泛的應用，但在式(2)的長時間積分迭代時，會從根源上引入積分誤差積累，使得陀螺儀的噪聲和漂移在積分過程中積累，進而影響融合算法輸出的精度。為避免誤差積累對計算精度的影響，從加速度信息求取姿態(tài)的角度入手，提出一種無積分積累的加加速度提取算法。

1.2 Jerk模型

Jerk(加加速度)，可以用加速度的一階導數(shù)直接計算，標記為Jd，表示如下：

(4)

式中Δt為時間常數(shù)，與IMU傳感器采樣頻率有關(guān)，本文采樣頻率為200 Hz。

本文基于陀螺儀和加速度計直接融合的思路，提出新的Jerk提取算法。此法直接對傳感器的輸出信號進行信息融合，在獲得更準確的姿態(tài)信息的同時，有效避免積分誤差積累。以X軸為例進行分析，首先對X軸的加速度ax進行微分操作，得到偏微分方程，如下：

(5)

根據(jù)式(3)，有：

(6)

將式(6)按式(5)的形式進行整理，再將式(2)代入，可得到X軸在連續(xù)時間域下的Jerk：

(7)

進一步簡化式(7)，可得：

(8)

以相同的方式處理Y和Z軸，獲得系統(tǒng)在離散時間域下的三軸Jerk的新計算方法，將其標記為Jf，則有：

(9)

1.3 Jerk抗噪的理論分析

假設IMU傳感器輸出的加速度a和角速度w的噪聲為互相獨立的高斯白噪聲na與nw，噪聲均值均為零，方差為σna和σnw。

首先，根據(jù)式(4)分析Jd的均值，則有：

(10)

式(10)Jd表明的均值與噪聲無關(guān)。

再分析Jd的方差，其計算關(guān)系式為

(11)

從式(11)可知，Jd的差分計算倍增了噪聲的影響。

其次，根據(jù)式(9)分析X軸加加速度Jfx的均值，有：

E(Jfx)=-E(az+na)E(wy+nw)+E(ay+na)E(wz+nw)

=E(-azwy+aywz)

(12)

式(12)表明Jfx的均值同樣與噪聲無關(guān)。

假定IMU傳感器各軸信號的方差一致，X軸Jfx的差分計算如下：

D(Jfx)=D[(ay+na)(wz+nw)-(az+na)(wy+nw)] =D[aywz-azwy+(ay-az)nw+(wz-wy)na] =D(aywz-azwy)+D[(ay-az)nw]+D[(wz-wy)na] =D(aywz-azwy)+2D(anw)+2D(wna)

(13)

從式(13)可以發(fā)現(xiàn)，噪聲對Jfx方差的影響分別通過角速度、加速度分量與噪聲相乘來實現(xiàn)。與卡爾曼濾波的核心一樣，2個高斯分布信號相乘得到1個新的高斯分布，為最優(yōu)估計。此時，得到方差更小、更準確的高斯分布。因此，本文提出的Jerk融合計算方法能減小噪聲的影響。

1.4 Jerk抗噪的實驗與分析

利用仿真實驗進一步驗證新的Jerk提取算法的抗噪性能。實驗采用Matlab中的y120p60r30.mat數(shù)據(jù)模型，在該模型中有7個運動階段：前3個分別是2 s內(nèi)偏航120°，接著1 s內(nèi)俯仰60°，然后0.5 s內(nèi)滾轉(zhuǎn)30°；后續(xù)的3個過程為前3個階段的反向運動；最后一個動作綜合了3個軸的轉(zhuǎn)動。

表1 不同Jerk算法在不同噪聲下的RMSE

表1的數(shù)據(jù)表明：直接差分計算的Jd的RMSE僅與加速度噪聲有關(guān)，并且隨著噪聲的增加，RMSE急劇增加；本文提出的Jf的RMSE與加速度噪聲以及陀螺儀噪聲有關(guān)，即使噪聲增加到極大的水平，Jf的RMSE仍保持較低的水平，展現(xiàn)了該法的優(yōu)秀抗噪能力。

為了進一步驗證算法在實際應用環(huán)境下的抗噪性，采用知名實驗數(shù)據(jù)集EuRoC，計算并對比直接差分算法與本文Jerk提取算法的各軸的均方根誤差，結(jié)果如表2所示。

表2 不同Jerk算法對EuRoc數(shù)據(jù)集的RMSE

表2的計算結(jié)果表明：本文Jerk提取算法的抗噪性能遠遠優(yōu)于直接差分計算，且與參考真值接近。

然而，EuRoC數(shù)據(jù)集的數(shù)據(jù)序列時間較短，為了進一步驗證算法的性能，應用知名實驗數(shù)據(jù)集TUM-VI[14]來分析算法的抗噪性能。利用數(shù)據(jù)集中長達111 h的實驗數(shù)據(jù)序列calib-imu-static2，計算出Jd×Δt的RMSE等于0.017 7，而Jf×Δt的RMSE低至0.000 7。這兩個數(shù)據(jù)的對比表明，本文Jerk算法的抗噪性能比差分法提高了2個量級，而且針對超長時間的實驗沒有誤差積累。

2 基于Jerk的加速度校正模型

在前面的討論中，基于融合算法提出了新的Jerk提取算法。該算法具有良好的抗噪性能，其計算模型僅僅取決于當前的IMU輸出，不存在積分誤差積累。但算法僅僅獲得Jerk信息，還未能獲得直接與姿態(tài)相關(guān)的數(shù)據(jù)。為了避免引入積分誤差積累，下面將進一步探討基于Jerk的加速度校正算法。

2.1 計算模型

IMU加速度計當前時刻i的輸出ai由重力加速度分量ag,i、運動加速度分量av,i、漂移bi和噪聲ni組成：

ai=ag,i+av,i+bi+ni

(14)

其中，重力加速度分量ag,i為求解目標?；谇耙恍」?jié)的討論，所提出的Jerk法在較高的干擾和噪聲情況下，仍然可獲得接近真值的Jerk信息，因而可以繞過式(14)，直接構(gòu)建關(guān)于求解目標ag,i的差分方程，表示如下：

(15)

同時，考慮求解目標ag,i在X，Y，Z3個軸上的分量，它們的重力加速度的幅度具有如下約束關(guān)系：

(16)

把式(15)和式(16)擴展到m個樣本，則有3m個未知變量，3(m-1)+m個方程。理論上僅僅需要3個樣本數(shù)據(jù)就可以求解一組目標參數(shù)。為了簡化計算，進一步整理式(15)，有：

(17)

把式(17)展開整理，再將式(16)的約束條件代入，可得到：

(18)

把式(18)擴展到更多樣本，可得：

(19)

把式(19)擴展到m個樣本，其中每個樣本i有3個未知變量(agx,i，agy,i,agz,i)以及m-1個約束方程，并把方程組簡化表示為

AΔaag,i=bΔa

(20)

由于式(20)為非線性方程，求解采用非線性數(shù)值計算方法，并構(gòu)建目標函數(shù)如下：

f=(|ag,i|-9.81)+λ(AΔaag,i-bΔa)

(21)

式中λ為調(diào)節(jié)系數(shù)，為了保證式的強約束，本文設置λ=1/(m-1)。

對式(21)的求解可采用牛頓迭代法、梯度下降法等非線性數(shù)值計算方法。本文具體實現(xiàn)采用Matlab中的非線性最小二乘求解器lsqnonlin，所有參數(shù)采用系統(tǒng)默認參數(shù)，搜索初始值為加速度信號的當前200個樣本的滑動平均值，并進行歸一化操作，使得三軸重力加速度的模為9.81，每次計算的樣本數(shù)量m同樣為200。

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

為了觀察本文提出的基于Jerk約束的加速度校正算法的性能，采用TUM-VI數(shù)據(jù)集中的Room6運動序列進行加速度校正實驗。其中，這一序列包含了從實際環(huán)境中采集的實際加速度信息及其對應真值，將三軸的加速度真值視為參考量，與經(jīng)過本文算法校正后的實際加速度進行比較。此時，3個軸的加速度的結(jié)果如圖1所示。

(a)X軸加速度與參考真值的對比

(b)Y軸加速度與參考真值的對比

(c)Z軸加速度與參考真值的對比圖1 三軸加速度與對應真值的對比

從圖1可知，經(jīng)過本文算法校正后的三軸加速度曲線與真值曲線幾乎是重合的，只是在個別區(qū)域出現(xiàn)一些細小的差別。由此表明了本文的加速度校正算法可以獲得較準確的目標加速度。

為了進一步證明本文算法的性能，通過TUM-VI數(shù)據(jù)集中的多個數(shù)據(jù)序列進行試驗。將本文算法與經(jīng)典卡爾曼濾波算法以及較新的神經(jīng)網(wǎng)絡算法[7]進行比較。其中，經(jīng)典卡爾曼濾波算法采用Matlab中的imufilter模型，該濾波器利用9個狀態(tài)向量跟蹤陀螺儀和加速度計的估計誤差，從而進行數(shù)據(jù)的更新。由于數(shù)據(jù)集未給出靜態(tài)序列calib-imu-static2的參考真值，所以靜態(tài)序列的真值設置為前10 s數(shù)據(jù)的平均值，并進行歸一化操作，重力加速度常數(shù)同樣取9.81。同樣將RMSE作為評價指標，實驗對比結(jié)果如表3所示。

表3 不同算法對TUM-VI數(shù)據(jù)集的RMSE

基于表3的數(shù)據(jù)進行分析：文獻[7]中的神經(jīng)網(wǎng)絡法利用數(shù)據(jù)序列Room1、Room3、Room5進行訓練，所以對相似環(huán)境的序列，即Room2、Room4、Room6，其性能表現(xiàn)較好；但在未訓練過的場景中，如靜態(tài)序列calib-imu-static2，其性能惡化到完全不能正常工作。本文算法與Matlab中的imufilter算法性能相當，二者的RMSE值都處在同一量級。此時，從靜態(tài)序列calib-imu-static2的結(jié)果中可以看出，imufilter濾波器與前一個狀態(tài)相關(guān)，其工作有一個收斂過程。

相比之下，本文提出的加速度校正算法不僅算法結(jié)構(gòu)簡潔，更重要的是每次計算僅僅依賴于當前的少量樣本，不需要任何的先驗狀態(tài)信息，與系統(tǒng)的歷史狀態(tài)毫無關(guān)聯(lián)，不需要事先訓練，也不需要跟imufilter濾波模型一樣必須根據(jù)具體傳感器設置相關(guān)參數(shù)，也不需要初始化過程。再者，本文算法在未對求解式進行針對性優(yōu)化的情況下，仍然具有較好的性能?？梢詷酚^地估計，如果對非線性約束方程做針對性優(yōu)化求解，將可進一步提高本文算法的性能。

3 結(jié)束語

本文提出了一種新的提取系統(tǒng)Jerk信息的方法，構(gòu)建了新的計算模型，該模型僅依賴當前的陀螺儀和加速度計的輸出來計算系統(tǒng)的Jerk信息。在TUM-VI和EuRoC數(shù)據(jù)集的數(shù)據(jù)基礎上，對2種Jerk法進行抗噪實驗發(fā)現(xiàn)，本文提出的Jerk法能夠更好地逼近真值，進一步證明了新Jerk法在不引入積分誤差積累的同時具有更好的抵抗噪聲干擾的能力。然后，在新的Jerk法的約束下，構(gòu)建了一個求解加速度的模型。實驗結(jié)果表明，通過此模型可得到較精準的加速度，且本文算法結(jié)構(gòu)簡單，易于實現(xiàn)，對于具有較長時間的數(shù)據(jù)序列，算法的性能較好。