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

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

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

0 引 言

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

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

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

1 運動軌跡恢復

1.1 預處理

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

設卡爾曼濾波器的離散模型為xk+1=φκxk+ωk,xk為系統狀態向量，φk為系統狀態轉移矩陣，ωk為系統噪聲。

在噪聲環境下，系統測量方程為zk=Hxk+vk，vk為測量噪聲，H為測量矩陣。

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

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

(1)

式中Uk為加速度狀態，Uk=[ax,ay,az]，且

(2)

1.2 積分變換原理

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

(3)

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

(4)

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

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

(5)

(6)

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

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

(7)

1.3 改進的Mahony互補濾波算法

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

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

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

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

(8)

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

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

2 多項式位移補償

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

設速度補償二次多項式為

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

(9)

位移補償多項式

(10)

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

v(0)k-s(0)

(11)

3 實驗與結果分析

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

實驗首先對原始加速度和角速度數據通過本文描述的卡爾曼濾波進行預處理，處理后的數據進行坐標系的旋轉變換，根據積分原理得出位移再進行補償得到軌跡。對三軸加速度數據預處理的前后對比發現本文描述的卡爾曼濾波對數據波動起到的平滑作用顯著，減小了隨機誤差。

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

圖1 手機內置傳感器坐標軸方向

圖2 實驗結果

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

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

4 結束語

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