姚文凯 邢砾文 黄莹

摘要：论文针对行人行走过程中导航带来的误差，提出采用多传感器数据融合的零速修正算法。首先，将惯性测量模块安置于行人腿部，实时获取行人行进的加速度和角速度参数;然后，通过采用SVM零速检测分类方法，快速准确识别出运动阶段和静止阶段，在静止阶段速度和角速度都为零，同时利用磁强计确定方位，从而对行人的速度、角速度、方位等参数进行误差修正;最后，利用扩展卡尔曼滤波方法对行人行走误差进行滤波处理，进而对惯性导航解算的位置和方向信息进行实时补偿，实现对行人轨迹的跟踪。

关键词：导航算法;零速修正;卡尔曼滤波

中图分类号：S762;TN713;TP212  文献标识码：A  文章编号：1672-9129（2020）02-0035-02

Abstract： aiming at the errors caused by navigation during pedestrian walking， this paper proposes a zero-speed correction algorithm based on multi-sensor data fusion. First， the inertial measurement module is placed on the legs of pedestrians to obtain the acceleration and angular velocity parameters of pedestrians in real time. Then， the zero-speed detection and classification method of SVM is adopted to quickly and accurately identify the moving stage and the stationary stage. In the stationary stage， the velocity and angular velocity are both zero. At the same time， the azimuth is determined by the magnetometer， so as to correct the error of the pedestrian's velocity， angular velocity， azimuth and other parameters. Finally， the extended kalman filter method is used to filter the pedestrian walking error， and then the position and direction information of the inertial navigation solution are compensated in real time， so as to track the pedestrian trajectory.

Keywords： navigation algorithm; Zero speed correction; Kalman filtering

1   引言

随着科学技术和信息化的快速发展，信息技术逐渐应用到人们的日常生活中，为人们的日常活动提供了更多的便利。在出行过程中，人们对于位置的需求以及路径的导航规划要求越来越高。将多元微惯性测量单元固定于移动目标上，通过实时测量目标移动过程中的加速度、角速度等数据参数，进行目标的位置和方向的导航解算，从而对目标位置和方位进行实时跟踪和定位。这一技术不仅可以用于日常民用导航，还可以被广泛应用于航空航天、军工等领域，具有重要的社会价值和学术意义。

2   SVM分类法

选取一组已知运动状态的数据集作为样本训练集，训练样本集记为：  T={（xi，yi）}={（x1，y1），（x2，y2），...，（xn，yn）}，i=1，2，...，n  其中，xi∈RN，为第i组提取的加速度数据和角速度数据组建的向量组;yi∈{+1，-1}，表示第i组数据的运动状态。当yi=+1时，表示行人行进状态;当yi=-1时，表示行人静止状态;为了能够正确划分训练数据集表征xi与yi的对应关系，通过学习和训练在 n 维数据空间中找到一个最优的超平面wxi+b=0，满足yi（wxi+b）1，使几何间隔最大。假设某样本点（xi，yi）到超平面方程的几何距离为：

λ=yi（wTxi+b）‖w‖

考虑到yi（wxi+b）1，则几何间隔最大，即：

（λm=max1‖w‖）‖（λm=min12‖w‖2）

于是幾何间隔最大问题转成对凸二次规划求解算法改善问题。为了得到最优化的结果，采用拉格朗日函数，并且使其满足以下公式：

L（w，b，a）=12‖w‖2-∑nj=1ai（yi（wTxi+b）-1）

式中，ai为拉格朗日乘子。根据对偶性原理，利用求解极值的方法得到拉格朗日最优化问题的解。首先分别对法向量w和截距b求偏导数，得到：w=∑naiyixi，∑naiyi

代回上式求得最优解a=（a1，a2，...，an），然后计算得到 w 和 b 值。因此，分离超平面方程可用公式wxi+b=0来表示，相应的分类决策函数为：f（x）=sign（wTxi+b）

3  多源导航零速修正算法构建

