呂 品，朱靜漪，賴際舟，袁 誠，王鵬宇

（1.南京航空航天大學(xué)自動(dòng)化學(xué)院 導(dǎo)航研究中心，南京 211106；2.北京自動(dòng)化控制設(shè)備研究所，北京 100074）

隨著科技發(fā)展水平的提高，人們對(duì)于位置服務(wù)[1]的需求愈發(fā)凸顯。然而，基于衛(wèi)星的導(dǎo)航定位系統(tǒng)[2]僅能在室外空曠環(huán)境實(shí)現(xiàn)高精度、長(zhǎng)待機(jī)的穩(wěn)定導(dǎo)航服務(wù)，面對(duì)室內(nèi)復(fù)雜環(huán)境時(shí)則會(huì)有失效的可能[3]。

針對(duì)室內(nèi)環(huán)境，目前實(shí)現(xiàn)行人導(dǎo)航的方案主要包含無線定位技術(shù)[4]和慣性導(dǎo)航技術(shù)[5,6]兩種。無線定位技術(shù)即在室內(nèi)場(chǎng)景下部署無線通信技術(shù)[7]（Wireless Fidelity,Wi-Fi）、藍(lán)牙[8]、超寬帶[9]等設(shè)施進(jìn)行環(huán)境信息測(cè)量，從而實(shí)現(xiàn)行人導(dǎo)航定位。與前者相比，慣性導(dǎo)航技術(shù)是一種完全自主式的導(dǎo)航定位解決方案。隨著微機(jī)電系統(tǒng)（Micro-Electro-Mechanical System,MEMS）[10]的快速發(fā)展，行人慣性導(dǎo)航系統(tǒng)主要采用MEMS 慣性傳感器件，該器件具有體積小、功耗低、價(jià)格低廉等優(yōu)勢(shì)[11]。但MEMS 器件精度較低，在進(jìn)行慣性數(shù)據(jù)采集的同時(shí)引入了大量噪聲。為了抑制行人導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間連續(xù)積分過程中導(dǎo)航誤差的發(fā)散，目前通常采用零速修正方法[12]。行人在室內(nèi)場(chǎng)景行走時(shí)，通常包括走路、跑步、上下樓等不同運(yùn)動(dòng)狀態(tài)，但由于不同運(yùn)動(dòng)狀態(tài)的動(dòng)態(tài)特性不同，固定零速閾值誤判率較高。因此，如何針對(duì)各類運(yùn)動(dòng)狀態(tài)提取準(zhǔn)確的零速區(qū)間進(jìn)行誤差修正，成為影響導(dǎo)航定位精度的關(guān)鍵。

針對(duì)該問題，目前較多研究采用基于經(jīng)驗(yàn)閾值法和機(jī)器學(xué)習(xí)方法。經(jīng)驗(yàn)閾值法對(duì)不同運(yùn)動(dòng)狀態(tài)設(shè)置相應(yīng)的零速判斷閾值，如雷明等[13]對(duì)站立、走路、跑步、上樓和下樓等五種主要運(yùn)動(dòng)模式設(shè)置相應(yīng)的零速判斷閾值。田曉春等[14]對(duì)足部傳感器輸出特征設(shè)置多條件閾值進(jìn)行約束。Zhang R 等[15]通過提取胸部加速度信息增加零速檢測(cè)的相應(yīng)閾值判斷條件。經(jīng)驗(yàn)閾值法的閾值設(shè)置對(duì)經(jīng)驗(yàn)依賴性強(qiáng)，且對(duì)不同行人、不同步態(tài)的適應(yīng)性有限，易出現(xiàn)“誤檢”、“漏檢”現(xiàn)象。機(jī)器學(xué)習(xí)方法通過設(shè)計(jì)模型判斷零速狀態(tài)，如紀(jì)志農(nóng)等[16]采用支持向量機(jī)對(duì)不同步態(tài)下的零速點(diǎn)和非零速點(diǎn)進(jìn)行分類，實(shí)現(xiàn)零速區(qū)間的提取。戴洪德等[17]對(duì)運(yùn)動(dòng)狀態(tài)進(jìn)行分類，采用改進(jìn)的K-menas 聚類方法劃分零速區(qū)間。機(jī)器學(xué)習(xí)方法雖然魯棒性較好，但需要依賴大量數(shù)據(jù)支持，且計(jì)算復(fù)雜度較高。

