專利名稱：考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法
技術(shù)領(lǐng)域：
本發(fā)明涉及慣性裝置的姿態(tài)感知技術(shù)領(lǐng)域，是一種適用于集成三軸陀螺儀、三軸加速度計(jì)和三軸磁場傳感器的慣性裝置的姿態(tài)感知和估計(jì)方法，具體是一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波(UKF，UnscentedKalman Filter)的慣性位姿跟蹤方法。

背景技術(shù)：
利用機(jī)電慣性測量組合技術(shù)進(jìn)行運(yùn)動載體位姿的跟蹤具有非常廣闊的前景。慣性跟蹤系統(tǒng)的基本原理是在目標(biāo)初始位置和姿態(tài)已知的基礎(chǔ)上，依據(jù)慣性原理，利用陀螺儀和加速度計(jì)等慣性敏感元件測量物體運(yùn)動的角速度和直線加速度，然后通過積分獲得物體的位置和姿態(tài)。由于慣性積分存在累積誤差效應(yīng)，通常需要附加磁場傳感器等其它感知元件，以便能夠在較長運(yùn)行時(shí)間內(nèi)保證系統(tǒng)的位姿感知精度。
對于集成三軸陀螺儀、三軸加速度計(jì)以及三軸磁場傳感器的檢測裝置而言，其位姿跟蹤算法通常利用四元數(shù)描述裝置相應(yīng)于初始時(shí)刻的姿態(tài)信息，然后借助于最陡下降法或者卡爾曼(Kalman)濾波技術(shù)實(shí)現(xiàn)裝置姿態(tài)的實(shí)時(shí)跟蹤。上述方法在對加速度信息進(jìn)行處理時(shí)，通常假設(shè)裝置本身的運(yùn)動加速度幅值遠(yuǎn)遠(yuǎn)小于重力加速度，以便能夠利用線性技術(shù)實(shí)現(xiàn)位姿跟蹤過程。上述假設(shè)所帶來的缺陷是姿態(tài)感知精度的降低，尤其在裝置本身存在較大幅度的運(yùn)動情況下。
Choukroun D.提出了一種基于四元數(shù)的Kalman濾波方法(見Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalmanfilter，IEEE Transactions on Aerospace and Electronic Systems，2006，Vol.42，No.1174-190)。該方法較為詳盡地描述了利用Kalman技術(shù)實(shí)現(xiàn)四元數(shù)姿態(tài)跟蹤的系統(tǒng)數(shù)學(xué)模型和噪聲模型，其突出貢獻(xiàn)是系統(tǒng)模型的偽線性化和狀態(tài)相關(guān)噪聲的協(xié)方差矩陣的更新方法。然而，遺憾的是，該方法仍然沒有考慮裝置自身加速度對于系統(tǒng)精度的影響，從而導(dǎo)致其精度在某些情況下不能得到保證。
本發(fā)明在Choukroun D.所提方法的基礎(chǔ)上，充分考慮到裝置自身加速度對系統(tǒng)模型的影響，對其帶來的非線性問題利用UKF技術(shù)對其進(jìn)行解決，從而提出了一種考慮加速度補(bǔ)償和基于UKF的慣性位姿跟蹤方法。
本發(fā)明申請人在申請?zhí)枮椤?00810114391.4”的中國專利“基于ZigBee無線單片機(jī)的微慣性測量裝置”中提供了一種可用于運(yùn)動載體姿態(tài)測量的裝置。在該申請中，采用六軸微慣性傳感器和三軸磁阻傳感器來測量運(yùn)動載體的姿態(tài)，通過基于ZigBee無線單片機(jī)對所測得的信號進(jìn)行姿態(tài)解算，并將解算得到的姿態(tài)信息以無線方式傳送給其他系統(tǒng)或者上位機(jī)，該申請?jiān)诒旧暾堉幸胱鳛閰⒖肌?br>

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，適用于集成三軸陀螺儀、三軸加速度計(jì)和三軸磁場傳感器的慣性裝置的慣性位姿跟蹤算法，該算法不但能夠克服系統(tǒng)誤差，而且能夠在裝置本身存在較大幅度的運(yùn)動情況下也能保持較高精度。
為了達(dá)到上述目的，本發(fā)明的技術(shù)解決方案是 一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波(UKF，Unscented KalmanFilter)的慣性位姿跟蹤方法，適用于以正交方式集成三軸陀螺儀、三軸加速度計(jì)和三軸磁場傳感器的姿態(tài)感知裝置；其系統(tǒng)狀態(tài)向量包含了裝置本身的運(yùn)動加速度，并對其進(jìn)行濾波估計(jì)；包括以下步驟 1保持裝置固定不動，當(dāng)前姿態(tài)稱為初始姿態(tài)；采集三軸加速度傳感器和三軸磁阻傳感器數(shù)據(jù)，得到初始姿態(tài)下的加速度矢量ao＝[aox，aoy，aoz]T和磁場矢量mo＝[mox，moy，moz]T； 2在k時(shí)刻采集三軸陀螺儀、三軸加速度傳感器和三軸磁阻傳感器數(shù)據(jù)，得到裝置當(dāng)前姿態(tài)下的旋轉(zhuǎn)角速度矢量ωt(k)＝[ωtx(k)，ωty(k)，ωtz(k)]T、加速度矢量at(k)＝[atx(k)，aty(k)，atz(k)]T和磁場矢量mt(k)＝[mtx(k)，mty(k)，mtz(k)]T； 3構(gòu)建系統(tǒng)狀態(tài)方程 定義系統(tǒng)狀態(tài)向量為 X(k)＝[qT(k)，μT(k)，abT(k)]T(1) 其中q(k)＝[q0(k)，q1(k)，q2(k)，q3(k)]T為描述當(dāng)前姿態(tài)同初始姿態(tài)之間相對關(guān)系的旋轉(zhuǎn)四元數(shù)矢量，μ(k)＝[μx(k)，μy(k)，μz(k)]T為三軸陀螺儀的累積誤差矢量，ab(k)＝[abx(k)，aby(k)，abz(k)]T為裝置自身的運(yùn)動加速度矢量； 依據(jù)上述狀態(tài)向量的系統(tǒng)狀態(tài)方程為 式中Δt為采樣周期，

