电缆隧道环境下工业智能巡检机器人定位研究

Research on Localization of Industrial Intelligent Inspection Robots in Cable Tunnel Environment

摘要

电缆隧道封闭狭长，存在重复布设的电缆架和相似的场景纹理，属于退化场景.针对该场景，提出了1种基于点线特征融合的视觉惯导SLAM(simultaneous localization and mapping)算法.该算法通过长度抑制和短线拟合来改进高维线特征，使其能够更有效地描述结构化显著的隧道场景.此外，针对电缆隧道中特征相似导致的回环检测失败问题，引入具有高效识别和精确位姿估计的ArUco标记，限定回环发生区域，并利用最小化位姿变换筛选最佳回环帧，从而提升回环检测的准确度和定位精度.最后，在电缆隧道内采集数据集并进行实验验证.结果表明，相对于VINS–Mono(visual inertial system–Mono)，所提算法的绝对轨迹精度平均提升了69.73%，满足了电缆隧道巡检的应用需求.

Abstract

The cable tunnel is closed and narrow， with repetitively laid cable racks and similar scene textures， which is a degraded scenario. To address this environment， a visual-inertial SLAM （simultaneous localization and mapping） algorithm based on point-line feature fusion is proposed. The algorithm improves the high-dimensional line features through length suppression and short line fitting to make it more effective in describing the structural features of tunnel scene. In addition， for the problem of loop closure detection failure due to feature similarity in cable tunnels， ArUco markers with efficient recognition and accurate pose estimation are introduced to limit the loop closure area， and the optimal loop closure frames are selected using the minimized pose transformation to improve detection accuracy and localization precision. Finally， dataset collection and experimental validation were conducted in actual cable tunnels. The results show that the absolute trajectory accuracy of the algorithm is improved by 69.73% on average relative to VINS–Mono（visual intertial system-Mono）， which meets the application requirements of cable tunnel inspection.

Graphical abstract

关键词

同步定位与地图构建 / 点线特征融合 / 回环检测 / 工业机器人 / 电缆隧道

Key words

simultaneous localization and mapping / point-line feature fusion / loop closure detection / industrial robot / cable tunnel

随着城市化进程的加速和电力需求的不断增长，电缆隧道已成为城市电力输配系统的重要组成部分.由于隧道环境容易积水、空气不流通以及高压等复杂因素，传统的人工巡检方式面临着效率低下和安全隐患的问题.因此，研发用于电缆隧道的工业智能巡检机器人显得尤为重要.同时，隧道的封闭环境阻碍了卫星信号的传输，导致在该场景下无法使用 GPS进行定位.因此，自主定位技术成为实现电缆隧道智能化巡检的关键.

自主定位是指机器人在没有外界干预的情况下，通过传感器和算法配合，准确确定其在环境中的位置和姿态.目前，常用的定位方法包括视觉、激光雷达以及多传感器融合技术.视觉定位通过相机获取图像特征信息，结合图像处理算法，实现环境感知与定位.以具有代表性的ORB-SLAM（oriented fast and rotated brief-simultaneous localization and mapping）^［1］为例，该算法涵盖特征追踪、局部地图构建、闭环检测及后端优化等模块，能够提供足够的定位精度.激光雷达通过扫描空间生成三维点云图，提供精确的距离和位置信息.LeGO-LOAM（lightweight and ground-optimized-lidar odometry and mapping）^［2］作为轻量级的定位算法，通过对点云信息进行滤波和配准实现位姿估计.考虑到电缆隧道封闭狭长、特征匮乏以及存在退化场景的特点，单一传感器无法满足工业智能巡检机器人的定位需求.因此，需要结合多传感器以及多特征融合的方法实现定位，以提高系统定位精度和鲁棒性.

