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)也逐漸顯露出來,，特別是在室內(nèi)、山洞和隧道內(nèi),，由于衛(wèi)星導(dǎo)航信號(hào)質(zhì)量較差,，進(jìn)而無法獲取導(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è)試人員的腰部,，通過行走頻率和加速度計(jì)方差估計(jì)出步長，從而得到行走速度和距離,，此方案無法適應(yīng)不同的步行者和不同的行走方式而局限性很大,。文獻(xiàn)[7]利用無線電信號(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è)量單元器件上開發(fā)出一套行人鞋式導(dǎo)航系統(tǒng),，用于驗(yàn)證算法的有效性,，為進(jìn)一步提高導(dǎo)航精度奠定基礎(chǔ)。

1 系統(tǒng)框圖

    本試驗(yàn)所采用的硬件以STM32F103芯片為控制核心,，通過定時(shí)采樣MPU6050的加速度,、角速度以及HMC5883的磁力信息，經(jīng)過姿態(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ì)說明各算法的設(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)方程退化的問題。

    由以上關(guān)系可將實(shí)時(shí)計(jì)算的四元數(shù)法與直觀的歐拉角法聯(lián)系起來,，經(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è)量,，通過擴(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ò)展卡爾曼濾波器,，通過對(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è)量和噪聲帶來的誤差,，導(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è)試人員的一只腳上，通過收集傳感器的輸出數(shù)據(jù),，繪制出圖3步行的加速度特征曲線和圖4步行的角速度特征曲線,。

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

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

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

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

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

2.4 零速校正算法

    基于零速檢測(cè),，用卡爾曼濾波算法對(duì)速度誤差和位置誤差進(jìn)行估計(jì)^[14]，實(shí)驗(yàn)中還融入對(duì)方位的估計(jì),，從而實(shí)現(xiàn)對(duì)速度和位置在各方位上的有效補(bǔ)償,。下面說明本算法的實(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)過零速檢測(cè)和零速校正對(duì)普通導(dǎo)航算法的補(bǔ)償,，實(shí)現(xiàn)了慣性器件的室內(nèi)步行導(dǎo)航,，證明了此算法的有效性，為進(jìn)一步提高精度和改進(jìn)算法奠定了基礎(chǔ),。

參考文獻(xiàn)

[1] 賀鋒濤,，趙勝利，周廣平,，等.基于模糊邏輯的室內(nèi)導(dǎo)航步長估計(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].中國慣性技術(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] 奔粵陽,，孫楓，高偉,，等.慣導(dǎo)系統(tǒng)的零速校正技術(shù)研究[J].系統(tǒng)仿真學(xué)報(bào),，2008，20(17)：4639-4642.

作者信息:

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

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