nGx為噪聲矢量，其協(xié)方差矩陣為

矩陣

I為相應(yīng)階次的單位矩陣； 4構(gòu)建系統(tǒng)觀測方程 式中，aob(k)＝ao+ab(k)，對于任意三維矢量r＝[rx，ry，rz]T，

為其歸一化單位方向矢量，||r||為其幅值，矩陣 其中，[□]×表示由相應(yīng)向量定義的反對稱矩陣；nGz為觀測噪聲矢量，其協(xié)方差矩陣為
5系統(tǒng)狀態(tài)Sigma點(diǎn)采樣根據(jù)k-1時(shí)刻的系統(tǒng)狀態(tài)X(k-1/k-1)和協(xié)方差矩陣P(k-1/k-1)進(jìn)行Sigma點(diǎn)采樣，得到21個(gè)點(diǎn)樣本為Xsi，i＝1，…，20； 6UKF預(yù)測根據(jù)方程(2)，對21個(gè)Sigma點(diǎn)進(jìn)行狀態(tài)預(yù)測 Xspi＝F(Xsi，ωt(k))i＝0，…，21(4) 利用上述采樣預(yù)測值確定系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終預(yù)測值為 Mk-1＝q(k-1/k-1)qT(k-1/k-1)+Pq(k-1/k-1) (8) 其中Pq(k-1/k-1)為矩陣P(k-1/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣；wi為相應(yīng)點(diǎn)樣本的權(quán)值； 7UKF更新對于Sigma點(diǎn)預(yù)測Xspi，令qi(k/k-1)為由向量Xspi前四個(gè)元素得到的歸一化四元數(shù)，根據(jù)觀測方程其觀測值計(jì)算為 Zi(k)＝G(Xspi(k/k-1))i＝0，…，21(9) 而最終觀測值計(jì)算為 系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終更新為 X(k/k)＝X(k/k-1)-KZ(k) (11) P(k/k)＝P(k/k-1)-KPZZKT(12) 其中 其中Pq(k/k-1)為矩陣P(k/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣； 8將X(k/k)中的四元數(shù)向量元素進(jìn)行歸一化處理，并利用四元數(shù)表示同歐拉角表示之間的關(guān)系將其轉(zhuǎn)換為具有較為直觀意義的俯仰角、橫滾角和航向角。
所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其所述系統(tǒng)狀態(tài)向量包含了裝置本身的運(yùn)動加速度，從而導(dǎo)致了系統(tǒng)的觀測方程非線性，因此采用UKF技術(shù)實(shí)現(xiàn)對于系統(tǒng)狀態(tài)的濾波估計(jì)。
所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其對于加速度矢量和磁場矢量分別依據(jù)其單位方向矢量構(gòu)建觀測方程Za1(k)和Zm(k)；為了能夠有效地估計(jì)出裝置本身的運(yùn)動加速度，觀測方程中的Za2(k)對加速度矢量進(jìn)行了幅值限制，從而在單位方向矢量一定的情況下，能夠唯一估計(jì)出加速度矢量ab(k)。
所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其系統(tǒng)狀態(tài)方程或系統(tǒng)觀測方程，其中的噪聲均與系統(tǒng)狀態(tài)向量相關(guān)，對于協(xié)方差矩陣的預(yù)測和更新均對此進(jìn)行了處理。
所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其由于系統(tǒng)狀態(tài)包含了裝置本身的運(yùn)動加速度，同以往忽略該項(xiàng)因素相比，能夠獲取更為精確的姿態(tài)估計(jì)結(jié)果，尤其在轉(zhuǎn)置本身運(yùn)動幅度較大的情況下；同時(shí)，估計(jì)得到的裝置本身的運(yùn)動加速度，通過積分實(shí)現(xiàn)轉(zhuǎn)置的粗略位置確定。
同傳統(tǒng)的忽略載體本身加速度的方法相比，本發(fā)明方法能夠克服系統(tǒng)誤差，給出更為準(zhǔn)確的估計(jì)結(jié)果，在裝置本身存在較大幅度的運(yùn)動情況下也能保持較高精度，同時(shí)，估計(jì)得到的裝置運(yùn)動加速度可以通過積分實(shí)現(xiàn)轉(zhuǎn)置的粗略位置確定，從而拓寬了系統(tǒng)應(yīng)用范圍，可應(yīng)用于機(jī)器人、飛行器、車輛、人體運(yùn)動等領(lǐng)域的位姿檢測。



圖1為本發(fā)明的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法的示意框圖； 圖2為本發(fā)明所適用的慣性位姿跟蹤裝置的結(jié)構(gòu)圖； 圖3為裝置沿X軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖； 圖4為裝置沿Y軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖； 圖5為裝置沿Z軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖； 圖6為裝置以較大加速度沿Y軸做往返平移運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖； 圖7為裝置以較大加速度沿Y軸做往返平移運(yùn)動時(shí)，本發(fā)明方法所提取出的裝置本身的自運(yùn)動加速度。

具體實(shí)施例方式 本發(fā)明一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法采用如圖1所示結(jié)構(gòu)，主要包括以下步驟 1)初始姿態(tài)標(biāo)定(1)保持裝置固定不動，當(dāng)前姿態(tài)稱為初始姿態(tài)，此時(shí)由裝置的三個(gè)正交軸所確定的坐標(biāo)系作為世界參考坐標(biāo)系；采集三軸加速度傳感器和三軸磁阻傳感器數(shù)據(jù)，得到描述于世界坐標(biāo)系下的初始加速度矢量ao＝[aox，aoy，aoz]T和磁場矢量mo＝[mox，moy，moz]T； 2)在k時(shí)刻采集三軸陀螺儀(2-1)、三軸加速度傳感器(2-2)和三軸磁阻傳感器(2-3)數(shù)據(jù)，得到描述于裝置坐標(biāo)系下的當(dāng)前旋轉(zhuǎn)角速度矢量ωt(k)＝[ωtx(k)，ωty(k)，ωtz(k)]T、加速度矢量at(k)＝[atx(k)，aty(k)，atz(k)]T和磁場矢量mt(k)＝[mtx(k)，mty(k)，mtz(k)]T； 3)構(gòu)建系統(tǒng)狀態(tài)方程(3) 定義系統(tǒng)狀態(tài)向量為 X(k)＝[qT(k)，μT(k)，abT(k)]T(1) 其中q(k)＝[q0(k)，q1(k)，q2(k)，q3(k)]T為描述當(dāng)前姿態(tài)同初始姿態(tài)之間，即裝置坐標(biāo)系同世界參考坐標(biāo)系之間相對關(guān)系的旋轉(zhuǎn)單位四元數(shù)矢量，μ(k)＝[μx(k)，μy(k)，μz(k)]T為三軸陀螺儀的累積誤差矢量，ab(k)＝[abx(k)，aby(k)，abz(k)]T為描述于世界坐標(biāo)系中的裝置自身的運(yùn)動加速度矢量。
用以描述裝置坐標(biāo)系同世界坐標(biāo)系之間關(guān)系的單位四元數(shù)q(k)滿足以下離散差分方程 其中，ω(k)＝[ωx(k)，ωy(k)，ωz(k)]T為裝置在k時(shí)刻的瞬時(shí)旋轉(zhuǎn)角速度矢量， 描述于裝置坐標(biāo)系；矩陣

