0.   引言

煤矿护盾式掘进机器人适用于巷道断面大、夹矸与片帮共存的复杂地质条件掘进作业。煤矿护盾式掘进机器人在掘进过程中大多采用激光指向仪指示巷道的设计中线，通过人工目测激光光斑的方式控制截割头截割，极易因工作面环境恶劣、作业现场光线差等原因造成超挖或欠挖等，无法满足掘进工作面智能化、无人化的要求，而掘进机的井下精确定位是煤矿掘进智能化、无人化的核心技术之一^[1-3]。因此，深入研究煤矿护盾式掘进机器人精确定位方法十分必要。

目前，煤矿护盾式掘进机器人一般采用单一传感器定位方法，主要采用全站仪、机器视觉、超宽带等技术进行定位。全站仪虽然定位精度高，但煤矿井下巷道环境恶劣，光路可能被遮挡，使测量结果不可靠^[4]。机器视觉测量精度容易受井下粉尘影响^[5]。超宽带虽能够解决传统无线技术传播问题，但其节点安装和维护是个难题，不适用于长距离井下掘进定位^[6-7]。因此，在井下环境复杂多变的情况下，单一传感器定位方法常常会导致定位精度和可靠性降低，针对上述问题，有学者提出了组合惯导定位方法。惯导系统是一种自主式定位定向系统，适用范围广，通过加速度计及陀螺仪采集掘进机的加速度及角速度^[8]，通过数值积分运算可获得掘进机器人的姿态、速度及位置信息，对掘进机器人定位具有较好的效果。当前，掘进机组合惯导定位方法有惯导/视觉、惯导/里程计、惯导/全站仪等。文献[9-10]通过融合惯导和视觉信息，可在粉尘浓度较小的情况下实现掘进机器人位姿精准测量，但在粉尘浓度较高的情况下工业相机会失效，从而导致惯导/视觉组合定位精度降低。文献[11-12]通过融合里程计和惯导信息，可在地质条件较好的巷道中有效测量掘进机的位姿信息，但是随着时间的增长，掘进机易受浮煤影响造成履带滑移及打滑等，此时里程计会产生较大误差，从而导致惯导/里程计组合定位精度降低。文献[13-14]提出了利用惯导与全站仪组合对掘进机进行自主定位定向的方法，实现了在粉尘浓度较小情况下掘进机的精确定位，但是井下环境复杂、粉尘较多时，远距离全站仪激光透视性差，从而导致惯导/全站仪组合定位精度降低。

针对现有煤矿掘进机器人定位方法存在定位精度低的问题，本文在惯导/全站仪组合定位方法的基础上，提出了捷联惯导+数字全站仪+位移传感器的组合定位方法，利用基于联邦滤波器的多信息融合算法有效融合三者的信息，实现煤矿护盾式掘进机器人系统的精确定位。

1.   煤矿护盾式掘进机器人系统

煤矿护盾式掘进机器人系统构成如图1所示，主要由截割机器人、临时支护机器人Ⅰ和Ⅱ、钻锚机器人、锚网运输机器人、电液控平台、运输与通风除尘系统组成。临时支护机器人Ⅰ与临时支护机器人Ⅱ之间通过推移油缸的推拉作用，实现整个煤矿护盾式掘进机器人系统的自主行驶。煤矿护盾式掘进机器人系统集探、掘、支、锚、运、通风与除尘等于一体，能够实现掘进与钻锚协同作业，对提高掘进效率、保证安全生产、实现快速掘进具有重要意义。

图  1  煤矿护盾式掘进机器人系统构成

Figure  1.  Structure of coal mine shield roadheader robot system

下载: 全尺寸图片 幻灯片

2.   组合定位方法原理

煤矿护盾式掘进机器人系统组合定位过程包括基于捷联惯导的机身位姿参数解算、基于数字全站仪的机身位置参数测量、基于位移传感器的机身位置参数推算、基于联邦滤波器的机身位置参数与位姿参数融合。

