0.   引言

综采工作面各类机械设备繁多，空间狭小且视觉环境差，加大了事故发生概率^[1-3]。在综采工作面发生的事故中，有近80%的事故是由监测不到位引起的^[4]。通过在综采工作面配置巡检机器人，可实现对工作面环境、设备状态、人员的实时监测，替代人工来完成日常巡检工作^[5]。

巡检机器人在工作时需要准确且实时获取自身当前位置，目前常见的机器人定位技术有GPS、RFID、UWB、地磁定位及惯性导航等。GPS定位需接收卫星信号才可实现准确定位，而煤矿井下由于岩土层遮蔽，无法接收到GPS信号^[6-8]。RFID定位需要读取对应的RFID标签才能更新当前位置信息，无法做到实时定位^[9-10]。UWB定位需要极高的硬件花费和部署成本^[11-13]。地磁定位需要外界磁场具备较强的特异性，由于井下磁场加上巡检机器人本体上的电动机磁场干扰大，无法满足定位精度要求^[14]。惯性导航因导航信息的全面性与自主性而成为一种可行的综采工作面巡检机器人定位技术，但受井下工作环境的影响，纯惯性导航的位置漂移较大^[15-18]。基于惯性导航/里程计组合的机器人定位将惯性导航元件安装在机器人本体上测量机器人姿态角，将里程计安装在机器人牵引部测量机器人运行速度，通过航位推算算法，根据机器人的姿态角和运行速度进行位置解算，能有效抑制纯惯性导航的误差^[19-22]。

目前井下巡检机器人中应用广泛的是轨道式机器人^[23-26]，而轨道连接件会随着轨道铺设产生形变和间隙^[27]，因此机器人经过轨道连接件时难免产生抖动，在抖动过程中机器人会出现向下或向上的加速，导致机器人车轮打滑、弹起悬空，使得里程计测得的里程数据小于实际运动里程，造成惯性导航/里程计组合定位误差增大^[28]。为了减少由于抖动引起的里程计误差对机器人定位的影响，本文在惯性导航/里程计组合导航算法的基础上，对抖动进行检测，采用分段滤波方法提高巡检机器人定位精度，能够有效抑制因机器人本体抖动而引起的误差，实现巡检机器人高精度自主定位。

1.   抖动检测

现有综采工作面巡检机器人运行轨道大多采用如图1所示的刚柔一体化轨道，轨道包括刚性导轨、轨道连接件和轨道支撑装置。轨道连接件使得2节刚性导轨之间可以有一定的扭动和位移，刚性导轨随着轨道支撑装置的偏移做小幅变动，保证轨道上巡检机器人的平稳运行。当巡检机器人运行在平整的刚性导轨上时，陀螺仪和加速度计的测量值应该在某一数值附近上下波动，而当巡检机器人经过轨道连接件时会产生抖动，影响机器人定位。惯性导航系统中，加速度计和陀螺仪可以敏感地检测出巡检机器人因抖动而产生的异常加速度和角速度，因此可通过这些异常值对抖动进行检测。

图  1  刚柔一体化轨道

Figure  1.  Rigid-flexible integrated track

下载: 全尺寸图片 幻灯片

巡检机器人运行过程中采集的一段陀螺仪和加速度计数据如图2所示，其中X轴指向机器人本体右侧，Y轴指向机器人前进方向，Z轴垂直于机器人本体向上。选取处于有效测量数据范围内的数据局部放大后，可发现存在周期性极值的现象，即每间隔一段数据便会存在1个极值数据。极值所对应的位置就是巡检机器人经过轨道连接件的位置，因此抖动检测的关键就是极值点检测。

图  2  陀螺仪和加速度计数据

Figure  2.  Data of gyroscope and acceleromete

下载: 全尺寸图片 幻灯片

受巡检机器人运动速度及轨道连接件转弯幅度影响，每个周期抖动时数据变化幅度也有一定变化，因此通过区间数据阈值判断抖动点并不可靠，需要对数据进行进一步的动态特性分析。选取变化相对稳定的陀螺仪X轴数据（即机器人俯仰角速度）进行分析。局部放大的X轴角速度数据如图3所示，可看出2个波形的变化方式不同，这是因为实际中轨道连接件低于钢性导轨高度，且机器人车轮宽度小于轨道连接件长度，由于惯性器件安装在机器人头部，所以机器人前轮和后轮经过轨道连接件时会产生2个不同的数据波动。机器人前轮经过轨道连接件时，X轴角速度先向上抖动再向下抖动；机器人后轮经过轨道连接件时，X轴角速度先向下抖动再向上抖动。

