0.   引言

尾矿库作为矿山安全生产的重大危险源之一，存在极大的潜在风险 ^[1-2]。对此，国家出台的《尾矿库安全监督管理规定》^[3]和GB 39496−2020《尾矿库安全规程》^[4]中要求定期组织尾矿库专项检查，对发现的事故隐患及时进行治理。尾矿库排洪隧洞主要用于泄洪和回收尾矿澄清水，其排洪能力和结构性能直接关系到矿山安全，因此必须对排洪隧洞进行安全巡检。目前排洪隧洞巡检以人工为主，因巡检线路较长，巡检过程存在安全风险，提出以探测机器人代替巡检人员完成巡检任务。探测机器人巡检需要在长度超过1 km的排洪隧洞内部精准定位隐患，所以有必要对探测机器人在巡检过程中的自主定位进行研究。

近年来，学者们对探测机器人自主定位进行了深入研究，提出了多种算法提高探测机器人实时位姿计算精度。文献[5-6]使用测程法进行定位，利用UMBmark算法修正探测机器人轮径和轴径两类系统误差对里程计定位造成的影响，提高了测程法定位精度。测程法易于实现且适用性广，但其鲁棒性和稳定性较差，且存在累计误差。文献[7]提出了一种基于概率估计的里程计模型。文献[8]使用有限记忆卡尔曼方法，以逐级滤波的方法融合多传感器信息，推算矿井机器人的航向和运动轨迹，减小了单一传感器测量误差的影响，但仍未解决累计误差问题，不适合长距离定位。文献[9]使用惯性模块（Inertial Measurement Unit, IMU）、里程计、全球导航卫星系统（Global Navigation Satellite System，GNSS）等传感器进行机器人定位，通过扩展卡尔曼滤波（Extended Kalman Filter, EKF）进行数据滤波，实现多传感器融合，提升了系统鲁棒性和稳定性，但累计误差的消除需依赖卫星提供的绝对位置信息，当卫星信号微弱时该类算法易因校正失败出现累计误差，受使用环境限制。文献[10]利用Fast−SLAM及包含激光雷达在内的多种传感器，利用基于粒子滤波的自适应蒙特卡洛算法构建环境栅格地图，实现了探测机器人在矿洞室内环境的精确定位，但Fast−SLAM算法在弱纹理场景中易出现粒子收敛缓慢情况，影响定位效果。文献[11-13]均利用图优化算法实现了探测机器人多传感器融合定位，并利用子图匹配实现回环检测，有效消除了定位过程的累计误差，但所用算法在弱纹理场景中匹配错误率较高，未考虑弱纹理场景的特殊性。文献[14-15]利用二维码阵列进行室内定位与导航，使得机器人能够在缺少纹理的室内环境实现精确定位，但均需要提前布置密集的二维码，且维护工作量极大，不适合大场景定位。文献[16]利用实验对比分析了目前常用标签的检测精度和效率，结果表明ArUco码的检测速度有明显优势，适用于对标签检测速度要求高的场合。

尾矿库排洪隧洞环境属于狭长室内环境，且隧洞内卫星信号弱，特征点少，现有探测机器人定位算法存在弱纹理室内场景定位失效的问题，难以实现此类环境中的精确定位和累计误差消除。本文提出了一种适用于尾矿库排洪隧洞环境的探测机器人多传感器融合定位算法。该算法将排洪隧洞环境的远距离定位以ArUco码为边界点，简化为多段短距离定位，利用EKF算法融合里程计和IMU信息实现短距离精确定位，利用预设的ArUco码作为路标消除EKF定位带来的累计误差，并实现位姿优化，最终实现狭长空间弱纹理场景下探测机器人的远距离高精度定位。该算法有效利用了多种传感器数据，避免了单一传感器定位误差，提升了定位鲁棒性。为验证探测机器人多传感器融合定位算法效果，设计了尾矿库排洪隧洞探测机器人系统实验平台，并在尾矿库排洪隧洞实际场景中运行和关闭多传感器融合定位算法，分别进行10组行进40 m的重复定位实验，结果证明了该算法具有较高的稳定性和精确性。

1.   多传感器融合定位算法

