多传感器行人航位推算方法和UKF融合算法

漆钰晖，郭　杭，邓林坤

(南昌大学信息工程学院，江西 南昌 330031)

随着基于位置服务的快速兴起，准确的位置信息显得尤为重要。目前，在室外空旷的环境，依靠GPS、北斗等系统基本可以满足定位需求；而在室内或无卫星信号的情况下，则需要利用多种传感器来辅助行人确定位置，如惯性测量单元、无线网络传感器和射频识别单元等。

随着惯性测量单元(IMU)硬件水平的不断提高，基于惯性测量单元的室内行人航位推算(PDR)越来越受到重视，通过加速度计和陀螺仪组合磁力计、气压计等来提高位置精度。

PDR算法可以分解为航向估算与步长估算两部分，通过航向估算判定行人行走的方向，通过步长估算在其行走方向的投影，可以获得行人在二维坐标系下的位置。田国会等[1]提出一种基于小波变换与UKF结合的滤波方法，利用小波对观测信号进行处理，降低了传统UKF由于噪声而导致滤波偏差的影响，从而提高其定位精度；但其步伐状态测定仅利用加速度检测，倘若受到外界因素干扰，其测定结果必定产生较大偏差；李金凤等[2]采用互补滤波融合多传感器数据，修正航向角误差，获得了一定的精度，但其步长估计采用简单的步长模型，倘若行人在行走过程中步频改变，步长测量将会产生较大误差，最终将导致定位精度的下降。

本文利用智能移动设备的惯性测量单元和磁力计，通过基于UKF对PDR方法的两大部分进行误差修正，步长利用ZUPT进行估计计算，航向角利用ZARU与磁力计融合的方式进行估算，从而提高行人位置精度。基本思路如图1所示。

图1　多传感器行人航位推算方法框架

1　初始对准

在进行捷联导航解算前，惯性导航系统必须要进行初始对准，从而确定初始姿态等信息，它的结果将直接影响到惯性导航系统的导航性能。其中，依照运动载体的状态，又可以分为静基座对准和动基座对准两种，而且都包含粗对准和精对准两个阶段。

粗对准一般利用加速度计来获取载体的俯仰角和横滚角，磁力计获取其航向角；而精对准一般需在粗对准的基础上使用滤波等算法，处理各传感器的误差信息，并且精确估计与真实坐标系的偏差，从而获得更准确的姿态及相关矩阵[3]。本系统采用的是静基座对准方法，导航坐标系为北西天坐标系。

由于UKF算法滤波效果受滤波初值影响较大，故其初始对准非常重要。

一般情况下，可认为加速度计的输出为

(1)

(2)

由于惯性测量单元中，低成本陀螺仪性能的限制，计算航向角会产生很大偏差。因此粗对准一般采用磁力计代替陀螺仪获取相应的磁感应电势来计算航向角。

(3)

(4)

ψ=arctan(My/Mx)-Md

(5)

Md表示本地磁偏角。

在静止4 min的情况下，利用加速度计和磁力计的值，计算初始姿态与加速度计和陀螺仪的初始姿态误差。

在此阶段，可将UKF的误差状态向量设为[4-5]

X=[δθ,δφ,δψ,δωx,δ,δrU,δvN,δvW,δvU,δax,δay,δaz]T

(6)

式中，导航坐标系为n；δθ、δφ和δψ分别表示姿态角误差；δωx、δωy和δωz分别表示在b系下陀螺仪误差，δrN、δrW和δrU分别表示在n系下北西天方向上的位置误差；δvN、δvW和δvU分别表示在n系下北西天方向上的速度误差；δax、δay和δaz分别表示在b系下的加速度误差。

由于UKF使用的是离散时间上的非线性模型，需要对系统模型进行离散化处理，系统离散化状态方程为

Xk+1=f(Xk)+Wk

(7)

系统离散化量测方程为

Zk=HXk+Vk

(8)