组合定位方法组成如图2所示（x轴指向设计巷道中线方向；y轴垂直于设计巷道中线；z轴垂直于oxy平面）。将捷联惯导和360棱镜安装在掘进机机身前段临时支护机器人Ⅰ上，将位移传感器安装在临时支护机器人Ⅰ和临时支护机器人Ⅱ之间的推移油缸中，使其随掘进机机体移动，将数字全站仪固定在掘进机机身后端顶锚杆处并设置为跟踪模式。开始定位前，地测人员测量掘进机器人机体所在位置的经纬度，并依据设计巷道确定任意2个坐标点，分别为数字全站仪后视点和测站点。根据2个坐标点构建数字全站仪坐标系，数字全站仪通过测量临时支护机器人Ⅰ上的360棱镜，解算出掘进机器人机身在当地地理坐标系下的坐标。利用位移传感器输出的位姿信息与捷联惯导输出的实时姿态转换矩阵进行航位推算，得到掘进机器人机身在当地地理坐标下的位置信息。捷联惯导根据初始经纬度及初始姿态角可实时测量出掘进机器人的位姿和速度。将捷联惯导测量的经纬度转换为当地地理坐标系下的坐标值，利用基于联邦滤波器的多信息融合算法将捷联惯导解算出的掘进机器人位置及姿态角信息与全站仪测量的掘进机器人位置信息及位移传感器推算的掘进机器人位置信息进行融合，减少捷联惯导随时间产生的位置累计误差，得到掘进机器人的位姿信息，最终实现其精确定位，定位原理如图3所示。

图  2  组合定位方法组成

1−后视棱镜；2−数字全站仪；3−临时支护机器人Ⅱ；4−推移油缸；5−捷联惯导；6−前视棱镜；7−临时支护机器人Ⅰ；8−截割机器人；9−横滚角；10−航向角；11−俯仰角。

Figure  2.  Composition of combination positioning method

下载: 全尺寸图片 幻灯片

图  3  组合定位方法原理

Figure  3.  Principle of combined positioning method

下载: 全尺寸图片 幻灯片

3.   基于联邦滤波器的多信息融合算法

3.1   基于联邦滤波器的多信息融合原理

本文提出的捷联惯导+数字全站仪+位移传感器的组合定位方法有捷联惯导、数字全站仪、位移传感器3个信息源，利用联邦滤波器对这些信息进行融合。依据联邦滤波器结构，设计1个主滤波器和2个子滤波器。子滤波器1处理捷联惯导与数字全站仪的融合信息，输出局部状态估计值$ {\hat {\boldsymbol{X}}_1} $及误差协方差矩阵$ {{\boldsymbol{P}}_1} $。若子滤波器1输出值有效，则代表数字全站仪光线不被遮挡，其值进入主滤波器参与融合，若无效则代表数字全站仪光线被遮挡，不参与此次主滤波器信息融合。子滤波器2处理捷联惯导与位移传感器的融合信息，输出局部状态估计值$ {\hat {\boldsymbol{X}}_2} $及误差协方差矩阵$ {{\boldsymbol{P}}_2} $，最后进入主滤波器。主滤波器参考各子滤波器状态估计值$ {\hat {\boldsymbol{X}}_{{i}}} $$(i=1,2) $及对应的误差协方差矩阵$ {{\boldsymbol{P}}_{{i}}} $进行信息融合，得到全局状态估计值$ {\hat {\boldsymbol{X}}_{\rm{g}}} $和误差协方差矩阵$ {\boldsymbol{P}}{}_{\text{g}} $。主滤波器按照信息分配原则向各子滤波器反馈更新后的全局状态估计值$ {\hat {\boldsymbol{X}}_{\rm{g}}} $和被放大的误差协方差矩阵$ \beta _i^{- 1}{{\boldsymbol{P}}_{\rm{g}}} $($\beta _i^{- 1} $为子滤波器i的放大倍数)，用来重置子滤波器的估计值。信息融合原理如图4所示。