3.1导航参数修正。由于器件本身在工作时具有累积误差，通过姿态解算，解算出来的姿态和速度需要进行修正。在零速时刻，角速度误差Δwk，速度误差Δvk可以表示如下：   Δwk=[wxkwykwzk]-[000]Δvk=[vxkvykvzk]-[000]

将航向角误差Δφk，角速度误差Δwk，速度误差Δvk作为观测量，记为：Zk=[ΔφkΔwkΔvk]T

得到行人运动观测方程为：Zk=HkXk+Vk

式中，Vk为测量噪声，服从均值为零、协方差为Rk的高斯白噪声分布;Hk是观测转移矩阵。

3.2扩展卡尔曼滤波。假设在静止的k时刻，行人运动特征误差的状态向量为：

X=E  N  U  δvE  δvN  δvU  δL  δλ  δh  εx  εy  εz SymbolQC@x SymbolQC@y SymbolQC@zT

其中，E、N、U为失准角;δvE、δvN、δvU分别为载体的东向、北向和天向速度误差;δL、δλ、δh分别为纬度误差、经度误差和高度误差;εx、εy、εz、SymbolQC@x、SymbolQC@y、SymbolQC@z分别为陀螺随机常值漂移和加速度计随机常值零偏。（下标E、N、U分别代表东、北、天）

基于卡尔曼滤波方程原理，可得EKF的递推过程和步骤如下：

状态一步预测：X^k/k-1=k，k-1X^k-1

状态估计：X^k=X^k/k-1+Kk（Zk-HkX^k/k-1）

滤波增益：Kk=Pk/k-1HTk（HkPk/k-1HTk+Rk）-1

一步预测均方误差：Pk/k-1=k，k-1Pk-1Tk，k-1+Qk-1

估计均方误差：Pk=（I-KkHk）Pk/k-1

随着采样时刻的增加和卡尔曼滤波过程的递推，可以得到每一个状态的状态向量误差。由于在递推过程中在静止段对采样和解算的加速度、角速度和方位进行了闭环反馈修正，可高效地降低行人步进过程中的累积误差，改善行人导航和定位准确度。

4  基于零速修正的多源导航算法实验

考虑到惯性导航具有自主性和实时性，并能为行人惯性导航系统提供帮助的参考信息，在研究行人惯性导航系统时采用荷兰Xsens公司提供的MEMS器件，其中惯性测量模块由三轴加速度计、三轴陀螺仪、三轴磁力计、气压高度计等组成，其数据的采样频率可选用 50Hz、100Hz 等，另外通讯方式可采用 USB 接口或 RS232 串口通讯方式，将数据信息实时传送并存储到计算机里，进行下一步的数据分析和解算。

对于绕圈行进姿态变化，如图1所示，可以清楚的看到横滚角和俯仰角变化较小，而航向角随着行人行进方向持续发生变化，

按照既定路径连续测量五次实验数据进行比较，得五次的测量数据如表1所示。

行走距离（m）相对误差（%）

表中可以看出，按圆形路径行走，得到的轨迹平均误差为 2%，最大误差不超过 3%，整体导航效果较好。

参考文献：

[1]Ladetto Q， Gabaglio V， Van Seeters J. Pedestrian navigation method and apparatus operative in a dead reckoning mode： U.S. Patent 6，826，477＼[P＼]. 2014.

[2]Hide C， Moore T， Botterill T. Low Cost Vision-Aided IMU for Pedestrian Navigation＼[J＼]. Journal of Global Positioning Systems， 2014， 10（1）： 3-10..

[3]吉訓生， 王寿荣. MEMS陀螺仪随机漂移误差研究 ＼[J＼]. 宇航学报， 2016， 27（4）： 640-642.

作者简介：姚文凯（1996- ），男，汉，江苏溧阳人，武警工程大学硕士研究生。研究方向：信息融合，导航制导与控制。  邢砾文（1997- ），女，汉，吉林长春人，武警工程大学硕士研究生。研究方向：故障诊断。  通讯作者：黄莹（1978～），女，汉，博士研究生，武警工程大学信息工程学院通信装备教研室副教授。研究方向：卫星导航，卫星通信，智能控制，数据融合。