一種適用于跑步狀態(tài)的慣性/零速/GPS室內(nèi)外無縫組合導(dǎo)航定位方法

徐麗敏，熊 智，王鉦淳，張 苗

（南京航空航天大學(xué)自動化學(xué)院，南京211106）

0 引言

隨著社會的進(jìn)步，國內(nèi)外掀起了研究行人導(dǎo)航定位技術(shù)的熱潮。目前，較為成熟的行人導(dǎo)航定位服務(wù)系統(tǒng)主要依賴于GPS［1］。針對大型商場、地下車庫、消防救援等應(yīng)用場景，GPS往往無法正常工作，這將給人類的日常生活帶來不便，甚至導(dǎo)致巨大的財(cái)產(chǎn)損失和人員傷亡。

針對GPS信號易受干擾的特性，基于微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）的自主式慣性定位導(dǎo)航系統(tǒng)應(yīng)運(yùn)而生并迅速發(fā)展［2］。其中，第1種方法為航位推算算法（Pedestrian Dead Reckoning，PDR），即利用慣性傳感器數(shù)據(jù)信息實(shí)時檢測出行人行走的步數(shù)，通過預(yù)測的每步步長及利用磁傳感器與陀螺儀估計(jì)的航向角共同決定行人的位置。第2種方法則是基于捷聯(lián)解算的行人導(dǎo)航算法，即將慣性傳感器固定在行人的足部、腿部、腰部等位置，通過處理輸出的加速度與角速度值，預(yù)估行人的姿態(tài)信息和位置。其中，F(xiàn)ox?lin［3］最早提出了在行人足部位置放置慣性傳感設(shè)備的行人導(dǎo)航定位系統(tǒng)。該系統(tǒng)采用零速更新算法進(jìn)行零速修正，能夠抑制慣性系統(tǒng)的誤差，但不能滿足高精度定位的要求，且成本高昂，不利于推廣應(yīng)用。哈爾濱工程大學(xué)團(tuán)隊(duì)［4］提出了一種僅僅適用于緩慢行走場景的、基于足部的IMU/GPS組合導(dǎo)航系統(tǒng)，其驗(yàn)證試驗(yàn)在開闊的操場上進(jìn)行，雖定位精度較好，但適用局限性較大。同時，近年來，國內(nèi)外專家學(xué)者主要針對行人正常行走步態(tài)下的行人導(dǎo)航定位方法［5?6］及零速檢測方法［7?8］進(jìn)行了相關(guān)研究，針對跑步狀態(tài)下的行人導(dǎo)航定位及零速檢測方法的研究較少。而在救援等應(yīng)用場景下，人體多為跑步步態(tài)。另外，在室內(nèi)外過渡點(diǎn)進(jìn)行定位的精度低，不能平滑自動切換。因此，針對行人跑步室內(nèi)外無縫組合的導(dǎo)航定位技術(shù)亟待研究。

針對以上不足，本文分析了行人在跑步狀態(tài)下的運(yùn)動特性，提出了一種適用于行人跑步狀態(tài)的綜合零速檢測方法，研究了基于Kalman濾波的行人跑步零速修正算法（Zero Velocity Update，ZUPT），并提出了1種基于BP神經(jīng)網(wǎng)絡(luò)的GPS可用信號篩選方法。最后，基于上述分析研究提出了1種適用于跑步狀態(tài)的慣性/零速/GPS室內(nèi)外無縫組合導(dǎo)航定位方法，實(shí)現(xiàn)了行人跑步狀態(tài)下室內(nèi)外無縫組合導(dǎo)航定位。

1 行人跑步零速檢測方法及GPS篩選方法

本文采用捷聯(lián)慣性導(dǎo)航解算算法，將慣性測量單元（Inertial Measuring Unit，IMU）固定在行人腳面上，省去了機(jī)電式的慣性平臺，其體積與質(zhì)量較平臺慣導(dǎo)大大減小，具有較高的靈活性與可靠性［9］。捷聯(lián)慣性導(dǎo)航解算算法包括姿態(tài)解算、位置解算和速度解算，如（1）式所示。

1.1 行人跑步狀態(tài)的零速檢測方法

在正常跑步狀態(tài)下，行人的足部與地面存在一個相對靜止的瞬間，即零速區(qū)間。將慣性測量元件固定在行人腳面上，行人在跑步運(yùn)動狀態(tài)下的實(shí)測慣性傳感器數(shù)據(jù)如圖1和圖2所示。

圖1 陀螺儀三軸數(shù)據(jù)Fig.1 Data of tri-axial gyroscope

圖2 加速度計(jì)三軸數(shù)據(jù)Fig.2 Data of tri-axial acceleration

由圖1和圖2可知，三軸加速度與三軸角速度數(shù)據(jù)具有周期性，且跑步較步行運(yùn)動狀態(tài)更為劇烈，傳感器的晃動將導(dǎo)致零速區(qū)間短且不穩(wěn)定。針對跑步狀態(tài)，本文零速檢測的條件如下：

（1）加速度模值與重力加速度的差值及其標(biāo)準(zhǔn)差

