0 引言

    隨著導航技術的發(fā)展，衛(wèi)星導航技術已成為成熟的、全方位的現(xiàn)代導航系統(tǒng)，但隨著應用領域的擴大，衛(wèi)星導航系統(tǒng)的缺點也逐漸顯露出來，特別是在室內、山洞和隧道內，由于衛(wèi)星導航信號質量較差，進而無法獲取導航數(shù)據(jù)，已成為衛(wèi)星導航技術的致命缺點。目前，基于MEMS的室內行人航跡推算作為一種新興的室內個人導航定位方法受到了廣泛關注^[1]。而隨著慣導技術的發(fā)展，以ADI公司為代表的MEMS慣性測量單元打破傳統(tǒng)慣導器件的體積大、價格昂貴的格局，從而使慣導應用走向個人^[2-3]。

    個人導航系統(tǒng)主要用于跟蹤定位徒步行走人員的實時位置^[4-5]。目前，個人導航研究取得了一系列成果。文獻[6]中提到的將MEMS器件安裝在被測試人員的腰部，通過行走頻率和加速度計方差估計出步長，從而得到行走速度和距離，此方案無法適應不同的步行者和不同的行走方式而局限性很大。文獻[7]利用無線電信號識別路標點，與地圖進行匹配，從而大大提高了定位進度，但是這要借助外部設備才能進行定位。文獻[4]、[5]將MEMS慣性測量單元安裝在鞋上，利用磁力計對方位修正，在一定程度上提高了導航精度。文獻[8]利用GPS/DR組合導航進行行人導航，并提出DR參數(shù)估計方法提高了航跡推算參數(shù)的精度。此外，還有其他的三角定位法、GPS/INS組合、UWB/MEMS微機電系統(tǒng)傳感器整合的PDR算法等^[9-10]。

    另一方面，器件的性能和價格很大程度上影響著行人慣性導航的計算精度和應用范圍。為此，本文在MEMS慣性測量單元器件上開發(fā)出一套行人鞋式導航系統(tǒng)，用于驗證算法的有效性，為進一步提高導航精度奠定基礎。

1 系統(tǒng)框圖

    本試驗所采用的硬件以STM32F103芯片為控制核心，通過定時采樣MPU6050的加速度、角速度以及HMC5883的磁力信息，經(jīng)過姿態(tài)解算和導航解算，經(jīng)串口把導航信息輸出，再用MATLAB對串口信息進行處理，最終得到導航信息的圖形化顯示，系統(tǒng)框圖如圖1所示。

2 算法結構

    該系統(tǒng)算法分為三個子算法，算法總體結構設計如圖2所示。 

    采用地理坐標系作為導航系，以東、北、天 3個方向作為導航系的x、y、z軸正方向，MPU6050的x、y、z 3軸與HMC5883的3軸、載體x、y、z 3軸均呈平行關系。描述載體運動的直觀方法為歐拉角法，本文采用習慣定義，載體的俯仰角、橫滾角和偏航角分別用θ、γ和表示。下面詳細說明各算法的設計。

2.1 算法基礎

2.1.1 姿態(tài)更新算法

    姿態(tài)更新算法采用四元數(shù)法，該算法只需求解4個未知量的線性微分方程組^[11]，算法簡單，易于操作，較為實用，克服了方向余弦法未知量多、計算量大的缺點，同時避免了歐拉角算法易出現(xiàn)方程退化的問題。

    由以上關系可將實時計算的四元數(shù)法與直觀的歐拉角法聯(lián)系起來，經(jīng)擴展卡爾曼濾波，即可得到準確的運載體姿態(tài)。

2.1.2 9DOF全姿態(tài)算法

    9DOF全姿態(tài)算法融合了三軸磁力計、三軸加速度計和三軸陀螺儀的數(shù)據(jù)，以四元數(shù)為狀態(tài)量，加速度計和磁力計原始輸出為觀測量，通過擴展卡爾曼濾波算法，得到全姿態(tài)歐拉角。

    四元數(shù)微分方程（即卡爾曼濾波的狀態(tài)方程）的連續(xù)形式為：

2.1.3 初始化

    在東北天坐標系下完成磁場強度的初始化，公式為：

    試驗前，采用橢球擬合的方法對慣導器件進行初始標定。然后，根據(jù)以上模型構造出擴展卡爾曼濾波器，通過對慣導器件和磁力計噪聲的數(shù)學模型研究，用試驗的方法調整其噪聲參數(shù)，最終得到準確的姿態(tài)輸出。根據(jù)試驗，為兼顧其他算法和姿態(tài)實時更新要求，姿態(tài)更新頻率最終確定為66 Hz。

2.2 捷聯(lián)式慣導算法

    在捷聯(lián)慣導中，陀螺和加速度計的輸出經(jīng)計算機按一定的算法處理后，獲得離散時間點上的姿態(tài)、航向、速度和位置等導航信息。

2.2.1 速度算法

    捷聯(lián)慣導的基礎——比力方程為：

2.2.2 位置算法

    采用普通的速度積分法，公式如下：      

    經(jīng)試驗，僅采用以上的速度和位置算法會因器件的測量和噪聲帶來的誤差，導致速度誤差急劇增大，而使位置快速發(fā)散，導航失敗。針對這種情況，需設計卡爾曼濾波器，對速度和位置進行修正，即零速檢測和零速修正算法^[12-13]。

