基于異構多源傳感的行人長航時變步長誤差修正技術

謝金池，王 偉，文丹丹，路永樂，王汶新，劉 宇*

(1.重慶郵電大學生物信息學院，重慶 400065；2.重慶郵電大學光電工程學院，重慶 400065)

載體的姿態信息和速度信息是確定物體在空間運動狀態的重要信息，人們通過它們可以更加便捷地確定物體的運動方向和運動距離。慣性測量單元(inertial measurement unit，IMU)是測量物體三軸姿態角(或角速率)以及加速度的裝置，被廣泛地應用于航天、軍事、航海、醫學可穿戴式等領域[1]。陀螺儀及加速度計是IMU的主要元件，其精度直接影響到慣性系統的精度。微機電系統(micro-electro-mechanical system,MEMS)慣性器件因其體積和成本的巨大優勢，已展現出良好的應用前景[2]。低成本IMU仍然受到高噪聲水平和短期穩定性的影響，經研究，影響其IMU精度的因素有慣性傳感器的確定性誤差和隨機性誤差，其中最重要的便是確定性誤差中的零位漂移誤差[3]。

劉宇等[4]針對陀螺零偏會影響其導航精度問題。提出通過離線校準方式，設計了靜態陀螺零位補償的算法，提高了在短航時下的定位精度。許鈺龍等[5]采用MEMS陀螺儀綁于腳面，判定零速時刻，提出航向自糾正算法，進行動態零位補償，定位精度可達到2%。張倫東等[6]利用MEMS陀螺儀以及MEMS加速度計組合的方式嵌入鞋底，分析了零速修正的行人導航算法，在足底零速狀態下進行步長以及航向的優化。

由于IMU的零位漂移所產生的誤差是姿態測量系統的主要誤差，因此需要在動態過程中對IMU進行零位補償的校準工作。以上學者的研究成果，在長航時下會出現定位精度不高以及在判定零速時刻會出現錯判漏判等狀況，基于行人足底姿態的特征，提出加入壓力傳感器，與IMU組合嵌入鞋跟，設計多約束條件的零位捕捉方法，滿足不同步態下的零位精準捕捉，并在零速時刻下通過誤差修正算法補償姿態測量系統的誤差，優化步長及航向，達到提高定位系統精度的目的。

1 系統設計框架

圖1為系統整體框架，首先采集陀螺、加速度計以及壓力傳感器的數據，通過陀螺以及加速度計的輸出值進行姿態解算，得到載體的實時速度和位置信息[7]。并且根據行人足底姿態的特征，通過陀螺、加速度計以及壓力傳感器的輸出值實時判斷載體是否相對靜止，當檢測為非靜止時，不對零位做任何操作，只更新時刻；當檢測為靜止時，理論上此時陀螺的輸出值為零，加速度計的輸出值為重力加速度，壓力傳感器的輸出值處在一個峰值附近，觸發零位捕捉算法，并對系統誤差進行修正，最后進行姿態解算，更新速度位置信息，實現系統高定位精度的目的。

圖1 系統設計框架圖

2 算法設計

在進行姿態解算時，如果不對零位進行有效的捕捉以及補償，最終更新得到的速度位置信息與真實信息會有較大偏差，系統誤差會隨著時間不斷累積，導致定位功能失效[8]。在行人行走過程中，會有足底與地面完全接觸的一段時間，稱為相對靜止階段[9]。利用行走過程中腳著地時刻的速度為零，采用本文設計的零位捕捉以及補償算法，并將補償后的零位反饋給慣性定位單元進行誤差校準，實現高精度定位功能。

2.1 行人步態分析

行人足底姿態特征如圖2所示。對行人正常行走時的足底姿態特征進行分析可得，這是一個周期性的運動，這一周期性運動大致可分為三個階段：足跟著地，全足著地以及足跟離地。因足跟著地到全足著地的時間可忽略不計，可認為足跟著地到足跟離地這一段時間為全足著地時間，也就是相對靜止階段(零速時刻)。

圖2 行人步態周期圖

2.2 多條件靜止檢測算法設計

當使用慣性導航算法時，速度和位置誤差會隨著時間累積，如果不對這些錯誤進行修改，則導致定位失敗[10]。對于正常的步行，有一個每一步接觸地面時靜止不動的時刻。這種靜止的力矩也稱為零速度間隔。準確檢測零速度間隔和對誤差的實時修改間隔可以有效地抑制誤差。在這項研究中，我們使用閾值方法結合了加速度、陀螺以及壓力計的模塊值。

2.2.1 陀螺儀與加速度檢測法

