超寬帶矢量測距與慣性傳感組合的精準步長計算算法

劉 宇，張 泉，路永樂，崔 巍，邸 克，郭俊啟，楊 勇,2

(1. 重慶郵電大學 光電工程學院，重慶 400065；2. 中國電子科技集團公司第二十六研究所，重慶 400060)

0 引 言

隨著基于位置服務應用的快速發(fā)展以及微電子機械系統(tǒng)技術的逐漸成熟, 基于慣性傳感器的行人慣性導航因其適用范圍廣、不依賴外部信息即可實現(xiàn)自主導航而成為研究熱點[1]。

慣性定位技術多采用行人航跡推算算法(pedestrian dead reckoning, PDR)[2]， PDR算法的優(yōu)化主要集中在航向[3]、步長估計[4]以及行為識別[5]3個方面。步長信息在人的步態(tài)分析與康復[6]、行為監(jiān)測[7]或步行導航[8]等方面均有應用。步長估計是PDR算法中的重要參數(shù)，通過建立合適的步長估計模型，選擇合適的特征參數(shù)，可以實現(xiàn)步長估計。文獻[9]提出基于反向傳播算法(backpropagation algorithm, BP)神經網絡來訓練步長模型，利用每步加速度最大值、最小值和步頻等參數(shù)計算步長，有效提升步長精度。但是該算法的魯棒性較差，當行人行走包含多種運動行為時，算法的精度下降嚴重。文獻[10]提出融合多節(jié)點姿態(tài)信息，結合行人雙腿長度信息，可以準確計算步長。但是該算法要求傳感器各節(jié)點時間高度同步，對硬件要求較高，而且需要根據(jù)實際情況從外部更改模型參數(shù)，實用性不高。文獻[11]將IMU和超聲波模塊固定于行人的左腿和右腿，利用零速檢測原理和超聲波定向測距來實現(xiàn)足間距離測量，進而輔助行人慣性導航。但是超聲波測距容易受到環(huán)境影響，如溫度、風速等其他障礙物干擾，都會影響步長的測量。

超寬帶(ultra wide band,UWB)具有功耗低、帶寬高、測距精度高、時間分辨能力強、抗干擾能力強等優(yōu)點，在測距定位上有眾多的應用[12-13]，理論上測距精度可以達到厘米級[14]。因此，本文提出一種超寬帶矢量測距與慣性傳感組合的精準步長計算算法。該算法主要包含4個階段：①根據(jù)行人行走時足部軌跡模型構造足間步長模型，得到步長的準確計算公式；②利用足部慣性傳感器檢測足部零速時刻，并在此時刻觸發(fā)超寬帶測距；③對超寬帶采集的數(shù)據(jù)進行信號處理，得出足間距離值；④結合零速時刻超寬帶的測距值、陀螺解算的角度和足間步長模型公式，實現(xiàn)對行人步長的測量。

1 系統(tǒng)模型

1.1 足間模型

結合行人行走時足部的運動特征，足部軌跡可大致描述為沿直線行走和沿一定角度的轉彎行走，如圖1所示。步長定義為兩腳相同部位、相鄰著地點之間的距離，跨步長定義為同一側腳跟連續(xù)兩次著地點間的直線距離，步寬是指兩腳之間的橫向距離。

圖1 行人行走足跡模型Fig.1 Pedestrian walking footprint model

根據(jù)人的行走狀態(tài)，本文由圖1構造出圖2—圖4所示的足間模型。連續(xù)足跡構成三角形，足間距離呈現(xiàn)出具有連續(xù)局部極大值的周期性行為。模型中的角度由位于足部的慣性器件陀螺解算得到，距離信息由UWB解算出。圖2—圖4中，SW表示步寬；Li-1、Li分別表示前一時刻和當前時刻兩腳之間的距離；SLleft、SLright表示左步長和右步長；SLi表示當前時刻的跨步長。

圖2 直行足間模型Fig.2 Straight walk between feet model

1.2 步長計算公式

1)當行人沿著直線行走時，步長和步寬計算公式為

SLleft=Li-1cosθ

(1)

(2)

(3)

跨步長計算公式為

SLi=SLleft+SLright

(4)

圖3 左轉足間模型Fig.3 Foot model of left turn

圖4 右轉足間模型Fig.4 Foot model of right turn

2)行人轉彎時，跨步長和步寬計算如下。

由正弦定理可知

(5)

(6)

當行人左轉時有

θ1=θ-θ2

(7)

當行人右轉時有