在多传感器融合定位方法的研究中，MSCKF（multi-state constraint Kalman filter）^［3］以多状态约束扩展卡尔曼滤波为核心，实现了视觉惯导特征融合优化；VINS-Mono^［4］构建了单目视觉与IMU（inertial measurement unit）的紧耦合系统，形成了完整的功能架构；Dang等^［5］开发了基于激光雷达、视觉、IMU及热学数据松耦合的多模态SLAM系统，该系统显著提升了地下环境的定位稳定性；Azpurua等^［6］提出了适用于地下隧道勘测的双SLAM并行系统，分别采用视觉和激光方案；Campos等^［7］设计了改进的ORB-SLAM3系统，该系统引入了基于最大后验推断的IMU初始化方法；为拓展系统适用性，GVINS（GNSS-visual-inertial system）^［8］整合全球导航系统，实现了室内外环境的平滑过渡；LE-VINS（lidar-enhanced-VINS）^［9］在因子图优化中融合视觉特征、激光雷达深度和惯性测量单位数据，特别适用于低速机器人应用；DGM-VINS（dynamic environments with joint geometry feature extraction and multiple object tracking-VINS）^［10］通过联合几何动态特征提取模块，有效提升了系统在动态场景下的定位精度；SM-VINS（stepwise marginalization-VINS）^［11］采用逐步边际化的VINS以及双向光流跟踪方法，在保证精度的同时降低了计算负担.在多特征融合方面，学者重点关注高维几何特征的应用.StructSLAM框架^［12］利用线特征构建约束完成位姿估算；在SVO（semi-direct monocular visual odometry）^［13］的基础上，Gomez-Ojeda等^［14］提出了PL-SVO（points and line segments-SVO），在最大似然估计中融合了点线特征；Fu等^［15］在VINS-Mono框架中引入参数控制的线特征提取，开发了高效的PL-VINS（point and line features-VINS）系统；Lim等^［16］使用点线结合的方式来应对退化情况；此外，PLD-VINS（point-line features and depth information-VINS）^［17］和PLF-VINS（point-line fusion and parallel-line fusion-VINS）^［18］同样将点线特征与IMU进行融合，进一步提高系统定位精度.

尽管多传感器融合与多特征融合定位技术已取得显著进展，但现有研究主要面向通用场景，鲜有针对电缆隧道场景的自主定位算法设计.在实际应用中，仍存在诸多问题，包括：在复杂光照条件、特殊场景结构下的特征提取、追踪问题；在相似纹理、结构信息下各模块的鲁棒性问题.为满足工业智能巡检机器人在电缆隧道环境下的自主定位需求，本文提出了基于点线特征融合的视觉惯导定位算法.该算法以图像信息和IMU信息作为输入，首先，在图像点特征的基础上引入表达结构信息更丰富的线特征，并结合电缆隧道特点对线特征提取进行改进，以保证前端视觉里程计特征匹配的正确率.随后，在回环检测中提出了1种融合ArUco标记的回环检测策略，以应对电缆隧道相似场景下的错误回环问题.最后，在实际电缆隧道场景下完成算法验证，实验结果表明，该算法能够满足工业智能巡检机器人在复杂隧道环境下的高精度自主定位需求.

1 电缆隧道工业智能巡检机器人定位算法框架

本文提出的定位算法整体框架如图1所示，主要包括三大并行线程：跟踪、后端优化和回环检测.

在跟踪线程中，系统对相机和IMU采集到的信息进行预处理操作，包括点线特征提取与跟踪以及IMU预积分.具体而言，在点特征处理方面，选用ORB^［19］特征点检测方法，并通过RANSAC（random sample consensus）算法进行误匹配滤除.在线特征处理方面，基于改进的ILSD（inside line segment dettction）算法进行特征提取，并利用LBD（line band descriptor）描述子实现稳定跟踪.在后端优化阶段，系统根据前端提供的位姿信息，构建包含点线特征和IMU观测值的约束项，采用滑动窗口策略联合最小化残差函数完成位姿优化.为提升系统在电缆隧道环境下的鲁棒性，回环检测模块融合了ArUco标记和词袋（bag of words， BoW）模型，通过筛选最佳回环匹配帧实现精准检测，最终完成所有关键帧的全局位姿优化.

2 点线特征融合的视觉里程计

2.1 点特征跟踪

ORB特征点整合了FAST（features from accelerated segment test）关键点检测算法和BRIEF（binary robust independent elementary features）描述子，在保持尺度不变性的同时具有较低的计算成本和较高的检测速度，成为兼顾性能与效率的理想选择.基于这一特性，本文选取ORB特征点作为基础特征，并通过均匀化提取策略优化特征点分布.图2展示了电缆隧道环境下提取的ORB特征点分布情况.

特征提取后的正确匹配是运动估计的重要前提，因此，本文设计了基于RANSAC算法的误匹配滤除方法.该方法通过随机抽样和迭代选择匹配的特征点对，构建模型并评估其一致性，从而剔除误匹配特征点并保留正确匹配的特征点.如图3所示，经过RANSAC算法处理后的特征匹配结果显著提升了电缆隧道环境下的特征跟踪可靠性，为后续位姿估计提供了更准确的数据基础.

2.2 线特征跟踪

电缆隧道包含较为丰富的结构信息，可提取大量高维的结构化特征.高维特征具有信息量更大、约束更强等优势，在特征追踪过程中更不易丢失、成功率更高，符合SLAM前端特征稳定性的要求.针对当前场景，考虑在点特征提取基础上，进一步提取线特征，以点线结合的方式来进一步提升SLAM定位精度.