此外，行人在室內(nèi)行走時(shí)通常包含上下樓梯場(chǎng)景，此時(shí)慣性導(dǎo)航系統(tǒng)的解算高度會(huì)快速發(fā)散，因此需要對(duì)行人在爬樓階段進(jìn)行高度修正。一些研究利用室內(nèi)高度的先驗(yàn)信息對(duì)行人高度進(jìn)行校正，如劉宇等[18]預(yù)先對(duì)半層樓高度進(jìn)行測(cè)量，在行人平走狀態(tài)時(shí)將高度修正至半層樓高的整數(shù)倍。采用先驗(yàn)信息的方法主要適用于已知合作場(chǎng)景，而在陌生場(chǎng)景中，較多采用氣壓高度計(jì)對(duì)高度進(jìn)行校正。Yang W 等[19]根據(jù)前后兩步的高度差判斷行人運(yùn)動(dòng)狀態(tài)，在上下樓狀態(tài)融合氣壓計(jì)信息進(jìn)行高度修正。然而，當(dāng)長(zhǎng)時(shí)間采用氣壓高度計(jì)進(jìn)行輔助時(shí)，其高度誤差會(huì)產(chǎn)生較大漂移。

針對(duì)上述問題，本文提出了一種基于行為概率分析的室內(nèi)行人導(dǎo)航方法。一方面，采用基于廣義似然比檢驗(yàn)的零速區(qū)間檢測(cè)器對(duì)零速區(qū)間進(jìn)行檢測(cè)，通過區(qū)間約束和周期劃分得到每一步的步態(tài)區(qū)間。在各步態(tài)區(qū)間內(nèi)提取零速檢測(cè)值最小的時(shí)刻，作為每一步的最大概率零速點(diǎn)并進(jìn)行導(dǎo)航誤差修正。另一方面，針對(duì)上下樓狀態(tài)下慣導(dǎo)高度誤差發(fā)散的問題，本文提出了一種基于慣性/氣壓傳感器的爬樓高度在線估計(jì)方法，對(duì)爬樓臺(tái)階數(shù)、臺(tái)階高度進(jìn)行估計(jì)。本文方法基于最大概率提取零速點(diǎn)，對(duì)不同步態(tài)適應(yīng)性強(qiáng)，且降低了不穩(wěn)定的氣壓計(jì)輸出對(duì)高度修正的影響。通過實(shí)驗(yàn)驗(yàn)證，該算法能夠在室內(nèi)場(chǎng)景下實(shí)現(xiàn)較高的定位精度。

1 算法總體框架

本文提出的行人導(dǎo)航方法總體框架如圖1 所示，主要包含零速檢測(cè)、上/下樓高度估計(jì)、擴(kuò)展卡爾曼濾波（Extended Kalman filter,EKF）誤差修正等部分。

圖1 算法總體框架Fig.1 Overall algorithm framework

本文采用基于最大概率的零速點(diǎn)提取方法實(shí)現(xiàn)各步態(tài)周期內(nèi)的零速檢測(cè)，提出一種慣性/氣壓聯(lián)合樓梯檢測(cè)方法實(shí)現(xiàn)上下樓運(yùn)動(dòng)臺(tái)階數(shù)的檢測(cè)，并基于樓梯高度不變的假設(shè)實(shí)現(xiàn)高度估計(jì)，同時(shí)提出了基于EKF的高度后處理優(yōu)化方法。采用EKF 進(jìn)行融合，在每個(gè)步態(tài)的最大概率零速點(diǎn)時(shí)刻對(duì)速度和高度進(jìn)行誤差修正，解算導(dǎo)航定位信息。

零速檢測(cè)方面，本文首先采用基于廣義似然比（Generalized Likelihood Ratio Τest,GLRΤ）的零速檢測(cè)方法對(duì)各運(yùn)動(dòng)類型的零速檢測(cè)值進(jìn)行初步分析。根據(jù)行人足部運(yùn)動(dòng)周期性對(duì)步態(tài)區(qū)間進(jìn)行約束，實(shí)現(xiàn)步態(tài)劃分，并在每一個(gè)步態(tài)內(nèi)提取最大概率零速點(diǎn)。

高度估計(jì)方面，本文首先搭建長(zhǎng)短期記憶網(wǎng)絡(luò)[20]（Long Short-Τerm Memory,LSΤM）實(shí)現(xiàn)上下樓的運(yùn)動(dòng)識(shí)別，結(jié)合氣壓高度計(jì)信息計(jì)算臺(tái)階平均高度，并提出了一種基于慣性/氣壓傳感器的爬樓高度在線估計(jì)方法，實(shí)現(xiàn)上下樓階段的高度估計(jì)。

2 基于步態(tài)檢測(cè)的最大概率零速點(diǎn)提取方法

