基于人體運動學輔助的可穿戴式行人導航系統

張 苗，許建新，黃 欣，殷德全，熊 智

（南京航空航天大學，南京211106）

0 引言

目前，較為成熟的個人移動定位服務系統主要依賴于衛星導航與無線通信定位技術。而當衛星信號不良時，系統面臨癱瘓的風險，無線通信信號易受到干擾，系統的穩定性與可靠性不強。尤其是在特種行業領域，如消防救援、野外搜救、井下勘探等復雜應用環境下，GPS以及無線通信信號往往失效不可用，給救援任務和特種作業帶來了極大的困難，甚至導致巨大的財產損失和人員傷亡。同時，特種作業人員在執行任務時，可能出現匍匐、跳躍、跑步等多種復雜運動模態，與行人正常的行走運動模態有較大差別，對傳感器的測量范圍、抗高溫、抗震動等能力，以及對算法的適應性具有較高的要求。

本文結合特種行業的個人定位要求，主要研究了基于人體運動學輔助的行人導航定位技術，并研制了可穿戴式行人導航系統。針對行人的多種運動模態進行了測試驗證，同時該系統可在上位機界面實時顯示行人的運動狀態和二維軌跡信息，便于外部人員對行人的實時監控和指揮。

1 系統算法原理

1.1 基于人體運動學模型的步態檢測算法

將可穿戴式MEMS慣性傳感測量元件固定于人體足部腳面位置，通過檢測運動時人體足部輸出的加速度信息和角速度信息來判斷人體處于何種運動模態，并使用判斷條件檢測出對應的零速時刻，進行速度和姿態的更新。圖1和圖2分別為人體正常行走時IMU的三軸加速度計和三軸陀螺儀原始數據輸出。

由圖1和圖2可看出，三軸加速度計與陀螺儀數據具有明顯的周期性。基于人體正常行走時的步態變化規律，可以對應到三軸加速度計和陀螺儀的數據變化規律。經過分析得知，足部與地面實際接觸時，陀螺三軸值和加速度計三軸值都較為平穩，且接近于零，認為這段時間為人體運動時的 “零速時刻”，即此時的速度為零。

由于零速檢測的準確性直接影響著導航精度，因此選擇合適的零速檢測方法至關重要。常用的零速檢測條件包括加速度計三軸模值與三軸標準差、陀螺儀三軸模值與三軸標準差。檢測條件具體如下：

綜合利用三軸加速度計和陀螺儀的輸出信息特征，本文設計了采用加速度計三軸模值、加速度計三軸標準差與陀螺儀三軸模值組合判斷零速時的判別方法。

a1和σ1是加速度計三軸模值與標準差，g1和σ2是陀螺儀三軸模值與標準差，當a1小于閾值εa1、σ1小于閾值εa2且g1小于閾值εω1、σ2小于閾值εω2時，零速檢測標志置為1，即成功檢測到零速區間。 其中，閾值 εa1、εa2與 εω1、εω2是根據多組實驗的加速度計三軸模值曲線和三軸標準差曲線、陀螺儀三軸模值曲線和三軸標準差曲線與對應周期的人體步行零速區間對比分析得到。

1.2 基于Kalman濾波的零速修正方法

Kalman濾波是由Kalman于1960年提出的一種濾波方法，其優點為高效率、可實時估計且具有遞推性，該方法由前一時刻的估計值結合當前時刻的測量值遞推得到當前時刻的估計值。本文將慣性傳感器固定于足部腳面上，當腳與地面實際接觸時速度為零，位置不變，可以將速度誤差、位置誤差作為觀測量，經Kalman濾波估計慣導系統誤差并進行反饋校正，從而抑制慣導系統的誤差，提高導航定位精度。

基于MEMS的慣性導航系統雖是連續系統，但只需線性化就可以采用離散Kalman濾波算法進行誤差狀態估計。Kalman濾波器狀態量為18維狀態變量，具體如下：

其中，φe、φn、φu為慣導平臺誤差角，δVe、δVn、δVu為速度誤差，δL、δλ、δh為位置誤差，εbx、εby、εbz為陀螺三軸零偏，εrx、εry、εrz為陀螺1階Markov過程隨機誤差，▽rx、▽ry、▽rz為加速度計1階Markov過程隨機誤差。

在檢測到零速區間時，認為這段時間人體運動的速度為零，位置保持不變，可以將慣導解算的速度信息與零速作差作為量測量，同時將慣導解算的位置信息與上一時刻慣導解算的位置信息作差作為量測量，所構成的系統量測方程為：

式中，[LINSλINShINS]為慣導解算的位置信息；[LZUPTλZUPThZUPT]為上一時刻慣導解算的位置信息；[veINSvnINSvuINS]為慣導解算的速度信息；[veZUPTvnZUPTvuZUPT]為速度零速，即[0 0 0]（單位：m/s）。 式中，量測矩陣 H（t）與量測噪聲矩陣NZUPT（t）分別為：

2 系統硬件架構

2.1 硬件平臺設計

本文所設計的可穿戴式行人導航系統硬件部分主要由導航計算機樹莓派3B、MTI-700 MEMS慣性傳感器、手機控制端、遠程監控端和電源模塊構成。硬件部分各模塊介紹如下：