1.1   融合定位算法流程

探测机器人多传感器融合定位算法如图1所示。首先选取UMBmark算法进行里程计标定，有效消除探测机器人轮径和轴径两类系统误差。然后利用EKF算法融合IMU数据和里程计数据，并利用测程法实现运动过程中探测机器人位置和姿态信息计算。最后选用ArUco码作为路标并固定于隧洞内部，探测机器人携带经张正友标定法^[17]标定后的相机对其进行识别与处理，利用图优化方式实现位姿优化，并根据ArUco码携带的信息实现累计误差消除，从而实现狭长空间弱纹理场景下探测机器人的高精度定位。

图  1  探测机器人多传感器融合定位算法

Figure  1.  Detection robot multi-sensor fusion positioning algorithm

下载: 全尺寸图片 幻灯片

探测机器人车身上安装的里程计、IMU、单目相机数据作为算法输入，其中里程计需利用UMBmark算法进行标定，以消除系统误差。

工控机通过串口获取到传感器数据后，将里程计与IMU数据进行时间戳对齐，并且判断是否检测到ArUco码。未检测到ArUco码时，将里程计和IMU数据通过EKF算法实现融合定位。当检测到ArUco码时，对当前探测机器人位姿状态利用图优化方法进行局部优化。随后将优化结果和参考位姿信息结合，修正融合定位结果，消除累计误差影响，最后将定位结果输出。

1.2   里程计与IMU数据融合

考虑单一传感器容易出现测量错误，且依靠里程计推算得到的偏航角容易出现较大误差，因此将里程计数据和IMU数据利用EKF算法进行融合，避免单一传感器测量误差影响定位结果。

EKF算法的实现包括预测和更新2个步骤。设$\gamma $时刻状态变量为$ {{\boldsymbol{x}}_\gamma } = {\left[ {{X_\gamma }\;{Y_\gamma }\;{\theta _\gamma }} \right]^{\rm{T}}} $，$ {X_\gamma },{Y_\gamma } $分别为$\gamma $时刻探测机器人二维坐标系的横纵坐标，$ {\theta _\gamma } $为$\gamma $时刻探测机器人偏航角。根据轮式里程计圆弧运动模型进行状态预测：

式中：$ {{\mathop {\boldsymbol{x}}\limits^ \vee} _{{\gamma}} } $为$\gamma $时刻状态变量的先验估计；$ {{\mathop {\boldsymbol{x}}\limits^ \wedge} _{{{\gamma}} -{{ 1}}}} $为$\gamma - 1$时刻状态变量的后验估计；$ \Delta {d_{\gamma - 1,\gamma }} $为$\gamma - 1$时刻到$\gamma $时刻的位移测量值；$ {\theta _{\gamma - 1}} $为$\gamma - 1$时刻探测机器人偏航角；$ \Delta {\theta _{\gamma - 1,\gamma }} $为$\gamma - 1$时刻到$\gamma $时刻探测机器人偏航角变化。

状态变量的协方差预测方程为

式中：$ {{\mathop {\boldsymbol{P}}\limits^ \vee} _{{\gamma}} } $和$ {{\mathop {\boldsymbol{P}}\limits^ \wedge } _{{{\gamma}} - {{ 1}}}} $分别为$\gamma $时刻状态变量的先验协方差矩阵和$\gamma - 1$时刻后验协方差矩阵；$ {{\boldsymbol{F}}_{{\gamma}} } $为$\gamma $时刻运动方程对状态变量求导得到的协方差矩阵；$ {\boldsymbol{R}} $为系统噪声协方差矩阵。

将IMU数据积分得到的位置信息$({X_{\gamma }},{Y_{\gamma }})$ 、偏航角信息${\theta _{\gamma }}$作为$\gamma $时刻观测值，则有如下观测方程：

式中${{\boldsymbol{V}}_\gamma }$为观测噪声。

根据式(3)和式(4)进行状态更新，卡尔曼增益计算公式为

式中：$ {{\boldsymbol{\kappa}} _{\boldsymbol{\gamma}} } $为$\gamma $时刻计算得到的卡尔曼增益；${\boldsymbol{\varTheta}}$为观测方程对观测值求偏导得到的雅克比矩阵；$ {{\boldsymbol{Q}}_{{\gamma}} } $为观测噪声的协方差矩阵。

