零速修正輔助的微慣性行人導(dǎo)航定位方法

李默涵 古翠紅

摘要：基于慣性測量組件足部安裝方式的行人導(dǎo)航系統(tǒng)，以捷聯(lián)慣性導(dǎo)航為主提供導(dǎo)航信息，通過行人足部運(yùn)動(dòng)信息進(jìn)行零速檢測觸發(fā)基于卡爾曼濾波的零速修正，補(bǔ)償導(dǎo)航系統(tǒng)誤差，并結(jié)合磁傳感器提供行人初始姿態(tài)信息，通過四元數(shù)算法提供行人行進(jìn)過程中的姿態(tài)信息。上述方案可以解決在GPS信號(hào)受屏蔽或嚴(yán)重受損無法進(jìn)行行人定位的問題。

本文將論述上述方案中涉及的捷聯(lián)慣性導(dǎo)航解算、零速檢測、卡爾曼濾波等關(guān)鍵技術(shù)中涉及到的基礎(chǔ)理論與基本原理進(jìn)行論述，并對(duì)實(shí)驗(yàn)過程中使用的IMU精度和微慣性行人導(dǎo)航系統(tǒng)的定位精度進(jìn)行分析。

關(guān)鍵詞：捷聯(lián)慣性導(dǎo)航解算;零速檢測;卡爾曼濾波

一、微慣性行人導(dǎo)航算法研究

以牛頓力學(xué)定律作為慣性導(dǎo)航系統(tǒng)的基礎(chǔ)原理，以求出所需要的導(dǎo)航參數(shù)（速度信息、位置信息）為目的，首先載體相對(duì)于坐標(biāo)系（由平臺(tái)系統(tǒng)物理平臺(tái)或捷聯(lián)系統(tǒng)數(shù)字平臺(tái)所建立）的加速度信息由慣性器件（陀螺儀、加速度計(jì)）測量，再通過積分變換得到速度變量，結(jié)合已知初速度，得到運(yùn)載體相對(duì)于所建立坐標(biāo)系的速度信息，再次積分得到位移，結(jié)合已知初始位置，得到運(yùn)載體相對(duì)于所建立坐標(biāo)系的位置信息，也可用于室內(nèi)定位應(yīng)用。上海交通大學(xué)儀器科學(xué)與工程系研究了一種基于人體動(dòng)作識(shí)別和步幅估計(jì)的步行者航位推算方法，建立足部動(dòng)作與步幅之間的非線性模型估計(jì)人體行走中的步幅，通過航位推算實(shí)現(xiàn)導(dǎo)航定位;

慣性導(dǎo)航系統(tǒng)由三部分組成：

一、慣性測量裝置，包括陀螺儀、加速度計(jì)（簡稱加表）、磁傳感器。

二、人工的物理平臺(tái)和計(jì)算機(jī)模擬數(shù)字平臺(tái)為兩種慣導(dǎo)平臺(tái)，加速度計(jì)上的信息由慣導(dǎo)平臺(tái)的陀螺儀模擬和跟蹤導(dǎo)航坐標(biāo)系，傳輸?shù)阶鴺?biāo)系上，載體即時(shí)的姿態(tài)與位置信息即可計(jì)算或模擬出。

三、導(dǎo)航計(jì)算機(jī)，平臺(tái)跟蹤回路的指令角速度可由導(dǎo)航解算計(jì)算。

捷聯(lián)慣性導(dǎo)航解算主要包括為兩個(gè)部分：對(duì)于姿態(tài)矩陣的解算和導(dǎo)航解算，也就是對(duì)數(shù)字平臺(tái)的解算和對(duì)位置速度的解算。以東北天指向的地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系。

系統(tǒng)啟動(dòng)后在靜態(tài)條件下，利用加速度信息，通過水平自對(duì)準(zhǔn)得到足部慣性傳感組件的初始橫滾角與俯仰角，并通過磁強(qiáng)計(jì)的信號(hào)輸出至導(dǎo)航計(jì)算機(jī)，結(jié)合初始橫滾角與俯仰角得到足部慣性/地磁傳感組件的初始航向角，進(jìn)而獲得初始姿態(tài)角;基于足部慣性/地磁傳感組件中的三軸陀螺儀數(shù)據(jù)，采用四元數(shù)法進(jìn)行姿態(tài)解算;把基于足部的慣性傳感器組件中的三軸加速度計(jì)原始輸出通過姿態(tài)轉(zhuǎn)移矩陣轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，解算出人體足部在地理系中的速度，進(jìn)一步求得人體的位置信息。