式中，k表示某一时刻；Vk和Wk分别表示量测噪声矩阵和系统噪声矩阵，而且它们为零均值的白噪声，以速度、位置、航向角和角速度为量测值，则相应的量测矩阵为

H=[010×2I10×10010×3]

(9)

利用上述模型及UKF的基本方程对该系统初始状态进行估计，利用闭环形式对系统进行误差补偿，实现精确而快速的初始对准，基本流程如图2所示。

图2　精对准流程图

2　移动测定

行人在行走过程中，其步伐需经历多种状态的周期性变化；本试验采用手托Ipad方式行走采集数据，各个传感器的3个轴均表现周期性的特征。目前大部分试验都是采用加速度计[6]或陀螺仪[7]来检测的，本文采用加速度计和陀螺仪联合检测[8]的方式来测定行人行走。

k时刻加速度的振幅及其均值为

(10)

(11)

(12)

角速度的振幅及其均值为

(13)

(14)

(15)

(16)

根据以上判定条件，行人行走是步伐状态如图3所示。

图3　步伐状态检测

为了清晰可见，本文仅截取了一段步态检测数据进行描述与分析，图中Y轴仅表示数值的大小，没有固定的意义，X轴为传感器采样样本数。由图3可知，行人行走时步伐状态呈现周期性变化，当步伐状态为移动时，表明行人处于移动状态，相应的静止则表明行人处于静止状态。

3　无迹卡尔曼滤波

无迹卡尔曼滤波作为一种典型的非线性估计的方法，其核心是利用无迹变换变化处理Sigma点集，使Sigma点集的均值、协方差与原系统统计特性匹配，再利用Sigma点集进行非线性映射，得到近似化的状态概率密度函数；无迹卡尔曼滤波使用Sigma点计算不同状态的更新和量测值的影响[9-10]。

3.1　状态传递

对于不同时刻k，由具有高斯白噪声uk的随机变量X和具有高斯白噪声vk的观测变量Z构成的非线性系统为

Xk=f(Xk-1,uk)

(17)

Zk=h(Zk-1,vk)

(18)

式中，f为非线性状态方程函数；h为非线性观测方程函数；设uk的协方差矩阵为Q；vk的协方差矩阵为R。

加速度计及陀螺仪真实输出为

(19)

(20)

式中，Va和Vω分别代表加速度计和陀螺仪的偏差量。

(21)

(22)

则方程(22)可表示为

(23)

3.2　无迹变换

无迹变换可以用来处理均值和协方差的非线性传递问题，其实现方法为：按一定规则选取一些采样点，将这些点代入非线性函数且得到相应的点集，通过这些点集求取变换后的矩阵和协方差[11]。采样点的选取是基于先验均值和先验协方差矩阵的平方根的相关序列。

计算(2n+1)个Sigma采样点

(24)

Xk,i=f(Xk-1,i,uk)

(25)

(26)

(27)

采样点相应的权重值为

(28)

3.3　量测更新

根据式(18)得到Sigma点集的观测预测值，通过加权求和可得到系统的预测均值和协方差为

(29)

(30)

(31)

卡尔曼增益矩阵为

(32)

状态更新及协方差更新为

(33)

(34)

4　ZUPT、ZARU与磁力计融合

行人在室内行走时，ZUPT和ZARU都是依据步伐处于静止状态时触发，即当行人步伐处于静止状态，理论上该时刻的速度和角速度应该为零，但是由于惯性传感器的电子噪声、量测误差等影响，实际上由加速度计计算得到的速度和陀螺仪获取的角速度并不为零。因此，可以将该时刻速度和角速度作为UKF的量测值进行误差修正[12]。

ZUPT的量测值及相应的量测矩阵为

Zk=Δvk=vk,k-1-[0,0,0]

(35)

H=[03×3,03×3,03×3,I3×3,03×3]

(36)

ZARU的量测值及相应的量测矩阵为

(37)