本文提出了一種基于步態(tài)檢測(cè)的最大概率零速點(diǎn)提取方法，流程圖如圖2 所示。

圖2 最大概率零速點(diǎn)提取方法流程圖Fig.2 Flow chart of maximum probability zero speed point extraction method

為了提高零速修正算法的魯棒性，本文首先采用GLRΤ 方法進(jìn)行零速檢測(cè)，并基于區(qū)間分析方法對(duì)零速檢測(cè)的結(jié)果進(jìn)行劃分，得到每一步的步態(tài)區(qū)間。同時(shí)，本文提出了一種基于最大概率的零速檢測(cè)方法，在每一個(gè)步態(tài)區(qū)間內(nèi)搜索最大概率零速點(diǎn)，實(shí)現(xiàn)跑步等動(dòng)態(tài)較大情況下的零速修正。

2.1 基于GLRΤ 的零速區(qū)間檢測(cè)方法

行人運(yùn)動(dòng)過程中的步態(tài)周期可劃分為運(yùn)動(dòng)階段和觸地階段。在運(yùn)動(dòng)階段，足部開始離開地面并在空中擺動(dòng)向前邁進(jìn)；在觸地階段，足部完全接觸地面，足部和地面保持一段時(shí)間的相對(duì)靜止，此段時(shí)間即為零速區(qū)間。為了抑制慣性行人導(dǎo)航系統(tǒng)誤差的發(fā)散，可以在零速區(qū)間對(duì)誤差進(jìn)行修正。

在實(shí)際的室內(nèi)運(yùn)動(dòng)場(chǎng)景中，行人不同運(yùn)動(dòng)的足部動(dòng)態(tài)大小存在差異。為了提高零速檢測(cè)方法的適應(yīng)性，本文首先采用基于GLRΤ 的零速區(qū)間判斷方法，分析行人在走路、跑步和上下樓運(yùn)動(dòng)狀態(tài)下的零速檢測(cè)值。

GLRΤ 綜合考慮加速度和角速度信息，根據(jù)式(1)計(jì)算k時(shí)刻窗口內(nèi)的檢測(cè)值。通過比較檢測(cè)值與閾值來判斷當(dāng)前樣本是否處于零速區(qū)間。

其中，Tk為k時(shí)刻零速檢測(cè)值；W為窗口大??；、分別為陀螺儀和加速度計(jì)的噪聲方差；ωk、fk分別為k時(shí)刻的陀螺儀和加速度計(jì)輸出值；g為當(dāng)?shù)刂亓铀俣?；為該窗口?nèi)三軸加速度均值。

當(dāng)檢測(cè)值小于零速檢測(cè)器閾值T時(shí)，可以認(rèn)為k時(shí)刻的樣本數(shù)據(jù)處于零速區(qū)間，此時(shí)零速判斷值z(mì)k為真，如式(2)所示。當(dāng)行人運(yùn)動(dòng)包含多種運(yùn)動(dòng)狀態(tài)時(shí)，設(shè)置合適的檢測(cè)器閾值，以檢測(cè)出各狀態(tài)下零速點(diǎn)可能存在的區(qū)間。考慮到跑步動(dòng)態(tài)較大，此處的零速檢測(cè)器閾值應(yīng)該設(shè)置得較大一些。

由于在行人不同運(yùn)動(dòng)狀態(tài)中傳感器動(dòng)態(tài)輸出和零速區(qū)間長(zhǎng)度存在差異，傳統(tǒng)方法在進(jìn)行零速檢測(cè)時(shí)需要針對(duì)不同運(yùn)動(dòng)狀態(tài)調(diào)整合適的零速閾值，并在零速區(qū)間內(nèi)進(jìn)行零速修正。為了減小由量測(cè)信息不準(zhǔn)引入的誤差量，本文在每個(gè)步態(tài)周期內(nèi)僅在最大概率零速點(diǎn)處進(jìn)行一次系統(tǒng)誤差修正?；贕LRΤ 劃分零速區(qū)間，采用基于區(qū)間分析的方法對(duì)零速區(qū)間進(jìn)行約束，完成步態(tài)周期的劃分，進(jìn)而實(shí)現(xiàn)在每一個(gè)步態(tài)周期內(nèi)提取最大概率零速點(diǎn)。

2.2 基于零速區(qū)間分析的步態(tài)周期劃分方法

