基于MEMS慣性傳感器和氣壓計(jì)的行人室內(nèi)導(dǎo)航系統(tǒng)設(shè)計(jì)*

鄒一凡，李 佳，王瑋冰

(1.中國科學(xué)院微電子研究所,北京 100029; 2.中國科學(xué)院大學(xué),北京 100049)

0 引 言

行人導(dǎo)航系統(tǒng)(pedestrian navigation system,PNS)被廣泛應(yīng)用于緊急救援、人員管理、健康護(hù)理等場景[1～3]，用于在全球定位系統(tǒng)(GPS)不可用的室內(nèi)環(huán)境中獲得行人的準(zhǔn)確定位信息。根據(jù)實(shí)現(xiàn)原理的不同，PNS可分為基于無線信號、基于視覺設(shè)備、基于慣性傳感器3種類別[4]。其中，基于慣性傳感器的PNS主要基于行人航跡推算(pedestrian dead reckoning,PDR)理論，具有不依賴于外部設(shè)備、低成本、高可靠性的特點(diǎn)，因此成為了該領(lǐng)域研究的熱點(diǎn)。

PDR系統(tǒng)通過將慣性測量單元(inertial measurement unit,IMU)安裝至可穿戴設(shè)備中，測量行人在運(yùn)動時的加速度、角速度、空間磁場、氣壓值等信息，經(jīng)過一定的數(shù)據(jù)融合算法，還原出行人運(yùn)動的空間軌跡。PDR算法可分為基于步長—方向模型的算法以及基于慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)的算法。其中，前者通過對傳感器數(shù)據(jù)進(jìn)行分析、建模，對行人每一步的步長與方向進(jìn)行估計(jì)，從而以步為單位更新行人位置；后者通過對傳感器的空間姿態(tài)進(jìn)行實(shí)時計(jì)算，將加速度轉(zhuǎn)換至導(dǎo)航坐標(biāo)系中進(jìn)行2次積分，分別獲得空間速度與位置。本文即基于后者進(jìn)行PNS的設(shè)計(jì)。

當(dāng)前，基于INS原理的PDR系統(tǒng)有一個通用的、經(jīng)受過實(shí)踐檢驗(yàn)的算法框架，即INS+擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)+零速更新(zero velocity update,ZUPT)框架，簡稱IEZ框架。這一框架最初由Foxlin E[5]提出，經(jīng)由Jiménez A R[6]改善，如今已在事實(shí)上成為該領(lǐng)域的基本算法思路[7～9]。為了修正系統(tǒng)在航向角上的誤差，在其基礎(chǔ)上進(jìn)一步添加零角速度更新(zero angular rate update,ZARU)、羅盤(compass)、啟發(fā)式偏移清除(heuristic drift elimination,HDE)、氣壓計(jì)修正等算法，即可獲得更加精確的定位結(jié)果。

本文基于IEZ+ZARU+HDE+氣壓計(jì)修正的算法思路，自主研發(fā)了軟硬件系統(tǒng)，設(shè)計(jì)并實(shí)現(xiàn)了完整的行人室內(nèi)導(dǎo)航系統(tǒng)方案。通過實(shí)驗(yàn)對系統(tǒng)性能進(jìn)行了驗(yàn)證，結(jié)果表明本文系統(tǒng)可滿足實(shí)際應(yīng)用需求。

1 基于IEZ框架的導(dǎo)航算法設(shè)計(jì)

1.1 系統(tǒng)總體框架

本文中，使用加速度計(jì)+陀螺儀+氣壓計(jì)的傳感器組合進(jìn)行數(shù)據(jù)采集。系統(tǒng)硬件中包含三軸磁力計(jì)，但并沒有使用，其原因在于室內(nèi)環(huán)境中，地磁場受到嚴(yán)重的干擾，無法用于對航向角進(jìn)行修正[10]。整體的算法框圖如圖1所示。由傳感器獲得的數(shù)據(jù)被直接用于INS解算，獲得對載體姿態(tài)、空間運(yùn)動速度以及位置的初步估計(jì)；使用EKF對INS系統(tǒng)的誤差進(jìn)行估計(jì)，其測量值來自多個來源，將在1.2節(jié)中進(jìn)行詳細(xì)介紹；當(dāng)腳步落地的零速區(qū)間被檢測到時，即執(zhí)行ZUPT與HDE過程。

圖1 系統(tǒng)算法框圖

1.2 INS算法

圖1中最上層為INS部分，利用慣性導(dǎo)航原理對載體姿態(tài)、速度與位置進(jìn)行更新。使用四元數(shù)q=[q0q1q2q3]T作為姿態(tài)表示方法，使用一階近似對四元數(shù)微分方程求數(shù)值解，可得離散化的四元數(shù)更新方程

(1)

式中ω=[ωxωyωz]為陀螺儀采集到的三軸角速度數(shù)據(jù)，T為采樣周期。利用上式計(jì)算后再將其歸一化，即完成了通過陀螺儀數(shù)據(jù)更新姿態(tài)。