图  4  基于联邦滤波器的多信息融合原理

Figure  4.  Multi-information fusion principle based on federated filter

下载: 全尺寸图片 幻灯片

3.2   联邦滤波器状态方程

本文选取捷联惯导与数字全站仪系统误差作为子滤波器1的状态量，位置差值作为量测量；选取捷联惯导与位移传感器系统误差作为子滤波器2的状态量，位置差值作为量测量；采用基于联邦滤波器的多信息融合算法对三者信息进行融合，获取掘进机器人的位姿。因此，需要对捷联惯导、数字全站仪、位移传感器的误差进行分析和建模。

捷联惯导为公共信息参考系统，其误差主要考虑随机漂移和噪声，主要包括加速度计的零偏误差和高斯白噪声、陀螺仪的漂移误差和高斯白噪声。关于捷联惯导更新算法中的姿态、速度及位置误差模型在文献[15-16]中有详细介绍，在此不再赘述。

数字全站仪测量误差主要考虑斜距测量误差和方位角误差，公式为

式中：$ e $为数字全站仪测量误差；$ {m} $为两点斜距测量误差；$ \theta $为方位角；$ S $为斜距；$ \rho $为弧度化秒因子，$ \rho = 206\;265 $；$ \Delta \theta $为方位角误差。

位移传感器输出的是推移掘进机器人前进的位移信息，无法单独将其用于对掘进机器人的位姿测量，因此需要将油缸推移掘进机器人前进的位移数据与开始推移时掘进机器人的姿态信息结合起来进行航位推算，得到其相对于每次推移起始点的相对位置。位移传感器对推移油缸行程的测量结果含有测量误差，本方法选用的位移传感器测量误差为0.1%，即在推移1 m的行程中，误差控制在1 mm以内。

联邦子滤波器1的状态变量为

式中：$ {\phi _{\rm{E}}} $，$ {\phi _{\rm{N}}} $，$ {\phi _{\rm{U}}} $分别为东、北、天向姿态角误差；$ {\upsilon _{\rm{E}}} $，$ {\upsilon _{\rm{N}}} $，$ {\upsilon _{\rm{U}}} $分别为东、北、天向速度误差；$ {\sigma _{\text{λ}}} $，$ {\sigma _{\rm{L}} } $分别为经度、纬度位置误差；$ {\nabla _{\rm{E}}} $，$ {\nabla _{\rm{N}}} $，$ {\nabla _{\rm{U}}} $分别为三向加速度计零偏误差；$ {\epsilon _{\rm{E}}} $，$ { \epsilon _{\rm{N}}} $，$ { \epsilon _{\rm{U}}} $分别为三向陀螺仪漂移误差。

依据上述系统状态变量可以得到联邦子滤波器1的状态方程：

式中：${{\boldsymbol{X}}_1}_{\left( t \right)} $为t时刻联邦子滤波器1的状态变量；${{\boldsymbol{\omega }} _{1\left( t \right)}}$为十五维状态转移矩阵；$ {{\boldsymbol{G}}_{1\left( t \right)}} $为联邦子滤波器1的噪声状态矩阵；$ {{\boldsymbol{W}}_{1\left( t \right)}} $为期望为0、方差为$ Q $的联邦子滤波器1高斯白噪声矩阵。

联邦子滤波器2的状态变量为

依据上述系统状态变量可以得到联邦子滤波器2的状态方程：

式中：${{\boldsymbol{X}}_2}_{\left( t \right)} $为t时刻联邦子滤波器2的状态变量；$ {{\boldsymbol{\omega }} _{2\left( t \right)}} $为十四维状态转移矩阵；$ {{\boldsymbol{G}}_{2\left( t \right)}} $为联邦子滤波器2的噪声状态矩阵；$ {{\boldsymbol{W}}_{2\left( t \right)}} $为期望为0、方差为$ Q $的联邦子滤波器2高斯白噪声矩阵。

3.3   联邦子滤波器量测方程

