基于UWB和IMU的煤矿机器人紧组合定位方法研究

郁露，唐超礼，黄友锐，韩涛，徐善永，付家豪

（安徽理工大学 电气与信息工程学院，安徽 淮南 232001）

0 引言

煤矿井下工作环境恶劣，对井下工作人员的人身安全造成极大威胁。煤矿无人开采或者少人开采日渐成为研究热点，引入煤矿机器人代替井下工作人员完成危险、繁重的井下工作，将为解决煤矿安全生产问题发挥重要作用[1]。

煤矿机器人可以应用于掘进、运输及救援等方面，精准定位是其智能化实现的基础。为解决煤矿机器人的精准定位问题，目前常见的定位方法包括基于射频识别定位技术、基于超声波定位技术、基于超宽带（Ultra Wide Band,UWB）定位技术、基于惯导定位技术等方法。张晓莉等[2]提出使用扩展卡尔曼滤波对射频识别信息和惯导解算信息进行融合的方法，实现了煤矿机器人实时高精度的定位。谭玉新等[3]提出了基于无损卡尔曼滤波的超声网络定位算法，使用无损卡尔曼滤波将超声网络定位和电子罗盘及光电码盘定位所得到的航向角度信息及位置坐标信息进行组合，达到了降低定位误差的目的。陈美蓉等[4]提出了一种基于超宽带的煤矿井下定位混合解算方法，使用头脑风暴优化和泰勒级数展开的混合定位方法对人−机−物位置进行解算，解决了使用泰勒级数展开需要较好初值的问题。马宏伟等[5]提出了基于捷联惯导和里程计融合的煤矿机器人定位方法，首先通过卡尔曼滤波校准捷联惯导，再将捷联惯导解算出的煤矿机器人位置信息与里程计解算的位置信息通过自适应卡尔曼滤波进行校正，得到组合定位结果，定位精度较高。杨金衡等[6]提出了基于自适应卡尔曼滤波的双惯导定位方法，使用2套惯导系统所获得的加速度信息及角速度信息建立了双惯导模型。但由于井下环境复杂，使得许多定位技术在井下使用受限。射频技术定位不能对煤矿机器人进行实时跟踪，超声波技术定位会受到多普勒效应等影响，定位精度低，且成本高。UWB作为一种新兴技术，有较高的时间分辨率[7]，将其应用在室内环境定位中，相比于其他定位技术有更高的稳定性及定位精度。但UWB技术在煤矿机器人定位中单独使用时，受UWB基站布置、多径效应、非视距误差等对定位精度的影响，其定位结果具有一定的波动性。惯性测量单元（Inertial Measurement Unit，IMU）工作时，环境因素对其干扰较小，更新速率高，在短时间内定位精度高，故可适应较为复杂的井下环境。但单独使用IMU定位的主要缺点是其定位误差会随着时间累计，导致长时间定位精度较低。为实现煤矿机器人精准定位，火元亨[8]将UWB测距信息与IMU定位信息通过扩展卡尔曼滤波进行松组合定位，定位精度有一定提高，但UWB测距过程中由于非视距误差等因素的影响，使定位结果仍存在一定的误差。

为了减小UWB测距的非视距误差，实现煤矿机器人的准确定位，本文提出了一种基于UWB和IMU的煤矿机器人紧组合定位方法。引入最小二乘支持向量机（Least Square Support Vector Machine，LSSVM）对UWB测距信息进行修正，以减小井下定位的非视距误差；利用误差状态卡尔曼滤波（Error-State Kalman Filter,ESKF）将修正后的UWB测距信息及通过惯性导航解算出的距离信息紧组合，更新煤矿机器人的位置信息，实现精确定位。使用UWB模块及IMU获得的煤矿机器人的实验数据，在Matlab中对紧组合定位方法进行静态和动态定位仿真实验，结果证明了该方法的可靠性。

1 紧组合定位方法

基于UWB和IMU的煤矿机器人紧组合定位方法原理如图1所示。利用UWB测距模块得到煤矿机器人与UWB基站之间的距离实测值，使用煤矿机器人与UWB基站之间的距离真实值和实测值训练LSSVM模型，得到LSSVM修正模型，并对煤矿机器人定位过程中UWB测距信息进行修正；利用IMU采集煤矿机器人的加速度及角速度信息[9]，使用惯性导航解算出煤矿机器人的状态信息。将经过LSSVM修正后的测距信息作为ESKF的量测输入，与惯性导航解算出的位置信息构成其量测方程，完成状态更新，得到更为精确的煤矿机器人位置信息，实现对煤矿机器人的精确定位。

图1 基于UWB和IMU的煤矿机器人紧组合定位方法原理Fig.1 Principle of tightly combined positioning method of coal mine robot based on UWB and IMU

1.1 UWB测距信息修正

UWB测距信息是非视距环境下所采集得到的[10]，严重的遮挡等因素导致UWB信号不能够直接传递，这将使信号在介质中传播的时间加长，测距信息不准确，最终导致定位效果较差。为了减小由环境带来的定位误差，需要对非视距环境下的UWB测距值进行处理。LSSVM模型适用于非线性估计，当样本数据较小时，也有很好的性能。本文使用LSSVM模型对测距误差进行修正。