本文在提取线特征时选择LSD（line segment detector）^［20］算法，尽管LSD算法在提取线特征的鲁棒性上表现良好，但在结构化电缆架众多的电缆隧道环境下，仍存在提取效率低、短线特征多等问题.因此，本文从两方面来处理短线段：一方面针对长线段因电缆架分割而产生的分段现象，将分段的线段拟合成实际场景原本的长线段；另一方面，为了减少噪声影响和提高匹配效率，对短线特征进行长度抑制.改进后的LSD（improved LSD，ILSO）算法流程图如图4所示.

在电缆隧道场景中，LSD算法会提取大量短线段特征，从而增加线特征误匹配的概率和计算成本.因此，需要进行长度（l）抑制，长度抑制公式如式（1）所示：

l_i\le \mu \times \mathrm{m}\mathrm{i}\mathrm{n}(W,H),i\in (\mathrm{1},n).(1)

其中：\mathrm{m}\mathrm{i}\mathrm{n}(W,H)为图像宽度和高度的较小值；\mu为长度比例经验阈值.针对电缆隧道场景的图像特性，并结合大量实验验证，本文将\mu设为 0.1，以确保线特征提取的质量和稳定性.

针对交叉线分段现象，Hamid等^［21］提出了1种基于线段合并策略的LSM（line segment merging）算法.该算法通过以下步骤实现线段拟合：首先选取2条线段中相距最远的2个端点作为新线段的端点；然后对拟合结果进行验证，若拟合后线段与基准线的角度偏差超过阈值，则取消本次拟合操作；最后对拟合后的线段在当前分组中迭代执行合并操作，直至无法继续合并为止.在电缆隧道场景的实际应用中，该算法存在一定的局限性：当处理电缆架边缘特征时，由于细微重叠现象的存在，原本应保持独立的多个电缆架线段被错误地合并为单一线段，如图5紫色边框区域所示.这种误拟合现象会影响后续特征匹配和位姿估计的准确性.

误拟合的线段主要表现为以下两种情况：①基于同一基准线发生多次复拟合；②最终拟合线段长度明显超出初始基准线长度.针对这类误拟合线段，基于合并次数以及拟合线段与初始基准线段的长度比值，进行筛选和滤除.此外，LSM算法的先分组再拟合策略会导致部分分段线段未发生拟合.因此，本文对该算法进行改进，取消其分组策略，针对每条基准线，直接在排序线段集合中遍历能与其发生拟合的线段，同时对拟合过程进行追踪，若发现误拟合，则将当前拟合过程逆向还原，保证线段拟合的准确度.改进后的线段拟合结果如图6所示，其中红色线段为经过优化算法处理后的拟合结果.实验结果表明，ILSD算法显著提高了线段拟合的准确性和可靠性.

提取线特征后，使用LBD描述子进行特征匹配.在构造LBD描述子的过程中，需要将线支持区域LSR（line support region）划分为多个相同宽度的条带B_j（j=1，2，…，m）.定义条带的2个方向，分别为直线方向向量{\mathit{d}}_{\mathrm{L}}以及顺时针垂直于{\mathit{d}}_{\mathrm{L}}的垂直方向向量{\mathit{d}}_{\perp }.以线段中点构建局部坐标系原点，并将LSR内的每个像素梯度(\nabla \mathit{g})投影到该局部坐标系下：

\nabla \mathit{g}=(\nabla {\mathit{g}}_{{\mathit{d}}_{\mathrm{L}}},\nabla {\mathit{g}}_{{\mathit{d}}_{\perp }}).(2)

分别计算条带B_j和相邻条带B_{j-\mathrm{1}}，B_{j+\mathrm{1}}之间的局部像素梯度，对于条带B_j中第k行，累积该行中4个方向上像素的梯度：

(3)

式中：\lambda =f_g(k)f_l(k)表示高斯权重；v{\mathrm{1}}_j^k，v{\mathrm{2}}_j^k，v{\mathrm{3}}_j^k和v{\mathrm{4}}_j^k分别为{\mathit{d}}_{\perp }方向，{\mathit{d}}_{\perp }反方向，{\mathit{d}}_{\mathrm{L}}方向和{\mathit{d}}_{\mathrm{L}}反方向4个方向上的像素梯度和.

最后，将每行像素梯度和进行联立，构成线特征描述子矩阵 C_j ：

