UWB辅助INS的室内组合定位方法

隋 心，高 嵩，黄俊尧，刘 韬，徐爱功

(1.辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000；2.武汉大学 卫星导航定位技术研究中心，湖北 武汉 430079)

0 引言

随着人工智能、智慧城市和无线网络的不断发展，无线定位技术越来越受到人们的关注。以全球卫星导航系统和组合导航系统为代表的室外定位技术相对趋于成熟[1-2]。在室内定位技术中，UWB定位技术具有测距定位精度更高、稳定性更强的定位能力，在视距(Line of Sight，LOS)环境下能达到厘米级甚至更高级别的测距定位精度[3]。UWB系统虽能提供高精度的定位结果，但其定位结果受NLOS误差影响明显。虽然采用NLOS鉴别与抑制方法可在一定程度上削弱该项误差影响，但是当UWB受到长时间的稳态NLOS误差影响时，目前没有有效的方法较好地解决这一问题。此外，UWB定位系统无法提供载体的姿态角信息，其数据更新速率较慢，难以满足实时快速定位需求[4-5]。INS系统短期定位精度高，不受外界干扰，自主性强，然而其导航定位误差随时间迅速累积[6-8]。UWB与INS组合能充分利用2个子系统的优点，一方面利用INS的短期高精度特性消除UWB中NLOS测距误差的影响；另一方面利用UWB系统修正INS的误差发散，从而得到稳定性更高、可靠性更强的定位结果，还能提供载体的姿态角信息，UWB/INS高频率的定位结果也能满足实时定位的需求[9-11]。

UWB测距方法有多种，由于基于往返时间(Round Trip Time，RTT)测距在测距精度和基站布设效率方面更具优势，因此被广泛关注，但该测距方法存在测距时间不同步问题[12-13]。在UWB/INS组合中，为了避免UWB测距的时间不同步误差影响，本文设计了UWB辅助INS的室内组合定位方法。该方法利用单个UWB RTT测距值和INS系统进行组合，采用一维的UWB/INS紧组合模型对组合系统观测数据进行处理，以此消除UWB测距的时间不同步误差影响。

1 UWB RTT测距误差分析

UWB测距方法通常包括：基于角度到达(Angle of Arrive，AOA)、基于接收信号强度(Received Signal Strength，RSS)、基于到达时间(Time of Arrive，TOA)、基于到达时间差(Time Difference of Arrive，TDOA)和基于RTT等5种测距方法。上述UWB测距方法中，基于TOA、基于TDOA和基于RTT方法均利用了UWB信号极高时间分辨率的特点，在测距定位过程中均能有效发挥UWB信号的特点与优势。相对于TOA和TDOA方法，RTT方法不需要保持UWB基准站之间、UWB基准站与流动站之间的时间同步，因此本文采用RTT测距方法进行UWB测距。

在采用RTT测距进行定位过程中，UWB流动站依次向各UWB基准站发送脉冲信号，UWB基准站识别接收到脉冲信号后再产生响应脉冲信号并将其发送出去，UWB流动站识别接收到响应脉冲信号后完成1次测距。UWB流动站重复上述过程直到与所有的基准站之间都完成测距。由于UWB流动站和基准站之间测距存在时间延迟(包括信号响应延迟、硬件延迟、信号传输时间和NLOS传播时间延迟)，每一次产生的时间延迟将累加到下一次测距过程中，导致UWB流动站的同一组测距信息中的时间不同步，UWB测距中的时间不同步情况如图1所示。

图1 UWB RTT测距中的时间不同步Fig.1 Time out of sync in UWB RTT Ranging

UWB测距中的时间不同步将限制UWB定位系统只能进行载体低速状态下的定位，极大地限制了UWB定位的应用范围，比如当UWB流动站与1个基准站之间的时间延迟为0.02 s，在载体运动速度为2 m/s的情况下将产生4 cm的定位误差，在4个基准站的情况下将造成16 cm的定位误差。因此，UWB定位的精度不仅会受到NLOS测距误差和测距异常值的影响，还会受到时间不同步的影响。

2 UWB辅助INS组合定位模型

2.1 INS误差方程

由文献[14]可得经纬高形式的INS误差方程为：

(1)

且有：

(2)

经纬高形式的INS误差方程以弧度为单位，在中小型区域内，纬度和经度误差较小，也存在与高度误差数值差距太大而产生舍入误差。UWB/INS组合场景多为室内环境，因此本文推导了ENU形式的位置误差方程。对惯性导航系统的经度、纬度和高度微分方程式变形可得：