LSSVM模型回归原理[11]可以描述如下。设有训练集{(xi,yi)}li=1， xi为UWB测距模块获得的实测值，作为LSSVM模型的输入值，yi为煤矿机器人与UWB基站之间的距离真实值，作为LSSVM模型的期望输出值，l为训练集样本数量。将样本中的数据映射到高维空间，可以得到回归方程：

式中：w 为权向量；ϕ(xi)为建立的输入数据和高维特征空间的映射，将非线性的样本变成线性可分的数据[12]；b为偏差。

LSSVM模型将SVM优化问题的非等式约束用等式约束替换。具有等式约束的LSSVM模型可以描述为

式中：η为正则化参数；ξi为随机拟合误差。

引入拉格朗日法来解决式（2）中的优化问题，构造函数为

式中：L 为拉格朗日函数；αi为拉格朗日乘子。

对式（3）求偏导，可得

决策函数定义为

式中：yj为UWB测距信息修正值；H为核函数，xj为煤矿机器人定位过程中UWB模块获得的实测值。

LSSVM模型选择高斯径向基函数（Radial Basis Function，RBF）作为核函数，核函数宽度为σ。

选择UWB测距实测值d′作为LSSVM模型的输入，选择UWB基站与煤矿机器人之间的真实值d作为输出，训练LSSVM模型，得到LSSVM修正模型。

将煤矿机器人在定位过程中UWB模块测得的实测值作为LSSVM修正模型的输入，通过LSSVM修正模型对UWB实测值进行修正，得到较为准确的距离信息。

1.2 IMU定位原理

导航坐标系（N系）采取UWB所在坐标系，载体坐标系（B系）使用左前上坐标系。从B系转换到N系需要通过旋转矩阵

式中：θ为航向角；θk为 k 时刻的航向角；Ts为采样周期；为k−1时刻在N系下的角速度。

IMU定位原理的数学模型[13]可以表示为

式中：pk为k时刻位置；vk为k时刻速度；qk为k时刻姿态；ak为k时刻加速度；g为 重力加速度；Ωk为k时刻四元数更新矩阵。

1.3 UWB和IMU紧组合定位

煤矿机器人搭载的UWB/IMU标签中内置IMU芯片，可以输出煤矿机器人的三轴加速度和三轴角速度信息；UWB模块可以测得煤矿机器人距离每个UWB基站的欧氏距离。紧组合将UWB和IMU作为一个测量传感器，利用IMU获取的煤矿机器人位置信息估算煤矿机器人与UWB基站之间的距离，与UWB测得的煤矿机器人与UWB基站之间的距离信息组合。UWB和IMU紧组合定位方法主要由UWB测距值修正部分及数据融合部分组成。

1.3.1 UWB测距值修正

UWB测距值修正具体实现步骤如下：

（1）在实验场景中，使用UWB测距模块对不同真实距离（1，2，…，18 m）测量300次并取其平均值作为不同真实距离的实测值。

（2）初始化LSSVM模型参数，并将18个真实值及其对应的18个实测值输入LSSVM模型中进行训练，得到LSSVM修正模型。

（3）将煤矿机器人运动过程中UWB测距实测值输入训练好的LSSVM修正模型中进行预测，得到较准确的UWB测距信息，用于ESKF数据融合。

1.3.2 数据融合

使用ESKF将UWB和IMU紧组合。这里设计煤矿机器人的位置误差 δpN（三维）、目标点的速度误差δvN（三维）、失准角φ（三维）、加速度零偏误差δba（ 三维）和陀螺零偏误差δbg（三维）为状态向量，得到状态方程:

式中：δX=[δpNδvNφ δbgδba]；F（t）为动态矩阵；G（t）为噪声输入系数矩阵[13]； s(t)为噪声。

由于IMU通常是高速率采样数据，所以需要将连续时间系统方程转换成相应的离散系统方程：

式中：ψk−1为状态转移矩阵，具体推导过程参考文献[14]；sk−1为 k−1时刻的噪声；I为单位矩阵。

利用IMU获取的煤矿机器人位置信息估算煤矿机器人与UWB基站之间的距离，与UWB测距信息的差值构造量测方程。ESKF的量测输入为经过LSSVM模型修正后的煤矿机器人距离每一个UWB基站的UWB测距修正值dk，卡尔曼状态更新方程[15-17]为

式中：Xk为 k时刻更新后状态；为k 时刻的先验状态估计值；K为卡尔曼增益；h为利用IMU获取的煤矿机器人位置信息估算的煤矿机器人与UWB基站之间的距离，其输入为惯性导航系统解算的煤矿机器人位置pk，输出为煤矿机器人与基站的距离。

式中c为UWB基站的位置。

2 实验分析