\begin{array}{l}{\mathit{C}}_j=\left[\begin{array}{cccc}v{\mathrm{1}}_j^{\mathrm{1}}& v{\mathrm{1}}_j^{\mathrm{2}}& \dots & v{\mathrm{1}}_j^n\\ v{\mathrm{2}}_j^{\mathrm{1}}& v{\mathrm{2}}_j^{\mathrm{2}}& \dots & v{\mathrm{2}}_j^n\\ v{\mathrm{3}}_j^{\mathrm{1}}& v{\mathrm{3}}_j^{\mathrm{2}}& \dots & v{\mathrm{3}}_j^n\\ v{\mathrm{4}}_j^{\mathrm{1}}& v{\mathrm{4}}_j^{\mathrm{2}}& \dots & v{\mathrm{4}}_j^n\end{array}\right]\in {\mathbf{R}}^{\mathrm{4}\times n},\\ n=\left\{\begin{array}{l}\mathrm{2}w,j=\mathrm{1}\mathrm{或}m,\\ \mathrm{3}w,\mathrm{其他}.\end{array}\right.\end{array}(4)

通过计算 C_j 的均值方差{\mathit{M}}_j^{\mathrm{T}}以及均值向量{\mathit{S}}_j^{\mathrm{T}}可以得到每个特征向量{\mathit{D}}_j=({\mathit{M}}_j^{\mathrm{T}},{\mathit{S}}_j^{\mathrm{T}}{)}^{\mathrm{T}}\in {\mathbf{R}}^{\mathrm{8}}，最终得到LBD描述子为

\mathrm{L}\mathrm{B}\mathrm{D}=({\mathit{M}}_{\mathrm{1}}^{\mathrm{T}},{\mathit{S}}_{\mathrm{1}}^{\mathrm{T}},{\mathit{M}}_{\mathrm{2}}^{\mathrm{T}},{\mathit{S}}_{\mathrm{2}}^{\mathrm{T}},\dots ,{\mathit{M}}_m^{\mathrm{T}},{\mathit{S}}_m^{\mathrm{T}})\in {\mathbf{R}}^{\mathrm{8}m}.(5)

在进行回环检测时，需要频繁计算特征向量之间的距离，这严重影响了SLAM系统的实时性.因此，为提高计算效率，将LBD转换为二进制描述子.与BRIEF描述子类似，二进制的LBD特征向量由许多个0和1组成，编码了72维浮点型特征向量中元素之间的大小关系.采用二进制的LBD后，计算汉明距离作为2个特征向量之间的距离度量，可显著提高其匹配的效率.基于LBD描述子的线特征匹配效果如图7所示，线特征的提取有效地描述了电缆隧道轮廓和结构化信息.

表1给出了改进前后线特征追踪耗时对比，相较于LSD算法，本文提出的线特征提取算法匹配效率更高.

3 后端优化与回环检测

3.1 基于滑动窗口的非线性优化

为保证优化效果和实时性，本文采用基于滑动窗口的非线性优化模型.该模型需要对滑动窗口中的状态向量进行联合优化，具体状态向量如下：

\mathit{\chi }=\left[{\mathit{x}}_{\mathrm{1}},{\mathit{x}}_{\mathrm{2}},\dots ,{\mathit{x}}_{k-\mathrm{1}},{\phi }_{\mathrm{1}},{\phi }_{\mathrm{2}},\dots ,{\phi }_{m-\mathrm{1}},O_{\mathrm{1}},O_{\mathrm{2}},\dots ,O_{n-\mathrm{1}}\right] .(6)

其中：\mathit{\chi }表示整个状态向量；{\mathit{x}}_i=\left[p_{b_i}^{\mathrm{w}},q_{b_i}^{\mathrm{w}},v_{b_i}^{\mathrm{w}},b_{\mathrm{a}},b_{\mathrm{g}}\right]表示第i个滑动窗口中的状态，由IMU位置信息p_{b_i}^{\mathrm{w}}、方向信息q_{b_i}^{\mathrm{w}}、速度v_{b_i}^{\mathrm{w}}、加速度计偏置b_{\mathrm{a}}和陀螺仪偏置b_{\mathrm{g}}组成；\phi表示点特征的逆深度；O表示空间线的正交.式（7）表示优化目标函数：

\underset{\mathit{\chi }}{\mathrm{m}\mathrm{i}\mathrm{n}}\left\{\sum _{k\in B}{\rho }_b{‖{\mathit{r}}_B\left({\widehat{\mathit{z}}}_{b_{k+\mathrm{1}}}^{b_k},\mathit{\chi }\right)‖}_{{\mathit{P}}_{b_{k+\mathrm{1}}}^{b_k}}^{\mathrm{2}}+\sum _{\left(i,j\right)\in F}{\rho }_f{‖{\mathit{r}}_F\left({\widehat{\mathit{z}}}_{f_i}^{c_j},\mathit{\chi }\right)‖}_{{\mathit{P}}_{f_i}^{c_j}}^{\mathrm{2}}+\sum _{\left(i,j\right)\in L}{\rho }_l{‖{\mathit{r}}_L\left({\widehat{\mathit{z}}}_{l_i}^{c_j},\mathit{\chi }\right)‖}_{{\mathit{P}}_{l_i}^{c_j}}^{{\mathrm{2}}_{c_j}}\right\}.(7)

其中： r_B\left({\widehat{\mathit{z}}}_{b_{k+\mathrm{1}}}^{b_k},\mathit{\chi }\right)表示IMU测量残差； r_F\left({\widehat{\mathit{z}}}_{f_i}^{c_j},\mathit{\chi }\right)表示点测量残差； r_L\left({\widehat{\mathit{z}}}_{l_i}^{c_j},\mathit{\chi }\right)表示线测量残差.具体而言，滑动窗口内包含以下观测数据：B为所有预积分的IMU测量值的集合；F，L分别为观测帧中点、线的测量值集合；{\mathit{P}}_{b_{k+\mathrm{1}}}^{b_k}，{\mathit{P}}_{f_i}^{c_j}，{\mathit{P}}_{l_i}^{c_j}分别表示IMU、点、线的协方差矩阵.针对点特征、线特征和IMU分别定义了对应的平均损失函数{\rho }_f，{\rho }_l，{\rho }_b.最后，通过最小化目标函数来优化滑动窗口内所有的状态变量.

3.2 回环检测

在电缆隧道中，由于场景结构的高度重复性，相邻帧间图像极为相似.除电缆箱、机器人充电桩等少数具有辨识度的特征点外，系统极易在不同空间位置采集到高度相似的图像数据.常用的基于词袋模型的回环检测方法主要通过计算图像相似度来完成回环候选帧的选择，进而造成图8所示的回环误检测现象.

针对这一问题，本文引入额外约束以辅助回环检测，并在基于词袋模型的回环检测基础上，提出了融合ArUco标记的回环检测算法.如图9所示，ArUco标记提供了唯一的ID信息，在电缆隧道内可以被快速检测和识别；此外，利用ArUco标记的4个角点坐标可以获得标记相对于相机的变换矩阵，进而筛选最佳回环帧.

该算法的实现主要包括以下2个部分.

1） 回环匹配帧对集合筛选策略.首先，利用ArUco标记的唯一ID信息约束回环检测的发生条件：仅当检测到ArUco标记时，系统才触发回环检测，且要求回环关键帧与候选帧必须包含相同的ArUco标记，图10为利用ArUco标记完成的回环检测.随后，筛选回环匹配帧对集合，筛选流程如图11所示，具体实现思想如下：

数据库构建：当相机首次经过某ArUco标记附近时，其全部图像帧会存入基于词袋模型构建、对应标记ID编号的数据库中.

回环触发与相似度计算：相机再次经过该标记附近时，从检测到该标记的第一帧开始，系统利用词袋模型计算当前帧与其前一帧的图像相似度\mathrm{s}\mathrm{i}{\mathrm{m}}_{\mathrm{1}}，并在对应标记ID的数据库中检索历史回环候选帧.

匹配帧对筛选：计算当前帧与历史最佳候选帧的图像相似度\mathrm{s}\mathrm{i}{\mathrm{m}}_{\mathrm{2}}，并将其与\mathrm{s}\mathrm{i}{\mathrm{m}}_{\mathrm{1}}进行比较.若满足预设条件，则将该帧对加入回环匹配帧对集合；否则，予以剔除.

2） 最佳回环匹配帧对策略.冗余的回环检测不仅无法提升定位精度，还会增加计算开销.因此，需要对回环匹配帧对进行筛选，选取最佳匹配帧对.其原理是通过ArUco标记求解PnP问题（perspective-n-point，透视n点问题：指利用至少n个3D-2D点的对应关系解算相机位姿），进而利用位姿估计模型解算出匹配帧对的相对位姿，具体实现过程见算法1.

算法1 最佳回环匹配帧对选择算法

输入：回环匹配帧对组合U，帧间相机光心距离d

输出：最佳回环帧及最佳回环候选帧\left(f_i,f_j\right)

1 选取U中1对回环匹配帧组合\left(f_m,f_n\right)；

2 对该组合的2帧图像，提取特征并完成匹配，利用对极几何求解帧间位姿变换矩阵{\mathit{T}}_{\mathrm{1}}；

3 根据ArUco码对2帧图像的观测，分别利用PnP求解世界坐标系到2帧图像坐标系的位姿变换矩阵{\mathit{T}}_i和{\mathit{T}}_j，代换求解帧间位姿变换矩阵{\mathit{T}}_{\mathrm{2}}；

4 对于第1张图像，d={\left({\mathit{T}}_{\mathrm{1}}\cdot \mathit{X}\right)}^{\mathrm{T}}\cdot \left({\mathit{T}}_{\mathrm{2}}\cdot \mathit{X}\right)，赋值\left(f_i,f_j\right)=\left(f_m,f_n\right)；

5 对于后续图像，如果{\left({\mathit{T}}_{\mathrm{1}}\cdot \mathit{X}\right)}^{\mathrm{T}}\cdot \left({\mathit{T}}_{\mathrm{2}}\cdot \mathit{X}\right)\le d，赋值\left(f_i,f_j\right)=\left(f_m,f_n\right)；

6 循环执行上述步骤，直至U中组合全部被选取，返回\left(f_i,f_j\right)，算法结束.

ArUco标记的位姿测量模型如图12所示，I_{\mathrm{1}}和I_{\mathrm{2}}分别为当前帧图像和回环帧图像.假设ArUco标记某一点在世界坐标系下的坐标为P_{\mathrm{a}}，在O_{\mathrm{1}}相机坐标系下的坐标为P_{\mathrm{1}}，在O_{\mathrm{2}}相机坐标系下的坐标为P_{\mathrm{2}}.已知世界坐标系到O_{\mathrm{1}}相机坐标系的变换矩阵为{\mathit{T}}_i，到O_{\mathrm{2}}相机坐标系的变换矩阵为{\mathit{T}}_j，则依据ArUco标记与相机坐标系间的位姿估算，可得

\left.\begin{array}{l}{P}_{\mathrm{1}}={\mathit{T}}_i\cdot P_{\mathrm{a}};\\ P_{\mathrm{2}}={\mathit{T}}_j\cdot P_{\mathrm{a}}.\end{array}\right\}(8)