綜合考慮走路、跑步、上下樓幾種運(yùn)動(dòng)情況對(duì)樣本進(jìn)行分析，首先，進(jìn)行步態(tài)區(qū)間長(zhǎng)度的約束。根據(jù)基于區(qū)間分析的步態(tài)劃分方法，認(rèn)為每個(gè)步態(tài)中的非零速區(qū)間長(zhǎng)度不小于0.12 s，且一個(gè)步態(tài)的區(qū)間長(zhǎng)度不小于0.6 s。計(jì)算非零速區(qū)間長(zhǎng)度，若長(zhǎng)度小于0.12 s則認(rèn)為出現(xiàn)了零速點(diǎn)誤判，此時(shí)實(shí)際上的零速點(diǎn)被誤判為非零速，將后一段區(qū)間劃分為零速區(qū)間。然后，根據(jù)行人運(yùn)動(dòng)時(shí)足部運(yùn)動(dòng)的周期性規(guī)律，以每一段零速區(qū)間的開始時(shí)刻為起點(diǎn)，以下一段非零速區(qū)間結(jié)束時(shí)刻為終點(diǎn)，將其視為一段完整的步態(tài)區(qū)間。步態(tài)區(qū)間劃分示意圖如圖3 所示。

圖3 步態(tài)區(qū)間劃分示意圖Fig.3 Gait interval division diagram

2.3 基于最大概率的零速點(diǎn)提取方法

傳統(tǒng)零速修正方法在檢測(cè)出來的所有零速點(diǎn)上均對(duì)誤差進(jìn)行修正，但在對(duì)量測(cè)和狀態(tài)進(jìn)行融合的同時(shí)也引入了大量的誤差，從而影響導(dǎo)航系統(tǒng)性能。

本文在進(jìn)行步態(tài)劃分時(shí)，已經(jīng)基于GLRΤ 實(shí)現(xiàn)了零速區(qū)間的識(shí)別。在一個(gè)步態(tài)周期中，足部速度為零的最大概率點(diǎn)即在零速檢測(cè)器檢測(cè)值最小值點(diǎn)附近，因此，在每個(gè)步態(tài)的零速區(qū)間內(nèi)可以進(jìn)一步提取最大概率零速點(diǎn)，認(rèn)為該時(shí)刻足跟完全接觸地面，并在下一時(shí)刻開始離開地面進(jìn)入擺動(dòng)期。

根據(jù)式(1)，檢測(cè)值Tk越小則表示k時(shí)刻為零速的概率越大，因此，只要在已獲得的零速區(qū)間內(nèi)尋找最小檢測(cè)值即可提取最大概率零速點(diǎn)，如圖4 所示。

圖4 最大概率零速點(diǎn)提取示意圖Fig.4 Maximum probability zero speed point extraction diagram

與傳統(tǒng)的零速修正算法相比，該方法在每個(gè)步態(tài)周期內(nèi)僅在最大概率零速點(diǎn)進(jìn)行一次誤差修正，簡(jiǎn)化了濾波器計(jì)算量，同時(shí)消除了由于運(yùn)動(dòng)狀態(tài)不同導(dǎo)致的零速誤判、漏判情況，提高了導(dǎo)航系統(tǒng)的魯棒性。

3 基于LSTM 的上下樓運(yùn)動(dòng)檢測(cè)方法

本文采用基于LSΤM 的深度學(xué)習(xí)網(wǎng)絡(luò)模型實(shí)現(xiàn)上下樓運(yùn)動(dòng)檢測(cè)，模型的輸入為66 維特征，樣本標(biāo)簽為四種。該深度學(xué)習(xí)網(wǎng)絡(luò)模型由輸入層、LSΤM 層、全連接層、softmax 層和輸出層構(gòu)成，包含64 個(gè)隱藏節(jié)點(diǎn)，采用Adam 優(yōu)化函數(shù)，最大迭代周期為100 次。

考慮性別、身高等因素，本實(shí)驗(yàn)共組織15 位人員進(jìn)行數(shù)據(jù)采集，其中男性7 名、女性8 名，身高范圍為1.56～1.82 m，體重范圍為52～75 kg。數(shù)據(jù)采集過程中，實(shí)驗(yàn)人員分別以走路、跑步、上樓、下樓步態(tài)進(jìn)行運(yùn)動(dòng)。

實(shí)驗(yàn)過程中采用藍(lán)牙無線傳輸數(shù)據(jù)，采集頻率為200 Hz，分別記錄三軸加速度、三軸角速度和氣壓高度信息。共采集數(shù)據(jù)樣本3601 份，各運(yùn)動(dòng)狀態(tài)樣本數(shù)量如表1 所示。

表1 數(shù)據(jù)集樣本數(shù)量Tab.1 Sample number of dataset

經(jīng)過該模型訓(xùn)練，運(yùn)動(dòng)狀態(tài)識(shí)別準(zhǔn)確率達(dá)98.6%，混淆矩陣如圖5 所示。

