一種用于足綁式行人慣性導(dǎo)航的區(qū)間搜索零速檢測器

陳 澤，潘獻(xiàn)飛，陳昶昊，吳美平

（國防科技大學(xué) 智能科學(xué)學(xué)院自動化系，長沙 410073）

行人導(dǎo)航技術(shù)具有重要的應(yīng)用價(jià)值，是當(dāng)前研究的熱點(diǎn)。GPS、UWB[1]、WIFI[2]等技術(shù)可以為用戶提供導(dǎo)航定位信息。但是這類方法需要提前對目標(biāo)場景布置，并且很容易受到外界環(huán)境干擾?；诘统杀綧IMU（Miniature Inertial Measurement Unit）的行人慣性導(dǎo)航系統(tǒng)不需要提前對室內(nèi)環(huán)境進(jìn)行測量，不易受到外部環(huán)境影響[3-5]。但是MIMU 精度低，定位誤差會隨著時(shí)間迅速累積，因此慣性行人導(dǎo)航系統(tǒng)需要通過其他約束條件對誤差進(jìn)行修正。零速信息可以對導(dǎo)航誤差進(jìn)行修正[6-8]：利用腳部運(yùn)動的零速狀態(tài)，通過卡爾曼濾波對導(dǎo)航誤差進(jìn)行估計(jì)，可以很好修正導(dǎo)航誤差。

實(shí)現(xiàn)零速修正的前提是準(zhǔn)確判斷出腳部的零速狀態(tài)，基于閾值的檢測法和基于機(jī)器學(xué)習(xí)的檢測法是目前主要的兩種方法?；陂撝档姆椒òü潭ㄩ撝捣ê妥赃m應(yīng)閾值法，固定閾值法如SHOE 檢測器[8,9]，SHOE 是目前固定閾值法中檢測性能較好的方法，然而固定閾值法在工程應(yīng)用中有較大的缺陷，需要根據(jù)實(shí)際情況調(diào)節(jié)閾值，算法魯棒性較差。自適應(yīng)閾值法主要是建立自適應(yīng)閾值調(diào)節(jié)模型，文獻(xiàn)[9]基于步態(tài)頻率和文獻(xiàn)[10]基于步態(tài)速度分別建立了對應(yīng)的自適應(yīng)閾值調(diào)節(jié)模型，雖然文中的效果都很好，然而實(shí)際中通用模型很難建立。機(jī)器學(xué)習(xí)的檢測方法不需要閾值，文獻(xiàn)[11]通過貝葉斯估計(jì)判斷零速狀態(tài)，設(shè)計(jì)了SHOE的自適應(yīng)版本：Adaptive-SHOE，在正常行走下效果較好，跑步等運(yùn)動并沒進(jìn)行驗(yàn)證。文獻(xiàn)[12]通過深度學(xué)習(xí)的方法，首先設(shè)計(jì)了運(yùn)動分類器，針對不同運(yùn)動，通過深度學(xué)習(xí)模型直接判斷慣性數(shù)據(jù)是否為零速狀態(tài)，取得了較好效果，深度學(xué)習(xí)的方法需要大量數(shù)據(jù)進(jìn)行訓(xùn)練，前期數(shù)據(jù)準(zhǔn)備工作量大，并且深度學(xué)習(xí)會出現(xiàn)過擬合的問題，該模型在其他數(shù)據(jù)上的可用性有待驗(yàn)證。

本文提出了一種基于區(qū)間搜索的自適應(yīng)零速檢測方法。利用腳部運(yùn)動中零速區(qū)間慣性數(shù)據(jù)分布的特征，對數(shù)據(jù)差異化放大。以數(shù)據(jù)收斂性為依據(jù)，搜索到最優(yōu)的零速區(qū)間。該方法立足慣性數(shù)據(jù)本身的特點(diǎn)，避免了閾值調(diào)節(jié)和運(yùn)動分類，可以準(zhǔn)確找到零速區(qū)間，提高了零速修正算法在工程應(yīng)用上的魯棒性。