消去ArUco标记坐标P_{\mathrm{a}}，则有

P_{\mathrm{2}}={\mathit{T}}_j\cdot {\mathit{T}}_i^{-\mathrm{1}}\cdot P_{\mathrm{1}}.(9)

最终得到相机坐标系O_{\mathrm{1}}到相机坐标系O_{\mathrm{2}}的位姿变换矩阵，{\mathit{T}}_{\mathrm{2}}={\mathit{T}}_j\cdot {\mathit{T}}_i^{-\mathrm{1}}.

得到帧间相机的位姿变换矩阵后，两相机光心O_{\mathrm{1}}到O_{\mathrm{2}}的距离d可通过变换矩阵的二范数值表述，令单位向量\mathit{X}={\left[\begin{array}{cccc}\mathrm{1}& \mathrm{1}& \mathrm{1}& \mathrm{1}\end{array}\right]}^{\mathrm{T}}，则有

d={\left({\mathit{T}}_{\mathrm{1}}\cdot \mathit{X}\right)}^{\mathrm{T}}\cdot \left({\mathit{T}}_{\mathrm{2}}\cdot \mathit{X}\right).(10)

对全部回环关键帧组合，利用ArUco标记位姿测量模型，分别计算得到多组帧间相机的光心距离，取距离最小的回环关键帧组合，即可得到最佳回环帧及其最佳回环候选帧.