圖5 步態(tài)識(shí)別混淆矩陣Fig.5 Confusion matrix of gait recognition

4 基于樓梯階數(shù)概率檢測(cè)的行人爬樓高度估計(jì)方法

目前，針對(duì)行人室內(nèi)導(dǎo)航環(huán)境的高度修正方法主要是采用氣壓高度計(jì)作為高度參考信息，但氣壓高度計(jì)容易受到溫度、濕度等環(huán)境因素影響，同時(shí)隨著行人運(yùn)動(dòng)幅度的變化，行人運(yùn)動(dòng)時(shí)氣壓高度計(jì)的輸出值存在較大波動(dòng)。

針對(duì)該問題，本文在行人同樓層運(yùn)動(dòng)狀態(tài)下不參考?xì)鈮焊叨扔?jì)信息，將前一步的高度作為參考進(jìn)行高度修正。由于建筑物內(nèi)臺(tái)階高度通常是一致的，因此本文在進(jìn)行行人上下樓高度估計(jì)時(shí)假設(shè)建筑物內(nèi)臺(tái)階高度不變。上下樓運(yùn)動(dòng)的高度估計(jì)算法具體流程如圖6 所示。

圖6 上下樓運(yùn)動(dòng)的高度估計(jì)算法流程圖Fig.6 Flow chart of height estimation algorithm for ascending and descending movements

4.1 慣性/氣壓聯(lián)合樓梯檢測(cè)

為了實(shí)現(xiàn)行人上下樓步態(tài)跨越臺(tái)階階數(shù)的判斷，本文結(jié)合慣性解算高度信息和氣壓高度計(jì)的信息進(jìn)行分析，算法流程圖如圖7 所示。在每個(gè)上下樓步態(tài)的最大概率零速點(diǎn)時(shí)刻，截取該步態(tài)區(qū)間內(nèi)的慣性解算高度數(shù)據(jù)和氣壓高度計(jì)數(shù)據(jù)。設(shè)置合適的窗口大小和步長(zhǎng)，采用滑動(dòng)窗口遍歷步態(tài)數(shù)據(jù)。

圖7 慣性/氣壓聯(lián)合樓梯檢測(cè)算法流程圖Fig.7 Flow chart of inertial/pneumatic stair detection algorithm

首先，在每個(gè)滑動(dòng)窗口內(nèi)分別對(duì)慣性解算高度和氣壓高度數(shù)據(jù)提取差分特征值，繼而計(jì)算慣性解算高度差分的能量函數(shù)和氣壓高度差分的能量函數(shù)。第k個(gè)步態(tài)的能量函數(shù)的計(jì)算方法如式(3)所示。

其中，z k為第k個(gè)步態(tài)的最大概率零速點(diǎn)；di,m、dp,m分別為m時(shí)刻慣性解算高度和氣壓高度差分值。式(3)的兩個(gè)能量函數(shù)分別反映了當(dāng)前數(shù)據(jù)窗口中慣性解算高度和氣壓高度變化的程度。

其次，根據(jù)預(yù)先設(shè)定的閾值對(duì)能量函數(shù)進(jìn)行判斷。由于上樓運(yùn)動(dòng)和下樓運(yùn)動(dòng)的動(dòng)態(tài)特性不同，需要分別設(shè)置相應(yīng)的能量函數(shù)閾值t1、t2。如果慣性解算高度差分和氣壓高度差分的能量函數(shù)均低于設(shè)定的閾值，則判斷當(dāng)前窗口步態(tài)片段未跨越臺(tái)階。反之則當(dāng)前窗口步態(tài)片段跨越了一個(gè)臺(tái)階。

最后，對(duì)當(dāng)前步態(tài)的所有窗口的臺(tái)階跨越情況進(jìn)行統(tǒng)計(jì)，得到行人上下樓步態(tài)中跨越臺(tái)階階數(shù)。

4.2 上下樓高度估計(jì)

為了減小氣壓高度計(jì)輸出不準(zhǔn)確對(duì)高度帶來的影響，結(jié)合運(yùn)動(dòng)狀態(tài)識(shí)別結(jié)果，本文僅在第一段上下樓運(yùn)動(dòng)時(shí)參考?xì)鈮焊叨扔?jì)高度信息并計(jì)算臺(tái)階平均高度?；谂_(tái)階高度不變假設(shè)，在后續(xù)上下樓運(yùn)動(dòng)時(shí)參考臺(tái)階高度估計(jì)信息進(jìn)行高度修正。