式（2）、式（3）中，fkx、fky、fkz為第 k時刻加速度計(jì)采集得到的比力信息，g為重力加速度，ωkx、ωky、ωkz為第k時刻陀螺儀采集得到的角速度信息，n為方差計(jì)算區(qū)間大小，則有

式（5）～式（8）中，λ1、λ2、λ3與λ4分別為4 種零速檢測方法的標(biāo)志位，εa1、εa2與 εw1、εw2分別為ak、σ1k、gk、σ2k所對應(yīng)的判別閾值。其中，閾值εa1、εa2與 εw1、εw2是根據(jù)多組試驗(yàn)中的 ak、σ1k、gk、σ2k曲線與所對應(yīng)的零速區(qū)間的對比分析而得到。

圖3為4種零速檢測方法的檢測結(jié)果對比曲線，圖 3（a）、圖 3（b）與圖 3（d）均存在誤判現(xiàn)象。本文同時針對不同路徑分別進(jìn)行了多次試驗(yàn)，其對比結(jié)果如表1所示。

圖3 4種零速檢測方法的結(jié)果圖Fig.3 Diagram of four zero velocity detection methods

由表1可知，方法3的檢測準(zhǔn)確率最高，同時受變量的約束較少，穩(wěn)定性較好，故采用基于加速度模值與重力加速度的差值ak、該差值的標(biāo)準(zhǔn)差σ1k、角速度模值gk的組合方法進(jìn)行零速檢測。

表1 4種零速檢測方法的檢測步數(shù)結(jié)果Table 1 Statistical results of four zero velocity detection methods

1.2 基于BP神經(jīng)網(wǎng)絡(luò)的GPS可用信息篩選方法

GPS在開闊的室外環(huán)境下精度較高，但信號容易受到高樓、高架等環(huán)境的干擾。為提高組合導(dǎo)航定位的精度，需對GPS信息進(jìn)行篩選。傳統(tǒng)的GPS可用信息篩選方法主要以特征量（如PDOP值）作為依據(jù)，較為片面化。針對傳統(tǒng)篩選方法的局限性，本文提出了一種基于BP神經(jīng)網(wǎng)絡(luò)的GPS可用信息篩選方法。

本文構(gòu)建了 “4輸入1輸出”與 “4個隱藏節(jié)點(diǎn)” 的神經(jīng)網(wǎng)絡(luò)，4個輸入為Δp、Δv、Δh和pd，分別代表GPS的位置變化量、速度變化量、航向角變化量及PDOP值。1個輸出為flag，代表GPS的標(biāo)志位（1代表有效，0代表無效，即誤差較大）。BP神經(jīng)網(wǎng)絡(luò)的架構(gòu)如圖4所示。

圖4 BP神經(jīng)網(wǎng)絡(luò)的架構(gòu)Fig.4 Structure of BP neural network

利用評估準(zhǔn)確的GPS數(shù)據(jù)對該BP神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練，BP神經(jīng)網(wǎng)絡(luò)預(yù)測誤差曲線如圖5所示。由于網(wǎng)絡(luò)輸出為標(biāo)志位flag（非0即1），將實(shí)時輸出flag與真實(shí)flag做差并取其絕對值，則形成的誤差非0即1。輸入實(shí)時GPS的位置變化量、速度變化量、航向角變化量及PDOP值，利用訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)輸出GPS標(biāo)志位，判斷實(shí)測GPS信息的有效性。將篩選后的有效GPS個數(shù)與實(shí)際有效GPS個數(shù)進(jìn)行比較，得到篩選結(jié)果成功的比例。具體5次試驗(yàn)的訓(xùn)練結(jié)果如圖6所示。

圖5 BP神經(jīng)網(wǎng)絡(luò)預(yù)測誤差曲線Fig.5 Error curve of BP neural network

圖6 篩選結(jié)果成功比例折線圖Fig.6 Successful ratio of screening results

2 基于慣性/零速/GPS的適用于行人跑步狀態(tài)的室內(nèi)外無縫組合導(dǎo)航定位方法的實(shí)現(xiàn)

慣性/GPS組合導(dǎo)航利用具有絕對位置信息的GPS不斷地修正慣導(dǎo)誤差，以抑制慣導(dǎo)誤差的增長，同時利用慣導(dǎo)短時、高精度的特性彌補(bǔ)GPS在短時間內(nèi)的失鎖和跳轉(zhuǎn)問題，使組合后的導(dǎo)航精度得到顯著提高。

本文構(gòu)建了18維的Kalman濾波器狀態(tài)量，具體如下

式（10）中，F(xiàn)為系統(tǒng)矩陣，G為系統(tǒng)噪聲矩陣，W為系統(tǒng)噪聲。

采用可變量測模型，主要可見以下3種情況：

1）當(dāng)僅檢測出零速區(qū)間時，鑒于零速區(qū)間的位置不變，速度為0，6維量測信息為Zzupt，量測方程如式（11）所示，即慣導(dǎo)解算的位置信息與上一時刻慣導(dǎo)解算的位置信息的差值，以及慣導(dǎo)解算的速度信息與零速的差值。