4 实验结果与讨论

本文在自建的电缆隧道数据集上进行测试，以验证所提算法的准确性和鲁棒性.该数据集主要分为两类：直道数据和弯道数据.为了更好地展示算法评估结果，本文从数据集介绍、系统评价指标介绍和定位结果分析三方面展开论述.

4.1 数据集介绍

本算法的应用场景为电缆隧道，其中包括较长的直道场景及半径相对小的弯道场景，由于隧道空间狭窄，履带式机器人只能沿中间窄过道前进、后退及进行角度较小的偏航角旋转运动.因此，为获取符合本文算法的数据集，其数据采集操作流程如下：

1） 固定机器人起始位置（标记1），以该固定位置侧前方（偏x轴负方向约0.2 m，偏y轴正方向约5 m）放置ArUco标记，机器人的运动坐标系如图13所示；

2） 开始录制数据集，机器人在起始固定位置附近进行初始化，分别对X，Y，Z三轴进行3次平移后，机器人回到起始标记1位置；

3） 机器人开始分别沿直道、弯道行驶至固定位置（标记2），然后按照来时路径回退折返至标记1位置，数据录制结束并保存.

4.2 系统评价指标介绍

在SLAM系统中，通用数据集包含了机器人运动轨迹每帧数据对应的真实位姿值.因此，可采用相对位姿误差RPE（relative pose error）和绝对轨迹误差ATE（absolute trajectory error）2个指标评价定位精度.在电缆隧道场景中，由于无法使用GPS等外部信号且动作捕捉系统成本高达数百万元，难以实现真值数据的实时获取.为评估本文算法的有效性，针对上述采集数据集，从以下两方面对方案效果进行评估：