(2)

(3)

之后，即可對加速度na進(jìn)行2次積分，分別得到空間速度nv與位置np。

1.3 EKF算法

EKF算法用于對系統(tǒng)中各個變量的誤差進(jìn)行估計(jì)。首先定義15維的狀態(tài)向量

xk=[δφδωδpδvδa]T

(4)

其中,包含了姿態(tài)估計(jì)誤差δφ,n系中的速度誤差δv和位置誤差δp，以及b系中的陀螺儀偏移δω和加速度偏移δa，均為三維向量。為了方便利用測量進(jìn)行更新，此處姿態(tài)誤差使用歐拉角進(jìn)行表示，在對姿態(tài)進(jìn)行誤差修正時，將-δφ/T代入式(1)即可。

寫出系統(tǒng)狀態(tài)更新方程

(5)

(6)

式中k為第k次采樣，wk為系統(tǒng)噪聲，I與0均為3×3矩陣，[nak×]為向量nak對應(yīng)的叉乘矩陣

(7)

之后，系統(tǒng)狀態(tài)即可隨著傳感器采樣進(jìn)行迭代更新，直至獲得測量值。本文系統(tǒng)中的測量值有：1)由ZARU獲得的δω測量值。2)由ZUPT獲得的δv測量值。3)由HDE獲得的δφ(3)測量值。4)由氣壓計(jì)數(shù)據(jù)獲得的δp(3)測量值。按這一順序，寫出包含所有量的測量矩陣H

(8)

可知，上述測量值并非在同一時間獲得：ZARU需要在完全靜止?fàn)顟B(tài)下測量，ZUPT與HDE需要在步行中的零速區(qū)間測量，氣壓計(jì)數(shù)據(jù)的更新取決于其采樣頻率。因此，當(dāng)進(jìn)行某一測量時，只需使用矩陣H中對應(yīng)行的元素，例如在零速區(qū)間進(jìn)行測量時，只需保留H的2,3行即可。

之后，即可利用卡爾曼濾波的基礎(chǔ)原理，對系統(tǒng)狀態(tài)進(jìn)行估計(jì)，這里不再進(jìn)行贅述。將估計(jì)所得的誤差值從INS所得量中減去，即可得到系統(tǒng)的最終估計(jì)值。

1.4 ZUPT與零速區(qū)間檢測算法

ZUPT利用行人腳步落地后速度為零的特點(diǎn)，將相鄰兩步內(nèi)經(jīng)由INS計(jì)算累積得的空間速度向量作為對系統(tǒng)誤差的測量，從而用于對系統(tǒng)狀態(tài)進(jìn)行估計(jì)。易知，對腳步落地區(qū)間的測量準(zhǔn)確度直接影響著ZUPT的應(yīng)用效果，需要對零速區(qū)間檢測算法進(jìn)行設(shè)計(jì)。相對于簡單的單傳感器閾值法，使用基于廣義似然比檢驗(yàn)(GLRT)的零速檢測算法具有更高的準(zhǔn)確度，即定義函數(shù)

(9)

1.5 HDE算法

在IEZ框架下，由于重力向量本身無法提供在水平面航向上的參考信息，因此,系統(tǒng)在航向角上的誤差會逐漸累積，使得定位結(jié)果逐漸偏離正常軌跡。同時，由于室內(nèi)環(huán)境下磁干擾嚴(yán)重，無法有效利用地磁向量作為航向的參考。此時，HDE算法即成為解決上述問題的較好方案。

HDE算法的核心思想來自于一個先驗(yàn)假設(shè)，即在樓房等室內(nèi)環(huán)境中，多數(shù)走廊之間呈現(xiàn)出互相垂直或夾角45°的規(guī)律。將這些方向設(shè)置為主方向，則行人在多數(shù)時間是沿著主方向行走的。運(yùn)用這一假設(shè)，即可利用行人運(yùn)動時解算所得的航向，與其鄰近的主方向進(jìn)行對應(yīng)，從而消除航向角的累積偏差。

同時，由于文獻(xiàn)[11]中原始的HDE算法不對運(yùn)動類型進(jìn)行區(qū)分，在行人作非直線運(yùn)動時反而會造成更大的誤差，因此，本文以文獻(xiàn)[11]所提出的iHDE算法為基礎(chǔ)進(jìn)行改進(jìn)，設(shè)計(jì)了下述算法。

首先，定義行人第k步的步長lk與方向θk如下

(10)

(11)

式中px,k與py,k分別為由INS獲得的第k步的x,y位置。之后，定位2個控制變量，分別用于判斷行人的步長與行進(jìn)方向

(12)

(13)

可見，當(dāng)單步步長大于閾值，且過去5步內(nèi)的航向變化小于閾值時，兩控制量值為1，這代表著行人正在以正常的步態(tài)進(jìn)行直線行走。只有在這種條件下，才利用θk與主方向之間的差值作為HDE測量，從而在其他不規(guī)則運(yùn)動狀態(tài)時禁用HDE，避免了可能帶來的定位精度下降。