θ1=θ+θ2

(8)

由余弦定理可得

(9)

(1)—(9)式中：θ表示左腳相鄰落地點之間的夾角；θ1表示同一只腳前后落地時與另一只腳之間的夾角；θ2表示右腳相鄰落地點之間的夾角。通過(1)—(9)式，可以計算出人在不同行走狀態(tài)下的步長和步寬。精準測量兩腳之間的距離是計算步長的關鍵所在，本文選擇測距精度高的UWB進行足間距離的測量。

2 組合步長計算算法設計

2.1 零速檢測

以ωx、ωy、ωz分別表示三軸角速度信息，ax、ay、az分別表示加速度信息，選擇連續(xù)的7個步態(tài)參數(shù)檢測波峰和波谷。具體方法如下。

(10)

(11)

(10)—(11)式中，St-3、St-2、St-1、St、St+1、St+2、St+3分別為S運動參數(shù)在t-3時刻、t-2時刻、t-1時刻、t時刻、t+1時刻、t+2時刻、t+3時刻的數(shù)值。當ps=1或vs=1時，代表運動參數(shù)S的波峰或者波谷檢測成功，否則為檢測失敗。

本文采集了平走過程足部傳感器的三軸加速度計和三軸陀螺儀數(shù)據(jù)，通過觀察發(fā)現(xiàn)ωx、ωy、ωz和az的數(shù)據(jù)出現(xiàn)周期性變化，如圖5所示。

進一步分析可得ωx和az的變化規(guī)律與實際的步態(tài)周期吻合度較高，能夠進行步態(tài)檢測，如圖6所示。圖6中，黑色圓圈標注的Z軸加速度波谷是腳后跟著地產生的，因此檢測腳后跟著地時刻即判斷Z軸加速度波谷出現(xiàn)時刻。每一個步態(tài)周期中，Z軸角速度先出現(xiàn)小波峰，后出現(xiàn)大波峰，Z軸加速度的波谷在兩個波峰之間。通過先檢測Z軸角速度的小波峰，再依次檢測Z軸加速度波谷和Z軸角速度的大波峰，可以對平走過程中的腳著地動作進行判斷。具體判斷方法如下。

(12)

(12)式中：ω1、ω2分別是檢測角速度大小波峰的閾值；a1是檢測加速度波谷的閾值。當r=1時，表示檢測腳跟著地動作成功，否則表示檢測失敗。

行走過程中，由于腳部的運動比較劇烈，傳感器的輸出可能會出現(xiàn)偽波峰和偽波谷，容易造成峰谷的誤判。為了解決這個問題，本文添加時間閾值T對Z軸角速度的大小波峰時間間隔ΔT進行約束，在ΔT間隔內對az的波谷進行檢測。結合圖與行人實際運動的狀態(tài)可得，角速度的大小波峰到加速度波谷的時間間隔差異較小。所以，角速度大小波峰之間的時間間隔可視為一段相對恒定的時間。綜上，本文選取合適的時間閾值T對ΔT時間內出現(xiàn)的角速度偽波峰進行過濾，可以進一步對腳著地動作進行判斷。判斷方法為

flag=(ΔT>T)&r

(13)

圖5 行人平走運動足部傳感器運動特征Fig.5 Movement characteristics of foot sensors for pedestrians in horizontal walking

圖6 足部傳感器Z軸數(shù)據(jù)特征Fig.6 Z-axis data characteristics of foot sensor

當flag=1時，表示腳跟著地時刻檢測成功，否則為檢測失敗。利用慣性傳感器判斷出雙腿同時落地時刻后，采集這個時刻位于足部UWB的數(shù)據(jù)信息，利用解算的距離信息和足間模型完成步長的解算。

2.2 UWB測距

UWB多基于飛行時間進行測距，一般使用兩個異步收發(fā)機之間信號飛行時間進行節(jié)點之間的距離測量。但是該種方法要求發(fā)送設備和接收設備必須始終同步，即使較小的時延也會產生較大的誤差。對UWB采集的數(shù)據(jù)先進行信號預處理，包括背景噪聲去除、雜波濾除等；再通過匹配濾波、脈沖壓縮進行解算距離。

在實際測量中，背景噪聲對含有目標信息的回波信號有一定的干擾，去除相干背景噪聲可以降低背景噪聲產生的影響。先采集放置目標物體前環(huán)境的回波信號S1，然后采集放置目標物體后的回波信號S2，從包含目標物體的回波中去除背景噪聲，可以得到只包含目標物體的回波信號S。即

