针对煤矿井下环境复杂，现有煤矿机器人定位方法受非视距误差等因素影响导致定位精度低、实时性不高等问题，提出了一种基于 UWB（超宽带）和 IMU（惯性测量单元）的煤矿机器人紧组合定位方法。首先利用UWB 模块测量煤矿机器人与 UWB 基站之间的距离，使用煤矿机器人与 UWB 基站之间的距离真实值和实测值训练最小二乘支持向量机（LSSVM）模型，得到 LSSVM 修正模型；然后将煤矿机器人定位过程中 UWB 模块测得的实测值作为 LSSVM 修正模型的输入，通过 LSSVM 修正模型对 UWB 实测值进行修正，减小非视距误差对定位精度的影响，得到较为准确的距离信息；最后将经过 LSSVM 修正模型修正后的测距信息作为误差状态卡尔曼滤波（ESKF）的量测入，与惯性导航解算出的位置信息构成量测方程，使用 ESKF 对 UWB 测距修正值与惯性导航解算的距离信息紧组合，完成状态更新，得到更为精确的位置信息，实现煤矿机器人的精确定位。 UWB 基站不同布置方案下的模拟实验结果表明：使用 LSSVM 修正模型可使 UWB 测距信息更为准确，进而提高定位精度。静态定位实验时，当 4 个 UWB 基站等高对称布置时，定位的均方根误差由 0.146 4 m 减小到 0.1398 m；当4 个 UWB 基站不等高对称布置时，均方根误差由 0.300 8 m 减小到 0.200 6 m；当 4 个基站无规律布置时，均方根误差由 0.317 5 m 减小到 0.314 2 m。因此，在实际场景中，应尽可能使 UWB 基站等高对称布置。动态定位实验时，通过 LSSVM 修正模型对 UWB 测距信息进行修正后的融合定位轨迹相较于修正前的融合定位轨迹更接近煤矿机器人的真实轨迹，验证了该紧组合定位方法能够减小非视距误差，提高定位精度。
0 引言
煤矿井下工作环境恶劣，对井下工作人员的人身安全造成极大威胁。煤矿无人开采或者少人开采日渐成为研究热点，引入煤矿机器人代替井下工作人员完成危险、繁重的井下工作，将为解决煤矿安全生产问题发挥重要作用[1]。
煤矿机器人可以应用于掘进、运输及救援等方面，精准定位是其智能化实现的基础。为解决煤矿机器人的精准定位问题，目前常见的定位方法包括基于射频识别定位技术、基于超声波定位技术、基于超宽带（Ultra Wide Band, UWB）定位技术、基于惯导定位技术等方法。张晓莉等[2]提出使用扩展卡尔曼滤波对射频识别信息和惯导解算信息进行融合的方法 ，实现了煤矿机器人实时高精度的定位 。谭玉新等[3]提出了基于无损卡尔曼滤波的超声网络定位算法，使用无损卡尔曼滤波将超声网络定位和电子罗盘及光电码盘定位所得到的航向角度信息及位置坐标信息进行组合，达到了降低定位误差的目的。陈美蓉等[4]提出了一种基于超宽带的煤矿井下定位混合解算方法，使用头脑风暴优化和泰勒级数展开的混合定位方法对人-机-物位置进行解算 ，解决了使用泰勒级数展开需要较好初值的问题。马宏伟等[5]提出了基于捷联惯导和里程计融合的煤矿机器人定位方法，首先通过卡尔曼滤波校准捷联惯导，再将捷联惯导解算出的煤矿机器人位置信息与里程计解算的位置信息通过自适应卡尔曼滤波进行校正，得到组合定位结果，定位精度较高。杨金衡等[6]提出了基于自适应卡尔曼滤波的双惯导定位方法，使用 2 套惯导系统所获得的加速度信息及角速度信息建立了双惯导模型。但由于井下环境复杂，使得许多定位技术在井下使用受限。射频技术定位不能对煤矿机器人进行实时跟踪，超声波技术定位会受到多普勒效应等影响，定位精度低，且成本高。 UWB 作为一种新兴技术，有较高的时间分辨率[7]，将其应用在室内环境定位中，相比于其他定位技术有更高的稳定性及定位精度。 但 UWB 技术在煤矿机器人定位中单独使用时，受 UWB 基站布置、多径效应、非视距误差等对定位精度的影响，其定位结果具有一定的波动性 。惯性测量单元（InertialMeasurement Unit，IMU）工作时，环境因素对其干扰较小更新速率高，在短时间内定位精度高，故可适应较为复杂的井下环境。但单独使用 IMU 定位的主要缺点是其定位误差会随着时间累计，导致长时间定位精度较低。为实现煤矿机器人精准定位，火元亨[8]将 UWB 测距信息与 IMU 定位信息通过扩展卡尔曼滤波进行松组合定位，定位精度有一定提高，但UWB 测距过程中由于非视距误差等因素的影响，使定位结果仍存在一定的误差。
为了减小 UWB 测距的非视距误差，实现煤矿机器人的准确定位 ，本文提出了一种基于 UWB 和IMU 的煤矿机器人紧组合定位方法。引入最小二乘支 持 向 量 机 （Least Square Support Vector Machine，LSSVM）对 UWB 测距信息进行修正，以减小井下定位的非视距误差；利用误差状态卡尔曼滤波（ErrorState Kalman Filter, ESKF）将修正后的 UWB 测距信息及通过惯性导航解算出的距离信息紧组合，更新煤 矿 机 器 人 的 位 置 信 息 ， 实 现 精 确 定 位 。 使 用UWB 模块及 IMU 获得的煤矿机器人的实验数据，在 Matlab 中对紧组合定位方法进行静态和动态定位仿真实验，结果证明了该方法的可靠性。
1 紧组合定位方法
基于 UWB 和 IMU 的煤矿机器人紧组合定位方法原理如图 1 所示。利用 UWB 测距模块得到煤矿机器人与 UWB 基站之间的距离实测值，使用煤矿机器人与 UWB 基站之间的距离真实值和实测值训练LSSVM 模型，得到 LSSVM 修正模型，并对煤矿机器人定位过程中 UWB 测距信息进行修正；利用 IMU采集煤矿机器人的加速度及角速度信息[9]，使用惯性导航解算出煤矿机器人的状态信息。将经过 LSSVM修正后的测距信息作为 ESKF 的量测输入，与惯性导航解算出的位置信息构成其量测方程，完成状态更新，得到更为精确的煤矿机器人位置信息，实现对煤矿机器人的精确定位。
1.1 UWB 测距信息修正
UWB 测距信息是非视距环境下所采集得到的[10]，严重的遮挡等因素导致 UWB 信号不能够直接传递，这将使信号在介质中传播的时间加长，测距信息不准确，最终导致定位效果较差。为了减小由环境带来的定位误差，需要对非视距环境下的 UWB测距值进行处理。 LSSVM 模型适用于非线性估计，当样本数据较小时 ，也有很好的性能 。本文使用LSSVM 模型对测距误差进行修正。
LSSVM 模型回归原理[11]可以描述如下。设有训练集 ， 为 UWB 测距模块获得的实测值，作为 LSSVM 模型的输入值， 为煤矿机器人与 UWB基站之间的距离真实值，作为 LSSVM 模型的期望输出值， 为训练集样本数量。将样本中的数据映射到高维空间，可以得到回归方程：
式中： 为权向量； 为建立的输入数据和高维特征空间的映射，将非线性的样本变成线性可分的数据[12]； 为偏差。
LSSVM 模型将 SVM 优化问题的非等式约束用等式约束替换。具有等式约束的 LSSVM 模型可以描述为
式中：η为正则化参数；ξi为随机拟合误差。
引入拉格朗日法来解决式（2）中的优化问题，构造函数为
式中：L为拉格朗日函数；αi为拉格朗日乘子。
对式（3）求偏导，可得
决策函数定义为
式中：yj 为 UWB 测距信息修正值； 为核函数， 为煤矿机器人定位过程中 UWB 模块获得的实测值。
LSSVM 模型选择高斯径向基函数（Radial BasisFunction，RBF）作为核函数，核函数宽度为 。
选择 UWB 测距实测值 作为 LSSVM 模型的输入，选择 UWB 基站与煤矿机器人之间的真实值作为输出 ，训练 LSSVM 模型 ，得到 LSSVM 修正模型。
将煤矿机器人在定位过程中 UWB 模块测得的实测值作为 LSSVM 修正模型的输入，通过 LSSVM修正模型对 UWB 实测值进行修正，得到较为准确的距离信息。
1.2 IMU 定位原理
导航坐标系（N 系）采取 UWB 所在坐标系，载体坐标系（B 系）使用左前上坐标系。 从 B 系转换到N 系需要通过旋转矩阵
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 数据融合。
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 标签固定在煤矿机器人上。
实验中使用的软件环境：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。
实验中，对搭载 UWB/IMU 标签的煤矿机器人在同一位置与 1 个 UWB 基站之间的距离进行多次测量，取 1 500 次 UWB 测距结果进行仿真实验。搭载标签的煤矿机器人与 UWB 基站之间的真实欧氏距离 为 9.6 m， 仿 真 结 果 如 图 3 所 示 。 可 看 出 经 过LSSVM 修正模型修正后的 UWB 测距值比实测值小，更接近煤矿机器人与 UWB 基站之间的真实距离，减小了 UWB 测距的非视距误差。
图 3 UWB 测距仿真结果
对 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 UWB/IMU 紧组合定位动态实验分析
动态实验过程中，煤矿机器人沿 L 型路线行驶，使用 Matlab 对获取的实验数据进行定位仿真，实验结果如图 5 所示。可看出使用 LSSVM 修正模型对UWB 测距值进行修正后的融合定位轨迹相较于未修正的融合定位轨迹更接近于实际轨迹，UWB 测距修正 后 定 位 均 方 根 误 差 比 修 正 前 降 低 了 4.5%； 将LSSVM 修正模型修正后的 UWB 测距信息用于紧组合定位解算，效果更优，更适用于煤矿机器人定位。
3 结论
（1） 提出了一种基于 UWB 和 IMU 的煤矿机器人紧组合定位方法，使用 LSSVM 修正模型对所测得的 UWB 测距信息进行修正，减小了非视距误差对 UWB测距的影响；使用 ESKF 对 UWB 和 IMU 紧组合，达到了提高定位精度的目的。
（2） 研究了 UWB 基站不同布置方案下 ，经过LSSVM 修正模型处理的 UWB 测距信息对煤矿机器人定位精度的影响。实验结果表明：当 UWB 基站在应用场景中等高对称布置时，相比于其他布置方式，其定位均方根误差较小 ，故在实际场景中 ，应使UWB 基站尽可能等高对称布置；对利用 LSSVM 修正模型修正前后的 UWB 测距信息分别进行 UWB和 IMU 紧组合实验，结果表明：经过 LSSVM 修正模型修正后的组合定位均方根误差有所减小，测距信息修正后融合定位轨迹相较于未修正的融合定位轨迹更接近煤矿机器人运动的真实轨迹。 LSSVM 修正模型对 UWB 测距信息修正后组合定位精度更高，更适用于煤矿机器人定位。
参考文献（References）：
[1]柳玉龙. 煤矿搜救机器人的研究现状及关键技术分析[J]. 矿山机械，2013，41（3）：7-12.LIU Yulong. Analysis on current research status and key technologies of mine search and rescue robots[J]. Mining & Processing Equipment，2013，41（3）：7-12.
[2]张晓莉，王张哲. 井下巡检机器人实时高精度定位方法[J]. 矿业研究与开发，2021，41（10）：158-161.ZHANG Xiaoli，WANG Zhangzhe. Real-time and high precision positioning method of underground patrol robot[J]. Mining Research and Development， 2021，41（10）：158-161.
[3]谭玉新，杨维. 一种基于UKF的井下机器人超声网络定位方法[J]. 煤炭学报，2016，41（9）：2396-2404.TAN Yuxin， YANG Wei. UKF-based ultra sonicnetwork localization for a mine robot[J]. Journal of China Coal Society，2016，41（9）：2396-2404.
[4]陈美蓉，王凯，张嘉纯，等. 煤矿井下超宽带定位混合解算方法[J]. 工矿自动化，2021，47（3）：53-59.CHEN Meirong， WANG Kai， ZHANG Jiachun， et al.Hybrid solution method for ultra-wideband positioning in coal mines[J]. Industry and Mine Automation，2021，47（3）：53-59.
[5]马宏伟，张璞，毛清华，等. 基于捷联惯导和里程计的井下机器人定位方法研究[J]. 工矿自动化，2019，45（4）：35-42.MA Hongwei， ZHANG Pu， MAO Qinghua， et al.Research on positioning method of underground robot based on strapdown inertial navigation and odometer[J].Industry and Mine Automation201945（4）：35-42.
[6]杨金衡，宋单阳，田慕琴，等. 基于自适应卡尔曼滤波的双惯导采煤机定位方法[J]. 工矿自动化，2021，47（7）：14-20，28.YANG Jinheng，SONG Danyang，TIAN Muqin，et al.Double inertial navigation shearer positioning methodbased on adaptive Kalman filter[J]. Industry and Mine Automation，2021，47（7）14-20，28.[ 6 ]
贺磊，魏明生，仇欣宇，等. 基于UWB的井下人员定位算法研究[J]. 工矿自动化，2022，48（6）：134-138.HE Lei，WEI Mingsheng，QIU Xinyu，et al. Research on positioning algorithm of underground personnel based on UWB[J]. Journal of Mine Automation， 2022，48（6）：134-138.
[7]火元亨. 基于惯性导航和UWB定位的室内无人车导航技术研究[D]. 哈尔滨：哈尔滨工业大学，2021.HUO Yuanheng. Indoor unmanned vehicle navigationbased on inertial navigation and UWB positioning[D].Harbin：Harbin Institute of Technology，2021.
[8]牛小骥，旷俭，陈起金. 采用MEMS惯导的小口径管道内检测定位方案可行性研究[J]. 传感技术学报，2016，29（1）：40-44.NIU Xiaoji，KUANG Jian，CHEN Qijin. Study on the possibility of the PIG positioning using MEMS-based IMU[J]. Chinese Journal of Sensors and Actuators，2016，29（1）：40-44.
[9]张 宝 军 ， 陈 曦 ， 廖 延 娜 ， 等 . 基 于 DL-LSTM的UWB/INS室内定位算法[J]. 传感器与微系统，2021，40（10）：147-150.ZHANG Baojun， CHEN Xi， LIAO Yanna， et al.UWB/INS indoor positioning algorithm based onDL-LSTM[J]. Transducer and MicrosystemTechnologies，2021，40（10）：147-150
[10]XU Yuan，LI Yueyang，CHOON K A，et al. Seamlessindoor pedestrian tracking by fusing INS and UWB
[11]measurements via LS-SVM assisted UFIR filter[J].Neurocomputing， 2020， 388： 301-308. DOI： 10.1016/j.neucom.2019.12.121.蒋来来. 基于LSSVM优化的电厂NO_x排放预测方法研究[D]. 保定：华北电力大学，2021.JIANG Lailai. Research on NO_x emission predictionmethod of power plant based on optimized LSSVM [D].Baoding：North China Electric Power University，2021.
[12]邹强 ，陆甫光 ，兰馗博 ，等. 基于ISRUKF的UWB/INS组合室内定位方法研究[J]. 天津大学学报（自然科学与工程技术版），2022，55（5）：496-503.ZOU Qiang，LU Fuguang，LAN Kuibo，et al. UWB/INScombined indoor positioning method based on ISRUKF[J]. Journal of Tianjin University （Science and Technology），2022，55（5）：496-503.
[13]SHIN E H. Estimation techniques for low-cost inertialnavigation[D]. Calgary： University of Calgary（Canada），2005.
[14]秦永元，张洪钺，汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安：西北工业大学出版社，2015.QIN Yongyuan， ZHANG Hongyu， WANG Shuhua.Kalman filtering and combinatorial navigation principle[M]. Xi'an： Northwestern Polytechnical University Press，2015.
[15]FENG Daquan， WANG Chunqi， HE Chunlong， et al.Kalman-filter-based integration of IMU and UWB for high-accuracy indoor positioning and navigation[J]. IEEE Internet of Things Journal， 2020， 7（4） ：3133-3146.
[16]徐爱功，刘韬，隋心，等. UWB/INS紧组合的室内定位定姿方法[J]. 导航定位学报，2017，5（2）：14-19.XU Aigong，LIU Tao，SUI Xin，et al. Indoor positioning and attitude determination method based on UWB/INS tightly coupled[J]. Journal of Navigation and Positioning，2017，5（2）：14-19.
[17]王川阳，王坚. 超宽带应急定位基站布设研究[J]. 测绘科学，2019，44（8）：174-181.WANG Chuanyang，WANG Jian. Study of base station layout of ultra wideband emergency positioning[J].Science of Surveying and Mapping，2019，44（8）：174-181.
班宁产品汇总
