基于改進Mahony互補濾波算法的三維運動軌跡恢復*

田紅麗， 孫永全， 劉洪普,3

(1.河北工業(yè)大學 人工智能與數(shù)據(jù)科學學院，天津 300401； 2.河北省大數(shù)據(jù)計算重點實驗室，天津 300401;3.華北科技學院 河北省物聯(lián)網(wǎng)數(shù)據(jù)采集與處理工程技術(shù)研究中心, 河北 廊坊 065201)

0 引 言

長期以來，對物體運動狀態(tài)的檢測大多都采用外部觀測的方法，即檢測系統(tǒng)與運動物體相互分離[1]。采用外部觀測方法精度固然很高，但成本相對較高，而且受制于外界條件，如全球定位系統(tǒng)(global positioning system,GPS)無法在室內(nèi)使用，視覺傳感器需要充足光線和合適角度等。近年來，隨著微機電系統(tǒng)(micro-electro-mechanical system，MEMS)[2,3]技術(shù)的發(fā)展，體積更小、成本更低、性能卓越的新型傳感器出現(xiàn)在大眾視野之中[4]。

一直以來，基于MEMS傳感器和捷聯(lián)慣性導航的運動物體姿態(tài)的研究是定位導航的熱點問題。文獻[6～8]為基于卡爾曼濾波(Kalman filtering，KF)[9]的姿態(tài)解算過程，文獻[10]將擴展卡爾曼濾波(extended Kalman filtering，EKF)[11]加入到低成本MEMS姿態(tài)解算中，但大多數(shù)研究局限于求解姿態(tài)角的范疇。2009年,劉勃等人[12]利用加速度計進行三維空間運動追蹤，實驗結(jié)果良好但實驗較為單一。2016年,仇立杰[13]使用MEMS傳感器采集數(shù)據(jù)，轉(zhuǎn)換為空間數(shù)據(jù)，從而記錄手勢的運動軌跡，實驗積分原理嚴謹，但呈現(xiàn)的空間曲線波動較大，準確性較差。

本文總結(jié)了前期算法中的精度上的不足和穩(wěn)定性上的差異，參考了四軸飛行器姿態(tài)解算的方法，在原有的基礎上使用四元數(shù)代替歐拉角，將Mahony互補濾波[14]算法求解姿態(tài)角的過程加以改進應用到三維運動軌跡恢復中，通過卡爾曼濾波處理加速度計帶來的誤差和漂移，經(jīng)過積分運算，最后使用二次多項式補償算法對速度和位移進行補償，得出三維軌跡。

1 運動軌跡恢復

1.1 預處理

本文通過采集的三軸加速度和三軸角速度數(shù)據(jù)來進行數(shù)學運算，傳感器在采集數(shù)據(jù)過程中一組或多組數(shù)據(jù)的隨機誤差都會對最終的積分運算結(jié)果產(chǎn)生影響。為了消除隨機誤差，采用卡爾曼濾波器對原始數(shù)據(jù)進行預處理。

設卡爾曼濾波器的離散模型為xk+1=φκxk+ωk,xk為系統(tǒng)狀態(tài)向量，φk為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣，ωk為系統(tǒng)噪聲。

在噪聲環(huán)境下，系統(tǒng)測量方程為zk=Hxk+vk，vk為測量噪聲，H為測量矩陣。

本文將加速度狀態(tài)作為控制量加入到濾波器中有

xk+1=φκxk+ωk+BkUk,zk+1=Hxk+fφκ

(1)

式中Uk為加速度狀態(tài)，Uk=[ax,ay,az]，且

(2)

1.2 積分變換原理

根據(jù)牛頓第二定律針對物體運動軌跡檢測，對物體產(chǎn)生的加速度進行一次積分得到移動的速度，進而二次積分得到位移。由于采集的加速度數(shù)據(jù)是離散的，設傳感器采樣時間間隔為Δt，根據(jù)迭代的方法得出

(3)

式中v(n)為tn時刻的瞬時速度，a(n)為tn時刻的瞬時加速度，s(n)為0～tn時間段的位移。推廣到三維空間，tn時刻沿加速度傳感器x,y,z軸方向上的運動位移分別為

(4)

則tn時刻目標物體的空間坐標位置為(sx(n)，sy(n)，sz(n))。在三維坐標系中，連接各個時刻的空間位置坐標點即可得出空間運動軌跡。

由于物體在空間運動中會發(fā)生方向的改變，傳感器所在的三軸坐標系也會隨之改變，為了得出空間軌跡，需要把測得的加速度轉(zhuǎn)換到恒定的參考坐標系中。設目標物體的地理坐標系為oxnynzn，傳感器的載體坐標系為oxbybzb。由于矩陣方程乘法不具有交換性，從地理坐標系到載體坐標系的以Z,X,Y軸轉(zhuǎn)動順序得出的變換關(guān)系為

(5)

(6)

通過上式的計算，可以得到目標物體所處空間的加速度。而涉及到旋轉(zhuǎn)矩陣的求解，分為歐拉角法[15,16]、方向余弦法[17]、四元數(shù)法。其中四元數(shù)法只要解四個未知量的線性微分方程即可，計算量小，易于操作，也滿足實時性要求，本文采用四元數(shù)法來求解旋轉(zhuǎn)矩陣。

歐拉角表示的方向余弦矩陣用四元數(shù)表示為

(7)

1.3 改進的Mahony互補濾波算法

本文將廣泛應用于四軸飛行器和平衡車上的Mahony互補濾波姿態(tài)解算算法，調(diào)整后應用到三維運動軌跡恢復實驗中。考慮到三軸的電子羅盤數(shù)據(jù)的誤差波動，算法只保留了加速度計的數(shù)據(jù)來修正陀螺儀快速解算帶來的誤差。