2 實(shí)驗(yàn)驗(yàn)證

2.1 行人室內(nèi)導(dǎo)航系統(tǒng)方案設(shè)計(jì)

本文面向?qū)嶋H應(yīng)用需求，設(shè)計(jì)了完整的行人室內(nèi)導(dǎo)航系統(tǒng)方案，包括數(shù)據(jù)采集模塊的軟硬件系統(tǒng)、上位機(jī)軟件以及上文所述的系統(tǒng)算法。系統(tǒng)整體架構(gòu)如圖2所示。將數(shù)據(jù)采集模塊安裝至行人足部后，對九軸傳感器(MARG)以及氣壓傳感器數(shù)據(jù)進(jìn)行采集、處理后，通過算法對行人當(dāng)前空間位置進(jìn)行估計(jì)，并將運(yùn)算結(jié)果通過藍(lán)牙傳輸至上位機(jī)。

圖2 行人室內(nèi)導(dǎo)航系統(tǒng)架構(gòu)

數(shù)據(jù)采集模塊以STM32 L031G6U6作為MCU，搭載九軸傳感器ICM—20948與氣壓傳感器BMP280，使用nRF52832通過BLE5.0與上位機(jī)進(jìn)行數(shù)據(jù)通信。模塊整體尺寸為35 mm×35 mm×10 mm，由鋰電池進(jìn)行供電，實(shí)物如圖3所示。

圖3 數(shù)據(jù)采集模塊實(shí)物

2.2 實(shí)驗(yàn)設(shè)計(jì)與結(jié)果分析

為了驗(yàn)證上述系統(tǒng)及算法的有效性，通過設(shè)定運(yùn)動路徑，進(jìn)行了多組實(shí)驗(yàn)。試驗(yàn)對象足部安裝采集模塊，以125 Hz采集慣性數(shù)據(jù)，以25 Hz采集氣壓數(shù)據(jù)，利用上述算法進(jìn)行了軌跡的還原。

1)走廊直線路徑實(shí)驗(yàn)

試驗(yàn)對象在靜止一段時間后，沿著一段長40 m，寬1.8 m的走廊邊緣，由一端行走至另一端，再沿另一邊緣折返回來至出發(fā)線。使用本文設(shè)計(jì)的算法流程進(jìn)行運(yùn)動軌跡還原，所得結(jié)果如圖4所示。

圖4 走廊直線路徑實(shí)驗(yàn)軌跡

可見，本文所設(shè)計(jì)算法能夠較好地還原出行人運(yùn)動的軌跡。圖4中終點(diǎn)坐標(biāo)與實(shí)際位置相差0.79 m，占總路程的0.96 %。

2)組合路徑實(shí)驗(yàn)

在實(shí)際應(yīng)用場景中，行人在室內(nèi)的運(yùn)動路徑是多樣的，既包括沿直線走廊的行走，也包括沿曲線行走、上下樓梯、乘坐電梯等。為了驗(yàn)證系統(tǒng)在多種運(yùn)動路徑下的定位功能，設(shè)計(jì)了組合路徑進(jìn)行實(shí)驗(yàn)。最終解算所得運(yùn)動軌跡如圖5所示。實(shí)驗(yàn)對象由出發(fā)點(diǎn)出發(fā)，先搭乘電梯，由6樓下降至1樓，隨后沿走廊行進(jìn)。點(diǎn)A至點(diǎn)B為室外行進(jìn)路段，由B點(diǎn)進(jìn)入建筑物內(nèi)后，從C處樓梯上至6樓，隨后通過走廊回到出發(fā)點(diǎn)處，即為完整的運(yùn)動路徑。

圖5 組合路徑實(shí)驗(yàn)軌跡

可見，由于將氣壓計(jì)應(yīng)用于對海拔高度進(jìn)行測量，軌跡在空間Z軸上的誤差極小。經(jīng)計(jì)算，完整的運(yùn)動距離(total traveles distance,TTD)約330 m，軌跡還原所得起始點(diǎn)間的誤差為1.10 m，由此可計(jì)算得系統(tǒng)定位誤差為0.33 %。

3 結(jié) 論

本文針對實(shí)際應(yīng)用中對行人室內(nèi)導(dǎo)航系統(tǒng)的需求，實(shí)現(xiàn)了對系統(tǒng)軟硬件及算法的設(shè)計(jì)和實(shí)現(xiàn)。通過將MEMS慣性傳感器以及氣壓計(jì)安裝至行人足部，利用IEZ框架，結(jié)合ZARU、HDE、氣壓計(jì)等多種校正方法，將傳感器數(shù)據(jù)融合以獲得對行人空間位置的估計(jì)。實(shí)驗(yàn)結(jié)果表明：系統(tǒng)對行人定位的空間誤差在1 %以內(nèi)，驗(yàn)證了其有效性。