为了验证基于UWB和IMU的煤矿机器人紧组合定位方法的定位精度，在模拟井下巷道中进行了实验。在实验中，首先对基站的不同布置方式进行静态定位实验，分析UWB基站所在位置对定位精度的影响，确定基站布置的最佳方案；然后使用LSSVM修正模型修正前后的UWB测距信息分别进行UWB和IMU紧组合定位实验，比较2种方式的定位误差；最后进行UWB和IMU紧组合定位动态实验，比较经过LSSVM修正模型修正前后融合的煤矿机器人运动轨迹。

2.1 实验器材及环境

本次实验所涉及的实验器材包括4个LD150型UWB基站、1个LD150−I型UWB/IMU标签（其中IMU型号为ICM−426005）、1台便携式计算机、1台Turtlebot2机器人、若干数据线及1个三脚架等。其中UWB/IMU标签采样间隔为10 ms。实验场景模拟井下巷道，如图2所示。4个UWB基站固定在墙上，UWB/IMU标签固定在煤矿机器人上。

图2 实验场景Fig.2 Experimental scenario

实验中使用的软件环境：Intel i5−8250处理器，Windows10操作系统，软件为Matlab2018a，串口调试助手为ATK XCOM V2.0版本。

2.2 UWB定位静态实验分析

针对煤矿机器人定位的实际情况，实验设置了3种UWB基站布置方案。方案1：4个基站等高对称分布[18]；方案2：将方案1中基站2和基站3的高度移动至1 m；方案3：将方案1中基站2和基站3无规律放置。使用均方根误差作为测试指标，分析不同布置方案及使用LSSVM修正模型对测距数据进行处理对定位精度的影响。不同UWB基站位置坐标见表1。

表1 UWB基站位置坐标Table 1 UWB base station layout coordinates m

实验中，对搭载UWB/IMU标签的煤矿机器人在同一位置与1个UWB基站之间的距离进行多次测量，取1 500次UWB测距结果进行仿真实验。搭载标签的煤矿机器人与UWB基站之间的真实欧氏距离为9.6 m，仿真结果如图3所示。可看出经过LSSVM修正模型修正后的UWB测距值比实测值小，更接近煤矿机器人与UWB基站之间的真实距离，减小了UWB测距的非视距误差。

图3 UWB测距仿真结果Fig.3 UWB ranging simulation results

对LSSVM修正模型处理前后的数据进行静态定位仿真实验。实验中搭载UWB/IMU标签的煤矿机器人的真实位置为（−1.8 m，7.2 m）。在表1中的3种方案中分别进行定位解算，3种基站布置方案的定位结果如图4所示。经过计算实际位置与使用UWB信息解算位置之间的均方根误差，得到3种基站布置方案的计算结果，见表2。

根据图4和表2可看出：当UWB基站等高对称布置时，定位结果更接近真实位置，其均方根误差最小，定位精度最高；当搭载UWB/IMU标签的煤矿机器人在基站布置的范围外（方案3），其定位精度最低。对比LSSVM修正模型修正前后的定位结果，可看出经过LSSVM修正模型对UWB测距信息修正后，在3种基站布置方案下，UWB和IMU紧组合定位均方根误差均有所减小。

表2 3种基站布置方案的实验均方根误差Table 2 Experimental root mean squareerror of three basestations layout schemes m

图4 3种基站布置方案的定位结果Fig.4 Positioning results of threebase stationslayout schemes

2.3 UWB/IMU紧组合定位动态实验分析

动态实验过程中，煤矿机器人沿L型路线行驶，使用Matlab对获取的实验数据进行定位仿真，实验结果如图5所示。可看出使用LSSVM修正模型对UWB测距值进行修正后的融合定位轨迹相较于未修正的融合定位轨迹更接近于实际轨迹，UWB测距修正后定位均方根误差比修正前降低了4.5%；将LSSVM修正模型修正后的UWB测距信息用于紧组合定位解算，效果更优，更适用于煤矿机器人定位。

图5 UWB/IMU紧组合定位结果Fig.5 UWB/IMU tightly combined positioning results

3 结论

（1）提出了一种基于UWB和IMU的煤矿机器人紧组合定位方法，使用LSSVM修正模型对所测得的UWB测距信息进行修正，减小了非视距误差对UWB测距的影响；使用ESKF对UWB和IMU紧组合，达到了提高定位精度的目的。

（2）研究了UWB基站不同布置方案下，经过LSSVM修正模型处理的UWB测距信息对煤矿机器人定位精度的影响。实验结果表明：当UWB基站在应用场景中等高对称布置时，相比于其他布置方式，其定位均方根误差较小，故在实际场景中，应使UWB基站尽可能等高对称布置；对利用LSSVM修正模型修正前后的UWB测距信息分别进行UWB和IMU紧组合实验，结果表明：经过LSSVM修正模型修正后的组合定位均方根误差有所减小，测距信息修正后融合定位轨迹相较于未修正的融合定位轨迹更接近煤矿机器人运动的真实轨迹。LSSVM修正模型对UWB测距信息修正后组合定位精度更高，更适用于煤矿机器人定位。