S=S2-S1

(14)

基于UWB收集的信號里存在目標物體以外的噪聲和其他障礙物反射回波，其中噪聲包括天線的耦合噪聲。本文先使用平均消除方法，從回波信號中減去信號均值，再使用漢明窗濾波，去除障礙物的直接反射回波。

當濾波器的頻率響應函數(shù)是輸入信號頻率響應函數(shù)的復共軛時，濾波器的輸出具有最大信噪比，接收機在接收雷達回波過程中使用匹配濾波器抑制噪聲信息，通過匹配濾波器對UWB回波數(shù)據(jù)進行處理，實現(xiàn)對信號的脈沖壓縮，并根據(jù)脈沖壓縮結果獲得目標與天線間的距離信息?；赨WB解算的測距結果中包含饋線長度，需要對數(shù)據(jù)進行擬合、校準來消除饋線的影響；同時將解算的距離與實際距離對比，去除系統(tǒng)誤差的影響。

3 實驗與分析

3.1 實驗條件

本文采用Time Domain(pulsOn440 monostatic radar module ,P440-MRM)的UWB傳感模塊和2個IMU，如圖7所示。采集數(shù)據(jù)后傳輸至PC端,使用Matlab進行處理。通過紅外線測量儀測得步長實際距離，當紅外線從測距儀發(fā)出碰到反射物被反射回來被測距儀接收到，再根據(jù)紅外線從發(fā)出到被接收到的時間及紅外線的傳播速度就可以算出距離，測試場景如圖8所示。

圖7 實驗設備圖Fig.7 Experimental equipment diagram

3.2 實驗結果

實驗采集了5名實驗人員在長50 m的水平路面往返過程中，沿直線行走和含有轉彎行走的數(shù)據(jù)，總共20組；將本文提出的算法與常用且精度較高的Weinberg模型進行比較。Weinberg模型是一種非線性步長模型，計算公式為

(15)

(15)式中：k為校準系數(shù)；Amax和Amin分別為每一步三軸加速度模值的最大值和最小值。

圖8 實驗場景圖Fig.8 Experimental scene graph

Weinberg非線性模型的步長估計如圖9—圖10所示。此種模型完全以加速度來計算步長，行走軌跡不會對步長計算產生影響，故本文僅選擇直行場景來進行測試。

圖9 合加速度峰值檢測Fig.9 Combined acceleration peak detection

圖10 非線性模型的步長估計Fig.10 Step length estimation of nonlinear model

直行時組合步長算法的計算結果如表1所示，轉彎時組合步長算法的計算結果如表2所示，兩種算法的結果對比如表3所示。

表1 直行時組合算法步長計算結果Tab.1 Calculation results of combination algorithm step length when going straight

表2 轉彎時組合算法跨步長計算結果Tab.2 Calculation results of combined algorithm stride length when turning

從表3可見，非線性步長模型計算的步長相對誤差均值為10.35%；本文算法在直行時相對誤差均值為1.86%，在轉彎時相對誤差均值為1.73%。本文算法的相對誤差明顯降低。

為更加直觀分析，采集水平路面上行走120步的數(shù)據(jù)，用兩種算法分別進行計算，結果如圖11—圖12所示。

表3 組合算法與傳統(tǒng)算法計算結果對比Tab.3 Comparison of calculation results between combined algorithm and traditional algorithm

圖11 步長估計結果對比圖Fig.11 Comparison chart of step length estimation results

圖12 步長曲線圖Fig.12 Step length curve diagram

從圖11—圖12可以看出，本文算法計算結果相比非線性步長模型更加接近行人行走的實際步長。

表4將5種不同步長估計方法與本方法進行了對比，通過對比可以看出，本文提出的方法具有較低的誤差率。

表4 不同行人步長估計方法誤差率Tab.4 Errors rate of different pedestrian step length estimation methods

4 結 論

本文針對PDR算法中步長計算精度較低的問題，提出了基于UWB矢量測距與慣性傳感組合的行人步長計算算法；通過分析行人行走過程中的足跡和足間模型，結合步態(tài)檢測對步長進行解算，并和PDR中常用步長估計算法進行對比。實驗表明，本文算法平均估計精度可達98.20%，相比于傳統(tǒng)算法誤差明顯下降。該算法精度高，參數(shù)直觀，解決了行人步長的精準測量和長航時精度保持問題，能適應復雜環(huán)境下變步長精準測量要求，在室內定位導航系統(tǒng)中有良好的應用前景。