根据式（5）可得状态变量的后验估计：

式中${{\mathop {{X}}\limits^ \vee} _\gamma }$，${{\mathop {{Y}}\limits^ \vee } _\gamma }$，${{\mathop {{\theta}} \limits^ \vee} _\gamma }$分别为$\gamma $时刻状态变量的先验估计值。

状态变量的后验协方差矩阵为

式中$ {\boldsymbol{I}} $为单位矩阵。

1.3   相对位姿关系

ArUco码到探测机器人坐标系需要经过多次坐标变换，假设以洞口位置探测机器人的车体中心为原点的三维世界坐标系下某点p的齐次坐标为${{\boldsymbol{p}}_{{{{\rm{wq}}}}}} = $$ {\left[ {{X_{\rm{w}}}\;\;{Y_{\rm{w}}}\;\;{Z_{\rm{w}}}\;1} \right]^{\rm{T}}}$ ，且其对应的相机坐标系下坐标为$ {{\boldsymbol{p}}_{{{\rm{c}}}}} = $$ {[{X_{\text{c}}}\;\;\;{Y_{\text{c}}}\;\;\;{Z_{\text{c}}}]^{\rm{T}}} $，则根据针孔相机模型可得到$ {{\boldsymbol{p}}_{{{\rm{c}}}}} $到像素坐标系下坐标$ {\boldsymbol{p}} = {[{m}\;\;\;{n}]^{\rm{T}}} $的变换关系：

式中：${\boldsymbol{K}}$为相机内参数矩阵，一般由标定得到；${f_{{m}}}$，${f_{{n}}}$分别为像素平面m轴和n轴上的尺度因子；${c_{{m}}}$，${c_{{n}}}$为相机光心偏移量；A为变换矩阵，$ {\boldsymbol{A}} = [{\boldsymbol{r}}\;\;\;{\boldsymbol{\tau}} ] $，$ {\boldsymbol{r}} $，${\boldsymbol{\tau}}$分别为相机当前位姿的旋转矩阵和平移矩阵。

实际安装时相机并不处于探测机器人坐标系中心，故某点p在探测机器人坐标系下的坐标需进行坐标变换得到。

式中：${{\boldsymbol{p}}_{{\rm{rq}}}}$，${{\boldsymbol{p}}_{{\rm{cq}}}}$分别为某点p在探测机器人坐标系、相机坐标系下的齐次坐标；${}_{\rm{c}}^{\rm{r}}{\boldsymbol{T}}$为探测机器人坐标系和相机坐标系之间的变换矩阵，由于相机固定在探测机器人车身上，所以其中参数均为常数；${}_{\rm{w}}^{\rm{c}}{\boldsymbol{T}}$为世界坐标系到相机坐标系的变换矩阵。

根据式( 8)和式( 9)可由像素坐标推算得到探测机器人坐标系与世界坐标系下ArUco码之间的位姿变换关系。

1.4   ArUco码识别与解算

由于相机固定在探测机器人车身上，所以求得像素坐标系相对于ArUco码所在的世界坐标系的位姿变换矩阵及像素坐标系与相机坐标系之间的位姿变换矩阵，即可得到相机和ArUco码的位置变换关系。

首先使用张正友标定法对相机进行标定，得到相机的内参矩阵和畸变参数。然后分别通过自适应滤波对图像进行二值化，利用Suzuki算法进行轮廓提取并再次进行滤波，采用Douglas−Peucker算法进行四边形轮廓拟合并且得到4个角点。因ArUco码外侧边界均为黑色，可据此进行筛选得到ArUco码的候选轮廓，从而得到其角点在世界坐标系的位置。最后通过求解单应矩阵即可得到2帧图像间探测机器人的运动估计。

