在火灾现场, 消防员根据导航定位信息对周围伤员搜救, 可提高救援效率。由于全球定位系统GPS(global positioning system)在森林茂密的林区信号易受干扰, 导致导航误差产生显著漂移, 很难实现消防员的定位。基于微机电惯性测量单元MEMS(micro-electro-mechanical system)的导航技术具有自主性和抗干扰能力强的特点, 可满足消防员的定位需求^[1]。

目前行人的导航定位方法分为2种:①行人航位推算PDR^[2](pedestrian dead reckoning), 根据人体运动模型推算行人步数、步长与方向, 采用描点法实现行人轨迹绘制；②其定位误差随时间呈抛物线形式漂移。因此, 基于零速更新ZUPT^[3](zero velocity update)的惯性导航成为定位的有效方法。它把行人步态分为零速区间和运动区间。在零速区间停止惯性导航, 并修正传感器误差, 减少了惯性导航的累积误差, 具有稳定性强, 精度高的优点^[4]。

零速更新方法通过估计惯性传感器的误差修正零速区间的姿态、速度、位置, 达到减小定位误差的目的, 目前应用最广泛的方法是卡尔曼滤波。文献[5]介绍了一种基于12维传感器误差的零速修正模型, 包括速度误差、姿态角误差、位置误差和地磁误差, 未考虑惯性传感器的测量误差, 随着时间积累误差较大, 不适合长时间定位。文献[6]介绍一种基于15维传感器误差变量的零速修正模型, 包括速度误差、姿态角误差、位置误差、陀螺仪误差和加速度计误差, 该方法将测量噪声假定为高斯白噪声, 当导航系统的非线性化程度严重, 而且噪声复杂非高斯时, 卡尔曼滤波器发散, 导致精度严重下降。文献[7]提出一种18维状态变量的零速修正模型, 但是此方法计算量大, 实时性不强。文献[8]应用标准粒子滤波器对室内行人定位, 通过墙体的参数限制粒子权重, 但是未考虑到零速状态下的位置漂移, 而且随着时间的推移, 粒子退化严重, 影响滤波精度。文献[9]提出一种粒子滤波应用于GPS组合导航中, 但是在森林茂密的林区GPS信号易受干扰, GPS不能正常工作, 导致单独使用惯性传感器定位会产生较大误差。惯性导航定位使用ZUPT方法可以有效减少定位误差, 利用惯性传感器在静止状态进行零速检测, 对零速区间的速度、位置误差进行修正。

目前, 经典的粒子滤波存在粒子退化现象, 减少粒子退化的主要方法是选择合适的重要性概率密度函数和增加重采样过程^[10]。针对现有的零速修正方法的缺陷, 本文提出一种自适应粒子滤波APF的零速修正方法, 利用状态空间中的随机样本来近似表示概率密度函数, 在重采样过程中引入自适应阈值和二次采样判断, 适用于非线性和非高斯系统, 同时改善了滤波精度低, 收敛效率低的缺陷^[11], 提高了行人导航定位精度。

1 行人导航算法的总体结构

如图 1所示为导航算法的总体结构。陀螺仪和加速度计对行人的零速点进行判断, 磁强计对行人的航向进行推算, 在零速区间内利用自适应粒子滤波算法对加速度计、陀螺仪得到的数据进行修正, 在运动区间内利用捷联惯性导航解算处理, 最后输出姿态、速度和位置信息。

2 零速检测方法

在行人行走过程中, 脚部的运动遵循周期性的规律, 在2个相邻运动状态之间脚部与地面接触时都会有一个相对静止的区间, 也称作是零速区间, 准确检测零速区间并在零速区间进行误差修正可以有效的抑制误差的积累, 本文采用加速度模值、加速度模值方差和角速度模值进行阈值判断^[12], 检测的条件如下:

1) 加速度模值检测

在零速区间内, 脚部加速度的模值接近于重力加速度, 采用加速度模值作为零速检测条件, 即

(1)

式中：为三轴加速度模值；a_x, a_y, a_z分别为x, y, z轴加速度测量值；a_min, a_max分别为加速度区间的上限和下限。

2) 角速度模值检测

在零速区间内, 脚部角速度的模值接近于零, 采用角速度模值作为零速检测条件, 即

(2)

式中：为三轴角速度模值；ω_x, ω_y, ω_z分别为x, y, z轴角速度测量值；ω_max为角速度区间阈值。