本文设计的联邦滤波器主滤波器不进行滤波，只对每个子滤波器的局部估计值进行融合得到最优估计，下面分别给出子滤波器的量测方程。

3.3.1   子滤波器1的量测方程

选取捷联惯导输出的位置$ {x_{{\rm{SINS}}}} $，$ {y_{{\rm{SINS}}}} $与数字全站仪解算的位置$ {x_{{\rm{DTS}}}} $，$ {{{y}}_{{\rm{DTS}}}} $差值为子滤波器1的量测值，则子滤波器1的量测方程为

式中：$ {{\boldsymbol{H}}_{1\left( t \right)}} $为子滤波器1的量测矩阵，$ {{\boldsymbol{H}}_{1(t)}} = $$ [ \begin{array}{*{20}{c}}{{0_{2 \times 6}}} & {{{\boldsymbol{I}}_{2 \times 2}}}&{{{\boldsymbol{I}}_{2 \times 3}}}&{{0_{2 \times 4}}}\end{array} ] $，I为单位矩阵；$ {{\boldsymbol{V}}}_{1(t)} $为期望为0、方差为$ {R_1} $的高斯白噪声矩阵。

3.3.2   子滤波器2的量测方程

选取捷联惯导输出的位置$ {x_{{\rm{SINS}}}} $，$ {y_{{\rm{SINS}}}} $与位移传感器推算的位置$ {x_{{\rm{DS}}}} $，$ {{{y}}_{{\rm{DS}}}} $差值为子滤波器2的量测值，则子滤波器2的量测方程为

式中：$ {{\boldsymbol{H}}_{2\left( t \right)}} $为子滤波器2的量测矩阵，$ {{\boldsymbol{H}}_{2(t)}} = $$ [\begin{array}{*{20}{c}}{{0_{2 \times 6}}}&{{{\boldsymbol{I}}_{2 \times 2}}}&{{{\boldsymbol{I}}_{2 \times 3}}}&{{0_{2 \times 3}}}\end{array}]$；$ {{\boldsymbol{V}}_{2({\text{t}})}} $为期望为0、方差为$ {R_2} $的高斯白噪声矩阵。

3.4   联邦滤波器设计

将子滤波器i的状态方程与量测方程离散化，获得子滤波器i的离散型状态方程和量测方程：

式中：X_i,k为k时刻的状态变量；$ {{\boldsymbol{\omega }} }_{{i}(k,k-1)} $为$k-1 $到k时刻的状态转移矩阵；$ {{\boldsymbol{G}}_{i,k - 1}} $为$k-1 $时刻噪声状态矩阵；$ {{\boldsymbol{W}}_{{{{{i}},{{k}} - 1}}}} $为$k-1 $时刻期望为0、方差为$ Q $的高斯白噪声矩阵；$ {{\boldsymbol{Z}}_{i,k}} $为k时刻观测量；$ {{\boldsymbol{H}}_{i,k}} $为k时刻量测矩阵；$ {{\boldsymbol{V}}_{i,k}} $为k时刻期望为0、方差为$ {R_i} $的高斯白噪声矩阵。

子滤波器与主滤波器之间遵守信息分配守恒原则，即

式中${\beta _{\rm{m}}}$为主滤波器的放大倍数。

本文设计的主滤波器不参与信息分配，即

主滤波器信息在子滤波器中的分配方式为

式中$ {{\boldsymbol{N}}_i} $，$ {{\boldsymbol{N}}_{\rm{g}}} $分别为子滤波器i的噪声协方差矩阵、主滤波器噪声协方差矩阵。

由式（8）可知，子滤波器依据系统上一时刻的状态变量预测下一时刻的状态变量，即子滤波器协方差矩阵的时间更新过程为

将式（12）代入式（13），得

由式（14）可知，子滤波器的状态误差协方差矩阵$ {{\boldsymbol{P}}_{i,\left( {k - 1} \right)}} $和噪声协方差矩阵$ {{\boldsymbol{N}}_{i,\left( {k - 1} \right)}} $都被放大了$ \beta _i^{{{ - }}1} $倍。

