陳 澤，潘獻飛，陳昶昊，吳美平

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

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

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

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

1 零速修正算法分析

1.1 零速修正算法框架

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

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

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

1.2 腳部運動分析

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

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

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

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

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

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

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

2.1 搜索單元確定

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

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

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

采用滑動窗口平均的方法對ωk進行濾波：

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

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

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

2.2 搜索空間映射

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

假設g是當?shù)刂亓铀俣确担琣ckc是第k時刻的合加速度幅值，ratiok是acck與g的比值：

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

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

如圖5 所示，選取“走”和“跑”兩種典型的腳部運動下的加速度數(shù)據(jù)。圖5(a)是在“走”的情況下，加速度幅值在一個搜索單元的分布，將其映射到搜索空間中，圖5(b)是零速區(qū)間在搜索空間中的結果，可以看出零速區(qū)間內(nèi)的數(shù)據(jù)平穩(wěn)且基本收斂在1附近。圖5(c)是“跑”的情況下，加速度幅值在一個搜索單元內(nèi)的分布，將其映射到搜索空間中，圖5(d)是零速區(qū)間在搜索空間中的結果，可以看出零速區(qū)間內(nèi)的數(shù)據(jù)平穩(wěn)且基本收斂在1 附近。由于“跑”運動劇烈，加速度變化劇烈，導致在搜索空間中零速區(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ū)間搜索

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

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

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

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

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

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

式中，ε是一個小值。

精搜索結果如圖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 粗搜索結果Fig.6 The result of rough search

圖7 精搜索結果Fig.7 The result of fine search

3 實驗驗證與分析

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

圖8 實驗所用MIMUFig.8 The MIMU used in the experiments

3.1 實驗一：正常行走

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

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

圖9 實驗1 解算軌跡Fig.9 The trajectory calculated in the experiment 1

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

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

表1 實驗一結果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 實驗一匹配度Tab.2 The matching degree of the experiment 1

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

3.2 實驗二：復雜運動

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

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

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

圖11 實驗2 解算軌跡Fig.11 The trajectory calculated in the experiment 2

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

表3 實驗二結果Tab.3 The result of the experiment 2

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

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

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

4 結 論

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

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