1 零速修正算法分析

1.1 零速修正算法框架

如圖1 所示，零速修正算法框架。MIMU 測量得到加速度和角速度，通過捷聯(lián)慣導(dǎo)算法解算得到位置、速度、姿態(tài)信息，通過零速檢測器檢測到零速狀態(tài)。利用檢測到的零速狀態(tài)，通過卡爾曼濾波方法對捷聯(lián)慣導(dǎo)算法計(jì)算的位置、速度、姿態(tài)進(jìn)行修正。

圖1 零速修正算法框架Fig.1 The framework of ZUPT

零速修正算法中，捷聯(lián)慣導(dǎo)算法和卡爾曼濾波算法是很成熟的算法，只要零速檢測正確，就能很好的修正導(dǎo)航誤差。因此零速檢測器的性能是決定導(dǎo)航精度的主要因素。

1.2 腳部運(yùn)動分析

腳部運(yùn)動是一個(gè)不斷“運(yùn)動-停止”的周期性運(yùn)動，如圖2 所示，在行走運(yùn)動下，腳部運(yùn)動周期主要分為步態(tài)支撐期和步態(tài)擺動期，步態(tài)支撐期就是“停止”階段，即零速區(qū)間[13]。

圖2 行走步態(tài)Fig.2 The gait of walking

將腳部運(yùn)動周期擴(kuò)展為“運(yùn)動-停止-運(yùn)動”，以此作為一個(gè)搜索單元?！巴Ｖ埂辈糠峙c“運(yùn)動”部分在慣性數(shù)據(jù)上有著顯著差別。在“停止”部分中，MIMU 測量的加速度值與重力值近似，角速度值近似為零；在“運(yùn)動”部分中，MIMU 測量的加速度值與重力值相差較大并且波動較大，角速度值不為零且波動較大：

式中，SC是一個(gè)搜索單元，Ps是“停止”部分，Pm1和Pm2是兩個(gè)“運(yùn)動”部分，M A和MG分別表示在Ps中所有測量的加速度和角速度的平均值。Pm1、Ps、Pm2是有序分布的，即在一個(gè)搜索單元中，中間部分是零速區(qū)間，兩端部分是非零速區(qū)間。根據(jù)慣性數(shù)據(jù)在搜索單元內(nèi)中“靜止”和“運(yùn)動”部分不同的分布特性，可以將零速區(qū)間從中分離出來。

2 區(qū)間搜索零速檢測器

區(qū)間搜索零速檢測器算法流程如圖3 所示，首先利用角速度確定基本的搜索單元，同時(shí)將加速度通過非線性函數(shù)映射到搜索空間中，在搜索空間中分別通過粗搜索和精搜索確定一個(gè)搜索單元中的零速區(qū)間。區(qū)間搜索法包括搜索區(qū)間確定、搜索空間映射、零速區(qū)間搜索三個(gè)部分。

圖3 算法流程圖Fig.3 The flow diagram of the algorithm

2.1 搜索單元確定

“運(yùn)動-停止-運(yùn)動”的基本搜索單元在腳部運(yùn)動中是一個(gè)步態(tài)周期。采用傳統(tǒng)的滑動窗口平均方法，對角速度進(jìn)行濾波處理，通過波峰確定一個(gè)搜索單元。

首先計(jì)算出MIMU 測量得到的角速度幅值ωk：

式中，ω k為第k時(shí)刻的角速度幅值，分別是第k時(shí)刻的陀螺儀的角速度測量值。

采用滑動窗口平均的方法對ωk進(jìn)行濾波：

式中，w是滑動窗口的大小，w取值為MIMU 采樣頻率的一半，是滑動平均后的角速度幅值。

如圖4 所示，綠色線條為原始角速度幅值，藍(lán)色線條為滑動平均濾波后的角速度信號。藍(lán)線上紅色峰值點(diǎn)對應(yīng)到原始角速度幅值上，兩個(gè)相鄰峰值點(diǎn)之間就是一個(gè)搜索單元。

