刘 宇，贺竹川，路永乐，邸 克，文丹丹，邹新海

(重庆邮电大学 智能传感技术与微系统重庆市高校工程研究中心，重庆 400065)

0 引 言

现代科学技术的发展，导航定位技术已经在社会的很多领域得到应用[1]，对室内外导航定位的需求也越来越多。目前，现行室外定位技术主要凭借全球定位系统(global positioning system,GPS)以及中国北斗卫星导航系统(BeiDou navigation satellite system,BDS)进行定位，其定位精度能达到亚米级[2]。由于卫星定位只适用于空旷无遮挡地带，在信号干扰源多、环境复杂多变的室内卫星信号的强度和质量急速下降，导致定位精度下降甚至无法完成定位。因此，工业界和科学界均在探求精度更高，可靠性更强的室内定位技术。

目前，利用单一惯性测量单元(inertial measurement unit,IMU)设备对多个移动节点组成的班组进行室内定位时，整个班组的整体性导航定位误差会随着时间的增长而累计，不适合于长时间的室内定位导航[3,4]。超宽带(ultra-wideband,UWB)定位能够达到较高精度，但由于需要提前搭设参考基站，在遇到突发状况时使用局限性较大，且UWB基站成本高，定位精度容易受到非视距(non line of sight,NLOS)噪声的干扰[5]。

Tong H B[6]、陈隆亮等人[7]均利用UWB和IMU松耦合进行组合定位，将IMU和UWB分别定位结果进行融合滤波，该方法需要设置UWB定位基站，定位效率不高。Olsson F等人[8]提出了选择以IMU为核心的基础上，将UWB测距数据作为位置观测值，加入到基于零速度的扩展卡尔曼滤波(EKF)算法中实现融合。然而，EKF本质上是观测方程的线性逼近，基于高度非线性的观测模型很难对UWB传感器实现很好的逼近。相比之下，当许多粒子分布在状态空间近似于状态概率分布时，粒子滤波可以解决大量的非线性或非高斯问题[9,10]。

本文选用粒子滤波算法将IMU测量数据以及UWB测距信息进行融合。将IMU数据作为粒子滤波的先验信息，并将UWB测距值作为粒子的观测值，对节点位置信息进行预测、更新，提高导航定位的精度。经实验证明，在利用粒子滤波融合的IMU和UWB室内定位系统的定位精度，相较于单一IMU惯性导航的定位精度明显提高。

1 IMU导航与UWB测距模型

1.1 IMU导航模型

IMU是测量物体运动的三轴角速度和三轴加速度的单元。本文将采用行人航迹推算(pedestrian dead reckoning，PDR)算法对IMU采集的移动节点的三轴角速度和三轴加速度进行解算[11]。采集无其他参考设备的环境下佩戴IMU运动人员在行进过程中的加速度、角速度等数据，并利用这些数据对行进人员进行步长与方向的推算，得到移动节点的坐标以及方位，从而达到对人员进行定位跟踪的目的，其中,主要涉及的过程有,步态检测、步长和方向计算。图1所示为PDR算法示意。

图1 PDR算法示意

PDR初始条件：已知初始位置S0(X0,Y0)，由IMU中内置的加速度计对行人的步态进行检测，再由陀螺仪对行人角度进行实时监测，从而利用下列公式对路径进行推算

(1)

根据以上PDR推算公式，可以得出任意时刻k的位置Sk(Xk,Yk)。

1.2 UWB测距模型

UWB测距具有穿透力强、功耗低、抗多径能力强，安全性能高等优势。本文UWB测距将采用双向飞行时间法(two way time of flight,TW-TOF)来完成。主要两个异步收发机(transceiver)之间的信号飞行时间来测距。每个收发机启动时会生成一条独立的时间戳。如图2所示，模块A的发射机在其时间戳上的Ta1发射请求性质的脉冲信号，模块B接收机在其时间戳Tb1接收到该信号。模块B在Tb2时刻发射一个响应性质的信号，被模块A在自己的时间戳Ta2时刻接收。由此可以计算出脉冲信号在两个模块之间的飞行时间，从而确定飞行距离S

图2 UWB测距原理

S=c[(Ta2-Ta1)-(Tb2-Tb1)]/2

(2)

将发送端发出的数据包和接收回应的时间间隔记为TTOT,接收端收到数据包和发出回应的时间间隔记为TTAT,那么数据包在空中单向飞行的时间TTOF可以简化为

TTOF=c(TTOT-TTAT)/2

(3)

2 UWB/IMU协同导航定位模型

2.1 协同导航定位模型建立

IMU对短期内位置变化的估计很准确，但对于长航时的误差累积非常敏感。特别是移动的节点来说，不规则运动对其影响很大。因此，利用UWB的测距数据来补偿IMU的误差，从而产生一种新的、更准确、更可靠的室内班组自主导航定位系统。

