CN115830070A - 一种牵引变电所巡检机器人红外激光融合定位方法 - Google Patents

Abstract

本发明公开了一种牵引变电所巡检机器人红外激光融合定位方法，保证巡检机器人能全天候，在恶劣环境下依然保证高精度的位姿估计与建图。本发明融合红外光图像和激光雷达，以精确的坐标变化进行联合标定获取坐标转换矩阵T1，通过红外图像的边缘特征点提取匹配，使用边缘和特征点融合作为红外图像的特征点进行跟踪并计算出红外视觉里程计L；利用位姿对激光点云进行运动畸变矫正，使用ICP匹配雷达帧间，获得激光雷达里程计M；利用红外视觉高频里程计辅助激光进行运动估计，通过相邻激光扫描帧间矫正后的点云进行匹配来估计相对位姿，最终再通过激光雷达帧间估计的位姿对视觉估计的位姿进行矫正，输出高频高精度的定位信息。

Description

一种牵引变电所巡检机器人红外激光融合定位方法

技术领域

本发明涉及机器人智能巡检技术领域，特别涉及一种牵引变电所巡检机器人红外激光融合定位方法。

背景技术

变电所巡检是保障高铁牵引供电系统稳定运行的重要工作。针对巡检机器人全天候、强电磁、室外等复杂背景下的特点，目前巡检机器人存在以下问题：

激光雷达易受到雨雪等恶劣环境影响，导致自主导航定位不稳定，容易出现机器人“走偏”，无法完成巡检任务。雨雪天等其他恶劣天气下，导致激光雷达和可见光摄像头都不能保证工作精度，不能完成巡检任务。

机器人的位姿估计精度较低，无法精确到达巡检目标点。目前巡检机器人主要以激光雷达和视觉融合作为导航，目前还不存在使用红外SLAM作为机器人定位的信息源，红外图像处理的难点在于，红外图像的纹理很弱，成像设备的噪声大，信噪比很低。

因巡检机器人装备了红外热成像，利用红外图像的优势，不受外界环境光线影响，在任何环境图像稳定，利用红外里程计和激光里程计融合，优化位姿，从而使得定位和建图精度更好。

发明内容

为了克服现有技术中的不足，本发明提供一种牵引变电所巡检机器人红外激光融合定位方法，保证巡检机器人能全天候，在恶劣环境下依然保证高精度的位姿估计。融合红外图像和激光雷达，提高机器人智能化和完全自主导航的能力，与现实中实际应用机器人对比，验证本发明具有更好的实践性。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

一种牵引变电所巡检机器人红外激光融合定位方法，包括以下步骤：

步骤1：红外相机与三维激光雷达的数据融合，需要以精确的坐标变化为前提，进行联合标定获取坐标转换矩阵T1；

步骤2：坐标转换矩阵T1，在红外相机的运动采集下，在红外图像下，每两副关键帧之间存在旋转矩阵R和平移向量t；

步骤3：对步骤2输入的图像提取ORB特征点F1和边缘B，作为视觉观测信息；

步骤4：对特征点F1进行粗跟踪/精跟踪，将特征点和边缘跟踪匹配结果融合，并通过三维激光雷达点云获取边缘和特征点的深度值ρ，估计当前红外相机的位姿T_KF；

步骤5：通过步骤4获取了红外里程计L，利用位姿对激光点云进行运动畸变矫正，使用ICP匹配雷达帧间，获得激光雷达里程计M；

步骤6：通过将红外里程计L和激光雷达里程计M，判断是否插入新的关键帧KF，对后端采用全局位姿图，通过插值融合的方式进行激光和红外定位信息的融合；

步骤7：通过进行绝对位姿误差的评估，以GNSS作为轨迹真值，与现有算法实验对比，验证本方法可行性。

进一步的，在步骤1中，具体包括以下步骤：

步骤11：设定红外相机坐标系为O-x-y-z，z轴为相机前方，x向右，y向下，0则为相机的感光光点；