行人正常行走，當全足著地時，處于相對靜止時刻，陀螺的理論輸出值為零，加速度的理論輸出值為重力加速度。傳統的閾值方法僅假設加速度應是重力加速度，容易出現對靜止時刻判斷不精準等情況，因此，提出一種組合條件方法，可通過三軸陀螺輸出值的平方和以及局部加速度方差選擇不同的閾值，計算公式為

(1)

(2)

(3)

(4)

式中：ωk(i)為三軸角速度信息；ωk為對三軸陀螺儀數據平方和處理后的值；tωmax和tωmin為陀螺儀零速時刻閾值；fω為陀螺儀的靜止時刻判斷條件；a(i)為三軸加速度計加速度信息；ak為對三軸加速度計數據方差處理后的值；tamax和tamin為加速度計靜止時刻閾值；fa為加速度計的靜止時刻判斷條件。

2.2.2 壓力計檢測法

上述提出的加速度計和陀螺儀檢測法，對于行人導航定位精度的提升并不理想，因此，設計加入壓力計進行輔助判斷零速時刻。

壓力計大多體積薄、質量輕，所以能夠很好地嵌入在鞋跟，目前常用的壓力計主要分為三類：壓電式、壓阻式、電容式。常用的壓電式和和電容式壓力計的實時性并不好，響應會滯后，最終導致行人運動狀態下的足部壓力值會失真。而壓阻式傳感器的材料是以單晶硅為基礎，具有實時性好、靈敏度高、動態響應快等優點。因此本設計采用壓阻式壓力傳感器，能夠很好地敏感行人足底壓力變化情況[11]。

對壓力計的數據處理方法，與加速度計和陀螺檢測法類似，為了準確檢測行人步態軌跡，傳統的方法是設置一個固定的壓力閾值，當壓力計敏感到的壓力值超過這個閾值，則表示足部已經著地，若沒有超過，則表示足部離地。但此方法有一個弊端，壓力計對行人足部著地時敏感到壓力值會隨著不同的行人出現不同的壓力值，并且在不同的環境，壓力值也會出現不同程度的變化，閾值不好設置，所以此方法并不實用。設計一種以壓力計的歷史數據為基礎的可變閾值調節法，公式為

T(k)=min[lG(k)]+α{max[lG(k)]-

min[lG(k)]}+β

(5)

lG(k)={F(k-l+1),F(k-l),…,F(k)}

(6)

式中：T(k)為第k個點所對應的閾值；α為加權系數；β為偏置量；lG(k)為包含l個壓力數據且連續的數據集；F(k)是第k個點的預測壓力值。

l、α、β共同決定這種可變閾值的大小和變化率，即使在不同行人穿戴以及不同環境下的運動，波形會發生變化，但可變閾值也會發生改變適應不同的環境，通過可變閾值就能測得足底是否離地或是著地。但是實驗證明，如果只通過閾值判斷，行人在高速運動下的壓力值由于波動大的原因，經常性出現連續壓力值峰值穿過閾值，會導致零速錯判。通過觀察行人步態周期可得一個規律，在一個周期里，同一個步態只能出現一次，也就是零速時刻在一個周期里只會出現一次。因此，本文設計一個窗口，窗口的長度由一個步態周期的長度決定，主要作用是將窗口里的出現波動的引起錯判的壓力值舍棄掉，窗口的長度也是一個可變值，公式為

W(n)=μ?(n)， 0<μ<1

(7)

?(n)=k(n)-k(n-1)

(8)

式中：?(n)表示第n次步態周期的長度；k(n)表示第n次步態周期的第k個點的值，而得到步態周期長度之后，就能得到窗口的長度W(n)；μ為窗口系數，決定著窗口的長度。

可以得到足底著地時刻的判斷條件fp為

(9)

可得三個判斷靜止時刻的條件fa、fω、fp。若采用與操作，則條件太過嚴格，容易將一些本來達標的時刻漏判，若采用或操作，容易誤判。因此，本文通過概率學知識，設計三個條件只要滿足其二，即可滿足條件，以fp為基礎條件，在滿足fp條件的前提下，只要再滿足fa和fω任意一種，就可滿足判斷靜止時刻的條件。此操作既不會發生錯判、漏判等問題。經過后續實驗驗證，此方法是可行的。判斷條件中各個系數的值需在使用中根據實際情況進行調整。

2.3 誤差修正算法設計

采用卡爾曼濾波算法對行人足底靜止時刻進行零速修正。但是由于卡爾曼濾波器的量測更新只在靜止時刻，這樣就會限制濾波器的估計效果，主要體現在方位誤差角不能得到有效估計。這樣，隨著導航時間的延長，方位誤差的影響就會逐漸取代器件誤差，成為MEMS個人導航系統的主要誤差來源，為解決這一問題，采用粒子濾波算法對航向角以及步長進行優化。

