尾矿库排洪隧洞探测机器人多传感器融合定位

周祎博，周唯逸，郭振武，王斌锐

（1.中国计量大学 机电工程学院,浙江 杭州 310018；2.国网浙江乐清市供电公司,浙江 温州 311799）

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 探测机器人多传感器融合定位算法Fig.1 Detection robot multi-sensor fusion positioning algorithm

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

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

1.2 里程计与IMU 数据融合

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

EKF 算法的实现包括预测和更新2 个步骤。设γ时刻状态变量为，Xγ,Yγ分别为 γ时刻探测机器人二维坐标系的横纵坐标，θγ为 γ时刻探测机器人偏航角。根据轮式里程计圆弧运动模型进行状态预测：

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

将IMU 数据积分得到的位置信息 (Xγ,Yγ)、偏航角信息 θγ作为 γ时刻观测值，则有如下观测方程：

式中Vγ为观测噪声。

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

式中：κγ为 γ 时刻计算得到的卡尔曼增益；Θ为观测方程对观测值求偏导得到的雅克比矩阵；Qγ为观测噪声的协方差矩阵。

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

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

式中I为单位矩阵。

1.3 相对位姿关系

ArUco 码到探测机器人坐标系需要经过多次坐标变换，假设以洞口位置探测机器人的车体中心为原点的三维世界坐标系下某点p的齐次坐标为pwq=[XwYwZw1]T，且其对应的相机坐标系下坐标为pc=[XcYcZc]T，则根据针孔相机模型可得到pc到像素坐标系下坐标p=[m n]T的变换关系：

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

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

式中：prq，pcq分别为某点p在探测机器人坐标系、相机坐标系下的齐次坐标；为探测机器人坐标系和相机坐标系之间的变换矩阵，由于相机固定在探测机器人车身上，所以其中参数均为常数；为世界坐标系到相机坐标系的变换矩阵。

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

1.4 ArUco 码识别与解算

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

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

考虑角点位于ArUco 所在平面，可设其为CTp+D=0，其中C为系数矩阵，D为常数。则可推导出2 帧图像的像素坐标系中一对匹配的角点对p1=的转换关系：

式中s为尺度因子。

根据匹配点参数即可求解出H3×3矩阵：

展开式（11）得

式中hαβ(α,β=1,2,3)为H3×3矩阵中第 α行 第 β列元素。

进一步消去尺度因子s，且当h33非 0 时，设h33=1，将H3×3矩阵看作列向量进行化简：

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

1.5 姿态和位置信息优化

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

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

式中：ηi为第i个观测点处的本体位姿；λj为观测到的第j个路标位置；wk为第k帧图像采集时刻的IMU 位置；qk为 第k帧图像采集时刻的IMU 姿态；vk为第k帧图像采集时刻的IMU 速度；分别为陀螺仪和加速度计的偏移。

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

式中：z,u分别为观测数据和输入数据；η,λ分别为待估计的位姿和路标位置。

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

式中：uk为第k帧的输入数据；ηk-1，ηk分别为第k-1帧和第k帧的位姿估计值；zk,j为第k帧探测机器人对第j个路标位置的观测值。

设运动模型噪声eu,k服从正态分布N(0,σu,k)，观测模型噪声ez,k,j服从正态分布N(0,σz,k,j)，σu,k,σz,k,j分别为2 类噪声的协方差矩阵。那么单独考虑相机观测方程zk,j=g(ηk,λj)并联立式（16）、式（17），同时对两侧取负对数并化简得

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

同理，单独考虑输入情况方程l(ηk-1,ηk)，有

待求解优化函数可表示为

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

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

图2 约束因子Fig.2 Constraint factor

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

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

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

图3 隧洞探测机器人系统Fig.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 尾矿库重复定位实验环境和实验平台Fig.4 Experimental environment and platform for repeated positioning of tailings pond

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

表1 探测机器人定位误差Table 1 Positioning error of detection robot

图5 探测机器人定位误差Fig.5 Positioning error of detection robot

运行多传感器融合定位算法实验组利用行进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。

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