0 引言

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

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

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

1 系統(tǒng)框圖

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

2 算法結(jié)構(gòu)

    該系統(tǒng)算法分為三個(gè)子算法，算法總體結(jié)構(gòu)設(shè)計(jì)如圖2所示。 

    采用地理坐標(biāo)系作為導(dǎo)航系，以東、北、天 3個(gè)方向作為導(dǎo)航系的x、y、z軸正方向，MPU6050的x、y、z 3軸與HMC5883的3軸、載體x、y、z 3軸均呈平行關(guān)系。描述載體運(yùn)動(dòng)的直觀方法為歐拉角法，本文采用習(xí)慣定義，載體的俯仰角、橫滾角和偏航角分別用θ、γ和表示。下面詳細(xì)說(shuō)明各算法的設(shè)計(jì)。

2.1 算法基礎(chǔ)

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

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

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

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

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

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

2.1.3 初始化

    在東北天坐標(biāo)系下完成磁場(chǎng)強(qiáng)度的初始化，公式為：

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

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

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

2.2.1 速度算法

    捷聯(lián)慣導(dǎo)的基礎(chǔ)——比力方程為：

2.2.2 位置算法

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

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

2.3 零速檢測(cè)算法

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

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

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

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

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

    由圖3可看出，靜止期間的合加速度值處于較低水平，因此可通過(guò)設(shè)置門(mén)限值來(lái)區(qū)分靜止和運(yùn)動(dòng)。

2.4 零速校正算法

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

    由于所選的觀測(cè)量只有在判斷為靜止即腳的速度為零時(shí)才能被觀測(cè)，所以，在運(yùn)動(dòng)期間，濾波器只做時(shí)間更新，在靜止期間做完整更新。

3 試驗(yàn)結(jié)果

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

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

4 結(jié)論

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

參考文獻(xiàn)

[1] 賀鋒濤，趙勝利，周廣平，等.基于模糊邏輯的室內(nèi)導(dǎo)航步長(zhǎng)估計(jì)方法研究[J].電子技術(shù)應(yīng)用，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] 崔瀟，秦永元，周琪，等.鞋式個(gè)人導(dǎo)航系統(tǒng)算法和試驗(yàn)研究[J].測(cè)控技術(shù)，2013，32(3)：138-142.

[5] 張金亮，秦永元，梅春波.基于MEMS慣性技術(shù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J].中國(guó)慣性技術(shù)學(xué)報(bào)，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] 田增山，朝磊，邢培基，等.行人導(dǎo)航系統(tǒng)中航跡推算參數(shù)估計(jì)方法的研究[J].電子技術(shù)應(yīng)用，2009，35(12)：84-87.

[9] 宋敏，申閆春.室內(nèi)定位航位推測(cè)算法的研究與實(shí)現(xiàn)[J].計(jì)算機(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ù)的前臂假肢手部自平衡的設(shè)計(jì)[J].電子技術(shù)應(yīng)用，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] 奔粵陽(yáng)，孫楓，高偉，等.慣導(dǎo)系統(tǒng)的零速校正技術(shù)研究[J].系統(tǒng)仿真學(xué)報(bào)，2008，20(17)：4639-4642.

作者信息:

錢(qián)文高1，陳靜杰1，馬紅巖2

（1.中國(guó)民航大學(xué) 電子信息與自動(dòng)化學(xué)院，天津300300；2.中國(guó)民航大學(xué) 基礎(chǔ)實(shí)驗(yàn)中心，天津300300）