基于異構(gòu)多源傳感的行人長(zhǎng)航時(shí)變步長(zhǎng)誤差修正技術(shù)

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

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

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

劉宇等[4]針對(duì)陀螺零偏會(huì)影響其導(dǎo)航精度問題。提出通過離線校準(zhǔn)方式，設(shè)計(jì)了靜態(tài)陀螺零位補(bǔ)償?shù)乃惴ǎ岣吡嗽诙毯綍r(shí)下的定位精度。許鈺龍等[5]采用MEMS陀螺儀綁于腳面，判定零速時(shí)刻，提出航向自糾正算法，進(jìn)行動(dòng)態(tài)零位補(bǔ)償，定位精度可達(dá)到2%。張倫東等[6]利用MEMS陀螺儀以及MEMS加速度計(jì)組合的方式嵌入鞋底，分析了零速修正的行人導(dǎo)航算法，在足底零速狀態(tài)下進(jìn)行步長(zhǎng)以及航向的優(yōu)化。

由于IMU的零位漂移所產(chǎn)生的誤差是姿態(tài)測(cè)量系統(tǒng)的主要誤差，因此需要在動(dòng)態(tài)過程中對(duì)IMU進(jìn)行零位補(bǔ)償?shù)男?zhǔn)工作。以上學(xué)者的研究成果，在長(zhǎng)航時(shí)下會(huì)出現(xiàn)定位精度不高以及在判定零速時(shí)刻會(huì)出現(xiàn)錯(cuò)判漏判等狀況，基于行人足底姿態(tài)的特征，提出加入壓力傳感器，與IMU組合嵌入鞋跟，設(shè)計(jì)多約束條件的零位捕捉方法，滿足不同步態(tài)下的零位精準(zhǔn)捕捉，并在零速時(shí)刻下通過誤差修正算法補(bǔ)償姿態(tài)測(cè)量系統(tǒng)的誤差，優(yōu)化步長(zhǎng)及航向，達(dá)到提高定位系統(tǒng)精度的目的。

1 系統(tǒng)設(shè)計(jì)框架

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

圖1 系統(tǒng)設(shè)計(jì)框架圖

2 算法設(shè)計(jì)

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

2.1 行人步態(tài)分析

行人足底姿態(tài)特征如圖2所示。對(duì)行人正常行走時(shí)的足底姿態(tài)特征進(jìn)行分析可得，這是一個(gè)周期性的運(yùn)動(dòng)，這一周期性運(yùn)動(dòng)大致可分為三個(gè)階段：足跟著地，全足著地以及足跟離地。因足跟著地到全足著地的時(shí)間可忽略不計(jì)，可認(rèn)為足跟著地到足跟離地這一段時(shí)間為全足著地時(shí)間，也就是相對(duì)靜止階段(零速時(shí)刻)。

圖2 行人步態(tài)周期圖

2.2 多條件靜止檢測(cè)算法設(shè)計(jì)

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

2.2.1 陀螺儀與加速度檢測(cè)法

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

(1)

(2)

(3)

(4)

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

2.2.2 壓力計(jì)檢測(cè)法

上述提出的加速度計(jì)和陀螺儀檢測(cè)法，對(duì)于行人導(dǎo)航定位精度的提升并不理想，因此，設(shè)計(jì)加入壓力計(jì)進(jìn)行輔助判斷零速時(shí)刻。

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