臺(tái)階高度估計(jì)量hs初始值為初始上下樓運(yùn)動(dòng)基于氣壓高度計(jì)計(jì)算的臺(tái)階平均高度，如式(4)所示。

其中，Δhp1為第一段上下樓運(yùn)動(dòng)的氣壓高度計(jì)高度差；ni為第i步跨越的臺(tái)階個(gè)數(shù)；p、q分別為第一段上下樓的起始和結(jié)束步態(tài)。

后續(xù)上下樓步態(tài)的高度變化量如式(5)所示。

為了進(jìn)一步提高上下樓高度估計(jì)的準(zhǔn)確性，本文提出了基于EKF 的后處理高度優(yōu)化方法。在兩個(gè)樓梯平臺(tái)之間的上下樓運(yùn)動(dòng)過程中，分別計(jì)算各步態(tài)區(qū)間內(nèi)高度變化量Δhk與慣性解算高度變化量Δhi,k的差值，尋找差值最小時(shí)的步態(tài)序號(hào)j。該段上下樓運(yùn)動(dòng)結(jié)束后，將臺(tái)階高度hs修正為此時(shí)基于慣性解算的臺(tái)階高度估計(jì)量Δhs,i，實(shí)現(xiàn)臺(tái)階高度估計(jì)的優(yōu)化。

基于慣性解算的臺(tái)階高度估計(jì)量Δhs,i如式(6)所示。

其中，nj為第j步跨越的臺(tái)階個(gè)數(shù)；Δhi,j為第j步的慣性解算高度變化量。

在下一段上下樓步態(tài)高度修正中，采用更新后的臺(tái)階估計(jì)高度hs。

5 基于EKF 的誤差修正算法

5.1 EKF 融合算法

本文采用EKF 算法對(duì)九維狀態(tài)誤差進(jìn)行估計(jì)，取狀態(tài)量如式(7)所示。

其中，Xk為k時(shí)刻的系統(tǒng)狀態(tài)量；Pk、Vk、ψk分別為系統(tǒng)三軸位置、速度、姿態(tài)角。

取系統(tǒng)量測(cè)量Zk以觀測(cè)k時(shí)刻的速度和高度。

其中，Zv,k為k時(shí)刻系統(tǒng)三軸速度理論值與實(shí)際值的差值；Zp,k為k時(shí)刻高度理論值與實(shí)際值的差值。

根據(jù)卡爾曼濾波的狀態(tài)空間模型，狀態(tài)量Xk和量測(cè)量Zk滿足式(9)。

其中，W為系統(tǒng)噪聲矩陣，R為量測(cè)噪聲矩陣。

F為系統(tǒng)矩陣：

其中，St為載體系加速度計(jì)在導(dǎo)航系投影的斜對(duì)稱矩陣；0i×j為i×j的零矩陣；Ii×j為i×j的單位矩陣。

H為量測(cè)矩陣：

其中，h21=[0 0 1]。

5.2 速度修正

為了使誤差修正時(shí)刻更加準(zhǔn)確，本文只在每個(gè)步態(tài)的最大概率零速點(diǎn)處進(jìn)行誤差修正。該時(shí)刻速度理論值為零，則速度修正量Zv,k為速度理論值與實(shí)際值的差值，如式(13)所示。

其中，Vx,k、Vy,k、Vz,k分別為k時(shí)刻速度三軸分量。

5.3 高度修正

根據(jù)基于步態(tài)類型約束的高度修正方案，對(duì)當(dāng)前步態(tài)運(yùn)動(dòng)類型進(jìn)行判斷。高度修正值Zp,k滿足式(13)。

其中，hi,k為k時(shí)刻最大概率零速點(diǎn)系統(tǒng)解算高度；hp,k為k時(shí)刻最大概率零速點(diǎn)氣壓高度計(jì)高度；lk為當(dāng)前上下樓步態(tài)跨越的臺(tái)階個(gè)數(shù)。

fk為當(dāng)前步態(tài)上下樓標(biāo)志：

6 實(shí)驗(yàn)結(jié)果和分析

6.1 實(shí)驗(yàn)設(shè)備和場(chǎng)景介紹

本文實(shí)驗(yàn)采用ForSense IMU614 MEMS-IMU 六軸傳感器，并將該傳感器固定于實(shí)驗(yàn)人員右足鞋墊內(nèi)足跟位置，如圖8 所示。

圖8 實(shí)驗(yàn)設(shè)備與安裝位置Fig.8 Experimental equipment and installation position

該MEMS-IMU 傳感器內(nèi)嵌三軸加速度計(jì)、三軸陀螺儀，性能參數(shù)如表2 所示。氣壓高度計(jì)內(nèi)嵌于MEMS-IMU 傳感器中，性能參數(shù)如表3 所示。