H=[03×3,I3×3,03×3,03×3,03×3]

(38)

由于行人在室内环境下，极易受到人工设施的干扰而导致磁极偏离地磁场，从而使磁场感应电势产生很大的偏差。

(39)

(40)

Δψk=ψk-ψmag(k)

(41)

式中，ψk为由修正后k时刻的姿态矩阵求得；ψmag(k)为由磁力计计算得到；Δψk为航向角偏差。

磁力计的量测值及相应的量测矩阵为

(42)

H=[[001],01×3,01×3,01×3,01×3]

(43)

单纯的利用磁力计或陀螺仪都很难获得较高精度的方向信息，针对这种问题本文建立了方向估计模型，将每一时刻的感应电势由载体坐标系转换为导航坐标系。利用ZUPT、ZARU与磁力计融合，构建相应的航向角误差模型。根据式(36)、式(38)和式(43)可以得到融合后相应的量测值和量测矩阵为

(44)

(45)

5　试验分析

本文提出了一种基于UKF低成本多传感器融合行人航位推算方法，试验设备采用苹果公司iPad air 2，此设备含有IMU、磁力计和气压计等传感器。通过SensorLog软件获取行人室内行走的数据，试验场地为长方形会议厅，设置采样频率为100 Hz，总测试路线长度为50.4 m，图4为使用Matlab软件仿真某一次试验方法对比图。

为了验证本文方法的优越性，同时引入了基于扩展卡尔曼滤波(EKF)的多传感器组合行人航位推算方法进行对比试验，从图4中可以看出，基于EKF利用ZUPT与磁力计融合的方法，在航向角方面，一开始直线行走行人航向角与实际偏差不大，但当经过第一个直角转弯时，由于设备传感器反应不灵敏且精度低，利用陀螺仪与磁力计组合计算航向角也会产生很大误差，而且基于EKF的方法将非线性问题线性化不可避免地产生线性误差，即一遇到转弯，该方法的航向角出现明显的波动；同样基于EKF的ZUPT未能有效地对速度误差修正，造成相邻步伐之间距离差别较大。

采用本文基于UKF利用ZUPT、ZARU与磁力计融合的方法，针对上述基于EKF和低成本传感器等问题，利用基于UKF的ZARU与磁力计融合部分，由步伐为静止状态触发角速度修正并融合磁力计对航向角偏差进行修正，有效地校正了行人转弯时航向角误差较大的问题；在速度误差方面，基于UKF的ZUPT部分同样在步伐处于静止状态触发速度修正，以贝叶斯理论和无迹变换为基础，有效地削弱了传感器误差累积，修正了行人行走时线性速度及位置漂移。从图4可以看出，基于UKF的方法在方向和相邻步伐的距离方面，两者表现得都非常平稳。

图4　试验测试对比结果

定位误差用试验终点和真实终点距离的差值与总路程的比值来表示。两种不同方法的对比见表1，一共进行了5次试验，时间包含初始对准4 min。

表1　定位误差分布

从表1中结果对比可知，基于UKF的ZUPT、ZARU与磁力计融合的方法，利用ZARU和磁力计构建修正航向角误差模型，有效地修正了航向角误差；利用ZUPT对速度误差修正，降低加速度计偏差及漂移，从而减小了相邻步伐间的误差，结合上述修正后的数据，最终提高室内行人位置精度。

由于本试验设备反应不灵敏且精度低等问题，惯性测量单元中的陀螺仪精度较低，而且磁力计易受外界干扰。故本试验使用智能移动设备的多传感器，采用基于UKF的ZUPT、ZARU与磁力计组合的方法同样达到了较好的精度。

上述试验结果表明：基于UKF的ZUPT、ZARU与磁力计组合的方法，使用低成本多传感器进行室内行人定位，同样可以获得较高的定位精度，验证了该方法的实用性和通用性。

6　结　论