考虑角点位于ArUco所在平面，可设其为$ {{\boldsymbol{C}}^{\rm{T}}}{{p}} + $$ {{D}} = 0 $，其中${\boldsymbol{C}}$为系数矩阵，$ {{D}} $为常数。则可推导出2帧图像的像素坐标系中一对匹配的角点对$ {{\boldsymbol{p}}_{{1}}} = $$ {[{m_1}\;\;\;{n_1}]^{\rm{T}}} $和$ {{\boldsymbol{p}}_{{2}}} = {[{m_2}\;\;\;{n_2}]^{\rm{T}}} $的转换关系：

式中s为尺度因子。

根据匹配点参数即可求解出${{\boldsymbol{H}}_{{{3}} \times {{3}}}}$矩阵：

展开式（11）得

式中${h_{\alpha \beta }}$$(\alpha ,\beta=1,2,3) $为${{\boldsymbol{H}}_{{\mathbf{3}} \times {\mathbf{3}}}}$矩阵中第$\alpha $行第$\beta $列元素。

进一步消去尺度因子$ s $，且当$ {h_{33}} $非0时，设$ {h_{33}} = 1 $，将${{\boldsymbol{H}}_{{\mathbf{3}} \times {\mathbf{3}}}}$矩阵看作列向量进行化简：

式中有$ {h_{11}} $，$ {h_{12}} $，$ {h_{13}} $，$ {h_{21}} $，$ {h_{22}} $，$ {h_{23}} $，$ {h_{31}} $，$ {h_{32}} $ 8个未知量，故利用4个角点的匹配点对即可建立方程组，完成相邻2帧图像间探测机器人位姿变换的初步计算。此外，根据提取的轮廓可进行二进制编码解算，由于每个ArUco码表示唯一，只有原始角点顺序下的ArUco码可在字典中找到编号，所以对提取的图像进行4次旋转，分别计算其编码值，即可得到一个唯一确定的编号。由于在首次完成ArUco码放置后，每个ArUco码在世界坐标系下的位姿均为固定值，根据其编号可确定ArUco码表示的位姿信息。

1.5   姿态和位置信息优化

探测机器人携带单目相机进入隧洞，当检测到ArUco码时，按照1.3和1.4节所述方法计算得到探测机器人和ArUco码之间的相对位姿关系及ArUco码代表的绝对位置信息。考虑到探测机器人在运动过程中传感器存在误差，因此，使用图优化方式进行姿态和位置信息的优化。将全过程待优化的状态变量记为$\chi $，则有

第k帧图像采集时刻的位姿为

式中：$ {{\boldsymbol{\eta}} _i} $为第$i$个观测点处的本体位姿；${\lambda _j}$为观测到的第$j$个路标位置；${w_k}$为第k帧图像采集时刻的IMU位置；${q_k}$为第k帧图像采集时刻的IMU姿态；${v_k}$为第k帧图像采集时刻的IMU速度；$b_k^{\rm{a}}$，$b_k^{\rm{g}}$分别为陀螺仪和加速度计的偏移。

在实际环境下，探测机器人在运动过程中存在噪声，基于高斯分布假设和最大似然估计，可建立如下目标函数：

式中：$ z,u $分别为观测数据和输入数据；$ {\boldsymbol{\eta}} ,\lambda $分别为待估计的位姿和路标位置。

假设各个时刻的输入数据和观测数据之间相互独立，则联合分布可分解为

式中：$ {u_k} $为第k帧的输入数据；$ {{\boldsymbol{\eta}} _{k - 1}} $，${{\boldsymbol{\eta}} _k}$分别为第$k-1 $帧和第k帧的位姿估计值；$ {z_{k,j}} $为第k帧探测机器人对第$j$个路标位置的观测值。

设运动模型噪声$ {{\boldsymbol{e}}_{u,k}} $服从正态分布$ N(0,{{\boldsymbol{\sigma}} _{u,k}}) $，观测模型噪声${{\boldsymbol{e}}_{z,k,j}}$服从正态分布$N(0,{{\boldsymbol{\sigma}} _{z,k,j}})$，$ {{\boldsymbol{\sigma}} _{u,k}},{{\boldsymbol{\sigma}} _{z,k,j}} $分别为2类噪声的协方差矩阵。那么单独考虑相机观测方程$ {z_{k,j}} = g({{\boldsymbol{\eta}} _k},{\lambda _j}) $并联立式（16）、式（17），同时对两侧取负对数并化简得