I為相應(yīng)階次的單位矩陣；Δt為采樣周期。旋轉(zhuǎn)角速度矢量ω(k)可由三軸陀螺儀感知得到，然而必然引入陀螺儀噪聲分量，即 ωt(k)＝ω(k)+nω1+nω2+μ(k)(3) 由上式可以看出，陀螺儀噪聲矢量由三部分組成電磁噪聲nω1為各軸相互獨(dú)立、且服從標(biāo)準(zhǔn)方差為σω1的零均值高斯白噪聲；漂移扭矩噪聲nω2為各軸相互獨(dú)立、且服從標(biāo)準(zhǔn)方差為

的零均值高斯白噪聲；μ(k)為截至到k時(shí)刻，各軸陀螺儀的累積誤差矢量。將(2)式代入(1)式，并經(jīng)過整理可得 其中， 累積誤差μ(k)可建模為由標(biāo)準(zhǔn)方差為

的零均值高斯白噪聲nω3驅(qū)動的隨機(jī)游動噪聲，即 μ(k)＝μ(k-1)+nω3(5) 裝置本體自身加速度矢量ab(k)描述于世界參考坐標(biāo)系，其變化可以視為具有適當(dāng)方差

的零均值高斯白噪聲

其合理性一方面在于采樣周期較小，相鄰兩時(shí)刻間的加速度變化可以理解為隨機(jī)擾動，而方差則描述了這種擾動的幅度；另一方面，在沒有其它信息可用的情況下，可以認(rèn)為k時(shí)刻的加速度以較高概率保持k-1時(shí)刻的值，并在一定范圍內(nèi)服從高斯分布。因此 綜合(4)、(5)和(6)式，可得系統(tǒng)狀態(tài)方程為 其中