在该组合导航模型当中，将每个节点自身携带的IMU测量惯性数据作为系统的状态量，当每个移动节点接收到IMU数据，则使用状态转移方程更新当前系统状态。当接收到班组区域内各节点之间的UWB测距数据，则使用该测距数据作为观测值，将两者融入一个概率框架之中，得到当前位置的联合后验概率分布，并根据IMU的信息对粒子进行更新后输出状态估计。最后，通过重新采样来缓解粒子衰减的问题。其融合算法示意如图3所示。

图3 传感器粒子滤波融合算法示意

2.2 粒子滤波融合算法

(4)

(5)

(6)

(7)

(8)

(9)

式中d(·)为节点i和节点j之间距离的平方根。在本文模型中，粒子滤波的步骤为三部分组成。

(10)

重采样：当有效的样本大小低于粒子数M的50%时，将生成一组新的粒子来替代之前的旧粒子样本集。

根据仿真验证，在粒子数为500时，系统的运行时间较低，定位产生的误差也较低，综合性能最优。所以，本文试验将粒子数M设定为500。

3 实验验证与结果分析

基于上述理论，为验证该方法的合理性及其定位精度，设计了UWB和IMU相结合与单一IMU相对比的实验方案。如图4所示，用到的IMU设备为实验室自行研制的IMU设备，UWB模块选用Time Domain公司的PulsOn440。本文实验场地选取重庆邮电大学室内体育馆中心较复杂的室内环境，实验的行走范围设定为50 m×60 m的矩形路径，实验由3名人员分别佩戴导航定位设备在室内行走。实验人员将IMU设备固定于腰间，为了避免人体对UWB信号产生遮挡，将UWB设备固定于头部。实验一共分为2组，每组实验围绕该矩形路径行走2圈，3名移动人员在起点处相隔1 m出发，并在实验过程中保持该距离行走。

图4 IMU和UWB组合导航定位系统

第一组实验由3名移动人员组成移动班组佩戴IMU设备从相同起始点出发，移动人员的定位信息由采样频率为50 Hz的IMU进行采集，并用PDR算法进行解算，得到IMU导航定位结果。

第二组实验由3名移动人员组成班组佩戴IMU和UWB导航定位设备从相同的起始点出发，其中，UWB的定位信息由串口相连接的IMU内设CPU进行采集，然后用粒子滤波算法进行融合滤波,得到融合定位导航结果。图5和图6分别为IMU定位和IMU/UWB组合定位实际轨迹图。对比可以得到，仅使用IMU进行导航定位的方案，受到IMU的累积误差以及航向角偏差的的影响，整个小组行进的路线与真实路线偏差较大，在行进途中各人员虽然保持同步，但轨迹却很快的发散，相互偏离。而由IMU/UWB组合的导航系统则与真实路线更加接近，人员轨迹基本保持同步。

图5 单IMU惯性导航定位效果

图6 IMU与UWB融合导航定位效果

由图7和图8得到，在仅用IMU导航的情况下，受累积影响，3个移动节点随着行走时间累计，单个节点以及班组的整体误差不断在增加，实验过程中单个节点在X和Y轴上的最大偏差可达到11.8 m和10.7 m，班组整体的组合轨迹偏差最大可达到6.4 m和6.3 m，整体路径偏离真实路径较大。而在添加了UWB测距量进行修正后，班组的整体组合轨迹重合度增大，单个节点和班组整体在X和Y轴上的最大偏差均小于2.1 m，轨迹的偏差明显降低。

图7 不同导航定位系统在X轴向的误差曲线

图8 不同导航定位系统在Y轴向的误差曲线

由表1可知，在单一IMU导航的情况下，在X轴向上，3个移动节点组成的班组区域在行走期间的平均累积误差为2.78 m，Y轴向上为3.08 m。在结合UWB后，班组区域平均累积误差在X轴上缩小为1.07 m，在Y轴向上缩小为1 m。误差较单一IMU惯性导航减小了61.5 %和67.5 %，有效地提高了班组的整体导航定位的精度。

表1 X轴和Y轴方向上不同系统的平均误差 m

4 结 论

单一IMU惯性导航的长航时累积误差会导致定位偏差，本文提出的无需外界辅助信号源，将IMU惯性信息和班组区域内的UWB测距信息结合导航定位的方法，相对单一IMU惯性导航，增加了精确的距离观测值，利用粒子滤波将2种数据进行融合，以提高导航定位的精度。通过实验验证，将IMU和UWB结合的方案与单一器件的定位误差相比在X轴和Y轴上各自降低61.5 %和67.5 %，班组实际行走轨迹与参考轨迹重合，导航定位精度得到明显的提升。