子滤波器的预测方程为

子滤波器的估计方程为

式中$ {{\boldsymbol{K}}_{i,k}} $为子滤波器滤波增益，${{\boldsymbol{K}}_{i,k}} = \dfrac{{{{\boldsymbol{P}}_{i,\left( {k,k - 1} \right)}}{\boldsymbol{H}}_{i,k}^{\rm{T}}}}{{{{\boldsymbol{H}}_{i,k}}{{\boldsymbol{P}}_{i,\left( {k,k - 1} \right)}}{\boldsymbol{H}}_{i,k}^{\rm{T}} + {R_i}}}$。

子滤波器的估计均方差更新为

子滤波器局部状态估计完成后，主滤波器依据局部估计值进行全局最优估计，其公式为

式中：${\boldsymbol{P}}_{{\rm{g}},k}^{ - 1}$为估计状态变量的协方差矩阵；${\hat {\boldsymbol{X}}_{{\rm{g}},k}}$为系统误差状态变量的全局最优估计结果。

4.   仿真与分析

利用基于联邦滤波器的多信息融合算法对组合定位方法进行信息融合仿真验证。假设陀螺仪零偏误差为0.01 $ {\text{deg}}/{\rm{h}} $，角度随机游走误差为0.000 1 $ {\text{deg}}/\sqrt {\rm{h}} $，加速度计零偏误差为$0.5 \times {10^{ - 6}}\; {{g}} $(g为重力加速度)，速度随机游走误差为$0.05 \times {10^{ - 6}}\; {{g}}/\sqrt {{\rm{H}}{\text{z}}} $，采样周期为0.01 s，采样时间为7 500 s，初始姿态角均为0，掘进机器人前进位移为50 m。将初始加速度及速度均设为0，仿真得到组合定位下掘进机器人静态姿态角误差，如图5所示。可看出掘进机器人静态姿态角误差稳定控制在0.06′（0.001°）内。

图  5  组合定位方法下掘进机器人静态姿态角误差

Figure  5.  Static attitude angle error of roadheader robot under combined positioning method

下载: 全尺寸图片 幻灯片

仿真组合定位下掘进机器人掘进过程，由于掘进机器人借助油缸按1 m截距通过反复推拉来实现行走，可定义掘进机器人直线行走1 m为1个行走周期，1个行走周期轨迹包括匀加速运动、匀速运动、匀减速运动、截割状态。匀加速运动过程加速度设为0.000 5 m/s²，速度设为0，持续时间为20 s；匀速运动过程加速度设为0，速度设为0.01 m/s，持续时间为80 s；匀减速运动过程加速度设为−0.000 5 m/s²，速度设为0，持续时间为20 s；截割状态加速度及速度均为0，持续时间为30 s。采集掘进机器人动态姿态角误差及位置解算误差，如图6、图7所示。从图6可看出，随着时间的增加，掘进机器人动态姿态角误差由1.5′（0.025°）收敛趋于0.5′（0.008 3°）内。从图7可看出，位置解算误差随着时间的增加慢慢趋于稳定，位置解算误差最大为0.005 m。

图  6  组合定位方法下掘进机器人动态姿态角误差

Figure  6.  Dynamic attitude angle error of roadheader robot under combined positioning method

下载: 全尺寸图片 幻灯片

图  7  组合定位方法下掘进机器人位置解算误差

Figure  7.  Position calculation error of roadheader robot under combined positioning method

下载: 全尺寸图片 幻灯片

在相同条件下做纯惯导定位位置解算仿真，位置解算误差如图8所示。可看出纯惯导位置解算误差随时间增加而增大，最大误差为0.3 m。

图  8  纯惯导定位位置解算误差

Figure  8.  Position calculation error of pure inertial navigation positioning

下载: 全尺寸图片 幻灯片

综上可看出：组合定位方法定位精度较高，且能够很好地抑制纯惯导位置解算误差累计。