為系統(tǒng)噪聲向量，其協(xié)方差矩陣Q計(jì)算為 4)依據(jù)加速度矢量at(k)和磁場矢量mt(k)構(gòu)建系統(tǒng)觀測方程(4) k時(shí)刻的加速度矢量at(k)為重力加速度矢量和裝置自身加速度矢量的復(fù)合。在以往方法中，往往假設(shè)裝置自身加速度矢量遠(yuǎn)遠(yuǎn)小于重力加速度矢量，并因此忽略裝置的自身加速度矢量，以期便于利用相關(guān)線性理論和方法對其進(jìn)行處理。盡管在大多數(shù)場合下，這種假設(shè)是成立的，然而在裝置進(jìn)行大幅度變化運(yùn)動時(shí)，往往會導(dǎo)致較大估計(jì)誤差的產(chǎn)生，從而影響系統(tǒng)性能。本發(fā)明對裝置自身加速度矢量和重力加速度矢量進(jìn)行綜合考慮，這一點(diǎn)不但體現(xiàn)在系統(tǒng)狀態(tài)向量的定義和系統(tǒng)狀態(tài)方程的構(gòu)建中，最為重要的是體現(xiàn)在觀測方程的構(gòu)建上。
理想情況下，描述于裝置坐標(biāo)系的加速度矢量at(k)同描述于世界坐標(biāo)系的初始加速度矢量(可以理解為純粹的重力加速度矢量)ao、裝置自身的運(yùn)動加速度矢量ab(k)之間存在以下關(guān)系 其中，aob(k)＝ao+ab(k)；各矢量的上標(biāo)q表示由0和該矢量所構(gòu)成的矢量四元數(shù)，

表示四元數(shù)乘積。實(shí)際上，由于存在感知誤差，上式通常難以得到滿足，四元數(shù)q(k)也只是在矢量at(k)和aob(k)具有相同幅值的情況下，才能對二者所在坐標(biāo)系之間的相對關(guān)系進(jìn)行正確描述。由于待估量ab(k)的存在，q(k)不能同時(shí)保證二者具有相同的幅值和方向，為此本發(fā)明針對加速度矢量的方向和幅度分別建立觀測方程。首先，無論aob(k)幅值如何，其必然和矢量at(k)具有相同方向，即二者滿足 其中為相應(yīng)矢量的歸一化四元數(shù)，即

為方差為

的零均值高斯白噪聲。(10)式兩端同時(shí)左乘q(k)/2，并對其進(jìn)行整理可得 其中，[□]×表示由相應(yīng)向量定義的反對稱矩陣。
矢量at(k)和aob(k)之間的幅值差異，可描述為具有較小方差σ|a|的零均值白噪聲n|a|，由此可建立方程 磁場矢量mt(k)和mo為同一地磁矢量在不同坐標(biāo)系下的描述，同加速度矢量相似，二者也應(yīng)該具有一致的方向和相同的幅值。由于mt(k)為傳感器檢測矢量，此處僅根據(jù)二者單位方向矢量之間的關(guān)系構(gòu)建觀測方程如式(15)，其推導(dǎo)過程同加速度矢量類似。
其中，