2.3 零速檢測算法

    首先對人的行走模態(tài)進行分析。由于所采用的器件精度較低，不能感受重力、地球的自轉角速度，也就不會產生牽連角速度，因而載體Z軸正方向指向導航系正方向靜止放置時，存在正的1 g的比力。將測試板裝配在測試人員的一只腳上，通過收集傳感器的輸出數(shù)據(jù)，繪制出圖3步行的加速度特征曲線和圖4步行的角速度特征曲線。

    由圖分析得，可將人的行走分為兩個過程，抬腳和放腳為運動過程，觸地穩(wěn)定后到第二次抬腳前為靜止過程。兩個過程的加速度和角速度都有明顯的不同，因此可通過設置適當?shù)乃惴▽⑦@兩個過程區(qū)分開來，本試驗采用以下3個判斷條件：

    (1)導航系的合加速度收斂

    根據(jù)對行走模態(tài)的加速度特征分析，在收腳期間，合加速度是減小的，通過對加速度連續(xù)多次采樣，判斷合加速度的收斂情況。本試驗采用連續(xù)7次采樣，滿足5次的后一次值減去前一次值為負即為收斂。

    (2)載體系的合加速度小于門限值f_bm

    由圖3可看出，靜止期間的合加速度值處于較低水平，因此可通過設置門限值來區(qū)分靜止和運動。

2.4 零速校正算法

    基于零速檢測，用卡爾曼濾波算法對速度誤差和位置誤差進行估計^[14]，實驗中還融入對方位的估計，從而實現(xiàn)對速度和位置在各方位上的有效補償。下面說明本算法的實現(xiàn)。狀態(tài)量為：

    由于所選的觀測量只有在判斷為靜止即腳的速度為零時才能被觀測，所以，在運動期間，濾波器只做時間更新，在靜止期間做完整更新。

3 試驗結果

    為驗證算法的有效性，在一塊室內矩形區(qū)域(如圖7所示)進行了步行試驗，各次試驗結果如圖5～圖7所示。

    其中，圖5是未加任何校正的導航結果實測圖，可看到未經(jīng)修正的純慣性導航的位置誤差發(fā)散很快，這主要是由水平姿態(tài)誤差和速度誤差的積累引起的。圖6是加速度和位置校正但未加入方位校正的導航實測圖，可看到方位誤差嚴重，解算的方位已偏離了真實路徑。圖7是加入速度、位置和方位校正的綜合算法實測圖，可看到導航結果得到明顯改善。

4 結論

    本文的個人鞋式導航算法在充分分析個人行走特點的基礎上，經(jīng)過零速檢測和零速校正對普通導航算法的補償，實現(xiàn)了慣性器件的室內步行導航，證明了此算法的有效性，為進一步提高精度和改進算法奠定了基礎。

參考文獻

[1] 賀鋒濤，趙勝利，周廣平，等.基于模糊邏輯的室內導航步長估計方法研究[J].電子技術應用，2016，42(11)：59-61.

[2] JIMENEZ A R，SECO F，PRIETO J C，et al.Indoor pedestrian navigation using an INS/EKF framework for yaw drift reduction and a foot-mounted IMU[C].The Workshop on Positioning Navigation & Communication.IEEE，2010：135-143.

[3] FISCHER C，GELLERSEN H.Location and navigation support for emergency responders：A survey[J].IEEE Pervasive Computing，2010，9(1)：38-47.

[4] 崔瀟，秦永元，周琪，等.鞋式個人導航系統(tǒng)算法和試驗研究[J].測控技術，2013，32(3)：138-142.

[5] 張金亮，秦永元，梅春波.基于MEMS慣性技術的鞋式個人導航系統(tǒng)[J].中國慣性技術學報，2011，19(3)：253-256.

[6] SHIN S H，CHAN G P，CHOI S.New map-matching algorithm using virtual track for pedestrian dead reckoning[J].ETRI Journal， 2010，32(6)：891-900.

[7] RENAUDIN V，YALAK O，TOME P，et al.Indoor navigation of emergency agents[J].European Journal of Navigation，2007，5(3)：36-45.

[8] 田增山，朝磊，邢培基，等.行人導航系統(tǒng)中航跡推算參數(shù)估計方法的研究[J].電子技術應用，2009，35(12)：84-87.

[9] 宋敏，申閆春.室內定位航位推測算法的研究與實現(xiàn)[J].計算機工程，2013，39(7)：293-297.

[10] PITTET S，RENAUDIN V，MERMINOD B，et al.UWB and MEMS based indoor navigation[J].Journal of Navigation，2008，61(3)：369-384.

[11] 樊炳輝，張凱麗，王傳江，等.基于四元數(shù)的前臂假肢手部自平衡的設計[J].電子技術應用，2016，42(5)：78-81.

[12] TIAN X，CHEN J，HAN Y，et al.A novel zero velocity interval detection algorithm for self-contained pedestrian navigation system with inertial sensors:[J].Sensors，2016，16(10)：1578.

[13] SKOG I，NILSSON J O，HANDEL P.Evaluation of zero-velocity detectors for foot-mounted inertial navigation systems[C].Indoor Positioning and Indoor Navigation(IPIN)，2010 International Conference on.IEEE，2010：1-6.

[14] 奔粵陽，孫楓，高偉，等.慣導系統(tǒng)的零速校正技術研究[J].系統(tǒng)仿真學報，2008，20(17)：4639-4642.

作者信息:

錢文高1，陳靜杰1，馬紅巖2

（1.中國民航大學 電子信息與自動化學院，天津300300；2.中國民航大學 基礎實驗中心，天津300300）