基于超宽带与惯性导航融合的定位研究

李锐君，胡代弟

（郑州西亚斯学院，河南郑州，451150）

0 引言

移动互联网的发展，有效的位置服务也越来越重要，地下停车场、隧道等GPS 信号弱的环境中需要代替GPS的精确位置服务，以满足各种定位需求。UWB (Ultra Wideband Technology)定位系统近年受到了高度重视和深入的研究[1]。基于TOA（Time Of Arrival）和TDOA（Time Difference of Arrival）的常见测距和定位技术，对基于UWB 的定位应用有了很大的改进[2-3]。但是针对移动目标的实时跟踪，只有当系统误差为零均值的高斯分布时，才能得到较高精度[4]。

惯性导航的突出优势在于导航定位的自主性与连续性，在三维空间下有良好的表现[5]。利用搭配磁力计、陀螺仪、加速度计的九轴惯导模块可精确测得物体的运动信息，通过航迹推算估计出物体运动的方向和距离，但是低成本陀螺仪漂移误差严重，磁力计容易受到各种磁干扰，无法获取准确的航向值，导致定位精度低，此外，由于航迹推算机制所固有的积分误差累积效应，也限制了惯导组件长时间的单独定位工作[6]。

鉴于UWB 带宽高、能耗低、多径分辨能力强，以及惯性导航定位的自身优势，将二者结合用于定位和测距，具有重要的应用价值。本文研究了二者融合算法并搭建平台验证了其在定位方面的实用性，并获取了较高的定位精度。

1 UWB 与惯性导航

■1.1 UWB 测距原理

UWB 定位原理，首先测量基站与目标节点之间的距离，再由算法求解目标节点的位置坐标，本文利用TOA 发进行测距。TOA 定位法假设目标结点发射信号的时刻为t，第i个已知参考基站收到目标节点信号的时刻为ti，i=1～6。已知第i 个参考基站的三维空间坐标为（xi,yi,zi），待测目标节点坐标为（x,y,z），建立方程如式（1），并得出第i 个已知参考基站与目标节点的直线距离。

式中，ζi为第i 个已知参考基站接收到的TOA 值。在理想无误差情况下，计算所得到的方程组的解只有一个，即有唯一交点，有误差时即为一个区域。

TOA 算法要求参与定位的所有参考节点都要与目标节点在时间上精确同步，否则定位精度将急速下降，此要求在实际中是很难达到[7]。

■1.2 惯性导航的运动轨迹

初始的四元数值根据初始的姿态角确定，将加速度计的测量信息经过余弦矩阵由地理坐标系转换到载体坐标系中，根据初始的经纬度、高度和速度值和变换后的比例信息进行指北方位系统的运动解算，求出指令角速度信息、速度信息和位置信息。式（2）为地理坐标系x、y、z 轴方向上的加速度值与其对应的角速度值、速度值的关系。

根据四元数和欧拉角之间的关系求解出新的姿态角。式（3）为姿态角与方向余弦矩阵的关系。

式中θ、γ、φ 分别为载体在三维空间中的俯仰角、横滚角和航偏角。

经过以上步骤即可对指北方位惯性导航运动轨迹进行解算。

2 UWB 与惯性导航融合定位算法

为解决定位精度偏低的问题，将UWB 和惯性导航的定位数据进行融合。利用九轴惯导模块获得待测目标节点的加速度、航偏角、横滚角等传感数据，待测节点运动的距离和方向通过互补滤波器进行航迹推算，互补滤波器如图1 所示。以三维空间室内定位为例，由已知固定基站初始位置对航迹推算系统的起始位置进行初始化，在目标待测节点运动过程中，获取UWB 与惯性导航的定位估值后，利用信赖度对两组数据进行加权融合，确定定位结果。

图1 互补滤波器

当仅用UWB 模块定位时，实际测得的接收信号强度RSS（Received Signal Strength，RSS）矢量表示为（rss1，…，rssn）。从第i 个定位基站收到的RSS 值rssi(i=1，2，..，n)，与初始化库中的RSS 进行对比，选取欧式距离最近的3 个RSS：，对应的样本点位置作加权质心计算定位结果。当已知参考基站与目标节点的直线距离R 与3 个样本点的最小欧式距离Dis=min(||R-R'||，||R-R''||，||R-R'''||)越小，定位结果可信度越高。当Dis 大于阈值等于μ 时，认为该定位结果无效。UWB 定位数据的可信度采用信赖度因子α ∈[0，1]描述。式（4）为信赖度因子α 与Dis 关系。

惯性导航系统时间越长，其误差累积越大，当导航时间超过x秒后，定位误差过大，认为其定位结果不可信。为描述惯导系统定位数据的可信度，定义信赖度因子β ∈[0,1]，该值在x秒内随时间以1/x 的斜率递减，β 与时间t 的关系如式（5）。

为有效利用惯性导航信息，当信赖度低于阈值ω 时，重新推算载体位置并更新计时。融合后的定位数据由式（6）计算获得。

其中loc1为UWB 定位估计位置，loc2为惯性导航估计位置，locend表示最终估计位置。

实测中，设UWB 模块采集周期为T1，惯性导航模块采集周期为T2，在一个RSS 采集周期内会有连续多个运动传感信息，即T1＞T2。在每个T1时刻由式（6）进行定位信息的融合，并将融合的位置作为每轮的初始位置，在T1时间间隔内由惯性导航推算载体的运动位置。此算法由于多种测量误差的影响会表现出运动轨迹的波动，可通过滑动平均等方法对位置信息进行滤波。

3 测试验证

为验证此融合算法，测试平台如图2 所示。本系统包括UWB 定位基站、待测目标节点、上位机。本测验首先需将基站固定安装，6 个基站放置在一个铁架的6 个点，彼此间距2 米，并设其中一个基站为三维坐标原点。通过WIFI建立可与上位机通信的网络。在物体定位跟踪过程中，目标节点将实时测得的RSS 值和惯性导航信息传送至上位机，由上位机使用基于信赖度的加权融合定位算法获得待测目标节点的位置信息。

图2 测试平台示意图

固定基站的硬件由爱尔兰DecaWave 公司的UWB 集成模块、意法半导体公司的STM32 芯片与外围电路组成；待测目标节点由UWB 集成模块、STM32 芯片及美国PNI公司的九轴惯导模块、WIFI 模块与外围电路组成。固定基站如图3 所示；待测节点如图4 所示。

图3 固定基站

图4 待测节点

采用MATLAB 进行上位机程序编写及仿真测试。测试地点为郑州西亚斯学院一个200m2的空旷房间，待测节点位置每0.5 秒在MATLAB 中记录1 次。仅用UWB 模块进行的三维定位效果如图5 所示，待测节点在室内移动时的定位显示会有较为明显的发散现象。UWB与惯导模块加权融合后的定位效果如图6 所示，将UWB 与惯导模块融合后再做定位测试，虽然采集时间更长，但待测节点正常移动时的发散现象明显下降。定位精度经与实际位置对比，定位误差都在20cm 以内，定位算法可靠有效。

图5 仅用UWB 模块的三维定位效果

图6 UWB 与惯导模块结合后的定位效果

4 结束语

本文介绍了基于TOA 测距的UWB 定位算法和惯性导航运动轨迹及姿态角解算步骤，提出了一个基于UWB 与惯性导航相互融合的加权算法，以提高对物体在三维空间长时间定位效果。最后利用硬件平台做了定位测验并在上位机进行了实测记录。实测表明，将UWB 与惯性导航进行加权融合的空间定位方案，有效提高了定位精度，在移动物体三维空间的定位应用中，具有一定的优越性与实用性。