式中：$ G $为常数；L为进行优化的位姿观测点数；E为观测到的路标数。

同理，单独考虑输入情况方程$l({{\boldsymbol{\eta }}_{k - 1}},{{\boldsymbol{\eta}} _k})$，有

待求解优化函数可表示为

式中：$B$为第k帧图像采集时刻IMU测量数据集合；$\mu $为比例因子，2帧图像之间探测机器人在ArUco码范围内识别的特征点信息越多且运动速度越慢，则$\mu $越小，表明该情况下依靠相机的位姿估计准确性更高；$\rho $为huber核函数，当匹配偏差超过阈值使其保持线性增长，从而降低错误匹配带来的影响；${M_{\rm{IMU}}}(\tilde Z_{{\rm{IMU}}k + 1}^{{\rm{IMU}}k},{\boldsymbol{\chi}} )$为IMU数据的残差项($\tilde Z_{{\rm{IMU}}k + 1}^{{\rm{IMU}}k} $为IMU的观测值)，由IMU测量预积分形成的观测值和实际值比较形成，用于约束k和k+1连续2帧；$W$为匹配良好的特征点对集合；${M_{\rm{f}}}(\tilde Z_{{f_j}}^{{c_i}},{\boldsymbol{\chi}} )$为相机残差项($\tilde Z_{{f_j}}^{{c_i}} $为相机观测值)，通过将特征点$ {c_i} $投影到第${f_j}$帧并与测量值计算重投影残差得到；${U_{{\rm{IMU}}}}$和${U_{{\rm{cam}}}}$分别为IMU和相机约束。

利用里程计信息、IMU信息和相机信息，根据图优化的方式建立如图2所示的约束因子，图中${t_k}$为时间戳对齐至第k帧的时间，v'_k为里程计测量速度。在ArUco码附近范围实现探测机器人位姿优化，其中里程计约束提供的是相对约束，转换到IMU坐标系中进行速度和位姿的限制。

图  2  约束因子

Figure  2.  Constraint factor

下载: 全尺寸图片 幻灯片

结合绝对位置信息和局部位姿优化结果更新探测机器人位姿，同时完成对IMU和里程计累计误差的修正。

2.   隧洞探测机器人系统设计

为验证多传感器融合定位算法在尾矿库排洪隧洞实际环境中的运行效果，研究并设计了隧洞探测机器人系统，如图3所示。

图  3  隧洞探测机器人系统

Figure  3.  Tunnel detection robot system

下载: 全尺寸图片 幻灯片

尾矿库排洪隧洞探测机器人系统以履带式探测机器人实验平台为本体，基于机器人操作系统（Robot Operating System，ROS）进行软件设计，采用分布式系统，将监视端工控机布置于隧洞入口，而运行多传感器融合定位算法的工控机安装在探测机器人本体上，上下位机之间利用无线网桥进行组网通信。为实现多传感器融合定位算法，隧洞探测机器人实验平台和洞口的监控平台需要进行相应的软硬件设计，且根据系统功能将隧洞探测机器人系统划分为4个子系统：驱动控制系统、主控系统、传感通信系统、人机交互系统。驱动控制系统对接主控系统，进行电动机控制和电动机相关数据传输，保证探测机器人的运动能力。主控系统对接驱动控制系统和传感通信系统，运行多传感器融合定位、定位校正等核心算法，实现探测机器人自主定位和定位校正等功能，并解析执行控制端操作人员指令，通过串口转RS232实现与驱动控制系统的通信，通过USB获取传感器数据，在进行数据分析后将关键数据和音视频通过传感通信系统传输至人机交互系统，并实现人机交互系统的各类指令下传，以实现对探测机器人本体的控制。传感通信系统对接主控系统和人机交互系统，负责探测机器人环境感知和通信，使洞口操作人员可实时查看探测机器人实验平台的运行状态。人机交互系统对接传感通信系统，负责解析操作人员指令并通过传感通信系统将指令发送到主控系统，将主控系统数据实时显示在监视端工控机界面中。

3.   实验验证和分析