2）當(dāng)僅GPS的信息為有效時，6維量測信息為ZGPS/INS，量測方程如式（12）所示，即慣導(dǎo)系統(tǒng)與GPS接收機(jī)得到的位置信息的差值，以及兩者所得出的速度信息的差值。

3）當(dāng)零速區(qū)間與有效GPS信息同時存在時，采用 12維量測信息 ［ZzuptZGPS/INS］，量測方程如下

式（13）中，H為量測矩陣，V為量測噪聲矩陣，L為緯度，RM和RN分別為子午圈和卯酉圈半徑，LINS、λINS、hINS為慣導(dǎo)解算的位置信息，LZUPT、λZUPT、hZUPT為上一時刻慣導(dǎo)解算的位置信息，νeINS、νnINS、νuINS為慣導(dǎo)解算的速度信息，νeZUPT、νnZUPT、νuZUPT為零速，λGPS、LGPS、hGPS為GPS接收機(jī)所給的位置信息，νeGPS、νnGPS、νuGPS為GPS接收機(jī)所給的速度信息。

結(jié)合上述模型，組合算法的具體流程如圖7所示。

圖7 慣性/零速/GPS組合導(dǎo)航定位算法Fig.7 Integrated navigation positioning algorithm of INS/ZUPT/GPS

3 仿真驗(yàn)證

試驗(yàn)采用AHRS低成本慣性測量單元，陀螺儀與加速度計(jì)的采集頻率為200Hz。經(jīng)靜態(tài)試驗(yàn)分析得到兩者的均值與均方差，其性能參數(shù)如表2所示。

表2 性能參數(shù)Table 2 Performance parameters

分別在室內(nèi)與室外場景下進(jìn)行測試，其中2組規(guī)劃路徑如圖8所示。圖8（a）中的黃線為室內(nèi)方形，總路徑長為200.46m。圖8（b）中的黃線為室外操場方形，總路徑長為346m。室內(nèi)外過渡場景如圖8（a）中的紅色虛線所示，總路徑長為123.16m。其中，試驗(yàn)路線的相對距離由高精度激光測距儀測量而得。

圖8 真實(shí)測試路徑Fig.8 Real testing trajectory

為對比零速檢測算法的優(yōu)劣，在捷聯(lián)導(dǎo)航解算算法的基礎(chǔ)上，加入不同的零速修正算法。如圖9（a）所示，藍(lán)色虛線表示應(yīng)用上文所提的第2種零速檢測方法的結(jié)果，紅色實(shí)線表示應(yīng)用上文所提的第3種零速檢測方法的結(jié)果，紅色軌跡基本與規(guī)劃路線相匹配，且首尾誤差約為1.5m。

如圖9（b）所示，藍(lán)色虛線表示應(yīng)用上文所提的第3種零速檢測方法的結(jié)果，綠色虛線表示應(yīng)用上文所提的第4種零速檢測方法的結(jié)果。相比較而言，藍(lán)色軌跡基本與規(guī)劃路線相匹配，誤差約為13.3m。若在加入零速修正的捷聯(lián)導(dǎo)航解算算法的基礎(chǔ)上，融合有效GPS進(jìn)行組合導(dǎo)航解算，如圖9（b）中的紅色實(shí)線所示。其與規(guī)劃路徑基本一致，同時誤差也減小到了3.13m。

圖9 部分測試路徑Fig.9 Partial testing trajectory

圖10（a）中的黃色虛線為標(biāo)準(zhǔn)路徑，紅色實(shí)線表示與經(jīng)篩選的GPS進(jìn)行組合的結(jié)果。圖10（b）中的藍(lán)色虛線表示與未篩選的GPS進(jìn)行組合的結(jié)果。由圖10可知，紅色軌跡基本與規(guī)劃路徑一致，其路徑誤差根據(jù)實(shí)測點(diǎn)與路線起終點(diǎn)、拐點(diǎn)的平均誤差計(jì)算而得，而藍(lán)色軌跡受到GPS的誤修正，與實(shí)際的軌跡相差較大。

圖10 室內(nèi)外路徑Fig.10 Indoor-outdoor trajectory

表3是不同試驗(yàn)場景的誤差結(jié)果匯總表，3組試驗(yàn)的誤差比均在跑步總里程的1%以內(nèi)。

表3 誤差結(jié)果Table 3 Error results

4 結(jié)論

本文在分析了行人跑步運(yùn)動特性的基礎(chǔ)上，對比研究并提出了可靠適用的行人跑步狀態(tài)下的零速檢測方法。設(shè)計(jì)了基于BP神經(jīng)網(wǎng)絡(luò)的GPS可用信號篩選方法，增強(qiáng)了GPS信息的有效性與可靠性。本文有效融合了慣性導(dǎo)航、零速、GPS信息，通過Kalman濾波的方法進(jìn)行了誤差修正，解決了慣導(dǎo)誤差快速累積的問題。仿真結(jié)果表明，基于慣性/零速/GPS的行人跑步室內(nèi)外無縫導(dǎo)航定位算法的平均定位誤差可減小到行人跑步總里程的1%以內(nèi)。