步骤12：设一空间点P[X，Y，Z]^T，经过光点O投影在像素平面的坐标为P′[X′，Y′，Z′]^T，根据物理成像平面到针孔的距离为f，记为焦距；

根据三角形相似可知：

进一步的，在步骤12中，包括以下步骤：

步骤121：根据步骤11中的相机坐标系，来建立成像平面坐标系o-u-v，以一个像素1为单位，则P的像素平面坐标为[u，v]^T，则投影点P′[X′，Y′，Z′]^T与像素坐标有：

其中，α、β为缩放因子，c_x、c_y为原点偏移量；

根据像素平面坐标和相机坐标系可知：

其中，f_x为像素坐标下宽度方向u的归一化焦距，f_x＝f/d_x；f_y为像素坐标下高度方向v的归一化焦距，f_y＝f/d_y；

步骤122：对步骤121中的坐标系关系存在以下等式：

其中K为旋转系数，即内参；

步骤123：将相机坐标系下坐标P用来描述实际物体运动下的坐标P_R，则存在着旋转矩阵R和平移矩阵t，可得到坐标转换矩阵T1，表达式为：

进一步的，在步骤3中，包括以下步骤：

步骤31：通过对步骤2步骤输入的图像进行滤波预处理和直方图均衡化，增强图像对比度，提取ORB特征点F1和边缘B，作为视觉观测信息；

步骤32：将提取的ORB特征点F1和边缘B分别在两个线程中同时进行，边缘B即图像灰度发生跳变的一系列一维曲线，其描述了场景的纹理或结构的形状，对图像检测边缘时，为了减少噪声的影响，首先用高斯核对图像进行平滑，然后使用Sobel算子检测边缘；

步骤33：特征点F1指的是图像灰度值发生剧烈变化的点，或者在图像边缘上曲率较大的点，即两个边缘的交点，ORB特征点使用F1检测子+改进的BRIEF描述子。

进一步的，在步骤4中，包括以下步骤：

步骤41：边缘跟踪匹配时，首先需要获得当前帧相对于关键帧KF的位姿T_KF，可以由基于特征点的跟踪得到，如果特征点跟踪没有成功，则用部分边缘点对T_KF初始化；

步骤42：在步骤41中得到的关键帧KF的位姿后，将KF中所有带有深度值ρ的边缘点投影到当前帧中，进行搜索匹配，其表达式为：

其中，P_K为位姿中的像素坐标，

为当前帧的匹配点；

搜索时，首先找到候选网络，然后再对其中的边缘点进行匹配，匹配时，首先排除法向方向差异过大的点，再排除描述子差异过大的点，然后再进行边缘匹配；

步骤43：如果上个步骤中特征点的匹配失败，则使用边缘对T_KF进行初始化，首先在KF的边缘点中，选择深度标准差最小的N个点，再在其中随机选择M个点，将这些点按照恒速模型预估的T_KF投影到当前帧中，然后以较大的半径搜索匹配，在完成匹配后，使用优化的方法，估计当前帧的位姿，设在KF中，第i个点的其次像素坐标为

逆深度为ρⁱ，法向量为

它在当前帧中的匹配点为

则构建残差：

将所有匹配点对的残差写成代价函数：

E(T_FK)＝∑_i wⁱ||rⁱ||_δ (4-3)

其中，wⁱ为第i对边缘点的描述子匹配权重，||||_δ为Huber范数；

残差rⁱ相对于T_FK的Jacobian矩阵为：

步骤431：如果特征点和边缘均跟踪成功，则根据特征点匹配结果和边缘匹配结果构造代价函数：

特征点匹配的代价函数，其中μⁱ为权重，它根据第i个特征点的尺度确定，

为特征点匹配残差：

其中，T_FK为KF在世界坐标系W中的位姿，

为第i个特征点在W系中的坐标，这两个参数在跟踪时被固定住，不参与优化，zⁱ是第i个特征点在图像中的像素坐标，