为探究搭载多传感器融合定位算法的尾矿库排洪隧洞探测机器人实验平台在实际环境中运行的稳定性，并验证多传感器融合定位算法的有效性，在尾矿库排洪隧洞实际场景中运行和关闭多传感器融合定位算法，分别进行10组行进40 m的重复定位实验，且每轮实验均记录探测机器人在20 m和40 m处的定位误差。运行多传感器融合定位算法需要在20 m处额外记录校正后误差。

在实验开始前需要标定和校正系统误差，探测机器人平均车轮直径为0.21 m，理论轮距为 0.88 m，使用UMBmark算法进行标定，根据实验数据求得校正后左轮直径为0.209 5 m，右轮直径为0.210 5 m，轮间距为0.84 m。

修改里程计参数后，在距起点20 m处的排洪隧洞墙壁上布设ArUco码。在探测机器人系统中利用ROS编写程序，对校正后的里程计和IMU数据进行EKF，利用OpenCV库进行ArUco码识别与相应图像处理操作，并利用Graph SLAM Tutorial实现图优化算法对探测机器人位姿进行优化并校正定位。在尾矿库排洪隧洞中进行重复定位实验，实验环境和实验平台实物如图4所示。

图  4  尾矿库重复定位实验环境和实验平台

Figure  4.  Experimental environment and platform for repeated positioning of tailings pond

下载: 全尺寸图片 幻灯片

根据实验设计运行和关闭多传感器融合定位算法，在相同环境中各进行10组40 m行进实验，并记录实验数据，各组实验20 m和40 m处定位误差如图5所示。根据图5数据统计定位误差，结果见表1。

图  5  探测机器人定位误差

Figure  5.  Positioning error of detection robot

下载: 全尺寸图片 幻灯片

表  1  探测机器人定位误差

Table  1.  Positioning error of detection robot

定位位置	平均定位误差/cm
	关闭算法实验组	运行算法实验组
行进20 m	22.51	19.77
行进40 m	58.11	21.23

下载: 导出CSV 

| 显示表格

运行多传感器融合定位算法实验组利用行进20 m时的ArUco码进行误差校正。使用米尺测量ArUco码中心和探测机器人车身中心的$X$，$Y$方向数据，与位姿估计结果比较并计算距离误差，得误差均值为4.2 mm。

从图5、表1可看出，使用多传感器融合定位算法的探测机器人在行进20 m时平均定位误差为19.77 cm，在行进40 m时的平均定位误差为21.23 cm。同样条件下，关闭多传感器融合定位算法实验组在行进20 m时的平均定位误差为22.51 cm，在行进40 m时的平均定位误差为58.11 cm。可见使用多传感器融合定位算法的探测机器人在运行过程中可保持更高定位精度，且在20 m的ArUco码放置处多传感器融合定位算法可有效修正累计误差，使校正后误差均值为4.2 mm。

4.   结论

（1） 针对现有定位算法不适用于网络信号弱、场景重复率高且属于狭长室内环境的尾矿库排洪隧洞探测机器人定位的问题，提出了一种探测机器人多传感器融合定位算法。将排洪隧洞环境的远距离定位以ArUco码为边界点，简化为多段短距离定位。未检测到ArUco码时，利用EKF算法融合里程计和IMU信息，可实现位姿信息的快速更新；检测到ArUco码时，则利用图优化算法将里程计、IMU、相机信息组成约束因子图，并结合ArUco码的绝对位置实现探测机器人局部位姿优化和定位校正，避免误差累计，最终实现狭长空间内探测机器人的远距离高精度定位。

（2） 为验证多传感器融合定位算法的有效性，设计了隧洞探测机器人系统，在尾矿库排洪隧洞实际场景中运行和关闭多传感器融合定位算法，分别进行10组行进40 m的重复定位实验，实验结果表明：多传感器融合定位算法具有较高的稳定性和精确性，能够实现隧洞探测机器人在排洪隧洞环境中的高精度定位，行进20 m的平均定位误差为19.77 cm，40 m的平均定位误差为21.23 cm，且20 m处校正后误差均值为4.2 mm。

致谢：本文得到了北京科技大学、福建马坑矿业股份有限公司、中国安全生产科学研究院和中国计量大学人工智能与机器人团队的支持，对此表示感谢！