設載體坐標系上加速度計測出的重力單位向量a=(ax,ay,az)，而v為陀螺積分后的姿態(tài)推算出的重力向量，均載體坐標系上的重力向量。各量的誤差向量就是陀螺積分后的姿態(tài)和加速度計測出的姿態(tài)間的誤差。向量之間的誤差，用叉乘來表示。叉乘的大小與陀螺積分誤差成正比，可以用于糾正陀螺儀。程序上可以對e進行PI控制[18]修正陀螺儀零偏,即

E(n)=E(n-1)+Ki×e

g(n)=g(n-1)+Kp×e+E(n)

(8)

式中g(shù)=(gx,gy,gz)為陀螺儀角速度數(shù)據(jù)。 通過調(diào)整Ki和Kp兩個參數(shù)能夠借助加速度數(shù)據(jù)快速修正陀螺儀角速度數(shù)據(jù)。

通過求解四元數(shù)的微分方程，進行四元數(shù)的更新，其中T為測量周期，傳統(tǒng)的求解采用一階龍格庫塔法求解,由于四元數(shù)更新是整個算法實現(xiàn)的關(guān)鍵一步，本文提出采用高精度的四階龍格庫塔法取代原有的一階龍格庫塔，使算法在四元數(shù)求解這一部分實時性準確性更佳。最終以歸一化后的四元數(shù)利用式(8)計算當前的旋轉(zhuǎn)矩陣，求出地理坐標系下的加速度，進行二次積分得出位移。

2 多項式位移補償

在實際中，累積誤差會導致系統(tǒng)的初始狀態(tài)和終止狀態(tài)與理論不符。為解決這個問題,文獻[19]論證了使用多項式對速度和位移積分結(jié)果重建的可行性，并通過一次多項式實現(xiàn)了該算法。本文拓展了多項補償理論，將系統(tǒng)的累積誤差分配到整個計算過程。具體算法如下：

設速度補償二次多項式為

vr(k)=v(k)+b1k2+b2k+b3

(9)

位移補償多項式

(10)

根據(jù)理想狀態(tài)下運動開始前和終止后速度為零，開始前加速度和位移也為零，即vr(0)=0,vr(ke)=0,ar(0)=0,sr(0)=0，ke為運動終止時間。速度與位移的補償多項式為

v(0)k-s(0)

(11)

3 實驗與結(jié)果分析

本文的實驗分為兩個部分，實驗一使用iPhone se手機內(nèi)置的傳感器，通過MATLAB MOBILE采集加速度數(shù)據(jù)和角速度數(shù)據(jù)，進行常規(guī)運動的軌跡還原實驗。實驗二使用MPU6050九軸運動傳感器，采集數(shù)據(jù)進行進一步實驗驗證。

實驗首先對原始加速度和角速度數(shù)據(jù)通過本文描述的卡爾曼濾波進行預處理，處理后的數(shù)據(jù)進行坐標系的旋轉(zhuǎn)變換，根據(jù)積分原理得出位移再進行補償?shù)玫杰壽E。對三軸加速度數(shù)據(jù)預處理的前后對比發(fā)現(xiàn)本文描述的卡爾曼濾波對數(shù)據(jù)波動起到的平滑作用顯著，減小了隨機誤差。

實驗一取手機在水平方向從左側(cè)到右側(cè)的進行一次簡單的平移。根據(jù)手機內(nèi)置傳感器軸向，如圖1所示，軌跡應當為一條平行于x-y平面的平滑曲線。為了檢測3種算法的實驗效率，實驗中將初始位置設為三維坐標原點，圖2(a)為使用手機內(nèi)置傳感器對應三種算法的空間軌跡。

圖1 手機內(nèi)置傳感器坐標軸方向

圖2 實驗結(jié)果

通過比較三條空間曲線，傳統(tǒng)Mahony互補濾波算法計算的三維曲線在z軸的波動巨大，接近8 m，算法的準確度較差。使用經(jīng)典四階龍格庫塔法更新四元數(shù)得出的曲線后段近似直線，z軸的波動極小，但從原點出發(fā)后的前段曲線在z軸和y軸的方向上有明顯的波動，且整條曲線不夠平滑，算法魯棒性不足。改進后的Mahony互補濾波算法實現(xiàn)，整條軌跡相對平滑，z軸的波動范圍控制在[-0.05,0]m,比較完整地復現(xiàn)手機運動的軌跡，改進后的算法較前兩種準確性和魯棒性明顯提高。

實驗二使用MPU6050九軸運動傳感器在單位立方空間中從上到下做斜線運動，同時比較三種算法的軌跡。圖2(b)可以看出，傳統(tǒng)Mahony互補濾波算法和經(jīng)典四階龍格庫塔法求解的軌跡在x軸方向迅速收斂，沒有達到預期的位置，四階龍格庫塔法的曲線在后段有向y軸的波動，平滑度不夠。改進后的互補濾波算法求解的軌跡接近預期的位置，且曲線平滑，軌跡恢復的完整度高，驗證了算法的準確性和魯棒性。

4 結(jié)束語

本文以定位和導航的理論為基礎，對室內(nèi)場景下運動物體的軌跡恢復進行了深入研究。將改進后的的Mahony互補濾波算法運用到實驗中，使用兩種傳感器測出的加速度和角速度數(shù)據(jù)，經(jīng)過預處理，坐標系變換，旋轉(zhuǎn)矩陣更新，位移補償進而求得運動物體的軌跡。實驗結(jié)果證明：改進Mahony互補濾波算法在準確性和魯棒性上有明顯優(yōu)勢，在三維運動軌跡恢復的研究中有很大應用價值。