1） 绝对评价指标：机器人起始位置p_{\mathrm{s}}=(x_{\mathrm{s}},y_{\mathrm{s}},z_{\mathrm{s}})与结束位置p_{\mathrm{e}}=(x_{\mathrm{e}},y_{\mathrm{e}},z_{\mathrm{e}})的重合度，即端点距离，该距离越小表明优化轨迹的累积误差越小，定位精度越高：

d\left(p_{\mathrm{s}},p_{\mathrm{e}}\right)=\sqrt[]{{\left(x_{\mathrm{s}}-x_{\mathrm{e}}\right)}^{\mathrm{2}}+{\left(y_{\mathrm{s}}-y_{\mathrm{e}}\right)}^{\mathrm{2}}+{\left(z_{\mathrm{s}}-z_{\mathrm{e}}\right)}^{\mathrm{2}}}.(11)

2） 相对评价指标：以机器人前进路径作为参考基准，对直道段轨迹进行直线拟合；随后计算返回路径中对应直道段的轨迹点到拟合直线的距离分布，包括均值距离、中值距离和最大距离.这3个距离越小，表明往返路径的轨迹重合度越高，间接反映系统位姿估计的精度越高.假设有n个3D空间点(x_{\mathrm{1}},y_{\mathrm{1}},z_{\mathrm{1}}),(x_{\mathrm{2}},y_{\mathrm{2}},z_{\mathrm{2}}),\dots ,(x_n,y_n,z_n)，以及待拟合直线L:ax+by+cz+e=\mathrm{0}，基于点到直线距离（d_i）的平方和构建误差函数：

f(a,b,c,e)={\displaystyle \sum _{i=\mathrm{1}}^n}{d}_i^{\mathrm{2}}={\displaystyle \sum _{i=\mathrm{1}}^n}{\left(\frac{a{x}_i+b{y}_i+c{z}_i+e}{\sqrt[]{a^{\mathrm{2}}+b^{\mathrm{2}}+c^{\mathrm{2}}}}\right)}^{\mathrm{2}}.(12)

式中，a,b,c,e表示待拟合直线方程中的待求解参数.

通过最小化该误差函数，可以得到最佳直线拟合参数.直道轨迹拟合后，对应该直道返回路径的空间点到拟合线段的均值距离、中值距离分别为

d_{\mathrm{a}\mathrm{v}\mathrm{g}}=\frac{{\displaystyle \sum _{i=\mathrm{1}}^k}\frac{\left|a{x}_i+b{y}_i+c{z}_i+e\right|}{\sqrt[]{a^{\mathrm{2}}+b^{\mathrm{2}}+c^{\mathrm{2}}}}}{k},(13)

d_{\mathrm{m}\mathrm{i}\mathrm{d}}=\mathrm{m}\mathrm{i}\mathrm{d}\left({\displaystyle \sum _{i=\mathrm{1}}^k}\frac{\left|a{x}_i+b{y}_i+c{z}_i+e\right|}{\sqrt[]{a^{\mathrm{2}}+b^{\mathrm{2}}+c^{\mathrm{2}}}}\right).(14)

其中：k表示回时轨迹对应的3D点数量；\left(x_i,y_i,z_i\right)表示第i帧图像对应的坐标.

4.3 定位结果分析

在电缆隧道的直道和弯道数据集上，将本文算法与VINS-Mono进行对比.表2给出了定量分析的结果.结果表明，线特征的引入和融合ArUco标记的回环检测使得本文算法获得了更好的指标.与VINS-Mono相比，在直道场景的数据集下，绝对定位精度提升了61.19%，相对评价指标中的均值距离、中值距离分别提升了41.03%，41.33%；在弯道场景的数据集下，本文算法的绝对定位精度提升了78.26%，相对评价指标中均值距离、中值距离分别提升了81.89%，82.42%.

此外，图14给出了本文算法回环优化轨迹与VINS-Mono里程计轨迹的对比效果.根据轨迹对比效果可以看出，在2种数据集下，无论是起始端点与终止端点的重合度，还是去时轨迹与回时轨迹的重合度，本文算法优化轨迹的起止端点更近、重合度更高.

5 结 语

针对电缆隧道复杂环境下工业智能巡检机器人的巡检需求，本文提出了基于点线特征融合的视觉惯导自主定位算法.该算法通过在视觉里程计中引入改进的高维线特征，使巡检机器人在低纹理、特征单一的隧道退化场景中能有效地获取图像特征并完成匹配，为后端优化提供充足条件.针对电缆隧道环境特征相似性导致的回环检测失效问题，本文通过引入ArUco标记，利用其ID信息和位姿测量模型，有效实现了机器人的正确回环识别，从而提升了系统的定位精度.利用自建的电缆隧道数据集进行实验，结果表明本文提出的算法与VINS-Mono相比，定位精度更高，满足隧道场景应用需求.在后续工作中，笔者计划进一步提高线特征的提取效率，提高算法性能.