二、零速檢測算法

純捷聯(lián)慣導(dǎo)解算對(duì)位置，速度的估計(jì)會(huì)隨著時(shí)間的增長而出現(xiàn)發(fā)散，因此需要引入其他輔助方法來抑制誤差的累積。本文中的ZUPT輔助工具通過定期清除誤差，可以有效地提高導(dǎo)航系統(tǒng)的精度。

MEMS慣性傳感器安裝在足部，可以靈敏地感知足部的加速度和角速度的變化，并真實(shí)反映足部的運(yùn)動(dòng)過程。

行人在運(yùn)動(dòng)過程中，足部的運(yùn)動(dòng)大致可分為兩個(gè)階段：靜態(tài)和動(dòng)態(tài)。動(dòng)態(tài)階段可分為三部分：腳尖離地，空中擺動(dòng)，腳跟著地。隨著腳尖離開地面，腳跟會(huì)有一個(gè)明顯的旋轉(zhuǎn)過程;抬起腳后，腳在空中擺動(dòng)，然后身體向前移動(dòng);足部進(jìn)入到腳跟著地階段后，加速度達(dá)到最大值;然后前腳著地，加速度值會(huì)有較小的振幅脈沖沖擊;當(dāng)接近零速相位時(shí)，角速度接近零，合成加速度的總和接近重力加速度。

三、卡爾曼濾波算法

Kalman濾波算法采用遞推形式，給出了一套在計(jì)算機(jī)上容易實(shí)時(shí)實(shí)現(xiàn)的最優(yōu)遞推濾波算法，適合處理多變量系統(tǒng)、時(shí)變系統(tǒng)和非平穩(wěn)隨機(jī)過程，克服了經(jīng)典維納濾波理論的缺點(diǎn)和局限性，避免了維納濾波器在頻域內(nèi)設(shè)計(jì)遇到的限制和障礙。卡爾曼濾波算法用于剔除機(jī)器人航向角信號(hào)中的高斯白噪聲，建立了基于LSTM深度學(xué)習(xí)算法的角速度預(yù)測模型，從而實(shí)現(xiàn)基于航位推算的相對(duì)定位。

采用改良后的卡爾曼濾波對(duì)慣性導(dǎo)航系統(tǒng)的速度誤差進(jìn)行實(shí)時(shí)校正，從而抑制定位誤差隨時(shí)間的發(fā)散，在室內(nèi)外環(huán)境中的實(shí)驗(yàn)驗(yàn)證表明，該方法的定位精度在行程的2%以內(nèi)。

卡爾曼濾波在遞推過程中存在兩個(gè)回路，它是依據(jù)上一時(shí)刻的濾波估算值和當(dāng)前時(shí)刻的量測值實(shí)現(xiàn)當(dāng)前時(shí)刻的最優(yōu)估。

為了對(duì)行人足部在零速時(shí)刻的誤差進(jìn)行估計(jì)，本文基于捷聯(lián)式導(dǎo)航誤差狀態(tài)方程建立卡爾曼濾波器。卡爾曼濾波器由時(shí)刻的15維誤差狀態(tài)量構(gòu)成。是數(shù)學(xué)平臺(tái)誤差角;是位置誤差;是東北天三向的速度誤差;是加表三軸向的一階馬爾科夫過程;是陀螺三軸向的隨機(jī)常值。以慣導(dǎo)誤差方程作為狀態(tài)方程，系統(tǒng)零速時(shí)刻解算的速度與零速差值作為速度誤差觀測量。

四、總結(jié)

本文首先研究了純捷聯(lián)慣性導(dǎo)航系統(tǒng)的工作原理及其工作流程，并針對(duì)捷聯(lián)慣性導(dǎo)航系統(tǒng)的不足，研究了基于零速修正的改進(jìn)方法，使得微慣性行人導(dǎo)航系統(tǒng)性能得到較大的提高。然而，行人在劇烈運(yùn)動(dòng)狀態(tài)下，足部慣性導(dǎo)航系統(tǒng)很可能由于故障或超量程無法進(jìn)行行人定位，而且足部慣性導(dǎo)航系統(tǒng)的航向角誤差無法通過零速修正的方式被修正，通過磁傳感器獲得足部航向角又容易受到地磁干擾的影響。

指導(dǎo)老師（通訊作者）：古翠紅 山東協(xié)和學(xué)院

山東協(xié)和學(xué)院 山東濟(jì)南 250200