0 引言
由于煤矿生产系统庞大复杂，工作环境恶劣，煤矿少人或无人化开采的研究已成为热点[1]，在煤矿生产过程中引入机器人技术将在未来煤矿少人或无人化开采中发挥重要作用[2]。机器人定位技术研究一直是机器人技术研究的重点，目前常用的机器人定位技术有GPS定位[3]、基于信号灯定位[4]、基于视觉定位[5]、基于超声定位[6-7]、基于激光定位[8]等技术，不过由于受煤矿井下环境恶劣、巷道转弯多、能见度低、煤壁对信号吸收严重等条件的影响，且井下地理环境十分复杂，不确定性较多，没有GPS信号，导致多数定位方法都存在设备安装困难、环境干扰较强、定位误差很大、无法连续监测等缺点。而捷联惯导在工作时不受外部信号的影响，受环境干扰破坏较少，且其具有数据更新率高、数据全面、短时定位精度高等优点[9]，能够较好地适应井下复杂环境。但利用捷联惯导进行定位的主要缺点就是其定位误差会随着时间累积，难以长时间独立工作[10]。目前解决这一问题有2种方法：一种是提高惯性传感器的精度，不过这种方法消耗的人力、物力、财力过多，且惯性传感器精度的提高有限[11]；另一种是采用组合定位技术，通常组合定位利用2个或多个传感器的互补特性，在现有惯性传感器的应用基础上，利用外部传感器对惯性传感器的误差进行校正，以产生一个比单独传感器独立工作时精度更高的组合定位系统。组合定位主要通过算法来提高定位精度[12]，而里程计工作自主性强，精度较高，所测载体速度信息正好可以修正捷联惯导的定位误差，为此，本文提出了一种基于捷联惯导和里程计的井下机器人定位方法。利用捷联惯导和里程计构成航位推算系统，采用Sage-Husa自适应滤波设计捷联惯导和里程计组合定位算法，以减小外界环境和机器人机身振动对定位精度的影响。同时，本文基于实验室捷联惯导和里程计的机载实验数据，通过Matlab编程仿真实现了初始对准和定位解算的全过程，实验结果表明，该定位方法有较高的准确度。
1 定位方法原理
基于捷联惯导和里程计的机器人定位方法原理如图1所示，在定位工作开始之前，首先利用卡尔曼滤波对捷联惯导进行初始对准，以此确定定位的初始坐标基准，得到初始姿态转换矩阵；再利用捷联惯导独立完成机器人位置解算，输出载体位置信息，同时利用里程计输出的速度信息与捷联惯导输出的实时姿态转换矩阵进行航位推算解算，解算再次得到机器人的位置信息，然后，将捷联惯导独立解算的位置信息与航位推算系统输出的位置信息对应相减作为量测，送入组合定位滤波器(Sage-Husa自适应滤波器)进行滤波，同时将系统误差作为滤波的状态值，经过滤波计算，获得系统状态的最优估计值；最后，利用系统误差估计值实时对捷联惯导和航位推算系统进行误差校正，并将校正后捷联惯导输出的位置信息作为整个机器人定位系统的输出。
图1 基于捷联惯导和里程计的井下机器人定位方法原理
Fig.1 Principle of positioning method of underground robot based on strapdown inertial navigation and odometer
2 捷联惯导的初始对准
捷联惯导上电启动后，其内部惯性传感器的三轴指向不固定，导致无法获取准确的方位，因此，捷联惯导在进入定位工作状态之前，必须确定惯导定位工作的初始基准，为之后的定位工作提供基础保障，此过程即捷联惯导的初始对准。要达到初始对准，首先需要先建立定位坐标系，再根据捷联惯导输出信息确定载体坐标系相对于导航坐标系的方位，得到姿态转换矩阵。捷联惯导的初始对准一般分为初始粗对准和初始精对准[13]。
2.1 定位坐标系建立
定位坐标系如图2所示，本文选取东-北-天(O-XnYnZn)地理坐标系为导航坐标系，记为n系，选取右、前、上方向(O′-Xb(m)Yb(m)Zb(m))为载体坐标系，载体坐标系又分为捷联惯导坐标系和里程计坐标系，分别记为b系、m系，载体坐标系即机器人坐标系，机器人坐标系的坐标原点O′位于机器人机身的重心处，X指向机器人机身的横轴方向，Y指向机器人机身的纵轴方向，Z指向机器人机身的竖轴方向，同时，记地球坐标系为e系。
图2 定位坐标系
Fig.2 Positioning coordinate system
2.2 初始对准粗对准
初始对准粗对准阶段主要任务就是利用惯导的加速度计和角速度计测量地球自转角速度ωie和重力加速度g，并通过简单的解算得到导航坐标系到载体坐标系的姿态转换矩阵
通过转换可确定以下矢量变换等式成立[14]：
(1)
(2)
(3)
式中：
为导航坐标系中地球自转角速度的分量；
为载体坐标系中地球自转角速度的分量；gn为重力矢量；fb为加速度计在b系测得的比力。
将式(1)、式(2)代入式(3)可以求出：
(4)
2.3 初始对准精对准
经过粗对准，捷联惯导可以获得粗略的姿态矩阵，也就是精度较低的3轴指向，其指向与实际地理坐标系存在一定失准角误差，一般情况下，水平失准角(东向和北向)可达数角分，因此，需要进行进一步的精对准，以减少失准角误差的影响。为了达到该目的，本文利用卡尔曼滤波解决精对准问题，并根据状态空间模型，选择系统误差作为滤波的状态，以此构建滤波估计的系统方程和量测方程，利用卡尔曼滤波的估计手段，对误差状态进行估计，从而实现精对准[15]。
本文采用3个姿态角误差和2个水平速度误差进行状态滤波，根据捷联惯导误差方程可得5个状态的误差方程，可以设初始系统模型为
(5)
式中：A为10×10维的状态转移矩阵；δx=[δVE δVN φN φE φD 
E N εE εN εU]T，δVE，δVN分别为东向、北向速度误差，φN，φE，φD分别是北向、东向、天向的的姿态角误差，E、N为加速度计常值零偏，εE，εN、εU为陀螺常值零偏；Hk为2×10维的量测矩阵；Zk为量测信息；Xk为状态估计值。
通过对该系统进行可观测分析[16]，可以确定E，N，εE为不可观状态变量，其余状态构成的系统是可观的，则状态变量为δVE，δVN，φN，φE，φD，εN，εU，从而建立最终7状态系统模型，得到系统方程和量测方程后需要对连续系统进行离散化，离散化后就可以根据离散卡尔曼滤波方程进行滤波，得到失准角的最优估计，从而对姿态矩阵进行校正，得到准确的坐标基准。
离散卡尔曼滤波方程包括状态一步预测方程、状态估值计算方程、滤波增益方程、一步预测均方误差方程、估计均方误差方程。
状态一步预测方程为
Ø
(6)
状态估值计算方程为
(7)
滤波增益方程为
(8)
一步预测均方误差方程为
Pk|k-1=Ø
(9)
估计均方误差方程为
Pk= (I-KkHk)Pk|k-1(I-KkHk)T+
(10)
式中：
为状态方程的状态一步预测；Øk|k-1为转移矩阵；
为前一时刻状态估计值；
为当前时刻的状态估计值；Kk为滤波增益方程；Pk|k-1为一步预测均方误差矩阵；Rk为观测噪声方差矩阵；Pk-1为状态估计误差的均方误差矩阵；σk-1为噪声矩阵；Qk-1为系统噪声方差矩阵；Pk为k时刻状态估计的均方误差矩阵；I为单位矩阵。
3 基于捷联惯导和里程计的机器人定位
3.1 捷联惯导位置解算
3.1.1 姿态更新
在得到捷联惯导输出的加速度和角速度信息后，须经过坐标系转换后才能完成对机器人姿态的解算。在进行机器人姿态解算时，本文采用四元数算法对姿态矩阵进行求解，通过四元数的变化对姿态矩阵进行更新，具体解算过程可参考文献[17-18]，可以得出载体坐标系(b系)相对于n系的姿态矩阵为
(11)
式中θ、γ、ψ为机器人机身的仰俯角、横滚角和航向角。
3.1.2 速度更新
有关机器人速度更新的解算过程已有文献描述，本文只对其结果进行描述，经过解算可以得到机器人3个方向的速度更新微分方程为[19]
(12)
式中：fx,fy,fz为加速度计测量的比力;vx，vy,vz分别为机器人在导航坐标系下东向、北向和天向的速度分量；RN为卯酉圈主曲率半径；RM为子午圈主曲率半径；L为机器人所在地的纬度；h为机器人所在地的高度。
3.1.3 位置更新
对速度积分即可得到位置矢量，机器人位置更新微分方程为
(13)
式中
分别为机器人当前时刻所在位置的纬度、经度和高度。
通过对式(13)积分即可以得到机器人所在位置的纬度、经度和高度。
3.2 航位推算位置解算
里程计的速度输出在里程计坐标系上可以表示为
(14)
vD为里程计测得的前向速度大小，右向和天向速度均为零，可以假设b系与m系的坐标轴重合，利用捷联惯导实时计算得出的姿态转换矩阵
对
进行转换，可得在导航坐标系下里程计的速度输出，即
(15)
由里程计速度
可得航位推算定位解算的微分方程为
(16)
式中：LD，λD，hD分别为航位推算的地理纬度、 经度和高度；RMhD=RMD+hD，RNhD=RND+hD，RMD和RND分别为使用航位推算地理位置计算的子午圈和卯酉圈主曲率半径，
同时采用里程计的路程增量给出航位推算数值更新算法，记里程计在一小段时间[tk-1,tk]内的路程增量为ΔSk，则路程增量在载体坐标系m系的投影为
(17)
对式(17)进行整理，可得
(18)
式中
为tk-1时刻的机器人姿态矩阵，记
将式(18)离散化，可以得到航迹推算位置更新方程为
(19)
4 组合定位系统模型
在井下移动机器人的工作过程中，环境非常复杂，导致噪声统计特性参数获取都会存在误差，此时若采用标准卡尔曼滤波，不仅会出现滤波精度下降，甚至可能导致滤波发散。为此，本文采用对噪声处理较好的Sage-Husa自适应滤波来进行组合定位定向滤波设计，并采用间接法滤波，选取系统误差作为组合定位滤波器的状态，系统误差由捷联惯导误差和航位推算误差组成。
4.1 捷联惯导误差方程
姿态误差方程为
(20)
速度误差方程为
b
(21)
位置误差方程为
(22)
式中：φ=[φE φN φU]T，为捷联惯导计算的平台误差角；εb为陀螺漂移；δvn=[δvE δvN δvU]，为速度误差；b为机器人坐标系中的加速度计零偏；δL、δλ、δh分别为纬度、经度和高度误差。
4.2 航位推算误差方程
已知惯导坐标系(b系)与里程计坐标系(m系)的变换矩阵
则通过解算可以得到航位推算的速度误差方程
(23)
(24)
式中：αθ、αγ、αψ为安装偏差角；
为初始失准角；
为里程计刻度系数误差。
根据式(23)、式(24)可以得出航位推算的位置误差方程为
(25)
式中：
为初始位置误差[20]。
4.3 组合定位方程
将惯导误差和航位推算误差合并在一起，组成如下状态矢量：
X=[φT(δvn)T(δpINS)T(δpD)T(εb)T(b)T
(26)
式中：φT为姿态误差；δvn为速度误差；δpINS，δpD分别为惯导解算位置误差和航位推算解算位置误差。
本文以惯导解算位置与航位推算位置之差构造观测量，可得量测方程为
Z=δpINS-δpD
(27)
由此可以建立捷联惯导和里程计组合定位状态空间模型为
(28)
式中：F为误差系统状态矩阵；G为相应维数的矩阵；W为系统噪声；H为量测矩阵；V为量测信号的测量白噪声。
整理可以得到Sage-Husa自适应滤波算法如下：
(29)
式中：yk为信息序列；dk为计算加权系数；b为遗忘因子(0<b<1，可以在0.95～0.99中选取)；
为观测噪声方差矩阵。
当估计参数变化较快时，b取值较小，而当估计参数变化较慢时，b取值较大。在滤波的过程中,根据组合定位的各项参数，选取合适的初值与遗忘因子b，实现在测量噪声统计特性发生变化时的稳定误差估计。
5 实验验证与数据分析
根据实验室现有的煤矿井下移动机器人、惯性导航模块和里程计搭建机器人定位实验平台。机器人定位实验平台主要包括履带式煤矿井下巡检机器人、TM352九轴捷联惯导传感器、里程计以及定位主机等。为操作方便，将捷联惯导传感器安装在车体上方，里程计安装于机器人前轮驱动轴上，为保证2个传感器指向准确性，工作前通过水平仪和光学瞄准仪将两者指向标校至一致。捷联惯导传感器和加速度计输出的数据通过USB-to-Serial TTL转换器传给定位主机，波特率为115 200 bit/s。捷联惯导的初始参数见表1。
表1 捷联惯导初始参数
Table 1 Initial parameters of trapdown inertial navigation
设置初始实验条件：初始位置为X0= (3,0,0),m；初始速度为V0=(0 0 0),m/s；初始航向角ψ=0°，仰俯角θ=0°，横滚角为γ=0°；地球自转角速度ω=7.272 20×10-5rad/s;重力加速度g=9.780 49 m/s2;地球半径RM=6 356 077.2 m，RN=6 385 297.0 m；初始纬度L0= 34.25°；初始经度λ0=117.18°；采样时间为0.05 s。惯性器件和里程计的数据采集频率均为200 Hz，卡尔曼滤波周期设定为1 s。里程计的初始刻度系数为0. 0120 m/P。
5.1 初始对准实验
针对初始对准的仿真，取一段捷联惯导的静态输出数据进行分析，该段数据采集时间为300 s，由于初始信息确定的载体坐标系和导航坐标系存在失准角，所以，仿真结果将对惯导北向失准角、东向失准角、方位失准角的变化进行分析，所得仿真结果如图3—图5所示。
图3 东向失准角变化曲线
Fig.3 Change curves of eastward misalignment angle
图4 北向失准角变化曲线
Fig.4 Change curves of northward misalignment angle
图5 方位失准角变化曲线
Fig.5 Change curves of azimuth misalignment angle
根据仿真结果可以看出，在完成了整个初始对准过程时，三轴失准角均有不同程度的收敛，达到了稳定状态，符合初始对准精对准的实验要求。
5.2 定位实验
实验时，机器人按定位主机设定的运动路线运行，假设定位主机设定的运动路线即为机器人的实际运动路线。固定在小车上的捷联惯导和里程计终端实时采集机器人的加速度、角速度和运动速度，并传送给定位主机，再通过Matlab对数据进行实时处理，得出捷联惯导解算的机器人速度和运动轨迹以及组合定位方法解算的机器人速度和运动轨迹，分别进行对比。
5.2.1 机器人运动速度分析
随机采集500 s的机器人运行数据，分别使用捷联惯导和组合定位方法对机器人的运动速度进行解算，速度误差对比结果如图6所示。
捷联惯导解算所产生的机器人速度误差随时间不断变大，组合定位方法解算得出的机器人速度误差很小，而且没有发散，通过与组合定位方法解算产生的速度误差进行对比，可以发现组合定位得出的速度相较捷联惯导独立解算得出的速度准确性以及稳定性有很明显的改善。
5.2.2 机器人运动轨迹分析
在定位坐标系下设定机器人的起始位置为[3,0,0]，操作机器人运动，让机器人在Y向运动4.3 m，Z向运动0.25 m，机器人运动轨迹如图7所示。在相同实验路况下重复该操作4次，分别采集机器人在该路径下的捷联惯导和里程计数据并进行解算，解算所产生的误差见表2。从图7和表2可看出，捷联惯导解算得到的Y向定位误差约为0.6 m，Z向定位误差约为0.03 m，组合定位方法解算得到的Y向定位误差约为0.25 m，Z向定位误差约为0.005 m。组合定位方法定位精度相较捷联惯导单独定位有明显提高。
(a) 北向速度误差
(b) 东向速度误差
图6 捷联惯导和组合定位方法解算出的速度误差对比
Fig.6 Comparison of speed error between inertial navigation and combined positioning method
表2 不同方法所产生的定位误差
Table 2 Positioning error of different methods m
图7 机器人运动轨迹
Fig.7 Robot motion track
实验结果表明，本文提出的基于捷联惯导和里程计的机器人定位方法可有效减少捷联惯导误差随时间累积的影响，且得到的机器人运动轨迹和预定轨迹基本相符，证明了该方法的可行性。
6 结论
(1) 针对煤矿井下机器人环境干扰较多、定位误差较大、无法连续监测等问题，利用捷联惯导和里程计不受外部信号影响、数据更新率高、数据全面等优点，提出一种基于捷联惯导和里程计的井下机器人定位方法。该方法采用卡尔曼滤波对捷联惯导进行了初始对准，从而快速提高了定位的反应能力，利用Sage-Husa自适应滤波设计捷联惯导和里程计组合定位算法，大幅减少了累积误差以及外部噪声对定位精度的影响。
(2) 实验结果表明，基于捷联惯导和里程计的井下机器人定位方法与其他方法相比较，定位精度有较大提高，取得了良好的效果。同时，由于捷联惯导和里程计在工作时既不受外部信号影响，也不会向外部辐射信号，所以，该定位方法具有自主性强、抗干扰能力强、对其他设备影响小的特点，未来在煤矿机器人的定位研究中将有广泛的应用前景。
参考文献：
[1] 谭得健.我国煤矿自动化现状及发展趋势[J].工矿自动化,2010,36(9):126-129.
TAN Dejian.Status of automation of coal mine and its developing trend in China[J].Industry and Mine Automation,2010,36(9):126-129.
[2] 柳玉龙.煤矿搜救机器人的研究现状及关键技术分析[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
[3] 纪龙,闻新,回亚斌,等.基于简易惯导/GPS组合和无线网络的搜救机器人群开发[J].全球定位系统,2013,38(3):30-34.
JI Long,WEN Xin,HUI Yabin,et al.The search and rescue robots developed simplified inertial navigation/GPS and based on wireless network[J].GNSS World of China,2013,38(3):30-34
[4] 曹岩,雷涛.基于交通灯的高精度车辆定位技术[J].计算机工程与应用,2015,51(3):212-215.
CAO Yan,LEI Tao.High precision vehicle positioning method based on traffic light[J].Computer Engineering &Applications,2015,51(3):212-215.
[5] 赵霞,袁家政,刘宏哲.基于视觉的目标定位技术的研究进展[J].计算机科学,2016,43(6):10-16.
ZHAO Xia,YUAN Jiazheng,LIU Hongzhe.Advances in vision-based target location technology[J].Computer Science,2016,43(6):10-16.
[6] 田子建,李宗伟,刘晓阳,等.基于电磁波及超声波联合测距的井下定位方法[J].北京理工大学学报,2014,34(5):490-494.
TIAN Zijian,LI Zongwei,LIU Xiaoyang,et al.A positioning method in mine tunnel based on joint electromagnetic wave and ultrasonic distance measurement [J].Transactions of Beijing Institute of Technology,2014,34(5):490-494.
[7] HAN J,CHANG R L.Research and design of range positioning system based on ultrasonic sensor for intelligent mobile robot[J].Advanced Materials Research,2011,201-203:1886-1890.
[8] 杜钊君,吴怀宇.基于激光测距与双目视觉信息融合的移动机器人SLAM研究[J].计算机测量与控制,2013,21(1):180-183.
DU Zhaojun,WU Huaiyu.Research of mobile robot SLAM based on laser range finder and binocular vision data fusion[J].Computer Measurement &Control,2013,21(1):180-183.
[9] 赵天贺,汪伟.基于MEMS技术的捷联惯导系统现状[J].飞航导弹,2017(10):17-21.
[10] 齐广峰,吕军锋.MEMS惯性技术的发展及应用[J].电子设计工程,2015,23(1):87-89.
QI Guangfeng,LYU Junfeng.Evolution and application of MEMS inertial technology[J].Electronic Design Engineering,2015,23(1):87-89.
[11] 薛连莉,陈少春,陈效真.2017年国外惯性技术发展与回顾[J].导航与控制,2018,17(2):1-9.
XUE Lianli,CHEN Shaochun,CHEN Xiaozhen.Development and review of foreign inertial technology in 2017[J].Navigation and Control,2018,17(2):1-9.
[12] 吕鹏,陆明泉,冯振明.车载卫星/惯导组合导航算法优化及精度分析[J].计算机仿真,2013,30(8):155-158.
LYU Peng,LU Mingquan,FENG Zhenming. Optimization algorithm of vehicle-mounted GPS and inertial integrated navigation and precision analysis[J].Computer Simulation,2013,30(8):155-158.
[13] 侯迎团,祖肇梓.一种机载惯导系统初始对准方法的研究与仿真[J].计算机仿真,2016,33(7):78-82.
HOU Yingtuan,ZU Zhaozi.Research and simulation of INS's initial alignment method[J].Computer Simulation,2016,33(7):78-82.
[14] 许彩,胡柏青,常路宾,等.捷联惯导惯性系初始对准算法研究与仿真[J].计算机仿真,2014,31(1):68-71.
XU Cai,HU Baiqing,CHANG Lubin,et al.Research on initial alignment for SINS in inertial frame based on wahba problem[J].Computer Simulation,2014,31(1):68-71.
[15] 许明成,戴邵武.卡尔曼滤波在惯导初始对准中的应用[J].电子设计工程,2017,25(23):43-46.
XU Mingcheng,DAI Shaowu.Kalman filtering and its application in INS initial alignment [J].Electronic Design Engineering,2017,25(23):43-46.
[16] 杨晓霞,阴玉梅.可观测度的探讨及其在捷联惯导系统可观测性分析中的应用[J].中国惯性技术学报,2012,20(4):405-409.
YANG Xiaoxia,YIN Yumei.Discussions on observability and its applications in SINS[J].Journal of Chinese Inertial Technology,2012,20(4):405-409.
[17] 任思颖.基于捷联惯导及四元数法的采煤机定位方法研究[J].煤矿机械,2014,35(8):79-81.
REN Siying.Research of coal winning machine positioning based on strapdown inertial navigation and quaternion method[J].Coal Mine Machinery,2014,35(8):79-81.
[18] 王小峰,董正芳.捷联惯导系统航姿算法的比较及仿真分析[J].现代导航,2016,7(2):103-106.
WANG Xiaofeng,DONG Zhengfang.Comparison and simulation analysis for attitude algorithms of SINS[J].Modern Navigation,2016,7(2):103-106.
[19] 张金尧,李威,杨海,等.采煤机捷联惯导定位方法研究[J].工矿自动化,2016,42(3):52-55.
ZHANG Jinyao,LI Wei,YANG Hai,et al.Study of shearer positioning method using strapdown inertial navigation system[J].Industry and Mine Automation,2016,42(3):52-55.
[20] 杨波,彭培林,王跃钢,等.里程计辅助捷联惯导运动基座对准方法[J].中国惯性技术学报,2013(3):298-301.
YANG Bo,PENG Peilin,WANG Yuegang,et al.A alignment method of strapdown inertial navigation system aided by odometer on moving base[J].Journal of Chinese Inertial Technology,2013,21(3):298-301.