3) 加速度方差检测

方差可以衡量变量与其均值之间的偏离程度, 为了提高零速判断的准确性, 使用加速度方差判定方法, 在静止状态下, 加速度的模值应该为平稳状态, 假设一个时间窗宽度为s, 计算在s时间段内加速度模值的方差, 采用加速度模值的方差作为零速检测的条件, 即

(3)

式中：为加速度模值的方差；σ_amax²为加速度模值方差的阈值；a^b为三轴加速度测量值；为三轴加速度均值。

当C₁, C₂, C₃条件同时满足时, 视为零速时刻, 即

(4)

3 自适应粒子滤波器设计

粒子滤波是一种基于蒙特卡洛原理的贝叶斯滤波^[13]方法, 通过对粒子后验概率的求解, 得到目标状态的最优估计值。基于此方法设计了一种零速区间内的自适应滤波方法, 根据零速区间的运动模型寻找一组粒子对概率密度函数进行近似。通过不断更新的粒子值与权重得到状态向量的最优估计值。

自适应粒子滤波算法流程如下:

Step1   初始粒子计算。在初始时刻t=0, 初始粒子x₀~p(x₀), 根据采样分布p(x₀)对其进行采样得到初始粒子集{x₀⁽ⁱ⁾}_i=1^N, 取粒子数为N=100, 每个粒子的初始权重为1/N。

Step2   重要性采样更新粒子和权重。构造重要性概率密度函数q(x_k⁽ⁱ⁾|y_k⁽ⁱ⁾), 根据公式(5)计算每个粒子x_k⁽ⁱ⁾对应的权重x_k⁽ⁱ⁾。

(5)

式中：p(x_k⁽ⁱ⁾|y_k⁽ⁱ⁾)为后验概率密度函数；q(x_k⁽ⁱ⁾|y_k⁽ⁱ⁾)为重要性概率密度函数；p(y_k⁽ⁱ⁾|x_k⁽ⁱ⁾)为观测量的似然函数, 由量测方程决定；p(x_k⁽ⁱ⁾|x_k-1⁽ⁱ⁾)为状态转移概率, 由目标状态模型定义；q(x_k⁽ⁱ⁾|x_0:k-1⁽ⁱ⁾, Y_k)为建议分布函数, 满足x_k-1⁽ⁱ⁾为粒子k-1时刻的权重；x_k⁽ⁱ⁾为粒子k时刻的权重。

对(5)式求得的权重进行归一化, 得到(6)式

(6)

式中：ω_k⁽ⁱ⁾为k时刻第i个粒子权重；为k时刻所有粒子权重和；为权重归一化后的值。

Step3  计算滤波值。根据归一化的粒子集, 可由k-1时刻的状态值得到k时刻的值, 即最优估计, 状态估计为

(7)

式中, x_k⁽ⁱ⁾为k时刻粒子。

Step4  粒子重采样。由于粒子在滤波过程中会出现退化现象, 粒子多样性逐渐减少, 解决此问题的方法是对粒子重采样, 从粒子集中舍弃权值较小的粒子, 重新抽取权值较大的粒子并复制, 建立新的粒子集。使粒子集的均值以最大概率(值为1)趋向数学期望。

粒子在迭代过程中权重逐渐增大, 使用固定权重阈值选择粒子会出现以下情况:在权重阈值较小时, 保留的粒子中有权重小的粒子, 在权重阈值较大时, 粒子退化速度更快, 导致滤波失效。因此本文采用自适应阈值法选择新的粒子, 其具体步骤如下:

1) 选取自适应阈值M。M满足等式(8)

(8)

式中：M的取值范围为[M_a, M_b]；M_b为上限；M_a是阈值初值, t是迭代次数；N_e为有效粒子数；K_e为比例系数；阈值M与迭代次数t成正比，与有效粒子数N_e成反比；N_e满足(9)式

(9)

利用有效粒子数N_e来衡量粒子权值的退化程度, N为粒子数量, σ²_ωk为粒子权重方差。N_e越大, 退化越严重。将(9)式代入(8)式得到

(10)

当M达到上限M_b时, M=M_a。

2) 保留粒子判断。如果x_k⁽ⁱ⁾>M, 则保留x_k⁽ⁱ⁾, 否则x_k⁽ⁱ⁾=x_k-1⁽ⁱ⁾。

3) 对粒子退化现象做二次判断