相对于T_FK的Jacobian矩阵为：

步骤432：使用BA优化上述的特征点和边缘匹配结果的代价函数，即可得到当前帧相对于KF的位姿，经过变换，得到当前帧在世界坐标中的坐标，即红外里程计L。

进一步的，在步骤5中，包括以下步骤：

步骤51：假设估计出雷达前后帧运动ξ＝[γ，β，α，t_x，t_y，t_z]，对每帧点云中的所有点按照接收的时间进行线性插值，获取在当前帧起始坐标系下的位姿T_KF；

步骤52：在步骤43中得到红外里程计L，在激光雷达帧间插入多帧视觉位姿，以此对雷达帧间运动进行非线性运动预测，在红外视觉里程计中，存储实时的相对位姿估计

步骤53：在步骤52中形成的每个相对位姿估计给定相应的时间戳信息，形成位姿估计序列，表达式如下：

进一步的，在步骤6中，包括以下步骤：

步骤61：通过对步骤5中形成的位姿估计序列，来检查是否存在新的关键帧KF，若是存在，则将其送入回环检测线程中重新获取位姿序列；

步骤62：将高精度的红外视觉里程计和低频高精度的激光雷达里程计融合，即在雷达位姿中插入视觉里程计所获得位姿，来进行全局位姿图的维护；

步骤63：通过关键帧策略，即当当前帧位姿与上一个关键帧位姿的相对位姿变化大于一个阈值，则可认为一个新的关键帧，并且加入到全局位姿图中。

进一步的，在步骤7中，包括以下步骤：

步骤71：进行绝对位姿误差的评估，分别LOAM，LeGO-LOAM实验对比，求解相机位姿估计值；

步骤72：将LOAM，LeGO-LOAM和步骤71得出估计值，以GNSS轨迹真值进行校验。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

1、本发明一种牵引变电所巡检机器人红外激光融合定位方法，利用红外视觉高频里程计定位辅助激光雷达实现初步的非线性运动模型去畸变，达到点云更加细致的去畸变，对比现有的常用算法突出本文提出方法具备更好的精确性和实践性。

2、本发明通过红外图像的边缘特征点提取匹配，便于更加精准的发现牵引变电所内各种状况的特征，便于进行人工智能分析。

3、本发明使用边缘和特征点融合作为红外图像的特征点进行跟踪，提高对变电所内故障分析的精准度。

4、本发明采用红外视觉里程计辅助激光去畸变和运动估计，通过激光雷达帧间估计进行视觉的漂移误差弥补，输出高频高精度的定位信息。

5、本发明具有低成本、高运营和易于控制的优点，可以辅助人工实现牵引变电所日常巡检。

6、本发明克服了传统智能巡检机器人监控视野易于受天气的影响，受雨雪粉尘环境较小，易于全天侯巡检。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：

图1是本发明一种牵引变电所巡检机器人红外激光融合定位方法的整体系统架构图；

图2是本发明一种牵引变电所巡检机器人红外激光融合定位方法中红外图像预处理流程图；

图3是本发明一种牵引变电所巡检机器人红外激光融合定位方法的边缘-特征点融合定位的流程图；

图4是本发明一种牵引变电所巡检机器人红外激光融合定位方法的边缘与特征点融合的原理示意图；

图5是本发明一种牵引变电所巡检机器人红外激光融合定位方法的全局位姿图；

图6是本发明一种牵引变电所巡检机器人红外激光融合定位方法边缘和特征点跟踪逻辑关系；

图7是本发明一种牵引变电所巡检机器人红外激光融合定位方法的轨迹平移误差。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本实施例公开了一种牵引变电所巡检机器人红外激光融合定位方法，包括以下步骤：

步骤1：红外相机与三维激光雷达的数据融合，需要以精确的坐标变化为前提，进行联合标定获取坐标转换矩阵T1；

步骤2：坐标转换矩阵T1，在红外相机的运动采集下，在红外图像下，每两副关键帧之间存在旋转矩阵R和平移向量t；