圖4 搜索單元Fig.4 The search unit

2.2 搜索空間映射

在一個(gè)搜索單元中，區(qū)分零速區(qū)間和非零速區(qū)間要將兩者的差異進(jìn)行放大。本文設(shè)計(jì)了一種非線性函數(shù)，將搜索單元中的加速度幅值映射到搜索空間中。在搜索空間中，零速區(qū)間內(nèi)的數(shù)據(jù)保持平穩(wěn)，非零速區(qū)間內(nèi)的數(shù)據(jù)會與零速區(qū)間內(nèi)的數(shù)據(jù)產(chǎn)生顯著差異。

假設(shè)g是當(dāng)?shù)刂亓铀俣确?，ackc是第k時(shí)刻的合加速度幅值，ratiok是acck與g的比值：

從式(4)可以看出，在零速區(qū)間內(nèi)ratiok應(yīng)當(dāng)接近于1。因此需要一種非線性函數(shù)，使零速區(qū)間和非零速區(qū)間的差異放大。采用非線性函數(shù)為：

式(5)中，f′（x）是f(x)的導(dǎo)函數(shù)。當(dāng)x∈(0,1)時(shí)，f′（x）恒小于0；當(dāng)x∈ (1, ∞)時(shí)，f′（x）恒大于0。因此，x=1是f(x)的最小值點(diǎn)。當(dāng)x離1 越遠(yuǎn)時(shí)，f(x)的值會迅速增大。這就意味著當(dāng)acck在零速區(qū)間時(shí)，f（ratiok）會近似為1，并且波動很小；當(dāng)acck在非零速區(qū)間時(shí)，f（ratiok）會遠(yuǎn)大于1，并且波動很大。

如圖5 所示，選取“走”和“跑”兩種典型的腳部運(yùn)動下的加速度數(shù)據(jù)。圖5(a)是在“走”的情況下，加速度幅值在一個(gè)搜索單元的分布，將其映射到搜索空間中，圖5(b)是零速區(qū)間在搜索空間中的結(jié)果，可以看出零速區(qū)間內(nèi)的數(shù)據(jù)平穩(wěn)且基本收斂在1附近。圖5(c)是“跑”的情況下，加速度幅值在一個(gè)搜索單元內(nèi)的分布，將其映射到搜索空間中，圖5(d)是零速區(qū)間在搜索空間中的結(jié)果，可以看出零速區(qū)間內(nèi)的數(shù)據(jù)平穩(wěn)且基本收斂在1 附近。由于“跑”運(yùn)動劇烈，加速度變化劇烈，導(dǎo)致在搜索空間中零速區(qū)間和非零速區(qū)間差異性極大。根據(jù)以上分析以及搜索空間中數(shù)據(jù)的收斂性可以搜索到零速區(qū)間。

圖5 走和跑情況下的加速度幅值的分布Fig.5 The distribution of the acceleration magnitude in the case of walking and running

2.3 零速區(qū)間搜索

本文設(shè)計(jì)一種分層迭代的方式，在搜索空間中對零速區(qū)間進(jìn)行搜索。首先采用粗搜索的方式，確定零速區(qū)間的近似范圍。然后采用精搜索的方式，以數(shù)據(jù)收斂性為基準(zhǔn)，對零速區(qū)間進(jìn)行高精度的確定。通過以上兩步，搜索到最優(yōu)的零速區(qū)間。

粗搜索的核心是剔除那些絕對不可能在零速區(qū)間的點(diǎn)，非零速點(diǎn)的加速度應(yīng)當(dāng)與重力值之間有較大的差異。首先需要對MIMU 靜態(tài)情況下的加速度幅值進(jìn)行統(tǒng)計(jì)，獲取初始靜態(tài)對準(zhǔn)階段的MIMU 慣導(dǎo)數(shù)據(jù)：

