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

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

0 引 言

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

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

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

1 運(yùn)動(dòng)軌跡恢復(fù)

1.1 預(yù)處理

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

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

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

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

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

(1)

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

(2)

1.2 積分變換原理

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

(3)

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

(4)

則tn時(shí)刻目標(biāo)物體的空間坐標(biāo)位置為(sx(n)，sy(n)，sz(n))。在三維坐標(biāo)系中，連接各個(gè)時(shí)刻的空間位置坐標(biāo)點(diǎn)即可得出空間運(yùn)動(dòng)軌跡。

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

(5)

(6)

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

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

(7)

1.3 改進(jìn)的Mahony互補(bǔ)濾波算法

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

設(shè)載體坐標(biāo)系上加速度計(jì)測(cè)出的重力單位向量a=(ax,ay,az)，而v為陀螺積分后的姿態(tài)推算出的重力向量，均載體坐標(biāo)系上的重力向量。各量的誤差向量就是陀螺積分后的姿態(tài)和加速度計(jì)測(cè)出的姿態(tài)間的誤差。向量之間的誤差，用叉乘來(lái)表示。叉乘的大小與陀螺積分誤差成正比，可以用于糾正陀螺儀。程序上可以對(duì)e進(jìn)行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ù)。 通過(guò)調(diào)整Ki和Kp兩個(gè)參數(shù)能夠借助加速度數(shù)據(jù)快速修正陀螺儀角速度數(shù)據(jù)。

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

2 多項(xiàng)式位移補(bǔ)償

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

設(shè)速度補(bǔ)償二次多項(xiàng)式為

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

(9)

位移補(bǔ)償多項(xiàng)式

(10)

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

v(0)k-s(0)

(11)

3 實(shí)驗(yàn)與結(jié)果分析

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

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

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

圖1 手機(jī)內(nèi)置傳感器坐標(biāo)軸方向

圖2 實(shí)驗(yàn)結(jié)果

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

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

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

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