本试验提出一种基于智能移动设备的多传感器行人航位推算方法，该方法不仅可以实现在室内连续自主的定位，而且通过引入ZARU与磁力计融合构建航向角估计模型，有效地修正行人行走时航向角的偏差，ZUPT对速度误差的修正减少了加速度计偏差及漂移，有效地修正了行人步伐的误差。与目前常用的行人航位推算系统比较，该系统使用低成本的传感器，获得了相当的定位精度。在一定时间内，该系统可以满足室内行人的定位需求。但是，由于试验设备传感器性能比较差，造成采集到的数据包含了许多其他误差与噪声，从而导致了时间越长，位置误差越大的结果。下一步需要以与视觉定位结合的方式，实现在室内高精度、长时间、长距离和实时定位的PDR系统。

参考文献：

[1] 田国会,张庆宾,丁娜娜.基于WT-UKF的PDR/GPS组合定位算法[J].控制与决策,2015，30(1):86-90.

[2] 李金凤,王庆辉,刘晓梅,等.基于MEMS惯性传感器的行人航位推算系统[J].传感器与微系统,2014,33 (12):85-90.

[3] 马宏阳,程鹏飞,王潜心.一种改进的UKF算法在捷联惯导初始对准中的应用[J].测绘通报,2015(7):18-22.

[4] 门爱东,姜竹青,王宇鹏,等.双重自适应UKF在SINS初始对准中的应用[J].北京邮电大学学报,2014,37(1):11-15.

[5] 苏宛新.自适应UKF滤波在SINS初始对准中的应用[J].中国惯性技术学报,2011,19(5):533-536.

[6] JIMENEZ A R，SECO F，PRIETO C，et al.A Comparison of Pedestrian Dead-Reckoning Algorithms Using a Low-cost MEMS IMU [C]∥The 6th IEEE International Symposium on Intelligent Signal Processing.Budapest：IEEE，2009:37-42.

[7] RAUL F，EDUARDO Z，JAIME G.Pedestrian Tracking Using Inertial Sensors[J].Journal of Physical Agents,2009,3(1):35-42.

[8] ZHENG X，YANG H，TANG W，et al.Indoor Pedestrian Navigation with Shoe-mounted Inertial Sensors[C]∥Multimedia and Ubiquitous Engineering.Berlin：Springer，2014:67-73.

[9] FRANCISCO Z，MOHAMMED K，PATRICK R，et al.Unscented Kalman Filter and Magnetic Angular Rate Update (MARU) for an Improved Pedestrian Dead-reckoning [C]∥Position Location and Navigation Symposium.[S.l.]：IEEE,2012:129-139.

[10]赵鹤，王喆垚.基于UKF的MEMS传感器姿态测量系统[J].传感技术学报，2011，24(5):642 -646.

[11]陈国良,张言哲,汪云甲,等.WiFi-PDR室内组合定位的无迹卡尔曼滤波算法[J].测绘学报,2015,44(12):1314-1321.

[12]BORENSTEIN J，OJEDA L，KWANMUANG S.Heuristic Reduction of Gyro Drift in IMU-based Personnel Tracking Systems[J].Journal of Navigation，2009，62(1):41-58.

[13]JIMENEZ A R，SECO F，PRIETO J C，et al.Indoor Pedestrian Navigation Using an INS/EKF Framework for Yaw Drift Reduction and a Foot-mounted IMU [C]∥Proc.of 7th Workshop on Positioning，Navigation and Communication (WPNC’10).Dresden：[s.n.]，2010:1-9.

[14]WEINBERG H.Using the ADXL202 in Pedometer and Personal Navigation Applications[EB/OL].[2017-01-02].www.BDTIC.com/ADI.

[15]JIMENEZ A R，SECO F，PRIETO C，et al.A Comparison of Pedestrian Dead-reckoning Algorithms Using a Low-cost MEMS IMU [C]∥The 6th IEEE International Symposium on Intelligent Signal Processing.Budapest：IEEE，2009:37-42.