步骤3：对步骤2输入的图像提取ORB特征点F1和边缘B，作为视觉观测信息；

步骤4：对特征点F1进行粗跟踪/精跟踪，将特征点和边缘跟踪匹配结果融合，并通过三维激光雷达点云获取边缘和特征点的深度值ρ，估计当前红外相机的位姿T_KF；

步骤5：通过步骤4获取了红外里程计L，利用位姿对激光点云进行运动畸变矫正，使用ICP匹配雷达帧间，获得激光雷达里程计M；

步骤6：通过将红外里程计L和激光雷达里程计M，判断是否插入新的关键帧KF，对后端采用全局位姿图，通过插值融合的方式进行激光和红外定位信息的融合；

步骤7：通过进行绝对位姿误差的评估，以GNSS作为轨迹真值，与现有算法实验对比，验证本方法可行性。

进一步的，在步骤1中，具体包括以下步骤：

步骤11：设定红外相机坐标系为O-x-y-z，z轴为相机前方，x向右，y向下，O则为相机的感光光点；

步骤12：设一空间点P[X，Y，Z]^T，经过光点O投影在像素平面的坐标为P′[X′，Y′，Z′]^T，根据物理成像平面到针孔的距离为f，记为焦距；

根据三角形相似可知：

进一步的，在步骤12中，包括以下步骤：

步骤121：根据步骤11中的相机坐标系，来建立成像平面坐标系o-u-v，以一个像素1为单位，则P的像素平面坐标为[u，v]^T，则投影点P′[X′，Y′，Z′]^T与像素坐标有：

其中，α、β为缩放因子，c_x、c_y为原点偏移量；

根据像素平面坐标和相机坐标系可知：

其中，f_x为像素坐标下宽度方向u的归一化焦距，f_x＝f/d_x；f_y为像素坐标下高度方向v的归一化焦距，f_y＝f/d_y；

步骤122：对步骤121中的坐标系关系存在以下等式：

其中K为旋转系数，即内参；

步骤123：将相机坐标系下坐标P用来描述实际物体运动下的坐标P_R，则存在着旋转矩阵R和平移矩阵t，可得到坐标转换矩阵T1，表达式为：

进一步的，在步骤3中，包括以下步骤：

步骤31：通过对步骤2步骤输入的图像进行滤波预处理和直方图均衡化，增强图像对比度，提取ORB特征点F1和边缘B，作为视觉观测信息，设计流程如图2所示；

步骤32：将提取的ORB特征点F1和边缘B分别在两个线程中同时进行，边缘B即图像灰度发生跳变的一系列一维曲线，其描述了场景的纹理或结构的形状，对图像检测边缘时，为了减少噪声的影响，首先用高斯核对图像进行平滑，然后使用Sobel算子检测边缘；

步骤33：特征点F1指的是图像灰度值发生剧烈变化的点，或者在图像边缘上曲率较大的点，即两个边缘的交点，ORB特征点使用F1检测子+改进的BRIEF描述子。

进一步的，在步骤4中，作为具体实例，结合图3步骤4中所述的跟踪匹配特征点和边缘，通过雷达获取对应的深度值ρ，估计当前帧位姿，包括以下步骤：

步骤41：边缘跟踪匹配时，首先需要获得当前帧相对于关键帧KF的位姿T_KF，可以由基于特征点的跟踪得到，如果特征点跟踪没有成功，则用部分边缘点对T_KF初始化；

步骤42：在步骤41中得到的关键帧KF的位姿后，将KF中所有带有深度值ρ的边缘点投影到当前帧中，进行搜索匹配，其表达式为：

其中，P_K为位姿中的像素坐标，

为当前帧的匹配点；

在进行搜索时，首先找到候选网络，然后再对其中的边缘点进行匹配，匹配时，首先排除法向方向差异过大的点，再排除描述子差异过大的点，然后再进行边缘匹配；