分別為mt(k)和mo的歸一化單位方向矢量；

為方差為

的零均值白噪聲。
綜合式(13)、(14)、(15)，可得系統(tǒng)的總體觀測方程為 式中

為觀測噪聲矢量，其協(xié)方差矩陣為 由系統(tǒng)狀態(tài)方程和觀測方程可以看出系統(tǒng)具有非線性性質(zhì)，無論是系統(tǒng)噪聲還是觀測噪聲也均同當(dāng)前時(shí)刻的狀態(tài)向量相關(guān)，因此不能采用傳統(tǒng)的線性Kalman濾波理論實(shí)現(xiàn)相關(guān)狀態(tài)的估計(jì)；同時(shí)，觀測方程的泰勒展開式由于不能保證收斂而無法使用擴(kuò)展Kalman濾波方法。因此，本發(fā)明采用UKF技術(shù)實(shí)現(xiàn)系統(tǒng)狀態(tài)的估計(jì)過程，并對系統(tǒng)噪聲和觀測噪聲的協(xié)方差矩陣依據(jù)相關(guān)狀態(tài)向量值進(jìn)行實(shí)時(shí)更新。為此，本發(fā)明仍包括以下幾個(gè)步驟 5)系統(tǒng)狀態(tài)Sigma點(diǎn)采樣(5)。根據(jù)k-1時(shí)刻的系統(tǒng)狀態(tài)X(k-1/k-1)和協(xié)方差矩陣P(k-1/k-1)進(jìn)行Sigma點(diǎn)采樣(依據(jù)統(tǒng)計(jì)量進(jìn)行樣本點(diǎn)抽取的一種技術(shù)，具體見文獻(xiàn)Julier，Simon J.and Jeffery K.Uhlmann.ANew Extension of the Kalman Filter to Nonl inear Systems.TheProceedings of AeroSenseThe 1lth International Symposium onAerospace/Defense Sensing，Simulation and Controls，Multi SensorFusion，Tracking and Resource Management 11，SPIE，1997)，得到21個(gè)點(diǎn)樣本為 Xs0＝X(k-1/k-1) i＝1，…，10(17) i＝11，…，20 其中

為矩陣平方根矩陣的第i列。
6)UKF預(yù)測(6)。根據(jù)系統(tǒng)狀態(tài)方程(7)，分別依據(jù)21個(gè)Sigma點(diǎn)樣本進(jìn)行狀態(tài)預(yù)測 Xspi＝F(Xsi，ωt(k))i＝0，…，21(18) 利用上述采樣預(yù)測值確定系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終預(yù)測值為 Mk-1＝q(k-1/k-1)qT(k-1/k-1)+Pq(k-1/k-1)(22) 其中，wi為相關(guān)點(diǎn)采樣的權(quán)值對于Xs0而言，

對于其它點(diǎn)樣本，

Pq(k-1/k-1)為矩陣P(k-1/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣；矩陣Qk-1的計(jì)算推導(dǎo)過程請參閱文獻(xiàn)Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalman filter，IEEE Transactions onAerospace and Electronic Systems，2006，Vol.42，No.1174-190。
7)UKF更新(7)。對于Sigma點(diǎn)預(yù)測Xspi，令qi(k/k-1)為由向量Xspi前四個(gè)元素得到的歸一化四元數(shù)，根據(jù)觀測方程其觀測值計(jì)算為 Zi(k)＝G(Xspi(k/k-1))i＝0，…，21(23) 而最終觀測值計(jì)算為 系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終更新為 X(k/k)＝X(k/k-1)-KZ(k) (25) P(k/k)＝P(k/k-1)-KPZZKT(26) 其中 其中Pq(k/k-1)為矩陣P(k/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣，協(xié)方差矩陣Ri的計(jì)算推導(dǎo)過程請參閱文獻(xiàn)Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalman filter，IEEE Transactions onAerospace and Electronic Systems，2006，Vol.42，No.1174-190。
8)歐拉角解算(8)將X(k/k)中的四元數(shù)向量元素進(jìn)行歸一化處理，并根據(jù)旋轉(zhuǎn)的四元數(shù)表示同歐拉角表示之間的關(guān)系將其轉(zhuǎn)換為具有較為直觀意義的俯仰角β、橫滾角α和航向角γ 本發(fā)明考慮加速度補(bǔ)償和基于UKF的姿態(tài)跟蹤方法適用于具有如圖2所示架構(gòu)的慣性姿態(tài)跟蹤裝置。該類裝置以正交方式集成了三個(gè)加速度傳感器、三個(gè)陀螺儀和三個(gè)磁場傳感器，三組傳感器所在的正交軸構(gòu)成了裝置坐標(biāo)系。
依據(jù)上述具體步驟，對其中的具體實(shí)現(xiàn)細(xì)節(jié)進(jìn)行如下說明 1)在步驟(1)初始姿態(tài)標(biāo)定過程中，由于各傳感器均存在感知噪聲，需要進(jìn)行多次采樣，對多次采樣值進(jìn)行平均后作為描述于世界坐標(biāo)系下的初始加速度矢量ao和磁場矢量mo； 2)在步驟(3)系統(tǒng)狀態(tài)方程構(gòu)建過程中，陀螺儀電磁噪聲nω1方差σω1＝0.2；陀螺儀漂移扭矩噪聲nω2方差

