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

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

0 引言

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

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

1 系統(tǒng)算法原理

1.1 基于人體運動學模型的步態(tài)檢測算法

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

由圖1和圖2可看出，三軸加速度計與陀螺儀數(shù)據(jù)具有明顯的周期性?；谌梭w正常行走時的步態(tài)變化規(guī)律，可以對應(yīng)到三軸加速度計和陀螺儀的數(shù)據(jù)變化規(guī)律。經(jīng)過分析得知，足部與地面實際接觸時，陀螺三軸值和加速度計三軸值都較為平穩(wěn)，且接近于零，認為這段時間為人體運動時的 “零速時刻”，即此時的速度為零。

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

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

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

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

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

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

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

在檢測到零速區(qū)間時，認為這段時間人體運動的速度為零，位置保持不變，可以將慣導(dǎo)解算的速度信息與零速作差作為量測量，同時將慣導(dǎo)解算的位置信息與上一時刻慣導(dǎo)解算的位置信息作差作為量測量，所構(gòu)成的系統(tǒng)量測方程為：

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

2 系統(tǒng)硬件架構(gòu)

2.1 硬件平臺設(shè)計

本文所設(shè)計的可穿戴式行人導(dǎo)航系統(tǒng)硬件部分主要由導(dǎo)航計算機樹莓派3B、MTI-700 MEMS慣性傳感器、手機控制端、遠程監(jiān)控端和電源模塊構(gòu)成。硬件部分各模塊介紹如下：

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

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

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

②板載BCM43143 WiFi模塊。行人導(dǎo)航設(shè)備可以通過該WiFi模塊連接網(wǎng)絡(luò)，為手機控制端的控制功能，以及遠程監(jiān)控端的網(wǎng)絡(luò)通信提供了極大便利性。

③板載低功耗（BLE）藍牙模塊?？梢酝ㄟ^藍牙模塊進行手機app與行人導(dǎo)航設(shè)備之間的藍牙通信，方便后續(xù)工作的可擴展性。

④1GB RAM。大內(nèi)存保證了程序的穩(wěn)定運行，保證了行人導(dǎo)航程序的長時穩(wěn)定運行。

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

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

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

⑧微型SD端口。用于下載操作系統(tǒng)以及存儲數(shù)據(jù)。

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

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

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

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

2.2 具體使用流程

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

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

3 系統(tǒng)測試與驗證

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

圖9為部分測試的二維/三維軌跡圖，表1為行人導(dǎo)航系統(tǒng)測試結(jié)果。

表1 行人導(dǎo)航系統(tǒng)測試結(jié)果Table 1 Test results of pedestrian navigation system

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

4 結(jié)論

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