步骤43：如果上个步骤中特征点的匹配失败，则使用边缘对T_KF进行初始化，首先在KF的边缘点中，选择深度标准差最小的N个点，然后再在其中随机选择M个点，将这些点按照恒速模型预估的T_KF投影到当前帧中，然后以较大的半径搜索匹配，在完成匹配后，使用优化的方法，估计当前帧的位姿，设在KF中，第i个点的其次像素坐标为

逆深度为ρⁱ，法向量为

它在当前帧中的匹配点为

则构建残差：

将所有匹配点对的残差写成代价函数：

E(T_FK)＝∑_i wⁱ||rⁱ||_δ (4-3)

其中，wⁱ为第i对边缘点的描述子匹配权重，||||_δ为Huber范数；

残差rⁱ相对于T_FK的Jacobian矩阵为：

步骤431：如果特征点和边缘均跟踪成功，则根据特征点匹配结果和边缘匹配结果构造代价函数：

特征点匹配的代价函数，其中μⁱ为权重，它根据第i个特征点的尺度确定，

为特征点匹配残差：

其中，T_FK为KF在世界坐标系W中的位姿，

为第i个特征点在W系中的坐标，这两个参数在跟踪时被固定住，不参与优化，zⁱ是第i个特征点在图像中的像素坐标，

相对于T_FK的Jacobian矩阵为：

步骤432：使用BA优化上述的特征点和边缘匹配结果的代价函数，即可得到当前帧相对于KF的位姿，经过变换，得到当前帧在世界坐标中的坐标，即红外里程计L。如图4所示，假设空间中有边缘E＝{E₁，…，E_m}和特征点P＝{P₁，…，P_n}(图中只显示了一条边缘Eⁱ和一个特征点P^j)。当相机从ξ₁移动到ξ₂时，记录了图像I₁和I₂。边缘Eⁱ和特征点P^j在图像I₁和I₂上的投影分别为

和

和

进一步的，在步骤5中，所述的利用红外视觉里程计对激光点云进行运动畸变矫正，再使用ICP匹配雷达帧间，获得激光雷达里程计。再消除红外视觉定位的漂移误差，优化红外里程计的位姿，原理如图5所示，包括以下步骤：

步骤51：假设估计出雷达前后帧运动ξ＝[γ，β，α，t_x，t_y，t_z]，对每帧点云中的所有点按照接收的时间进行线性插值，获取在当前帧起始坐标系下的位姿T_KF；

步骤52：在步骤43中得到红外里程计L，在激光雷达帧间插入多帧视觉位姿，以此对雷达帧间运动进行非线性运动预测，在红外视觉里程计中，存储实时的相对位姿估计

步骤53：在步骤52中形成的每个相对位姿估计给定相应的时间戳信息，形成位姿估计序列，表达式如下：

进一步的，在步骤6中，所述的判断是否插入新的关键帧KF。若插入了新的KF关键帧，则将其送入回环检测线程。后端采用全局位姿图，通过插值融合的方式进行激光和红外定位信息的融合，包括以下步骤：

步骤61：通过对步骤5中形成的位姿估计序列，来检查是否存在新的关键帧KF，若是存在，如图6所示，将其送入回环检测线程中重新获取位姿序列；

步骤62：将高精度的红外视觉里程计和低频高精度的激光雷达里程计融合，即在雷达位姿中插入视觉里程计所获得位姿，来进行全局位姿图的维护；

步骤63：通过关键帧策略，即当当前帧位姿与上一个关键帧位姿的相对位姿变化大于一个阈值，则可认为一个新的关键帧，并且加入到全局位姿图中。

进一步的，在步骤7中，包括以下步骤：

步骤71：进行绝对位姿误差的评估，分别LOAM，LeGO-LOAM实验对比，求解相机位姿估计值；

步骤72：使用开源数据集验证本发明提出的方法，包含全天24小时时段不同时段的录制的数据。通过进行绝对位姿误差的评估，与现有开源方案LOAM激光里程计与地图构建，LeGO-LOAM实验对比，GNSS作为轨迹真值，测量数据如表1所示。