参考文献

[1] Mur-Artal R， Montiel J M M， Tardós J D. ORB-SLAM： a versatile and accurate monocular SLAM system［J］. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.

[2] Shan T X， Englot B. LeGO-LOAM： lightweight and ground-optimized lidar odometry and mapping on variable terrain［C］//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Madrid： IEEE， 2018： 4758-4765.

[3] Mourikis A I， Roumeliotis S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］//Proceedings 2007 IEEE International Conference on Robotics and Automation. Rome： IEEE， 2007： 3565-3572.

[4] Qin T， Li P L， Shen S J. VINS-Mono： a robust and versatile monocular visual-inertial state estimator［J］. IEEE Transactions on Robotics， 2018， 34（4）： 1004-1020.

[5] Dang T， Mascarich F， Khattak S， et al. Autonomous search for underground mine rescue using aerial robots［C］//2020 IEEE Aerospace Conference. Big Sky： IEEE， 2020： 1-8.

[6] Azpurua H， Campos M F M， Macharet D G. Three-dimensional terrain aware autonomous exploration for subterranean and confined spaces［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）. Xi’an： IEEE， 2021： 2443-2449.

[7] Campos C， Elvira R， Rodríguez J J G， et al. ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM［J］. IEEE Transactions on Robotics， 2021， 37（6）： 1874-1890.

[8] Cao S Z， Lu X Y， Shen S J. GVINS： tightly coupled GNSS-visual-inertial fusion for smooth and consistent state estimation［J］. IEEE Transactions on Robotics， 2022， 38（4）： 2004-2021.

[9] Tang H L， Niu X J， Zhang T S， et al. LE-VINS： a robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots［J］. IEEE Transactions on Instrumentation and Measurement， 2023， 72： 8502113.

[10] Song B Y， Yuan X F， Ying Z M， et al. DGM-VINS： visual-inertial SLAM for complex dynamic environments with joint geometry feature extraction and multiple object tracking［J］. IEEE Transactions on Instrumentation and Measurement， 2023， 72： 8503711.

[11] Li M L， Zhang H， Shen T A， et al. SM-VINS： a fast and decoupled monocular visual-inertial sensors SLAM system with stepwise marginalization［J］. IEEE Sensors Journal， 2024， 24（20）： 33240-33251.

[12] Zhou H Z， Zou D P， Pei L， et al. StructSLAM： visual SLAM with building structure lines［J］. IEEE Transactions on Vehicular Technology， 2015， 64（4）： 1364-1375.

[13] Forster C， Pizzoli M， Scaramuzza D. SVO： fast semi-direct monocular visual odometry［C］//2014 IEEE International Conference on Robotics and Automation（ICRA）. Hong Kong： IEEE， 2014： 15-22.

[14] Gomez-Ojeda R， Briales J， Gonzalez-Jimenez J. PL-SVO： semi-direct monocular visual odometry by combining points and line segments［C］//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Daejeon： IEEE， 2016： 4211-4216.

[15] Fu Q， Wang J， Yu H， et al. PL-VINS： real-time monocular visual-inertial SLAM with point and line features［J］. arXiv preprint arXiv， 2020： 2009.07462.

[16] Lim H， Kim Y， Jung K， et al. Avoiding degeneracy for monocular visual SLAM with point and line features［C］//2021 IEEE International Conference on Robotics and Automation （ICRA）. Xi’an： IEEE， 2021： 11675-11681.

[17] Zhu Y Q， Jin R， Lou T S， et al. PLD-VINS： RGBD visual-inertial SLAM with point and line features［J］. Aerospace Science and Technology， 2021， 119： 107185.

[18] Lee J， Park S Y. PLF-VINS： real-time monocular visual-inertial SLAM with point-line fusion and parallel-line fusion［J］. IEEE Robotics and Automation Letters， 2021， 6（4）： 7033-7040.

[19] Rublee E， Rabaud V， Konolige K， et al. ORB： an efficient alternative to SIFT or SURF ［C］//2011 IEEE International Conference on Computer Vision. Barcelona： IEEE， 2011： 2564-2571.

[20] Von Gioi R G， Jakubowicz J， Morel J M， et al. LSD： a line segment detector［J］. Image Processing on Line， 2012， 2（4）： 35-55.

[21] Hamid N， Khan N. LSM： perceptually accurate line segment merging［J］. Journal of Electronic Imaging， 2016， 25（6）： 061620.

基金资助

国家自然科学基金资助项目(62473092)

国家自然科学基金资助项目(62303097)

辽宁省自然科学基金资助项目(2023JH2/101700356)