(3)

对式(3)右边等式两边同时求导数，整理成矩阵形式，即可得到ENU形式的INS位置误差方程：

(4)

且有：

(5)

同时，对纬度和高度进行替换，可得ENU形式的INS姿态、速度误差方程：

(6)

且有：

(7)

整理可得ENU形式的INS误差方程为：

(8)

2.2 状态模型构建

(9)

式中，F(t)为系统状态转移矩阵；G(t)为系统的噪声映射矩阵；W(t)为陀螺仪和加速度计的随机噪声，假设其为零均值的高斯白噪声，其协方差矩阵为Qk，结合式(8)，有：

(10)

式中，wg,wa分别为陀螺仪和加速度计随机噪声。

2.3 量测模型构建

在UWB/INS组合中，利用INS解算的位置反算UWB流动站天线与UWB基准站天线之间的距离为dINS，利用UWB的RTT方法量测的UWB流动站天线与UWB基准站天线之间的距离为dUWB，假设UWB流动站天线与UWB基准站天线之间的真实距离为dtrue，则：

(11)

(12)

(13)

(14)

从而得到式(13)的近似表达式为：

(15)

UWB量测的RTT测距信息可以表示为：

dUWB=dtrue+εd，UWB。

(16)

为了避免UWB测距的时间不同步误差影响，本文设计了UWB辅助INS的室内组合定位方法，即利用单个UWB RTT测距值和INS系统进行组合，通过式(15)和式(16)，可得UWB辅助INS的量测方程为：

Zk=HkXk+Vk，

(17)

式中，Hk为量测矩阵；Vk为量测噪声向量，假设为零均值的高斯白噪声，其协方差矩阵为Rk，且有：

(18)

2.4 组合滤波方法

由于标准卡尔曼滤波只能处理离散方程，因此需要将线性连续形式的状态方程进行离散化处理，对于离散化后的线性系统，卡尔曼滤波的基本流程为[13-16]：

① 计算预测状态向量及其协方差矩阵：

(19)

② 计算增益矩阵，计算新的状态向量估值及其协方差矩阵：

(20)

由于UWB/INS组合系统中UWB的RTT测距信息会受到NLOS测距误差和测距异常值的影响，因此UWB辅助INS系统采用自适应抗差方法提高组合系统的精度和稳定性，其关键步骤为：

(21)

(22)

通过上述过程可以完成UWB辅助INS系统中卡尔曼滤波的自适应滤波和抗差估计处理，从而提高组合系统的精度、稳定性和可靠性。

3 实验与分析

利用一组室内实验对本文所述方法进行分析验证。实验中以Time Domain的PulsON400通信与测距模块作为UWB数据采集模块，其带宽为3.1～5.3 GHz，数据采集频率为2 Hz；利用国产某组合导航产品进行陀螺仪和加速度计的数据采集，该组合导航产品内置NovAtel的OEM617D专业型GNSS板卡、GNSS与INS的时间同步系统和惯性测量单元，其陀螺仪的零偏稳定性每小时不超过0.1°，零偏重复性每小时不超过0.1°，其加速度计的零偏稳定性不超过60 ug，零偏重复性不超过60 ug；利用Leica TS09建立UWB定位的坐标系统，其精度为1.5 mm+2 ppm，利用其测量UWB基准站天线的三维坐标并假定没有误差。实验中采用硬件设备如图2所示。

图2 UWB/INS组合实验中采用的硬件设备Fig.2 The hardware used in the UWB and INS combined experiment

UWB与INS组合实验中，将UWB模块固定组合导航产品上方，利用螺丝将组合导航产品固定在实验铝板上。实验开始前测量UWB天线中心与INS仪器中心的杆臂，UWB时间采用电脑时间，利用组合导航产品对实验笔记本电脑进行授时，保持UWB系统和INS系统的时间同步。

为了验证分析文中UWB辅助INS的组合定位方法，在实验过程中分别采用以下5种方法进行定位解算：

方法1：标准卡尔曼滤波的UWB定位(常规UWB)；

方法2：自适应抗差卡尔曼滤波的UWB定位(改进UWB)；

方法3：UWB/INS松组合导航定位(松组合)；

方法4：自适应抗差卡尔曼滤波的UWB/INS紧组合定位(紧组合)；

方法5：自适应抗差卡尔曼滤波的UWB辅助INS定位(辅助)。