图  3  陀螺仪X轴角速度

Figure  3.  X-axis angular velocity of gyroscope

下载: 全尺寸图片 幻灯片

针对图3中X轴角速度的变化特性，考虑实时性，采用滑动窗口方法对X轴角速度数据进行分析。窗口长度w可根据巡检机器人速度v和轨道连接件长度d来设定，即w=d/v。由于机器人经过轨道连接件时X轴角速度会发生剧烈变化，所以可将窗口内最大值与最小值之差作为初步判断条件，以消除随机误差引起的曲线抖动影响。当巡检机器人经过轨道连接件时，测量数据会出现最大上升沿和下降沿，且交替出现。可通过求导数判断局部最大上升沿和局部最大下降沿，但陀螺仪数据会有一定的随机跳动，使得窗口内数据不一定存在连续单调性。为了判断窗口内整体数据的变化趋势，可采用导数和作为判断特征。

最大上升沿和最大下降沿的具体判断过程：首先在初始窗口确定陀螺仪数据曲线波峰位置，同时以波峰的前后一段数据作上升沿次级窗口和下降沿次级窗口，2个窗口长度与初始窗口一致；然后取次级窗口内数据的最大值N_max与最小值N_min，并记录其在窗口数据数组内的索引号D_max和D_min；最后通过比较2个索引号的大小及次级窗口内的数组导数和的正负来判断最大上升沿或下降沿。上升沿的判断式为

式中：${T_{{\omega _X}}} $为消除随机误差的阈值，根据前期静置过程及平稳运动过程中的数据选取；${\omega _X}^\prime $（t）为次级窗口内t时刻X轴角速度的导数。

下降沿的判断式为

当判断出最大上升沿与最大下降沿交替出现时，即认为识别到轨道连接件，且最大上升沿和最大下降沿的极值点对应的时刻，即为机器人经过轨道连接件端点的时刻。由此即可完成因轨道连接件造成的机器人抖动检测，同时也能完成对轨道连接件的定位。由于有阈值加2级窗口的判断，即便外界干扰引起机器人振动，也不会产生误判。

2.   分段滤波

巡检机器人在相邻3节轨道上的运动如图4所示，其中图4（a）和图4（c）可视为同一运动状态，该状态下巡检机器人在同一节刚性导轨上运动，称为平稳运行状态。钢性导轨可视为理想平直化轨道，机器人在上面作直线运动，而在经过轨道连接件时，机器人会产生抖动，此时称为抖动状态。

图  4  巡检机器人在轨道上的运动状态

Figure  4.  Motion state of inspection robot on track

下载: 全尺寸图片 幻灯片

在机器人运动过程中，根据抖动检测结果，便可知机器人运动状态。当机器人处于平稳运行状态时，理论上机器人不会产生角度变化，即陀螺仪数据应该稳定在零值附近，由此可矫正陀螺仪误差，同时采用惯性导航/里程计组合导航方式进行滤波解算，实现机器人定位。选用惯性导航解算速度v_INS与里程计解算速度v_D之间的误差构成观测量：

则可建立卡尔曼滤波的状态空间模型：

式中：X为状态向量；F为系统的误差转移矩阵；G为系统噪声分配矩阵；W为系统噪声矩阵；H为观测矩阵；V为观测噪声矩阵。

机器人平稳运行状态下的滤波解算精度主要取决于里程计的精度，而当机器人处于抖动状态时，里程计可能会由于车轮打滑、弹起悬空而产生误差。此时如再采用上述滤波方式，会使得滤波解算结果产生较大误差。然而抖动状态一般持续时间较短，且加速度计短时间内精度较高，此时采用纯惯性导航即可减小里程计引起的误差。实际运行过程中，机器人前轮或后轮每经过1个轨道连接件时，代表机器人前轮或后轮走完了1节轨道，实际中轨道长度是固定的，因此在机器人前轮或后轮经过轨道连接件后，可用轨道长度对机器人位置进行进一步的修正。综上所述，通过抖动检测，即可实现机器人在不同状态下的分段滤波，不仅可减小在平稳运行状态下陀螺仪的误差，同时能降低抖动状态下里程计误差对机器人定位精度的影响，从而实现机器人高精度自主定位。

3.   实验验证

3.1   模拟实验