式中，是靜態(tài)情況下k時(shí)刻的加速度幅值，biask是k時(shí)刻的加速度幅值與重力值的偏差，PMax是在靜態(tài)下標(biāo)定的最大加速度測量誤差百分比，與所用MIMU 性能相關(guān)。在粗搜索中，放寬測量誤差，將其擴(kuò)大一個(gè)數(shù)量級，得到粗搜索的基準(zhǔn)值VMax：

在搜索空間中的點(diǎn)只要大于VMax就剔除，小于等于VMax的點(diǎn)就留下來進(jìn)行下一步的精搜索。

圖6(a)對應(yīng)圖5(a)的數(shù)據(jù)，對“走”的搜索單元經(jīng)過粗搜索之后，可以得到三個(gè)區(qū)間。通過圖5(a)可知，精確的零速區(qū)間在interval2 中，在精搜索中對其進(jìn)行剔除，從而得到精確的零速區(qū)間。采用同樣的方法對“跑”的情況進(jìn)行處理，圖6(b)對應(yīng)圖5(c)的數(shù)據(jù)，粗搜索得到兩個(gè)區(qū)間，通過圖5(c)所標(biāo)的零速區(qū)間，精確的零速區(qū)間在interval2 中，在精搜索中對其進(jìn)行剔除，從而得到精確的零速區(qū)間。

精搜索的核心是要找到最符合零速區(qū)間數(shù)據(jù)分布的最優(yōu)區(qū)間。在搜索空間中，零速區(qū)間的數(shù)學(xué)期望理論值為1?；谶@一前提，在搜索空間中如果存在一個(gè)區(qū)間，該區(qū)間的數(shù)學(xué)期望與1 近似，同時(shí)如果舍去區(qū)間中任意的點(diǎn)對區(qū)間的數(shù)學(xué)期望影響不大，則可以認(rèn)為該區(qū)間是零速區(qū)間。根據(jù)運(yùn)動數(shù)據(jù)特點(diǎn)，非零速點(diǎn)應(yīng)當(dāng)在區(qū)間的兩端，在搜索空間中比較兩端點(diǎn)的大小，將值大的點(diǎn)作為可能的非零速點(diǎn)進(jìn)行剔除。基于以上的模型，剔除非零速點(diǎn)前區(qū)間的數(shù)學(xué)期望Ep，剔除非零速點(diǎn)后區(qū)間的數(shù)學(xué)期望為Ec，再將剔除前后數(shù)學(xué)期望的比值通過f(x) 映射到搜索空間中。經(jīng)過多次迭代f（E c/g）和f（E cEp）應(yīng)當(dāng)收斂在1 附近：

式中，ε是一個(gè)小值。

精搜索結(jié)果如圖7 所示。其中圖7(a)和7(b)分別是f（E c/g）和f（E cEp）。在“走”的情況下零速區(qū)間搜索收斂情況，可以看出基本都收斂于1。圖7(c)中綠色是最終搜索到的零速區(qū)間。圖7(d)和7(e)是“跑”情況下的數(shù)據(jù)收斂情況，也同樣最終基本收斂于1。圖7(f)中綠色是最終搜索到的零速區(qū)間。

圖6 粗搜索結(jié)果Fig.6 The result of rough search

圖7 精搜索結(jié)果Fig.7 The result of fine search

3 實(shí)驗(yàn)驗(yàn)證與分析

實(shí)驗(yàn)所用MIMU 如圖8 所示，MIMU 為自主設(shè)計(jì)加工，加速度計(jì)零偏穩(wěn)定性約0.05 mg，陀螺零偏穩(wěn)定性約10 °/h，采樣頻率400 Hz。為了驗(yàn)證算法的有效性，采用固定閾值法SHOE 和自適應(yīng)Adaptive-SHOE進(jìn)行比較，對一般性的行走和復(fù)雜情況下走、跑等混合運(yùn)動進(jìn)行實(shí)驗(yàn)。

圖8 實(shí)驗(yàn)所用MIMUFig.8 The MIMU used in the experiments