实验中仅有3个UWB基准站，且实验区域很平缓，故利用全站仪观测UWB流动站天线的高度，将其作为高度约束加入到上述5种方法中，将高度误差大小设为5 cm。

实验环境为辽宁工程技术大学惠和楼207会议室，利用三脚架安装3个UWB基准站，在通电后利用全站仪确定其三维坐标并保持不动，实验平台上去掉GNSS天线和连接线。实验过程中墙柱和实验人员对UWB信号造成遮挡，由人推着小车沿固定轨迹移动，每隔一段时间利用全站仪测量UWB流动站的天线中心坐标，并将其作为参考真值，整个实验时长750 s，前300 s为静止状态，后450 s为运动状态。实验中运动部分的UWB原始测距信息如图3所示，可以看出UWB原始测距中存在部分的NLOS测距误差和测距异常值。陀螺和加速度计输出信号分别如图4和图5所示。

图3 室内实验中的UWB测距信息Fig.3 UWB ranging information in the indoor experiment

图4 室内实验中陀螺数据Fig.4 Gyro data in the indoor experiment

图5 室内实验中加速度计数据Fig.5 Accelerometer data in the indoor experiment

实验中UWB解算的平面轨迹和UWB/INS组合解算的平面轨迹如图6和图7所示。图中红色五角星为全站仪测量的UWB流动站天线的坐标。实验小车共停10次，每次利用全站仪确定UWB流动站天线坐标并将其作为参考真值，在全站仪观测期间利用5种方法解算的位置平均值与全站仪观测的位置进行比较。5种方法的东向误差和北向误差如图8和图9所示。5种方法在东方向和北方向的误差统计信息如表1所示。

图6 室内UWB解算的平面轨迹Fig.6 Calculated plane trajectory used by UWB

图7 室内UWB/INS组合解算的平面轨迹Fig.7 Calculated plane trajectory used by indoor UWB and INS integrated method

图8 室内实验中5种方法在东方向的位置误差Fig.8 East position error of 5 methods in the indoor experiment

图9 室内实验中5种方法在北方向的位置误差Fig.9 North position error of 5 methods in the indoor experiment

表1 室内实验中5种方法的误差统计信息Tab.1 Error statistics of 5 methods in the indoor experiment

通过5种方法实验结果和误差统计可以得到：

① 在实验过程中，每次静止时由于人的遮挡较为频繁，UWB的原始测距中频繁的短时间的NLOS测距误差较多。此时常规UWB定位方法的结果波动明显，稳定性很差；而自适应抗差卡尔曼滤波方法能利用提前设定的阈值判断NLOS测距误差，并降低含有NLOS测距误差项的测距信息的权重，结合自适应估计方法降低系统模型的误差，能大幅度提升定位的精度和稳定性。

② 实验中UWB/INS松组合的定位精度和自适应抗差卡尔曼滤波的UWB方法精度水平相当，且在单独UWB的基础上略微提高；由于UWB/INS紧组合和UWB辅助INS定位方法中加入了抗差估计方法，对原始测距中的NLOS测距误差和测距异常值具有较好的抗干扰能力，能提升系统的定位精度，其平面误差达到了0.05 m；UWB辅助INS定位方法避免了UWB同一组测距中的时间不同步，能在UWB/INS紧组合的基础上进一步提高系统的精度和稳定性，其平面位置精度优于0.05 m。

③ 需要说明的是，由于室内定位中很难精确地测定UWB流动站天线坐标，且实验过程中受到了人为的频繁干扰，实验环境的不同也可能会对实验结果产生影响，因此室内定位实验中不同方法的误差精度仅代表各方法的大体精度水平。

4 结束语

在室内定位技术中，UWB与INS组合定位可实现优势互补，是当前研究热点。目前已有的组合方法通常认为同一组UWB测距是时间同步的，但经过分析，UWB RTT测距方法存在同一组测距时间不同步现象，针对该问题本文设计了UWB辅助INS数学模型，利用单个UWB RTT测距值和INS系统进行组合，以此消除UWB测距的时间不同步误差影响。室内定位实验表明，UWB/INS组合系统比单独UWB系统有更高的精度和更强的稳定性，UWB/INS紧组合方法能有效减小UWB定位过程中产生的误差，UWB辅助INS定位方法在UWB/INS紧组合的基础上能有效避免UWB同一组测距中的时间不同步误差，进一步提高了室内定位的精度、稳定性和可靠性。