为验证基于抖动检测的分段滤波方法在轨道式巡检机器人自主定位中的有效性，采用下载自www.psins.org.cn网站的轨道机器人巡检数据进行模拟实验验证。但该数据中缺少里程计数据，因此当判断出机器人处于平稳运行状态时，只能根据陀螺仪数据应该稳定在零值附近的特性来矫正陀螺仪误差，从而提高机器人定位精度。模拟实验总时长为400 s，经过300 s静态对准后，机器人在轨道上行驶300 m。纯惯性导航采用基于抖动检测的分段滤波方法前后的机器人定位结果如图5所示。

图  5  模拟实验结果

Figure  5.  Simulation experiment result

下载: 全尺寸图片 幻灯片

由图5（a）和图5（b）可看出，由于只有陀螺仪数据，随着时间推移机器人定位误差逐渐增大，但基于抖动检测的分段滤波方法仍能在一定程度上提高定位精度，在300 m的行进过程中，定位精度提高了10%左右。由图5（c）可看出， 大部分相邻抖动点之间的间距在0.5 m左右，与实际相邻轨道连接件之间的间距相符。

3.2   实物实验

为验证基于抖动检测的分段滤波方法在轨道式巡检机器人自主定位中的实用性，采用激光陀螺捷联惯性导航系统进行巡检机器人定位实验，装置如图6所示。巡检机器人前后车轮轴心距为0.585 m。实验中有5节刚性导轨，每节刚性导轨长1.542 m，各节钢性导轨由轨道连接件连接，轨道连接件长度为0.1 m，实际中轨道连接件会因刚性导轨的拼接状态而有所拉伸或压缩，导致轨道连接件长度可能会有所变化。在测试长度有限的条件下，为了模拟实际综采工作面环境，各节刚性导轨设置一定的高度差，且在机器人运行过程中给予一定的外界振动干扰。

图  6  实验装置

Figure  6.  Experimental device

下载: 全尺寸图片 幻灯片

机器人车轮直径小于轨道连接件长度，因此一排车轮经过同一个轨道连接件会抖动2次，即整个机器人本体经过1次轨道连接件会抖动4次。实验轨道总共5节刚性导轨，机器人从第一节刚性导轨中部移动到最后一节刚性导轨中部，整个过程机器人会完整驶过3节刚性导轨，经过4个轨道连接件，因此解算结果中应检测到16个抖动点。

实验持续时间400 s，机器人总行驶路程约为7 m。机器人行驶前静止5 min，进行惯性导航的初始对准，可得机器人的初始姿态角，其中俯仰角为0.120 9°，横滚角为0.409 3°，航向角为−0.981 0°。实验开始时，机器人按设定的0.1 m/s速度沿轨道运动。捷联惯性导航和里程计终端实时采集机器人的加速度、角速度和运动速度并存储。实验结束后，由采集的数据可解算出机器人运动轨迹。

采用基于抖动检测的分段滤波方法得到的机器人导航定位结果如图7所示，可看出识别到的抖动点共有16个，与理论分析相符。根据抖动点的分布，可以分别解算出机器人前轮和后轮完整驶过的刚性导轨长度，结果见表1。由表1和图7可看出，基于抖动检测的分段滤波方法能使巡检机器人的平均定位误差小于5 mm。

图  7  基于抖动检测的分段滤波方法组合导航定位结果

Figure  7.  Positioning result of integrated navigation by piecewise filtering method based on jitter detection

下载: 全尺寸图片 幻灯片

表  1  刚性导轨长度解算结果及误差

Table  1.  Calculation result and error of rigid guides length

轨道编号	前轮/后轮解算	刚性导轨长度解算结果/m	误差/m
1	前轮	1.540 3	−0.001 7
	后轮	1.542 7	0.000 7
2	前轮	1.538 3	−0.003 7
	后轮	1.552 1	0.010 1
3	前轮	1.546 6	0.004 6
	后轮	1.545 0	0.003 0

下载: 导出CSV 

| 显示表格

4.   结语

为实现综采工作面巡检机器人自主定位，通过滑动窗口方法对机器人俯仰角速度进行分析，完成对机器人因轨道连接件而产生的抖动检测；基于抖动检测实现对机器人运动状态的识别，以此为基础，采用分段滤波的方法实现巡检机器人高精度定位。实验结果表明，基于抖动检测的分段滤波方法能有效提高轨道式巡检机器人定位精度，平均定位误差小于5 mm，满足综采工作面精确定位要求。后续将研究在定位算法中融合更多的传感器数据，如三维点云数据，从而进一步提高巡检机器人定位精度。