對(duì)壓力計(jì)的數(shù)據(jù)處理方法，與加速度計(jì)和陀螺檢測(cè)法類似，為了準(zhǔn)確檢測(cè)行人步態(tài)軌跡，傳統(tǒng)的方法是設(shè)置一個(gè)固定的壓力閾值，當(dāng)壓力計(jì)敏感到的壓力值超過這個(gè)閾值，則表示足部已經(jīng)著地，若沒有超過，則表示足部離地。但此方法有一個(gè)弊端，壓力計(jì)對(duì)行人足部著地時(shí)敏感到壓力值會(huì)隨著不同的行人出現(xiàn)不同的壓力值，并且在不同的環(huán)境，壓力值也會(huì)出現(xiàn)不同程度的變化，閾值不好設(shè)置，所以此方法并不實(shí)用。設(shè)計(jì)一種以壓力計(jì)的歷史數(shù)據(jù)為基礎(chǔ)的可變閾值調(diào)節(jié)法，公式為

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個(gè)點(diǎn)所對(duì)應(yīng)的閾值；α為加權(quán)系數(shù)；β為偏置量；lG(k)為包含l個(gè)壓力數(shù)據(jù)且連續(xù)的數(shù)據(jù)集；F(k)是第k個(gè)點(diǎn)的預(yù)測(cè)壓力值。

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

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

(7)

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

(8)

式中：?(n)表示第n次步態(tài)周期的長(zhǎng)度；k(n)表示第n次步態(tài)周期的第k個(gè)點(diǎn)的值，而得到步態(tài)周期長(zhǎng)度之后，就能得到窗口的長(zhǎng)度W(n)；μ為窗口系數(shù)，決定著窗口的長(zhǎng)度。

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

(9)

可得三個(gè)判斷靜止時(shí)刻的條件fa、fω、fp。若采用與操作，則條件太過嚴(yán)格，容易將一些本來(lái)達(dá)標(biāo)的時(shí)刻漏判，若采用或操作，容易誤判。因此，本文通過概率學(xué)知識(shí)，設(shè)計(jì)三個(gè)條件只要滿足其二，即可滿足條件，以fp為基礎(chǔ)條件，在滿足fp條件的前提下，只要再滿足fa和fω任意一種，就可滿足判斷靜止時(shí)刻的條件。此操作既不會(huì)發(fā)生錯(cuò)判、漏判等問題。經(jīng)過后續(xù)實(shí)驗(yàn)驗(yàn)證，此方法是可行的。判斷條件中各個(gè)系數(shù)的值需在使用中根據(jù)實(shí)際情況進(jìn)行調(diào)整。

2.3 誤差修正算法設(shè)計(jì)

采用卡爾曼濾波算法對(duì)行人足底靜止時(shí)刻進(jìn)行零速修正。但是由于卡爾曼濾波器的量測(cè)更新只在靜止時(shí)刻，這樣就會(huì)限制濾波器的估計(jì)效果，主要體現(xiàn)在方位誤差角不能得到有效估計(jì)。這樣，隨著導(dǎo)航時(shí)間的延長(zhǎng)，方位誤差的影響就會(huì)逐漸取代器件誤差，成為MEMS個(gè)人導(dǎo)航系統(tǒng)的主要誤差來(lái)源，為解決這一問題，采用粒子濾波算法對(duì)航向角以及步長(zhǎng)進(jìn)行優(yōu)化。

2.3.1 零速修正

零位補(bǔ)償算法是基于卡爾曼濾波模型理論構(gòu)建的導(dǎo)航誤差方程。經(jīng)SINS簡(jiǎn)化后的誤差模型為

(10)

(11)

式(11)中：X(t)為t時(shí)刻的系統(tǒng)狀態(tài)；F(t)為t時(shí)刻對(duì)系統(tǒng)的控制量；W(t)為t時(shí)刻的噪聲；Zk為k時(shí)刻的測(cè)量值；H為系統(tǒng)測(cè)量的參數(shù)；Xk為k時(shí)刻的系統(tǒng)狀態(tài)。

卡爾曼濾波由靜止時(shí)刻檢測(cè)觸發(fā)，卡爾曼濾波由靜止時(shí)刻間隔在每個(gè)步驟的開始處觸發(fā)。卡爾曼濾波離散化的時(shí)間不固定。將每一步的持續(xù)時(shí)間作為卡爾曼濾波。當(dāng)檢測(cè)到每一步足底靜止時(shí)刻的開始時(shí)，進(jìn)行卡爾曼濾波。

2.3.2 步長(zhǎng)優(yōu)化

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

(12)

(13)