中的參數(shù)σω2＝4.0；激勵(lì)陀螺儀累積誤差的零均值高斯白噪聲nω3的方差

中的參數(shù)σω3＝0.001；裝置本身運(yùn)動加速度的噪聲方差

設(shè)為重力加速度g。
3)在步驟(4)系統(tǒng)觀測方程構(gòu)建過程中，加速度矢量

方差

噪聲n|a|的方差σ|a|＝0.05；磁場噪聲矢量

各分量的方差為
4)在UKF濾波過程中，系統(tǒng)狀態(tài)向量X的初始值設(shè)置為 X(0/0)＝[1，0，0，0，0，0，0，0，0，0]T 協(xié)方差矩陣P的初始值設(shè)置為P(0/0)＝0.5I10×10 5)本發(fā)明所述方法的系統(tǒng)采樣和濾波周期為Δt＝30ms。
采用本發(fā)明考慮加速度補(bǔ)償和基于UKF的姿態(tài)跟蹤方法，可以取得以下效果 一方面，在裝置本體自身運(yùn)動幅度較小的情況下，能夠取得同傳統(tǒng)Kalman濾波方法相似甚至更好的效果，這一點(diǎn)從圖3、圖4和圖5所示的、裝置分別沿X軸、Y軸和Z軸做純旋轉(zhuǎn)運(yùn)動時(shí)的跟蹤效果對比可以看出。圖3為裝置沿X軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖；其中，帶有圓形標(biāo)記的曲線為采用傳統(tǒng)Kalman濾波技術(shù)得到的估計(jì)結(jié)果曲線；帶有十字標(biāo)記的曲線為采用本發(fā)明所得到的濾波估計(jì)結(jié)果曲線。圖4為裝置沿Y軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖；其中，帶有圓形標(biāo)記的曲線為采用傳統(tǒng)Kalman濾波技術(shù)得到的估計(jì)結(jié)果曲線；帶有十字標(biāo)記的曲線為采用本發(fā)明所得到的濾波估計(jì)結(jié)果曲線。圖5為裝置沿Z軸做純旋轉(zhuǎn)運(yùn)動時(shí)，本發(fā)明方法和傳統(tǒng)Kalman濾波方法的姿態(tài)跟蹤曲線圖；其中，帶有圓形標(biāo)記的曲線為采用傳統(tǒng)Kalman濾波技術(shù)得到的估計(jì)結(jié)果曲線；帶有十字標(biāo)記的曲線為采用本發(fā)明所得到的濾波估計(jì)結(jié)果曲線。
實(shí)際上，由于本發(fā)明方法對加速度進(jìn)行了補(bǔ)償，在一定程度上排除了導(dǎo)致傳統(tǒng)方法誤差產(chǎn)生的一個(gè)重要因素，因此，應(yīng)該比傳統(tǒng)方法具有更高的精度。只是在裝置本體自身運(yùn)動加速度較小的情況下，這種精度的改善幅度不大。
另一方面，當(dāng)裝置本體自身運(yùn)動幅度較大時(shí)，本發(fā)明方法對于系統(tǒng)精度的提高效果是非常明顯的。圖6所示為當(dāng)裝置沿Y軸以較大加速度進(jìn)行往返平移運(yùn)動時(shí)，利用傳統(tǒng)Kalman濾波技術(shù)和利用本發(fā)明方法所感知到的裝置姿態(tài)的變化曲線，其中，帶有星形標(biāo)記的曲線為采用傳統(tǒng)Kalman濾波技術(shù)得到的估計(jì)結(jié)果曲線；帶有十字標(biāo)記的曲線為采用本發(fā)明所得到的濾波估計(jì)結(jié)果曲線。理論上，當(dāng)裝置做純平移運(yùn)動時(shí)，其姿態(tài)保持不變，然而由于傳統(tǒng)Kalman濾波技術(shù)忽略了裝置本身自運(yùn)動加速度對于感知過程的影響，將裝置沿Y軸方向上的加速度歸因于繞X軸或繞Z軸的旋轉(zhuǎn)運(yùn)動所致，從而導(dǎo)致如圖6中所示的X軸方向和Z軸方向上的歐拉角具有較大偏差。本發(fā)明方法對裝置本身自運(yùn)動加速度進(jìn)行了充分考慮和補(bǔ)償，從而取得較為理想的跟蹤效果。圖7所示為裝置進(jìn)行上述運(yùn)動時(shí)，利用本發(fā)明方法所提取出的裝置本身的自運(yùn)動加速度。
權(quán)利要求
1.一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，適用于以正交方式集成三軸陀螺儀、三軸加速度計(jì)和三軸磁場傳感器的姿態(tài)感知裝置；其特征在于，系統(tǒng)狀態(tài)向量包含了裝置本身的運(yùn)動加速度，并對其進(jìn)行濾波估計(jì)；
包括以下步驟
1)保持裝置固定不動，當(dāng)前姿態(tài)稱為初始姿態(tài)；采集三軸加速度傳感器和三軸磁阻傳感器數(shù)據(jù)，得到初始姿態(tài)下的加速度矢量ao＝[aox，aoy，aoz]T和磁場矢量mo＝[mox，moy，moz]T；
2)在k時(shí)刻采集三軸陀螺儀、三軸加速度傳感器和三軸磁阻傳感器數(shù)據(jù)，得到裝置當(dāng)前姿態(tài)下的旋轉(zhuǎn)角速度矢量ωt(k)＝[ωtx(k)，ωty(k)，ωtz(k)]T、加速度矢量at(k)＝[atx(k)，aty(k)，atz(k)]T和磁場矢量mt(k)＝[mtx(k)，mty(k)，mtz(k)]T；
3)構(gòu)建系統(tǒng)狀態(tài)方程
定義系統(tǒng)狀態(tài)向量為
X(k)＝[qT(k)，μT(k)，abT(k)]T(1)
其中q(k)＝[q0(k)，q1(k)，q2(k)，q3(k)]T為描述當(dāng)前姿態(tài)同初始姿態(tài)之間相對關(guān)系的旋轉(zhuǎn)四元數(shù)矢量，μ(k)＝[μx(k)，μy(k)，μz(k)]T為三軸陀螺儀的累積誤差矢量，ab(k)＝[abx(k)，aby(k)，abz(k)]T為裝置自身的運(yùn)動加速度矢量；
依據(jù)上述狀態(tài)向量的系統(tǒng)狀態(tài)方程為
式中Δt為采樣周期，
nGx為噪聲矢量，其協(xié)方差矩陣為
矩陣
I為相應(yīng)階次的單位矩陣，σω1、σω2、σω3和
為相關(guān)噪聲變量的標(biāo)準(zhǔn)方差常數(shù)；
4)構(gòu)建系統(tǒng)觀測方程
式中，aob(k)＝ao+ab(k)，對于任意三維矢量r＝[rx，ry，rz]T，
為其歸一化單位方向矢量，||r||為其幅值，矩陣
其中，
表示由相應(yīng)向量定義的反對稱矩陣；nGz為觀測噪聲矢量，其協(xié)方差矩陣為
σ|a|和
為相關(guān)噪聲變量的標(biāo)準(zhǔn)方差常數(shù)；
5)系統(tǒng)狀態(tài)Sigma點(diǎn)采樣根據(jù)k-1時(shí)刻的系統(tǒng)狀態(tài)X(k-1/k-1)和協(xié)方差矩陣P(k-1/k-1)進(jìn)行Sigma點(diǎn)采樣，得到21個(gè)點(diǎn)樣本為Xsi，i＝1，…，20；
6)UKF預(yù)測根據(jù)方程(2)，對21個(gè)Sigma點(diǎn)進(jìn)行狀態(tài)預(yù)測
Xspi＝F(Xsi，ωt(k)) i＝0，…，21 (4)
利用上述采樣預(yù)測值確定系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終預(yù)測值為
Mk-1＝q(k-1/k-1)qT(k-1/k-1)+Pq(k-1/k-1)(8)
其中Pq(k-1/k-1)為矩陣P(k-1/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣；wi為相應(yīng)點(diǎn)樣本的權(quán)值；
7)UKF更新對于Sigma點(diǎn)預(yù)測Xspi，令qi(k/k-1)為由向量Xspi前四個(gè)元素得到的歸一化四元數(shù)，根據(jù)觀測方程其觀測值計(jì)算為
Zi(k)＝G(Xspi(k/k-1)) i＝0，…，21(9)
而最終觀測值計(jì)算為
系統(tǒng)狀態(tài)向量和協(xié)方差矩陣的最終更新為
X(k/k)＝X(k/k-1)-KZ(k) (11)
P(k/k)＝P(k/k-1)-KPZZKT(12)
其中
其中Pq(k/k-1)為矩陣P(k/k-1)中相應(yīng)于四元數(shù)向量的協(xié)方差子陣；
8)將X(k/k)中的四元數(shù)向量元素進(jìn)行歸一化處理，并利用四元數(shù)表示同歐拉角表示之間的關(guān)系將其轉(zhuǎn)換為具有較為直觀意義的俯仰角、橫滾角和航向角。
2.根據(jù)權(quán)利要求1所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其特征在于所述系統(tǒng)狀態(tài)向量包含了裝置本身的運(yùn)動加速度，從而導(dǎo)致了系統(tǒng)的觀測方程非線性，因此采用UKF技術(shù)實(shí)現(xiàn)對于系統(tǒng)狀態(tài)的濾波估計(jì)。
3.根據(jù)權(quán)利要求1所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其特征在于對于加速度矢量和磁場矢量分別依據(jù)其單位方向矢量構(gòu)建觀測方程Za1(k)和Zm(k)；為了能夠有效地估計(jì)出裝置本身的運(yùn)動加速度，觀測方程中的Za2(k)對加速度矢量進(jìn)行了幅值限制，從而在單位方向矢量一定的情況下，能夠唯一估計(jì)出加速度矢量ab(k)。
4.根據(jù)權(quán)利要求1所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其特征在于系統(tǒng)狀態(tài)方程或系統(tǒng)觀測方程，其中的噪聲均與系統(tǒng)狀態(tài)向量相關(guān)，對于協(xié)方差矩陣的預(yù)測和更新均對此進(jìn)行了處理。
5.根據(jù)權(quán)利要求1所述的考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，其特征在于由于系統(tǒng)狀態(tài)包含了裝置本身的運(yùn)動加速度，同以往忽略該項(xiàng)因素相比，能夠獲取更為精確的姿態(tài)估計(jì)結(jié)果，尤其在轉(zhuǎn)置本身運(yùn)動幅度較大的情況下；同時(shí)，估計(jì)得到的裝置本身的運(yùn)動加速度，通過積分實(shí)現(xiàn)轉(zhuǎn)置的粗略位置確定。
全文摘要
一種考慮加速度補(bǔ)償和基于無跡卡爾曼濾波的慣性位姿跟蹤方法，用于集成三軸微陀螺儀、三軸微加速度計(jì)和三軸磁阻傳感器的慣性測量裝置，該方法用轉(zhuǎn)置檢測到的旋轉(zhuǎn)角速度矢量、加速度矢量和磁場傳感器矢量，借助濾波技術(shù)實(shí)現(xiàn)裝置載體的位姿跟蹤估計(jì)。該方法包括1)將加速度矢量視為裝置載體本身加速度矢量和重力加速度矢量的復(fù)合，并對其幅值和歸一化的方向矢量分別構(gòu)建觀測方程；2)利用位姿描述四元數(shù)、陀螺儀累積誤差矢量、裝置載體本身加速度矢量構(gòu)建系統(tǒng)狀態(tài)向量；3)因觀測方程非線性，用無跡卡爾曼濾波技術(shù)實(shí)現(xiàn)系統(tǒng)的濾波估計(jì)過程。同傳統(tǒng)忽略載體本身加速度的方法相比，本發(fā)明不但能夠給出更為準(zhǔn)確的估計(jì)結(jié)果，且拓寬了系統(tǒng)應(yīng)用范圍。
文檔編號G01C21/10GK101726295SQ200810224898
公開日2010年6月9日 申請日期2008年10月24日 優(yōu)先權(quán)日2008年10月24日
發(fā)明者杜清秀, 鄒偉, 原魁 申請人:中國科學(xué)院自動化研究所