2.3.1 零速修正

零位補償算法是基于卡爾曼濾波模型理論構建的導航誤差方程。經SINS簡化后的誤差模型為

(10)

(11)

式(11)中：X(t)為t時刻的系統狀態；F(t)為t時刻對系統的控制量；W(t)為t時刻的噪聲；Zk為k時刻的測量值；H為系統測量的參數；Xk為k時刻的系統狀態。

卡爾曼濾波由靜止時刻檢測觸發，卡爾曼濾波由靜止時刻間隔在每個步驟的開始處觸發。卡爾曼濾波離散化的時間不固定。將每一步的持續時間作為卡爾曼濾波。當檢測到每一步足底靜止時刻的開始時，進行卡爾曼濾波。

2.3.2 步長優化

粒子濾波算法是通過使用一組加權的隨機樣本來近似表示系統狀態p(Xk，Z1:k)的后驗概率分布[13]。新的狀態分布可以通過這些隨機樣本的貝葉斯遞歸，公式為

(12)

(13)

有效粒子Neff可定義為

(14)

(15)

(16)

3 實驗平臺搭建

整個系統可分為上位機和下位機兩部分，下位機主要包括小型IMU和壓力傳感器，慣性器件選擇SCR2100，它包含陀螺儀和加速度計，壓力傳感器選擇IMS-C20A，如圖3所示。

圖3 IMU-I300和IMS-C20A壓力計

將IMU和壓力傳感器嵌入在鞋跟。將硬件嵌好之后，上位機和下位機通過433 d線模塊進行通信，上位機實時接收傳感器解算后的數據，得到位置信息，并實時在3D界面顯示出來。圖4為行人定位系統實物圖。

圖4 行人定位系統實物

4 實驗驗證

為驗證行人步態檢測算法和誤差修正算法的可行性以及最后考量行人定位導航的精度，需要對本文所設計進行驗證。

首先驗證行人在運動時，各個傳感器的數據是否滿足所設計的步態檢測算法。行人先做一段直行運動，根據前面行人步態規律可知，傳感器數據會成一個周期性變化的趨勢。圖5為陀螺儀數據仿真圖，在足底著地時刻，陀螺的數值在零附近，即可在此處設置閾值進行零速時刻判斷。圖6為角速度計數據仿真圖，在足底著地時刻，加速度計的數值在重力加速度附近，滿足本文所設計步態檢測算法。圖7為壓力計數據仿真圖，當足底離地時，壓力傳感器的數值是處在一個峰值附近變化，當足底著地時，對地會產生壓力值。即可利用上文的理論進行行人步態的判斷。

圖5 陀螺數據仿真圖

圖6 加速度計數據仿真圖

圖7 壓力計數據仿真圖

為了驗證本文所設計的零速檢測算法的可靠性以及準確性，提前規劃出某行人需要走100步的路程，即理想狀態下有100次零速狀態。做10組實驗，檢測在本文所設計的算法基礎上能檢測出零速狀態的次數，結果如表1所示。

表1 零速狀態檢測次數

當行人步態檢測算法滿足要求之后，下一步需要檢測的是零位是否得到有效的補償。采集陀螺 1 h 未經處理的零位輸出以及經過本文所設計的零位補償算法之后零位的輸出。如圖8所示。紅色的數據即為經過算法補償之后數據，雖然隨著時間的增加，也有一定的漂移，但是對比原始零位的漂移，零位是得到有效的補償。

圖8 零位輸出仿真圖

最后一步驗證在零速檢測算法以及濾波算法的基礎上，行人定位導航精度是否得到有效的提升。規劃出一段環形路線，以傳統的足綁式導航與本文的多傳感器零速判斷的定位導航的方式繞此路線行走各兩圈，采集數據并做仿真，結果如圖9所示。觀察其導航精度。

圖9 行人導航對比圖

5 結論

針對MEMS慣性定位導航系統中陀螺儀零位會隨著時間產生偏移以及傳統足綁式導航定位系統對于零速時刻的判斷會出現錯判和漏判等問題，根據行人足底姿態的周期性變化，本文提出一種以壓力傳感器輔助的結合慣性傳感器的多源異構傳感檢測零速時刻的方法，在此基礎上，建立卡爾曼濾波算法以及粒子濾波算法。有效地對微慣性器件的零位進行了補償，提高了行人定位導航的精度，精度提高一個數量級。