1）樹莓派3B：本文中所使用的導航計算機是樹莓派3B，樹莓派（Raspberry Pi）是一款基于Linux的單板機電腦，由英國的樹莓派基金會所開發，樹莓派3B實物如3圖所示。

樹莓派3B硬件平臺的各項參數如下：

①1.2GHz四核博通（Broadcom）BCM2837 64位ARMv8處理器。4核處理器滿足了多線程行人導航設備對處理器的計算需求，可以大幅提高程序的運行效率，保證了算法的實時精確解算。

②板載BCM43143 WiFi模塊。行人導航設備可以通過該WiFi模塊連接網絡，為手機控制端的控制功能，以及遠程監控端的網絡通信提供了極大便利性。

③板載低功耗（BLE）藍牙模塊。可以通過藍牙模塊進行手機app與行人導航設備之間的藍牙通信，方便后續工作的可擴展性。

④1GB RAM。大內存保證了程序的穩定運行，保證了行人導航程序的長時穩定運行。

⑤4個USB端口。該行人導航設備的處理器與傳感器之間通過USB串口進行通信，方便傳感器的擴展。

⑥40針擴展GPIO。引腳可以根據特殊需求進行配置，提供了更加豐富的接口以備使用。

⑦HDMI和RCA視頻輸出。外接顯示器，可以在處理器上直接調試導航設備程序，為野外工作提供了便利。

⑧微型SD端口。用于下載操作系統以及存儲數據。

2）MTI-700 MEMS慣性傳感器：MTI-700慣性傳感器主要是向行人導航系統提供IMU數據，是整個行人導航解算中最重要的傳感模塊，其精度直接決定了行人導航系統的性能，如圖4所示。

MTI-700由三軸加速度計、三軸陀螺儀、三軸磁強計、16位的AD、內部DSP、非易失性存儲器和串口收發器組成。各個傳感模塊的頻率可以根據需求進行配置，以滿足導航解算對數據采集頻率的需要。MTI-700系統主要功能是實時采集行人的運動信息，并將采集到的數據傳輸給處理器進行解算。

3）手機控制端：手機控制端由智能手機構成，智能手機通過Vnc Server登錄樹莓派3B的界面系統，通過智能手機即可控制導航程序的開始和終止，同時行人可以通過智能手機實時觀察到自己的位置及運動信息。手機登錄樹莓派界面系統如圖5所示。

4）遠程監控端：本文研究的遠程監控端是通過Qt軟件開發平臺開發的實時人機交付操作系統，該操作系統通過TCP網絡與行人導航終端進行網絡通信，實時獲取行人的運動和位置信息。同時，該界面還可以實時顯示行人的運動軌跡，并將運動信息保存下來，以支持事后軌跡回放和數據分析。行人在進行特種作業時，監控端可以根據需要發出特定的指令，以保證作業的順利完成。遠程監控界面如圖6所示。

2.2 具體使用流程

本文研制的基于人體運動學輔助的可穿戴式行人導航系統具體使用流程如圖7所示。

首先，用戶使用手機通過Vnc Server登錄處理器界面，同時在手機控制端啟動行人導航程序；用戶在原地靜止約5s時間進行初始對準；然后用戶可以開始運動，足部固定的傳感器可以實時采集人體運動數據，并實時發送到處理器進行導航信息解算；用戶通過手機控制端可以看到自己的行走軌跡，遠程監控端也可以實時顯示出行人的運動軌跡；用戶可以通過手機控制端終止行人導航程序，終止行程。用戶的運動信息被保存下來，以支持事后軌跡回放和數據分析。

3 系統測試與驗證

為驗證本文所設計系統的可靠性和適應性，測試人員穿戴該系統進行了多場景測試，測試場地有操場、足球場、籃球場、辦公樓等。測試人員穿戴效果如圖8所示。

圖9為部分測試的二維/三維軌跡圖，表1為行人導航系統測試結果。

表1 行人導航系統測試結果Table 1 Test results of pedestrian navigation system

在實際測試過程中，測試人員運動較為隨意，不受行人導航設備的影響，行走的路線涉及直線、多邊形、半圓、上下樓梯等多種場景。在操場繞行3圈后，行走誤差僅為7m；在上下樓梯1圈后，誤差僅為0.17m。同時，高度方向解算較為精確，可以辨識出樓層的變化。由以上可知，本文研制的行人導航系統在15min運動時間內可保持高精度定位，行走路線長度在1.2km內定位精度保持不變，行人導航系統的誤差平均在路線總長度的1.1%左右，最大不超過行走距離的1.7%，行人導航的精度不受具體行走路線復雜程度的限制。

4 結論

本文首先基于人體運動學原理研究了步態檢測算法，并引入基于Kalman濾波的零速修正輔助算法，估計慣導誤差并進行反饋校正，從而抑制慣導系統的誤差，提高導航定位精度。研制了一種集信息采集、數據傳輸、導航解算與監控顯示于一體的可穿戴式行人導航系統，體積小、佩戴便攜、使用簡易、續航時間長，可滿足行人長時、多動態、多環境下的高精度三維定位需求，具有較高的工程應用價值。