表2 ForSense IMU614 傳感器相關(guān)參數(shù)Tab.2 ForSense IMU614 sensor related parameters

表3 氣壓高度計(jì)相關(guān)參數(shù)Tab.3 Barometric altimeter related parameters

實(shí)驗(yàn)地點(diǎn)位于南京航空航天大學(xué)自動(dòng)化學(xué)院一號(hào)樓三樓至五樓，樓層平面圖如圖9 所示。單一步態(tài)場(chǎng)景下，實(shí)驗(yàn)人員于起點(diǎn)A 處出發(fā)，以走路姿態(tài)沿A-B-C-D-A 繞行兩圈后回到起點(diǎn)A?；旌喜綉B(tài)場(chǎng)景下，實(shí)驗(yàn)路徑1：實(shí)驗(yàn)人員于五樓樓梯口起點(diǎn)A 處出發(fā)，沿A-B 方向走路行進(jìn)至B 處，隨即下兩層樓梯至三樓樓梯口B 位置，以跑步姿態(tài)沿B-A 方向至三樓樓梯口A 處，最后上兩層樓梯回到終點(diǎn)A；實(shí)驗(yàn)路徑2：實(shí)驗(yàn)人員于五樓樓梯口起點(diǎn)A 處出發(fā)，沿A-B-C-D-A走路行進(jìn)一圈后回到起點(diǎn)A，隨即下兩層樓梯至三樓樓梯口位置A，同樣以跑步姿態(tài)沿A-B-C-D-A 進(jìn)行一圈后回到三樓樓梯口A 位置，最后上兩層樓梯回到終點(diǎn)A。

圖9 樓層平面圖與實(shí)驗(yàn)路徑Fig.9 Floor plan and experimental path

6.2 最大概率零速點(diǎn)誤差修正算法驗(yàn)證與分析

為了測(cè)試本文零速檢測(cè)方法的導(dǎo)航定位效果，分別采用Openshoe 方法和AdaptiveOpenshoe 方法進(jìn)行對(duì)比實(shí)驗(yàn)。Openshoe 方法采用固定零速檢測(cè)閾值，而AdaptiveOpenshoe 方法在前者的基礎(chǔ)上實(shí)現(xiàn)了零速檢測(cè)閾值的自適應(yīng)調(diào)整。

6.2.1 零速檢測(cè)方法對(duì)比結(jié)果

三種零速檢測(cè)算法在行人各運(yùn)動(dòng)狀態(tài)下的零速檢測(cè)結(jié)果對(duì)比如圖10 所示。

圖10 各狀態(tài)下三種算法零速檢測(cè)結(jié)果對(duì)比Fig.10 Comparison of zero speed detection results of three algorithms in different state

為驗(yàn)證最大概率零速檢測(cè)方法的適用性，針對(duì)不同人員采集的實(shí)驗(yàn)數(shù)據(jù)，三種零速檢測(cè)算法的檢測(cè)情況如表4 所示。

表4 三種算法步態(tài)檢測(cè)情況對(duì)比Tab.4 Comparison of gait detection by three algorithms

由圖10 和表4 可知，在行人走路狀態(tài)下，三種零速檢測(cè)方法實(shí)現(xiàn)零速點(diǎn)提取的成功率均較高；在行人上下樓狀態(tài)下，Openshoe 算法和AdaptiveOpenshoe算法零速點(diǎn)提取成功率有所降低；在行人跑步狀態(tài)下，Openshoe 算法成功率僅有11.36%，AdaptiveOpenshoe算法與前者相比零速檢測(cè)成功率有所提升。對(duì)三種零速檢測(cè)算法的零速點(diǎn)提取平均成功率進(jìn)行分析可得,最大概率零速修正算法在四種行人運(yùn)動(dòng)狀態(tài)下實(shí)現(xiàn)零速點(diǎn)提取的成功率最高，與前兩種零速檢測(cè)算法相比，平均成功率分別提高了24.44%和16.21%。

6.2.2 定位結(jié)果對(duì)比

1）單一步態(tài)場(chǎng)景

單一步態(tài)下三種算法導(dǎo)航定位結(jié)果和定位誤差如圖11 和表5 所示。

表5 單一步態(tài)下三種算法定位誤差對(duì)比Tab.5 Comparison of positioning errors of three algorithms under a single gait

圖11 單一步態(tài)下三種零速檢測(cè)算法定位結(jié)果對(duì)比Fig.11 Comparison of positioning results of three zero speed detection algorithms under a single gait

2）混合步態(tài)場(chǎng)景