5.   工业性试验与分析

在陕煤榆北煤业小保当煤矿对捷联惯导+数字全站仪+位移传感器组合定位方法在煤矿护盾式掘进机器人系统中的应用进行了工业性试验，试验采用MG045捷联惯导、XI−1001Q数字全站仪、GUC1200A位移传感器、360棱镜、工控机等设备。

5.1   姿态角静态漂移试验

在井下掘进工作面采集煤矿护盾式掘进机器人的静止姿态角数据，时长为3 h，结果如图9所示。可看出航向角、俯仰角、横滚角的变化趋势均稳定保持在0.005°以内，说明组合定位方法可以精确测量煤矿护盾式掘进机器人静止姿态信息，并且在较长时间内保持较高的姿态解算精度。

图  9  静止状态时姿态角变化趋势

Figure  9.  Change trend of attitude angle in static state

下载: 全尺寸图片 幻灯片

5.2   姿态角动态稳定性试验

为了验证本文提出的组合定位方法在煤矿护盾式掘进机器人系统掘进过程中姿态测量的有效性，选取掘进机器人静态姿态角信息作为其掘进时的姿态角参考值，与组合定位方法测量得到的姿态信息进行对比，对比结果如图10所示。从图10可看出：当煤矿护盾式掘进机器人系统处于掘进状态时，整体姿态角变化趋势呈现动态稳定状态，航向角、俯仰角、横滚角最大误差分别为0.031 1、0.048 4、0.039 7°，测量精度较高，可以准确反映出掘进机器人系统在掘进过程中的姿态信息。

图  10  掘进状态时姿态角变化趋势

Figure  10.  Change trend of attitude angle in driving state

下载: 全尺寸图片 幻灯片

5.3   组合定位与分析

井下跟班9 h，煤矿护盾式掘进机器人按照初始方向行进22 m，油缸每次推移量为1 m。在试验中，煤矿护盾式掘进机器人实际位置无法得知，以数字全站仪测量的位置数据为掘进机器人实际位置。捷联惯导、数字全站仪和位移传感器三者组合定位数据曲线如图11所示，可看出煤矿护盾式掘进机器人融合定位曲线与实际位置曲线基本重合。组合定位数据与实际值之间的误差曲线如图12所示，可看出组合定位方法下煤矿护盾式掘进机器人在x轴和y轴上的位置误差分别控制在±0.03 m和±0.02 m，有效消除了纯惯导解算所造成的累计误差，保证了系统的精确性、稳定性和可靠性。

图  11  组合定位方法下煤矿护盾式掘进机器人位置曲线

Figure  11.  Position curves of coal mine shield roadheader robot under combined positioning method

下载: 全尺寸图片 幻灯片

图  12  组合定位方法下煤矿护盾式掘进机器人位置误差曲线

Figure  12.  Position error curve of coal mine shield roadheader robot under combined positioning method

下载: 全尺寸图片 幻灯片

6.   结论

（1） 基于捷联惯导+数字全站仪+位移传感器的组合定位方法通过数字全站仪和位移传感器共同测量的位置信息辅助修正捷联惯导系统解算得到的煤矿护盾式掘进机器人的位置及姿态角信息，能够有效抑制惯导系统随时间所产生的累计位置误差。利用基于联邦滤波器的多信息融合算法将捷联惯导解算得到的位置及姿态角信息、全站仪测量得到的位置信息及位移传感器推算得到的位置信息融合，从而得到煤矿护盾式掘进机器人的精确位姿信息，实现较高精度定位。

（2） 仿真及工业性试验结果表明：基于捷联惯导+数字全站仪+位移传感器的组合定位方法有效克服了单一辅助测量方式与惯导组合测量方法的不稳定、不可靠问题，能够很好地抑制纯惯导位置解算误差累计，实现了煤矿护盾式掘进机器人的精确定位，x轴和y轴上的位置误差分别控制在±0.03 m和±0.02 m，满足井下掘进工作面要求。