有效粒子Neff可定義為

(14)

(15)

(16)

3 實(shí)驗(yàn)平臺(tái)搭建

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

圖3 IMU-I300和IMS-C20A壓力計(jì)

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

圖4 行人定位系統(tǒng)實(shí)物

4 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證行人步態(tài)檢測(cè)算法和誤差修正算法的可行性以及最后考量行人定位導(dǎo)航的精度，需要對(duì)本文所設(shè)計(jì)進(jìn)行驗(yàn)證。

首先驗(yàn)證行人在運(yùn)動(dòng)時(shí)，各個(gè)傳感器的數(shù)據(jù)是否滿足所設(shè)計(jì)的步態(tài)檢測(cè)算法。行人先做一段直行運(yùn)動(dòng)，根據(jù)前面行人步態(tài)規(guī)律可知，傳感器數(shù)據(jù)會(huì)成一個(gè)周期性變化的趨勢(shì)。圖5為陀螺儀數(shù)據(jù)仿真圖，在足底著地時(shí)刻，陀螺的數(shù)值在零附近，即可在此處設(shè)置閾值進(jìn)行零速時(shí)刻判斷。圖6為角速度計(jì)數(shù)據(jù)仿真圖，在足底著地時(shí)刻，加速度計(jì)的數(shù)值在重力加速度附近，滿足本文所設(shè)計(jì)步態(tài)檢測(cè)算法。圖7為壓力計(jì)數(shù)據(jù)仿真圖，當(dāng)足底離地時(shí)，壓力傳感器的數(shù)值是處在一個(gè)峰值附近變化，當(dāng)足底著地時(shí)，對(duì)地會(huì)產(chǎn)生壓力值。即可利用上文的理論進(jìn)行行人步態(tài)的判斷。

圖5 陀螺數(shù)據(jù)仿真圖

圖6 加速度計(jì)數(shù)據(jù)仿真圖

圖7 壓力計(jì)數(shù)據(jù)仿真圖

為了驗(yàn)證本文所設(shè)計(jì)的零速檢測(cè)算法的可靠性以及準(zhǔn)確性，提前規(guī)劃出某行人需要走100步的路程，即理想狀態(tài)下有100次零速狀態(tài)。做10組實(shí)驗(yàn)，檢測(cè)在本文所設(shè)計(jì)的算法基礎(chǔ)上能檢測(cè)出零速狀態(tài)的次數(shù)，結(jié)果如表1所示。

表1 零速狀態(tài)檢測(cè)次數(shù)

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

圖8 零位輸出仿真圖

最后一步驗(yàn)證在零速檢測(cè)算法以及濾波算法的基礎(chǔ)上，行人定位導(dǎo)航精度是否得到有效的提升。規(guī)劃出一段環(huán)形路線，以傳統(tǒng)的足綁式導(dǎo)航與本文的多傳感器零速判斷的定位導(dǎo)航的方式繞此路線行走各兩圈，采集數(shù)據(jù)并做仿真，結(jié)果如圖9所示。觀察其導(dǎo)航精度。

圖9 行人導(dǎo)航對(duì)比圖

5 結(jié)論

針對(duì)MEMS慣性定位導(dǎo)航系統(tǒng)中陀螺儀零位會(huì)隨著時(shí)間產(chǎn)生偏移以及傳統(tǒng)足綁式導(dǎo)航定位系統(tǒng)對(duì)于零速時(shí)刻的判斷會(huì)出現(xiàn)錯(cuò)判和漏判等問題，根據(jù)行人足底姿態(tài)的周期性變化，本文提出一種以壓力傳感器輔助的結(jié)合慣性傳感器的多源異構(gòu)傳感檢測(cè)零速時(shí)刻的方法，在此基礎(chǔ)上，建立卡爾曼濾波算法以及粒子濾波算法。有效地對(duì)微慣性器件的零位進(jìn)行了補(bǔ)償，提高了行人定位導(dǎo)航的精度，精度提高一個(gè)數(shù)量級(jí)。