混合步態(tài)下三種算法導(dǎo)航定位結(jié)果如圖12 所示。

圖12 混合步態(tài)下三種零速檢測(cè)算法定位結(jié)果對(duì)比Fig.12 Comparison of positioning results of three zero speed detection algorithms under mixed gait

為了更好地對(duì)定位結(jié)果進(jìn)行分析，本文采用導(dǎo)航軌跡的首尾誤差與總距離的比值作為精度指標(biāo)。三種算法的定位誤差和定位精度對(duì)比如表6-7 所示。

表6 混合步態(tài)下三種算法定位誤差對(duì)比Tab.6 Comparison of positioning errors of three algorithms under mixed gait

表7 混合步態(tài)下三種算法平均定位精度對(duì)比Tab.7 Comparison of the average positioning errors of three algorithms under mixed gait

表8 高度修正后的兩種算法高度誤差對(duì)比Tab.8 Comparison of height errors of two algorithms after height correction

由圖12 和表6-7 可知，由于Openshoe 算法在跑步階段幾乎無法檢測(cè)出零速時(shí)刻，定位誤差發(fā)散更加明顯；AdaptiveOpenshoe 算法由于能夠自適應(yīng)調(diào)整零速閾值，一定程度上可以減小定位誤差的發(fā)散；最大概率零速點(diǎn)修正算法由于可以在所有步態(tài)周期內(nèi)實(shí)現(xiàn)零速點(diǎn)提取，定位精度與前兩者相比有所提高。與前兩種算法相比，所提算法平均定位性能分別提高了55.14%和39.15%。

6.3 高度修正后的導(dǎo)航定位算法驗(yàn)證與分析

在最大概率零速點(diǎn)速度誤差修正的基礎(chǔ)上，本文分別采用兩種高度修正方法進(jìn)行對(duì)比。其中，采用文獻(xiàn)[20]的高度修正算法作為對(duì)照，該方法在上下樓運(yùn)動(dòng)時(shí)融合氣壓高度計(jì)輸出進(jìn)行高度修正，在同樓層運(yùn)動(dòng)時(shí)參考前一步態(tài)高度進(jìn)行高度修正。兩種算法的導(dǎo)航定位結(jié)果對(duì)比如圖13 所示。

圖13 高度修正后的兩種算法定位結(jié)果對(duì)比Fig.13 Comparison of positioning results of two algorithms after height correction

由圖14 可知，基于氣壓高度計(jì)的高度修正算法在上下樓梯時(shí)受噪聲影響，高度隨氣壓高度計(jì)輸出波動(dòng)；本文提出的高度修正方法只在第一段下樓時(shí)段參考?xì)鈮焊叨扔?jì)輸出，在后續(xù)上下樓步態(tài)中采用基于慣性/氣壓傳感器的爬樓高度在線估計(jì)方法,每一層樓梯平臺(tái)高度均保持一致，減小了高度誤差。

圖14 高度修正后的兩種算法高度對(duì)比Fig.14 Height comparison between two algorithms after height correction

本文以最終高度誤差與樓層總高度的比值作為高度估計(jì)精度，計(jì)算公式如式(16)所示：

其中，hend為最終高度誤差；htotal為樓層總高度。

經(jīng)測(cè)量，本實(shí)驗(yàn)場(chǎng)景下兩層樓的總高度為7.7 m，相較于基于氣壓高度計(jì)的高度修正算法，所提算法的高度估計(jì)精度由7.79%降低到0.39%。

7 結(jié)論

本文提出了一種室內(nèi)場(chǎng)景下的慣性行人導(dǎo)航方法，根據(jù)廣義似然比檢測(cè)器識(shí)別零速區(qū)間，實(shí)現(xiàn)行人步態(tài)周期劃分，并在每個(gè)步態(tài)周期內(nèi)提取最大概率零速點(diǎn)以實(shí)現(xiàn)零速修正；采用長(zhǎng)短期記憶深度學(xué)習(xí)網(wǎng)絡(luò)進(jìn)行上下樓步態(tài)識(shí)別，采用基于概率的樓梯檢測(cè)方法檢測(cè)樓梯高度，通過聯(lián)合使用慣性測(cè)量單元和氣壓高度計(jì)的信息判斷行人上下樓步態(tài)中的臺(tái)階跨越情況，降低了由于氣壓高度計(jì)輸出不穩(wěn)定引入的高度誤差。實(shí)驗(yàn)結(jié)果表明本文算法能有效提升室內(nèi)場(chǎng)景行人導(dǎo)航的定位精度，具有一定的應(yīng)用價(jià)值。