表1三种方法下的GNSS

Method	TIME：17：00	TIME：20：00
			LOAM	8.543	11.699
LeGO--LOAM	0.118	0.137
			Proposed	0.112	0.092

一条估计轨迹P_esti,i和真实轨迹P_gt,i，其中i＝1，...，N。均方根误差(Root MeanSquare Error，RMSE)，这种误差可以刻画两条轨迹的旋转和平移误差，仅考虑平移误差条件下，从而可以定义绝对平移误差(Average Translational Error)：

其中trans表示取括号内部变量的平移部分，误差曲线如图7所示。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (8)

1.一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，包括以下步骤：

步骤1：红外相机与三维激光雷达的数据融合，需要以精确的坐标变化为前提，进行联合标定获取坐标转换矩阵T1；

步骤2：坐标转换矩阵T1，在红外相机的运动采集下，在红外图像下，每两副关键帧之间存在旋转矩阵R和平移向量t；

步骤3：步骤2输入的图像提取ORB特征点F1和边缘B，作为视觉观测信息；

步骤4：对特征点F1进行粗跟踪/精跟踪，将特征点和边缘跟踪匹配结果融合，并通过三维激光雷达点云获取边缘和特征点的深度值ρ，估计当前红外相机的位姿T_KF；

步骤5：通过步骤4获取了红外里程计L，利用位姿对激光点云进行运动畸变矫正，使用ICP匹配雷达帧间，获得激光雷达里程计M；

步骤6：通过将红外里程计L和激光雷达里程计M，判断是否插入新的关键帧KF，对后端采用全局位姿图，通过插值融合的方式进行激光和红外定位信息的融合；

步骤7：进行绝对位姿误差的评估，以GNSS作为轨迹真值，与现有算法实验对比，验证本方法可行性。

2.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤1中，具体包括以下步骤：

步骤11：设定红外相机坐标系为O-x-y-z，z轴为相机前方，x向右，y向下，0则为相机的感光光点；

步骤12：设一空间点P[X，Y，Z]^T，经过光点0投影在像素平面的坐标为P′[X′，Y′，Z′]^T，根据物理成像平面到针孔的距离为f，记为焦距；

根据三角形相似可知：

3.根据权利要求2所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤12中，包括以下步骤：

步骤121：根据步骤11中的相机坐标系，来建立成像平面坐标系o-u-v，以一个像素1为单位，则P的像素平面坐标为[u，v]^T，则投影点P′[X′，Y′，Z′]^T与像素坐标有：

其中，α、β为缩放因子，c_x、c_y为原点偏移量；

根据像素平面坐标和相机坐标系可知：

其中，f_x为像素坐标下宽度方向u的归一化焦距，f_x＝f/d_x；f_y为像素坐标下高度方向v的归一化焦距，f_y＝f/d_y；

步骤122：对步骤121中的坐标系关系存在以下等式：

其中K为旋转系数，即内参；

步骤123：将相机坐标系下坐标P用来描述实际物体运动下的坐标P_R，则存在着旋转矩阵R和平移矩阵t，可得到坐标转换矩阵T1，表达式为：

4.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤3中，包括以下步骤：

步骤31：通过对步骤2步骤输入的图像进行滤波预处理和直方图均衡化，增强图像对比度，提取ORB特征点F1和边缘B，作为视觉观测信息；

步骤32：将提取的ORB特征点F1和边缘B分别在两个线程中同时进行，边缘B即图像灰度发生跳变的一系列一维曲线，其描述了场景的纹理或结构的形状，对图像检测边缘时，为了减少噪声的影响，首先用高斯核对图像进行平滑，然后使用Sobel算子检测边缘；

步骤33：特征点F1指的是图像灰度值发生剧烈变化的点，或者在图像边缘上曲率较大的点，即两个边缘的交点，ORB特征点使用F1检测子+改进的BRIEF描述子。

