文獻標識碼： A
DOI：10.16157/j.issn.0258-7998.181128
中文引用格式： 錢文高，陳靜杰，馬紅巖. MEMS個人導航系統設計與試驗研究[J].電子技術應用，2018，44(10)：93-96，101.
英文引用格式： Qian Wengao，Chen Jingjie，Ma Hongyan. Design and experimental study of MEMS personal navigation system[J]. Application of Electronic Technique，2018，44(10)：93-96，101.
0 引言
隨著導航技術的發展，衛星導航技術已成為成熟的、全方位的現代導航系統，但隨著應用領域的擴大，衛星導航系統的缺點也逐漸顯露出來，特別是在室內、山洞和隧道內，由于衛星導航信號質量較差，進而無法獲取導航數據，已成為衛星導航技術的致命缺點。目前，基于MEMS的室內行人航跡推算作為一種新興的室內個人導航定位方法受到了廣泛關注[1]。而隨著慣導技術的發展，以ADI公司為代表的MEMS慣性測量單元打破傳統慣導器件的體積大、價格昂貴的格局，從而使慣導應用走向個人[2-3]。
個人導航系統主要用于跟蹤定位徒步行走人員的實時位置[4-5]。目前，個人導航研究取得了一系列成果。文獻[6]中提到的將MEMS器件安裝在被測試人員的腰部，通過行走頻率和加速度計方差估計出步長，從而得到行走速度和距離，此方案無法適應不同的步行者和不同的行走方式而局限性很大。文獻[7]利用無線電信號識別路標點，與地圖進行匹配，從而大大提高了定位進度，但是這要借助外部設備才能進行定位。文獻[4]、[5]將MEMS慣性測量單元安裝在鞋上，利用磁力計對方位修正，在一定程度上提高了導航精度。文獻[8]利用GPS/DR組合導航進行行人導航，并提出DR參數估計方法提高了航跡推算參數的精度。此外，還有其他的三角定位法、GPS/INS組合、UWB/MEMS微機電系統傳感器整合的PDR算法等[9-10]。
另一方面，器件的性能和價格很大程度上影響著行人慣性導航的計算精度和應用范圍。為此，本文在MEMS慣性測量單元器件上開發出一套行人鞋式導航系統，用于驗證算法的有效性，為進一步提高導航精度奠定基礎。
1 系統框圖
本試驗所采用的硬件以STM32F103芯片為控制核心，通過定時采樣MPU6050的加速度、角速度以及HMC5883的磁力信息，經過姿態解算和導航解算，經串口把導航信息輸出，再用MATLAB對串口信息進行處理，最終得到導航信息的圖形化顯示，系統框圖如圖1所示。
2 算法結構
該系統算法分為三個子算法，算法總體結構設計如圖2所示。
采用地理坐標系作為導航系，以東、北、天 3個方向作為導航系的x、y、z軸正方向，MPU6050的x、y、z 3軸與HMC5883的3軸、載體x、y、z 3軸均呈平行關系。描述載體運動的直觀方法為歐拉角法，本文采用習慣定義，載體的俯仰角、橫滾角和偏航角分別用θ、γ和
表示。下面詳細說明各算法的設計。
2.1 算法基礎
2.1.1 姿態更新算法
姿態更新算法采用四元數法，該算法只需求解4個未知量的線性微分方程組[11]，算法簡單，易于操作，較為實用，克服了方向余弦法未知量多、計算量大的缺點，同時避免了歐拉角算法易出現方程退化的問題。
由以上關系可將實時計算的四元數法與直觀的歐拉角法聯系起來，經擴展卡爾曼濾波，即可得到準確的運載體姿態。
2.1.2 9DOF全姿態算法
9DOF全姿態算法融合了三軸磁力計、三軸加速度計和三軸陀螺儀的數據，以四元數為狀態量，加速度計和磁力計原始輸出為觀測量，通過擴展卡爾曼濾波算法，得到全姿態歐拉角。
四元數微分方程（即卡爾曼濾波的狀態方程）的連續形式為：
2.1.3 初始化
在東北天坐標系下完成磁場強度的初始化，公式為：
試驗前，采用橢球擬合的方法對慣導器件進行初始標定。然后，根據以上模型構造出擴展卡爾曼濾波器，通過對慣導器件和磁力計噪聲的數學模型研究，用試驗的方法調整其噪聲參數，最終得到準確的姿態輸出。根據試驗，為兼顧其他算法和姿態實時更新要求，姿態更新頻率最終確定為66 Hz。
2.2 捷聯式慣導算法
在捷聯慣導中，陀螺和加速度計的輸出經計算機按一定的算法處理后，獲得離散時間點上的姿態、航向、速度和位置等導航信息。
2.2.1 速度算法
捷聯慣導的基礎——比力方程為：
2.2.2 位置算法
采用普通的速度積分法，公式如下：
經試驗，僅采用以上的速度和位置算法會因器件的測量和噪聲帶來的誤差，導致速度誤差急劇增大，而使位置快速發散，導航失敗。針對這種情況，需設計卡爾曼濾波器，對速度和位置進行修正，即零速檢測和零速修正算法[12-13]。
2.3 零速檢測算法
首先對人的行走模態進行分析。由于所采用的器件精度較低，不能感受重力、地球的自轉角速度，也就不會產生牽連角速度，因而載體Z軸正方向指向導航系正方向靜止放置時，存在正的1 g的比力。將測試板裝配在測試人員的一只腳上，通過收集傳感器的輸出數據，繪制出圖3步行的加速度特征曲線和圖4步行的角速度特征曲線。
由圖分析得，可將人的行走分為兩個過程，抬腳和放腳為運動過程，觸地穩定后到第二次抬腳前為靜止過程。兩個過程的加速度和角速度都有明顯的不同，因此可通過設置適當的算法將這兩個過程區分開來，本試驗采用以下3個判斷條件：
(1)導航系的合加速度收斂
根據對行走模態的加速度特征分析，在收腳期間，合加速度是減小的，通過對加速度連續多次采樣，判斷合加速度的收斂情況。本試驗采用連續7次采樣，滿足5次的后一次值減去前一次值為負即為收斂。
(2)載體系的合加速度小于門限值fbm
由圖3可看出，靜止期間的合加速度值處于較低水平，因此可通過設置門限值來區分靜止和運動。
2.4 零速校正算法
基于零速檢測，用卡爾曼濾波算法對速度誤差和位置誤差進行估計[14]，實驗中還融入對方位的估計，從而實現對速度和位置在各方位上的有效補償。下面說明本算法的實現。狀態量為：
由于所選的觀測量只有在判斷為靜止即腳的速度為零時才能被觀測，所以，在運動期間，濾波器只做時間更新，在靜止期間做完整更新。
3 試驗結果
為驗證算法的有效性，在一塊室內矩形區域(如圖7所示)進行了步行試驗，各次試驗結果如圖5～圖7所示。
其中，圖5是未加任何校正的導航結果實測圖，可看到未經修正的純慣性導航的位置誤差發散很快，這主要是由水平姿態誤差和速度誤差的積累引起的。圖6是加速度和位置校正但未加入方位校正的導航實測圖，可看到方位誤差嚴重，解算的方位已偏離了真實路徑。圖7是加入速度、位置和方位校正的綜合算法實測圖，可看到導航結果得到明顯改善。
4 結論
本文的個人鞋式導航算法在充分分析個人行走特點的基礎上，經過零速檢測和零速校正對普通導航算法的補償，實現了慣性器件的室內步行導航，證明了此算法的有效性，為進一步提高精度和改進算法奠定了基礎。
參考文獻
[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] 崔瀟，秦永元，周琪，等.鞋式個人導航系統算法和試驗研究[J].測控技術，2013，32(3)：138-142.
[5] 張金亮，秦永元，梅春波.基于MEMS慣性技術的鞋式個人導航系統[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] 田增山，朝磊，邢培基，等.行人導航系統中航跡推算參數估計方法的研究[J].電子技術應用，2009，35(12)：84-87.
[9] 宋敏，申閆春.室內定位航位推測算法的研究與實現[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] 樊炳輝，張凱麗，王傳江，等.基于四元數的前臂假肢手部自平衡的設計[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] 奔粵陽，孫楓，高偉，等.慣導系統的零速校正技術研究[J].系統仿真學報，2008，20(17)：4639-4642.
作者信息:
錢文高1，陳靜杰1，馬紅巖2
（1.中國民航大學 電子信息與自動化學院，天津300300；2.中國民航大學 基礎實驗中心，天津300300）