3.1 實(shí)驗(yàn)一：正常行走

2020年9月23日，在戶外開闊場地隨機(jī)行走，實(shí)驗(yàn)人員攜帶u-blox-NEO 模塊獲取GPS 信號，通過事后差分獲得基準(zhǔn)軌跡，差分精度可以在10 cm 以內(nèi)。運(yùn)動的起點(diǎn)與終點(diǎn)為同一點(diǎn)。行走時(shí)間約680.34 s，行走距離約503.65 m。

如圖9 所示，黑色線條是差分GPS 獲得的基準(zhǔn)軌跡，綠色線條是區(qū)間搜索法解算的軌跡，紅色線條是SHOE-1e5(1e5是SHOE 的閾值，無單位)解算的軌跡，藍(lán)色線條是Adaptive-SHOE 解算的軌跡，三種方法解算的結(jié)果基本與差分GPS 軌跡一致。SHOE 的結(jié)果是需要經(jīng)過參數(shù)調(diào)節(jié)后才能得到，如圖10 所示，如果選取的閾值不當(dāng)，導(dǎo)航誤差就不能被很好修正，這就是固定閾值法在工程應(yīng)用中的缺陷。

圖9 實(shí)驗(yàn)1 解算軌跡Fig.9 The trajectory calculated in the experiment 1

統(tǒng)計(jì)結(jié)果如表1 所示，實(shí)驗(yàn)中起點(diǎn)和終點(diǎn)為同一點(diǎn)，SHOE-1e5、Adaptive-SHOE 和區(qū)間搜索法對應(yīng)的起點(diǎn)/終點(diǎn)誤差分別為0.65 m、1.01 m 和0.72 m。通過與差分GPS 解算的結(jié)果比較，計(jì)算得到對應(yīng)的RMS值分別為1.76 m、2.23 m 和1.39 m。區(qū)間搜索法在導(dǎo)航解算精度上與SHOE-1e5和Adaptive-SHOE 的性能相當(dāng)。

圖10 不同閾值解算軌跡Fig.10 The trajectory calculated by different thresholds

表1 實(shí)驗(yàn)一結(jié)果Tab.1 The result of the experiment 1

表1 中的零速數(shù)量表示判定為零速狀態(tài)的IMU 幀數(shù)。定義零速匹配度，例如：SHOE-1e5檢測的零速數(shù)量為177501 幀，區(qū)間搜索法檢測的零速數(shù)量為179296幀，兩種方法檢測到相同的零速數(shù)量為173134 幀，則其匹配度約為97.54%(173134/177501)。如表2 所示，區(qū)間搜索法同Adaptive-SHOE 的匹配度為98.51%。

表2 實(shí)驗(yàn)一匹配度Tab.2 The matching degree of the experiment 1

實(shí)驗(yàn)一中，在實(shí)驗(yàn)者正常行走情況下，區(qū)間搜索法的性能與SHOE-1e5和Adaptive-SHOE 相當(dāng)。區(qū)間搜索法和Adaptive-SHOE 可以自適應(yīng)找到零速區(qū)間，而SHOE 需要對閾值進(jìn)行調(diào)節(jié)從而找到最合適的閾值。實(shí)驗(yàn)一表明，在正常行走的情況下，區(qū)間搜索法可以在不調(diào)節(jié)參數(shù)的情況下實(shí)現(xiàn)零速的準(zhǔn)確檢測。

3.2 實(shí)驗(yàn)二：復(fù)雜運(yùn)動

2020年9月3日，在室內(nèi)走廊中沿著同一條路線進(jìn)行多種運(yùn)動綜合實(shí)驗(yàn)。運(yùn)動包括走、跑、跳。運(yùn)動時(shí)間約1020.51 s，運(yùn)動距離約1247.21 m，運(yùn)動的起點(diǎn)和終點(diǎn)為同一點(diǎn)。

