超宽带矢量测距与惯性传感组合的精准步长计算算法

刘 宇，张 泉，路永乐，崔 巍，邸 克，郭俊启，杨 勇,2

(1. 重庆邮电大学 光电工程学院，重庆 400065；2. 中国电子科技集团公司第二十六研究所，重庆 400060)

0 引 言

随着基于位置服务应用的快速发展以及微电子机械系统技术的逐渐成熟, 基于惯性传感器的行人惯性导航因其适用范围广、不依赖外部信息即可实现自主导航而成为研究热点[1]。

惯性定位技术多采用行人航迹推算算法(pedestrian dead reckoning, PDR)[2]， PDR算法的优化主要集中在航向[3]、步长估计[4]以及行为识别[5]3个方面。步长信息在人的步态分析与康复[6]、行为监测[7]或步行导航[8]等方面均有应用。步长估计是PDR算法中的重要参数，通过建立合适的步长估计模型，选择合适的特征参数，可以实现步长估计。文献[9]提出基于反向传播算法(backpropagation algorithm, BP)神经网络来训练步长模型，利用每步加速度最大值、最小值和步频等参数计算步长，有效提升步长精度。但是该算法的鲁棒性较差，当行人行走包含多种运动行为时，算法的精度下降严重。文献[10]提出融合多节点姿态信息，结合行人双腿长度信息，可以准确计算步长。但是该算法要求传感器各节点时间高度同步，对硬件要求较高，而且需要根据实际情况从外部更改模型参数，实用性不高。文献[11]将IMU和超声波模块固定于行人的左腿和右腿，利用零速检测原理和超声波定向测距来实现足间距离测量，进而辅助行人惯性导航。但是超声波测距容易受到环境影响，如温度、风速等其他障碍物干扰，都会影响步长的测量。

超宽带(ultra wide band,UWB)具有功耗低、带宽高、测距精度高、时间分辨能力强、抗干扰能力强等优点，在测距定位上有众多的应用[12-13]，理论上测距精度可以达到厘米级[14]。因此，本文提出一种超宽带矢量测距与惯性传感组合的精准步长计算算法。该算法主要包含4个阶段：①根据行人行走时足部轨迹模型构造足间步长模型，得到步长的准确计算公式；②利用足部惯性传感器检测足部零速时刻，并在此时刻触发超宽带测距；③对超宽带采集的数据进行信号处理，得出足间距离值；④结合零速时刻超宽带的测距值、陀螺解算的角度和足间步长模型公式，实现对行人步长的测量。

1 系统模型

1.1 足间模型

结合行人行走时足部的运动特征，足部轨迹可大致描述为沿直线行走和沿一定角度的转弯行走，如图1所示。步长定义为两脚相同部位、相邻着地点之间的距离，跨步长定义为同一侧脚跟连续两次着地点间的直线距离，步宽是指两脚之间的横向距离。

图1 行人行走足迹模型Fig.1 Pedestrian walking footprint model

根据人的行走状态，本文由图1构造出图2—图4所示的足间模型。连续足迹构成三角形，足间距离呈现出具有连续局部极大值的周期性行为。模型中的角度由位于足部的惯性器件陀螺解算得到，距离信息由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)式，可以计算出人在不同行走状态下的步长和步宽。精准测量两脚之间的距离是计算步长的关键所在，本文选择测距精度高的UWB进行足间距离的测量。

2 组合步长计算算法设计

2.1 零速检测

以ωx、ωy、ωz分别表示三轴角速度信息，ax、ay、az分别表示加速度信息，选择连续的7个步态参数检测波峰和波谷。具体方法如下。

(10)

(11)

(10)—(11)式中，St-3、St-2、St-1、St、St+1、St+2、St+3分别为S运动参数在t-3时刻、t-2时刻、t-1时刻、t时刻、t+1时刻、t+2时刻、t+3时刻的数值。当ps=1或vs=1时，代表运动参数S的波峰或者波谷检测成功，否则为检测失败。

本文采集了平走过程足部传感器的三轴加速度计和三轴陀螺仪数据，通过观察发现ωx、ωy、ωz和az的数据出现周期性变化，如图5所示。

进一步分析可得ωx和az的变化规律与实际的步态周期吻合度较高，能够进行步态检测，如图6所示。图6中，黑色圆圈标注的Z轴加速度波谷是脚后跟着地产生的，因此检测脚后跟着地时刻即判断Z轴加速度波谷出现时刻。每一个步态周期中，Z轴角速度先出现小波峰，后出现大波峰，Z轴加速度的波谷在两个波峰之间。通过先检测Z轴角速度的小波峰，再依次检测Z轴加速度波谷和Z轴角速度的大波峰，可以对平走过程中的脚着地动作进行判断。具体判断方法如下。