5.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤4中，包括以下步骤：

步骤41：边缘跟踪匹配时，首先需要获得当前帧相对于关键帧KF的位姿T_KF，可以由基于特征点的跟踪得到，如果特征点跟踪没有成功，则用部分边缘点对T_KF初始化；

步骤42：在步骤41中得到的关键帧KF的位姿后，将KF中所有带有深度值ρ的边缘点投影到当前帧中，进行搜索匹配，其表达式为：

其中，P_K为位姿中的像素坐标，

为当前帧的匹配点；

搜索时，首先找到候选网络，然后再对其中的边缘点进行匹配，匹配时，首先排除法向方向差异过大的点，再排除描述子差异过大的点，然后再进行边缘匹配；

步骤43：如果上个步骤中特征点的匹配失败，则使用边缘对T_KF进行初始化，首先在KF的边缘点中，选择深度标准差最小的N个点，然后再在其中随机选择M个点，将这些点按照恒速模型预估的T_KF投影到当前帧中，以较大的半径搜索匹配，在完成匹配后，使用优化的方法，估计当前帧的位姿，设在KF中，第i个点的其次像素坐标为

逆深度为ρⁱ，法向量为

它在当前帧中的匹配点为

则构建残差：

将所有匹配点对的残差写成代价函数：

E(T_FK)＝∑_iwⁱ||rⁱ||_δ (4-3)

其中，wⁱ为第i对边缘点的描述子匹配权重，|| ||_δ为Huber范数；

残差rⁱ相对于T_FK的Jacobian矩阵为：

步骤431：如果特征点和边缘均跟踪成功，则根据特征点匹配结果和边缘匹配结果构造代价函数：

特征点匹配的代价函数，其中μⁱ为权重，它根据第i个特征点的尺度确定，

为特征点匹配残差：

其中，T_FK为KF在世界坐标系W中的位姿，

为第i个特征点在W系中的坐标，这两个参数在跟踪时被固定住，不参与优化，zⁱ是第i个特征点在图像中的像素坐标，

相对于T_FK的Jacobian矩阵为：

步骤432：使用BA优化上述的特征点和边缘匹配结果的代价函数，即可得到当前帧相对于KF的位姿，经过变换，得到当前帧在世界坐标中的坐标，即红外里程计L。

6.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤5中，包括以下步骤：

步骤51：假设估计出雷达前后帧运动ξ＝[γ，β，α，t_x，t_y，t_z]，对每帧点云中的所有点按照接收的时间进行线性插值，获取在当前帧起始坐标系下的位姿T_KF；

步骤52：在步骤43中得到红外里程计L，在激光雷达帧间插入多帧视觉位姿，以此对雷达帧间运动进行非线性运动预测，在红外视觉里程计中，存储实时的相对位姿估计

步骤53：在步骤52中形成的每个相对位姿估计给定相应的时间戳信息，形成位姿估计序列，表达式如下：

7.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤6中，包括以下步骤：

步骤61：通过对步骤5中形成的位姿估计序列，来检查是否存在新的关键帧KF，若是存在，则将其送入回环检测线程中重新获取位姿序列；

步骤62：将高精度的红外视觉里程计和低频高精度的激光雷达里程计融合，即在雷达位姿中插入视觉里程计所获得位姿，来进行全局位姿图的维护；

步骤63：通过关键帧策略，即当当前帧位姿与上一个关键帧位姿的相对位姿变化大于一个阈值，则可认为一个新的关键帧，并且加入到全局位姿图中。

8.根据权利要求1所述的一种牵引变电所巡检机器人红外激光融合定位方法，其特征在于，在步骤7中，包括以下步骤：

步骤71：进行绝对位姿误差的评估，分别LOAM，LeGO-LOAM实验对比，求解相机位姿估计值；

步骤72：将LOAM，LeGO-LOAM和步骤71得出估计值，以GNSS轨迹真值进行校验。

Images