如圖11 所示，藍(lán)色線條是SHOE-8.6e4解算的軌跡，紅線是區(qū)間搜索法解算的軌跡，綠色圓點(diǎn)是運(yùn)動的起點(diǎn)。圖12 中的藍(lán)色線條是Adaptive-SHOE 解算的軌跡，軌跡直接發(fā)散，Adaptive-SHOE 在該實(shí)驗(yàn)中失效，Adaptive-SHOE 需要另外設(shè)計(jì)運(yùn)動分類器和對應(yīng)的零速判斷模型，否則無法應(yīng)對復(fù)雜運(yùn)動的情況。表3 中雖然SHOE-8.6e4和區(qū)間搜索法的起點(diǎn)/終點(diǎn)誤差相近，實(shí)際實(shí)驗(yàn)中是沿著同一條路線反復(fù)運(yùn)動，從圖11 中可以看出，閾值法的軌跡重合性不如無閾值法好。

如圖13，采用SHOE-1e4、SHOE-5e4和SHOE-1e5解算的軌跡，雖然SHOE-1e5在實(shí)驗(yàn)一中解算的結(jié)果較好，從圖13 可以直觀看出其在實(shí)驗(yàn)二中解算結(jié)果較差。對于固定閾值法，無法確定一個(gè)通用閾值應(yīng)對所有情況，因此需要根據(jù)不同情況設(shè)定閾值，算法的通用性較差。

圖11 實(shí)驗(yàn)2 解算軌跡Fig.11 The trajectory calculated in the experiment 2

圖12 Adaptive-SHOE 解算軌跡Fig.12 The trajectory calculated by Adaptive-SHOE

表3 實(shí)驗(yàn)二結(jié)果Tab.3 The result of the experiment 2

在表3 中，SHOE-8.6e4和區(qū)間搜索法測量得到的零速區(qū)間數(shù)量相近，但實(shí)際上兩種方法檢測的每個(gè)零速區(qū)間不同。由于多種運(yùn)動混合在一起，閾值法要找到適配所有運(yùn)動的閾值是很難的。如圖14 紅圈中，為了適應(yīng)跑步狀態(tài)，行走狀態(tài)下會出現(xiàn)誤判，缺失零速區(qū)間。SHOE-8.6e4和區(qū)間搜索法檢測到相同的零速數(shù)量為193226 幀，匹配度約為89.12%。實(shí)驗(yàn)二中，區(qū)間搜索法在復(fù)雜運(yùn)動情況下仍然具有較好的性能，并且不需要對參數(shù)進(jìn)行調(diào)節(jié)，算法的魯棒性較好。

圖13 不同閾值解算軌跡Fig.13 The trajectory calculated by different thresholds

圖14 零速區(qū)間檢測結(jié)果Fig.14 The result of the zero-velocity interval

4 結(jié) 論

本文提出了一種區(qū)間搜索零速檢測算法。針對腳部運(yùn)動特點(diǎn)，將“運(yùn)動-停止-運(yùn)動”作為搜索單元。在搜索單元中，基于慣性數(shù)據(jù)區(qū)分的特點(diǎn)，設(shè)計(jì)了一種數(shù)據(jù)轉(zhuǎn)換的方法，將加速度數(shù)據(jù)映射到搜索空間中進(jìn)行分析和處理。在搜索空間中，通過粗搜索和精搜索分層迭代的方式，找到最優(yōu)的零速區(qū)間。該算法可以自適應(yīng)地對零速區(qū)間進(jìn)行搜索，不需要對不同情況進(jìn)行閾值設(shè)置。

通過實(shí)驗(yàn)對算法進(jìn)行了測試，以固定閾值法SHOE 和自適應(yīng)檢測法Adaptive-SHOE 作為參考。區(qū)間搜索零速檢測器可以達(dá)到SHOE 在最優(yōu)閾值狀態(tài)下檢測效果。特別是在多種運(yùn)動混合的情況下，Adaptive-SHOE 出現(xiàn)失效，但是區(qū)間搜索法仍然能夠正常工作，具有較好的運(yùn)動適應(yīng)性。