(11)

(12)

当满足H₁+H₂=1时, 返回Step1对粒子进行重新采样, 重新采样粒子的权重为1/N, N为粒子数。新的粒子集x_k⁽ⁱ⁾作为下一时刻的初始粒子。否则返回Step2进行下一次迭代, 直至循环结束。

本文结合零速时刻速度和加速度稳定的特点, 对零速区间内速度和位置进行滤波和估计。行人导航模型选取当地的地理坐标系(东、北、天坐标系)为导航坐标系, 惯性测量单元所处的坐标系(右、前、上坐标系)为载体坐标系。设导航定位系统的状态变量为

(13)

式中：V_e为载体东向的速度分量；V_n为载体北向的速度分量；V_u为载体天向的速度分量。

建立系统连续的状态方程

(14)

设采样时间为T, 将连续系统离散化, 得到系统离散的状态方程为

(15)

式中

(16)

式中：X_k和X_k-1分别为k时刻和k-1时刻状态值；W_k为系统的过程噪声；Φ_{k, k-1}为系统状态转移矩阵；Δa_x, Δa_y, Δa_z分别为x, y, z轴加速度漂移。

将零速区间解算的速度值作为观测量, 建立系统的观测方程并离散化得到

(17)

(18)

(19)

式中：V_e, V_n, V_u分别为东、北、天方向的速度；H为系统观测矩阵；ΔV_e, ΔV_n, ΔV_u分别为东、北、天方向的速度观测噪声。

4 实验验证

实验装置选用美国应美盛(Invensense)公司的MPU9150模块, 该模块为9轴组合传感器, 由6轴惯性测量单元(3轴加速度计和3轴陀螺仪)和3轴磁强计组成。实验过程中, 将传感器安装在右脚脚面上, 单片机安装在实验人员腰部, 实验的硬件模块及安装方式如图 2所示。

为了验证本文提出算法的优势及可行性, 设置对比实验进行验证, 实验人员穿戴测量模块实验, 沿直线行走共35 m, 航向角为190°。实验沿西向直线行走约34.46 m, 南向约6.08 m, 时间为130 s。对行人行走过程中的步态信息进行采集, 行人初始位置定为(0, 0, 0)m, 设定粒子滤波参数, 如表 1所示。

为了便于自适应粒子滤波性能分析, 在相同实验条件下, 分别使用扩展卡尔曼滤波(EKF)算法^[14]、经典粒子滤波(PF)和自适应粒子滤波(APF)算法进行零速修正。3种滤波方法在零速区间的速度误差和位置误差曲线如图 3和4所示。

图 3和表 2所示为3种方法在零速区间的速度误差对比, 利用均方根误差RMSE(root mean square error)作为衡量滤波精度的指标。从图表可以看出, 相比EKF和PF方法, APF方法RMSE值较小, 该方法减小了噪声干扰, 滤波效果较好。

图 4和表 3描述了3种滤波方法在零速区间内东、北、天方向的位置误差对比。从图表可以看出, APF方法计算得到的曲线更平滑且更接近于真实值。

行人实际行走的东向实际位移为-34.46 m, 北向实际位移为-6.08 m, 天向实际位移为0 m。表 4所示为轨迹误差对比, 从表 4可以看出, 本文提出的APF方法相比于EKF方法总体误差减少了40.6%, 相比于PF方法总体误差减少了19.4%, 由此可见, 相比传统的EKF和PF方法, APF方法可以获得更高的定位精度。

图 5所示为二维实验轨迹, 由图可以看出APF计算的轨迹更接近于真实轨迹, 增强了行人导航定位的可靠性。

为了更好地验证所提出算法的有效性, 行人穿戴导航系统在相同实验条件下分别做10组实验, 表 5所示为3种滤波方法的定位滤波效果对比。从表 5可以看出, 即使传感器存在长时间的累积误差, 经过APF算法都得到了有效的抑制。同时定位效果优于另外2种算法。

5 结论

本文在传统ZUPT算法的基础上, 分析并检测步态过程中的零速状态和运动状态, 并对零速状态进行修正, 进而提出一种自适应粒子滤波方法, 实验表明, 该方法根据零速区间的变化规律修正了传感器误差, 解决了粒子退化的问题, 减少了零速区间的速度和位置误差。在IMU平台验证了该方法的有效性和可行性, 适用于林区消防定位场合, 具有应用价值。