(12)

(12)式中：ω1、ω2分别是检测角速度大小波峰的阈值；a1是检测加速度波谷的阈值。当r=1时，表示检测脚跟着地动作成功，否则表示检测失败。

行走过程中，由于脚部的运动比较剧烈，传感器的输出可能会出现伪波峰和伪波谷，容易造成峰谷的误判。为了解决这个问题，本文添加时间阈值T对Z轴角速度的大小波峰时间间隔ΔT进行约束，在ΔT间隔内对az的波谷进行检测。结合图与行人实际运动的状态可得，角速度的大小波峰到加速度波谷的时间间隔差异较小。所以，角速度大小波峰之间的时间间隔可视为一段相对恒定的时间。综上，本文选取合适的时间阈值T对ΔT时间内出现的角速度伪波峰进行过滤，可以进一步对脚着地动作进行判断。判断方法为

flag=(ΔT>T)&r

(13)

图5 行人平走运动足部传感器运动特征Fig.5 Movement characteristics of foot sensors for pedestrians in horizontal walking

图6 足部传感器Z轴数据特征Fig.6 Z-axis data characteristics of foot sensor

当flag=1时，表示脚跟着地时刻检测成功，否则为检测失败。利用惯性传感器判断出双腿同时落地时刻后，采集这个时刻位于足部UWB的数据信息，利用解算的距离信息和足间模型完成步长的解算。

2.2 UWB测距

UWB多基于飞行时间进行测距，一般使用两个异步收发机之间信号飞行时间进行节点之间的距离测量。但是该种方法要求发送设备和接收设备必须始终同步，即使较小的时延也会产生较大的误差。对UWB采集的数据先进行信号预处理，包括背景噪声去除、杂波滤除等；再通过匹配滤波、脉冲压缩进行解算距离。

在实际测量中，背景噪声对含有目标信息的回波信号有一定的干扰，去除相干背景噪声可以降低背景噪声产生的影响。先采集放置目标物体前环境的回波信号S1，然后采集放置目标物体后的回波信号S2，从包含目标物体的回波中去除背景噪声，可以得到只包含目标物体的回波信号S。即

S=S2-S1

(14)

基于UWB收集的信号里存在目标物体以外的噪声和其他障碍物反射回波，其中噪声包括天线的耦合噪声。本文先使用平均消除方法，从回波信号中减去信号均值，再使用汉明窗滤波，去除障碍物的直接反射回波。

当滤波器的频率响应函数是输入信号频率响应函数的复共轭时，滤波器的输出具有最大信噪比，接收机在接收雷达回波过程中使用匹配滤波器抑制噪声信息，通过匹配滤波器对UWB回波数据进行处理，实现对信号的脉冲压缩，并根据脉冲压缩结果获得目标与天线间的距离信息。基于UWB解算的测距结果中包含馈线长度，需要对数据进行拟合、校准来消除馈线的影响；同时将解算的距离与实际距离对比，去除系统误差的影响。

3 实验与分析

3.1 实验条件

本文采用Time Domain(pulsOn440 monostatic radar module ,P440-MRM)的UWB传感模块和2个IMU，如图7所示。采集数据后传输至PC端,使用Matlab进行处理。通过红外线测量仪测得步长实际距离，当红外线从测距仪发出碰到反射物被反射回来被测距仪接收到，再根据红外线从发出到被接收到的时间及红外线的传播速度就可以算出距离，测试场景如图8所示。

图7 实验设备图Fig.7 Experimental equipment diagram

3.2 实验结果

实验采集了5名实验人员在长50 m的水平路面往返过程中，沿直线行走和含有转弯行走的数据，总共20组；将本文提出的算法与常用且精度较高的Weinberg模型进行比较。Weinberg模型是一种非线性步长模型，计算公式为

(15)

(15)式中：k为校准系数；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步的数据，用两种算法分别进行计算，结果如图11—图12所示。

表3 组合算法与传统算法计算结果对比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矢量测距与惯性传感组合的行人步长计算算法；通过分析行人行走过程中的足迹和足间模型，结合步态检测对步长进行解算，并和PDR中常用步长估计算法进行对比。实验表明，本文算法平均估计精度可达98.20%，相比于传统算法误差明显下降。该算法精度高，参数直观，解决了行人步长的精准测量和长航时精度保持问题，能适应复杂环境下变步长精准测量要求，在室内定位导航系统中有良好的应用前景。