鄒一凡, 李 佳, 王瑋冰

(1.中國科學院 微電子研究所, 北京 100029; 2.中國科學院大學, 北京 100049)

0 引 言

相對于其他行人室內(nèi)定位方法,基于慣性傳感器的方法具有無需事先布置、實現(xiàn)成本低的優(yōu)勢,正在受到越來越多的重視[1,2]。其中,基于慣性導航系統(tǒng)(inertial navigation system,INS)的方法相較于基于步長—方向模型的方法,具有運算量少、實時性好、可以重建完整運動軌跡的優(yōu)點[3]。同時,如何消除其定位的累積誤差,是該領域的研究重點。本文在INS框架基礎上,進行了新的定位系統(tǒng)方案設計。

1 INS原理

對于INS,其原理框圖如圖1所示。由9軸磁、角速度、重力(mageric,angular rate and gravity,MARG)傳感器所采集到的數(shù)據(jù),在誤差補償后用于對載體的姿態(tài)進行實時的估計,從而將加速度數(shù)據(jù)由傳感器坐標系(S系)轉(zhuǎn)換至地球坐標系(E系),經(jīng)2次積分得到空間速度與位移。之后,利用零速更新(zero velocity update,ZUPT)算法對定位誤差進行補償,從而得到最終的定位結果[4]。

圖1 INS框架

2 姿態(tài)估計算法

2.1 四元數(shù)基礎運算

使用符號?表示四元數(shù)乘法,公式如下

(1)

(2)

(3)

式中 右上標*為四元數(shù)共軛。當Q=[q0q1q2q3]T時,Q*=[q0-q1-q2-q3]T。

2.2 擴展互補濾波

2020年,Madgwick S O H為了解決其先前提出的基于梯度下降的姿態(tài)估計算法[5]存在的問題,設計了新的算法,命名為擴展互補濾波(extended complementary filtering,ECF)[6]。本文即基于這一成果,進行了進一步的改進與應用?，F(xiàn)將算法運算過程介紹如下：

(4)

(5)

(6)

式中Sak與Smk分別為加速度計與磁力計的測量值。括號內(nèi)的運算分別為將指向下的向量[0 0 0 1]T與指向東的向量[0 0 1 0]T投影至傳感器坐標系,且略去結果的第一個元素,以參與三維向量運算。

由此,即可實現(xiàn)對角速度的修正,通過調(diào)整參數(shù)以實現(xiàn)最佳性能。在實際應用場景中,加速度數(shù)據(jù)與磁力計數(shù)據(jù)的特性有所不同,因此將參數(shù)K分為2個參數(shù)Ka和Km,即將式(4)進一步重寫為

(7)

當檢測到較大的運動加速度或磁干擾時,即降低對應的參數(shù)值,從而改善姿態(tài)估計結果。

2.3 快速收斂算法

在算法開始工作后,需要一定的時間進行迭代以收斂至初始姿態(tài),因此行人需靜止一段時間才能開始運動,這與實際應用場景是不符的。同時,當進入ZUPT后,若姿態(tài)收斂較慢,可能將姿態(tài)誤差延續(xù)至下一步,影響定位結果。為解決上述問題,本文提出一種姿態(tài)快速收斂(fast convergence,FC)算法。

首先計算

(8)

可見,向量X為加速度的測量向量A與重力的推測向量G經(jīng)由叉乘得來,其方得到向量向與兩向量垂直,模長為兩向量夾角θ的正弦值。若能通過向量X實現(xiàn)這一夾角的旋轉(zhuǎn),即可直接收斂于真實姿態(tài)。首先將其長度轉(zhuǎn)化為θ,得到向量

(9)

(10)

對于向量Sak×Smk的處理同理,這里不再贅述。該算法能夠在2個采樣周期內(nèi)實現(xiàn)初始對準,能夠在進入ZUPT后迅速修正姿態(tài)誤差,且運算過程與ECF本身相契合。使用FC算法前后,初始對準時姿態(tài)估計所得歐拉角變化曲線對比如圖2所示。其中,圖2(a)中使用10倍于常值的K值；圖2(b)中使用FC算法。值得一提的是,單次FC算法僅占用2個采樣周期,其余時刻仍依照式(7)迭代。

圖2 初始對準過程對比

2.4 算法整體流程

上述算法整體的流程如圖3所示。在獲得載體姿態(tài)后,即可對加速度數(shù)據(jù)進行積分,并通過ZUPT進行誤差修正。這里不再進行贅述。

圖3 姿態(tài)估計算法框圖

3 實驗驗證與分析

3.1 導航模塊設計

針對實驗需求,面向?qū)嶋H應用場景,設計了一種慣性導航專用模塊。通過對MCU進行編程,以125 Hz的采樣頻率,通過SPI接口從ICM—20948傳感器中讀取九軸數(shù)據(jù),在運算更新姿態(tài)、位置信息后,通過BLE通信將位置解算結果傳輸至上位機。

實驗過程中,模塊被粘貼至實驗對象鞋尖上方,使用筆記本電腦實時接收并展示運動軌跡。模塊實物如圖4所示。

圖4 導航模塊實物

3.2 實驗內(nèi)容

為了驗證本文所提出系統(tǒng)方案的有效性,設計路徑進行了軌跡還原實驗,并與其他基于互補濾波(complementary filtering,CF)的方案進行結果對比,分別為基于梯度下降的算法(GDA)以及經(jīng)典CF[7]。各姿態(tài)估計算法均通過實驗取得最佳參數(shù)設置。

實驗對象由6樓出發(fā),下樓梯至4樓,經(jīng)過一段走廊后,繞正方形軌跡2周,再沿原路返回。最終得到的平面軌跡如圖5(a)所示。可見使用GDA或CF還原出的軌跡有相當大的誤差,其原因在于無法利用磁力計數(shù)據(jù)進行有效的姿態(tài)校正。去掉這2條軌跡后,得到Y-Z平面的對比如圖5(b)。

圖5 各算法還原軌跡對比

可見,使用ECF算法能夠較為精確地還原出組合路徑下的運動軌跡。添加FC算法后,能夠優(yōu)化其在Z軸上的偏移。各實驗條件下的返回原位置誤差(return position error,RPE)如表1所示。

表1 實驗精度分析

4 結 論

本文以MARG傳感器為基礎,針對室內(nèi)定位的低成本應用需求,基于足部捷聯(lián)慣性導航原理,設計了一種導航定位系統(tǒng)方案。針對姿態(tài)估計中的問題,設計了FC算法,應用于初始收斂以及零速時姿態(tài)估計的快速修正。通過實驗證明,使用本文所設計的ECF+FC方案定位結果優(yōu)于其他算法。系統(tǒng)平面定位誤差小于0.5 %,空間定位誤差小于1 %,能夠適應較為復雜的應用場景,為將SINS應用于室內(nèi)定位提供了有效的解決方案。

后續(xù)研究中,將考慮以下方面：1)與氣壓計融合以提高空間定位精度;2)利用先驗信息,如地板為平面、樓層高度為固定值等,在上位機對最終定位結果進行修正。