一种用于电力巡检与作业的视觉惯性slam方法
阅读说明：本技术 一种用于电力巡检与作业的视觉惯性slam方法 (Visual inertia SLAM method for power inspection and operation ) 是由 张秋雁 杨忠 方千慧 曾鹏 许昌亮 张弛 朱傥 张海永 黄浩 于 2021-07-15 设计创作，主要内容包括：本发明公开了一种用于电力巡检与作业的视觉惯性SLAM方法,首先同时提取双目图像中的ORB点特征和LSD线特征并联合IMU测量信息进行位姿估计以及地图跟踪；然后构建由点线特征的重投影误差和IMU误差共同组成的联合误差函数并对位姿进行迭代优化；接着将追踪丢失分为短期与长期,短期丢失时利用IMU预积分信息进行位姿预测,长期丢失则重新建立局部地图；最后使用新型点线词袋模型对地图中所有关键帧进行匹配并用于回环检测和地图融合。本发明的方法可以在无人机电力巡检与作业时针对场景中丰富的线特征实现稳定和准确的跟踪,同时建立包含三维点线的全局完整地图,保证了无人机在定位与建图任务中的及时性与可靠性。(The invention discloses a visual inertia SLAM method for power inspection and operation, which comprises the steps of simultaneously extracting ORB point characteristics and LSD line characteristics in a binocular image and carrying out pose estimation and map tracking by combining IMU measurement information; then, a joint error function which is composed of the reprojection error of the point-line characteristics and the IMU error is constructed, and iterative optimization is carried out on the pose; then, dividing tracking loss into a short term and a long term, performing pose prediction by utilizing IMU pre-integration information when the tracking loss is in the short term, and reestablishing a local map when the tracking loss is in the long term; and finally, matching all key frames in the map by using a novel dotted bag-of-word model and using the key frames for loop detection and map fusion. The method can realize stable and accurate tracking on rich line characteristics in a scene during power inspection and operation of the unmanned aerial vehicle, and meanwhile, a global complete map containing three-dimensional point lines is established, so that the timeliness and the reliability of the unmanned aerial vehicle in positioning and map building tasks are ensured.)
技术领域
本发明涉及一种用于电力巡检与作业的视觉惯性SLAM方法，属于同时定位与建图技术领域。
背景技术
随着无人机在变电站巡检中的广泛应用，对无人机自主定位与导航的需求也应运而生。同步定位与建图(SLAM)技术是无人驾驶和移动机器人领域中的重要技术，通过激光、雷达、相机和惯性测量单元(IMU)等常见传感器即可实现机器人的自主定位和环境地图的建立。传统的视觉SLAM系统一般由视觉里程计、回环检测、后端优化和地图构建四个部分组成。根据相机类型可将视觉里程计分为单目、双目和深度(RGB-D)三个种类，根据不同的位姿估计方式又可分为特征法和直接法。由于直接法对图像灰度梯度的高依赖性以及图像灰度值不变的强假设，当相机发生快速运动或光照变化时系统极易丢失跟踪。因此，特征法仍是视觉SLAM系统中使用的主流方法。
传统视觉SLAM系统多使用点特征进行跟踪，而变电站由大量的输电线路组成，因此线特征更适用于变电站环境中的追踪定位与导航。同时线特征由于其相较于点特征携带了更多的结构信息，融合了线特征的SLAM系统所建立的三维地图相较于纯点云地图可储存更丰富的纹理特征。针对无人机在巡检过程中频繁的速度与方向变化所导致的易跟踪失败问题，基于多传感器融合的方法也被逐渐引入SLAM系统中。IMU以其轻量级、低成本和短时间内的高精度的特点被广泛用于视觉里程计以进行传感器的互补，在视觉追踪失败时转而使用IMU对位姿进行估计，可在一定程度上解决由相机快速运动导致的追踪丢失问题。尽管国内外在视觉惯性SLAM领域已取得一些成果，但现有的视觉惯性SLAM系统多基于点特征，对于变电站等线特征丰富的环境追踪效果较差，且没有一种基于图优化的点线特征与IMU融合方法。
发明内容
本发明解决的技术问题是：针对无人机在变电站巡检时的快速移动或转向情况下，传统视觉SLAM系统易丢失追踪问题，设计了一种用于电力巡检与作业的视觉惯性SLAM方法，加入线性特征与IMU以补偿追踪过程中的不利影响，在线特征丰富的变电站中可实现稳定和准确的跟踪，同时建立包含三维点线的全局完整地图，保证了无人机在定位与建图任务中的及时性与可靠性。
本发明采取的技术方案为：一种用于电力巡检与作业的视觉惯性SLAM方法，主要包括如下步骤：
步骤1，同时提取双目图像中的ORB点特征和LSD线特征并联合IMU测量信息进行位姿估计以及地图跟踪；
步骤2，构建由点线特征的重投影误差和IMU误差共同组成的联合误差函数，对位姿进行迭代优化；
步骤3，将追踪丢失分为短期与长期，短期丢失时利用IMU预积分信息进行位姿预测，长期丢失则重新建立局部地图；
步骤4，使用新型点线词袋模型对地图中所有关键帧进行匹配，用于回环检测和地图融合。
优选的，上述步骤1中，设直线l经过点P1和P2，它们的齐次坐标分别表示为P1＝(x1,y1,z1,w1)和P2＝(x2,y2,z2,w2)，则l可以表示为一个六维向量：
其中，和是P1和P2的非齐次坐标形式，w1和w2表示齐次因子，n和v分别表示直线l的法向量和方向向量。
直线l在世界坐标系Lw和相机坐标系Lc之间的转换关系可表示为：
其中，Rcw和tcw分别表示由世界坐标系到相机坐标系的旋转矩阵和平移向量,nc和vc是相机坐标系中的普吕克坐标向量表示。
对普吕克坐标中的矩阵[n|v]进行QR分解：
由三角函数矩阵将(||n||,||v||)参数化可以得到矩阵W：
则正交表示法可以表示为：
(U W)∈SO(3)×SO(2)
其中，U为从相机坐标系到线性坐标系的旋转矩阵，SO(3)是由三维空间的旋转矩阵所组成的特殊正交群，SO(2)是由二维空间的旋转矩阵所组成的特殊正交群。
正交表示到普吕克坐标的转换关系为：
其中，ui表示矩阵U的列向量。
优选的，上述步骤2中，点特征的重投影误差由投影点与观测点之间的距离定义，而线特征的重投影误差则通过投影线段的两个端点到观测线段的距离来计算：
其中，pci,j为空间点Pw在图像帧上的投影，poi,j为空间点Pw的观测位置，π(·)为世界坐标系中的点Pw到图像平面上的像素η＝[u v]T的映射，πh(·)是π(·)其对应的齐次表示，是相机的外参矩阵即第i帧的相机位姿，Rw∈SO(3)和分别为相机姿态在世界坐标系中的旋转矩阵和平移向量表示，li,k为垂直于由线特征的两个端点和相机光心所形成平面的单位法向量，表示为：
其中，mci,k和nci,k是空间线Lw在第i帧图像帧上的投影线段lc的2D端点。
滑动窗口中包含点、线以及IMU的全状态向量χ为：
χ＝[Rw,pw,Pw,Mw,k,Nw,k,vB,bg,ba]
其中，vB为IMU的速度，bg和ba分别代表陀螺仪和加速度计的偏置。
构建由点线特征的重投影误差和IMU误差所共同组成的联合误差函数：
其中，为IMU误差项：
其中，和分别为视觉与惯性集合，和分别为点和线的集合。eR、ev和ep分别为IMU的旋转、速度和位移误差，eb＝bk-bj为IMU偏置误差。ΣI和ΣR分别为IMU预积分的信息矩阵和随机游走误差。
优选的，上述步骤3中，IMU的状态向量为：
x＝[p v q bg ba]T
其中，p是位移，v速度，四元数q是世界坐标系中的旋转，bg和ba分别为陀螺仪和加速度计的偏差。
IMU观测模型可以用由真值、噪声和偏差组成的原始IMU数据表示为：
其中，和分别为在IMU机体坐标系下的角速度和加速度测量值，ωb和aw为角速度和加速度在IMU机体坐标系和世界坐标系下的真值，bg和ba分别为陀螺仪和加速度计的偏差，ng和na分别为陀螺仪和加速度计的噪声，四元数qbw为从世界坐标系到IMU机体坐标系的旋转。
建立IMU运动学模型为：
由IMU观测模型和运动学模型计算可得到第i时刻到第j时刻的IMU测量积分：
从IMU积分模型中可得到IMU预积分：
通过两时刻间的估计值与测量值差值可以计算预积分的误差：
其中，rp、rq和rv分别为位移、旋转和速度的误差，rba和rbg分别为IMU的加速度计和陀螺仪偏置误差。
优选的，上述步骤4中，引入结合点线特征的新型词汇模型，在回环检测和重定位时对所有被存储在关键帧数据库中的关键帧进行相似度计算。将训练好的ORB点特征和LSD线特征的二进制描述子放入同一个视觉词典中并分为点和线两组，通过对点和线特征描述符进行分层K-均值聚类构造了一个全新的点线词汇树结构。
将多个地图构成地图集，并支持地图复用和地图融合。每个插入的关键帧在重定位时都要在地图集中经过多次验证，以确保检测是否存在重复区域的准确性。如果匹配到的关键帧在当前活动地图中，则认为检测到了回环并执行全局BA以优化位姿和地图；若匹配到的关键帧在非活动地图中，则对多个地图的数据进行关联并将当前活动地图与匹配到的地图合并。由于地图融合较为耗时，将其分为几个步骤：首先，定义一个由匹配到的关键帧及其共视关键帧组成的融合窗口；然后将匹配后的地图融合到新的活动地图中，剔除冗余的地图点和地图线；接着对焊接窗口中的关键帧进行局部BA优化，优化后的关键帧可立即用于跟踪；最后，执行位姿图优化将校正扩散到其余的已合并地图中。
本发明的有益效果：与现有技术相比，本发明的效果如下：
1)本发明的方法设计的用于电力巡检与作业的视觉惯性SLAM方法，针对变电站中存在的大量线特征以及相机快速移动或旋转情况，无人机仍可更稳定地进行定位建图任务；
2)本发明方法构建的点线特征与IMU联合误差函数可以有效地降低系统的跟踪误差，从而实现更为准确的追踪效果；
3)本发明方法设计的多地图融合系统将线特征加入三维地图中，在长期跟踪失败时开始新地图的构建，所有局部地图构成了全局地图，实现了更具结构纹理和更为完整的三维地图；
4)本发明方法将新型点线词袋模型用于回环检测过程中，有效降低了回环检测的时间。
附图说明
图1为本发明所设计用于电力巡检与作业的视觉惯性SLAM算法的总体系统结构示意图；
图2为本发明方法构建的联合误差优化示意图；
图3为本发明方法所应用的新型点线词汇树结构图；
图4为本发明方法中相机与IMU的不同输出频率对比图；
图5为本发明方法所设计的地图融合方法示意图；
图6为实施例中系统建立的三维点线地图。
具体实施方式
下面，结合附图及具体的实施例对本发明进行进一步介绍。
实施例1：如图1～图6所示，一种用于电力巡检与作业的视觉惯性SLAM方法，主要包括如下步骤：
步骤1，同时提取双目图像中的ORB点特征和LSD线特征并联合IMU测量信息进行位姿估计以及地图跟踪；
步骤2，构建由点线特征的重投影误差和IMU误差共同组成的联合误差函数，对位姿进行迭代优化；
步骤3，将追踪丢失分为短期与长期，短期丢失时利用IMU预积分信息进行位姿预测，长期丢失则重新建立局部地图；
步骤4，使用新型点线词袋模型对地图中所有关键帧进行匹配，用于回环检测和地图融合。
优选的，上述步骤1中，设直线l经过点P1和P2，它们的齐次坐标分别表示为P1＝(x1,y1,z1,w1)和P2＝(x2,y2,z2,w2)，则l可以表示为一个六维向量：
其中，和是P1和P2的非齐次坐标形式，w1和w2表示齐次因子，n和v分别表示直线l的法向量和方向向量。
直线l在世界坐标系Lw和相机坐标系Lc之间的转换关系可表示为：
其中，Rcw和tcw分别表示由世界坐标系到相机坐标系的旋转矩阵和平移向量,nc和vc是相机坐标系中的普吕克坐标向量表示。
对普吕克坐标中的矩阵[n|v]进行QR分解：
由三角函数矩阵将(||n||,||v||)参数化可以得到矩阵W：
则正交表示法可以表示为：
(U W)∈SO(3)×SO(2)
其中，U为从相机坐标系到线性坐标系的旋转矩阵，SO(3)是由三维空间的旋转矩阵所组成的特殊正交群，SO(2)是由二维空间的旋转矩阵所组成的特殊正交群。
正交表示到普吕克坐标的转换关系为：
其中，ui表示矩阵U的列向量。
优选的，上述步骤2中，点特征的重投影误差由投影点与观测点之间的距离定义，而线特征的重投影误差则通过投影线段的两个端点到观测线段的距离来计算：
其中，pci,j为空间点Pw在图像帧上的投影，poi,j为空间点Pw的观测位置，π(·)为世界坐标系中的点Pw到图像平面上的像素η＝[u v]T的映射，πh(·)是π(·)其对应的齐次表示，是相机的外参矩阵即第i帧的相机位姿，Rw∈SO(3)和分别为相机姿态在世界坐标系中的旋转矩阵和平移向量表示，li,k为垂直于由线特征的两个端点和相机光心所形成平面的单位法向量，表示为：
其中，mci,k和nci,k是空间线Lw在第i帧图像帧上的投影线段lc的2D端点。
滑动窗口中包含点、线以及IMU的全状态向量χ为：
χ＝[Rw,pw,Pw,Mw,k,Nw,k,vB,bg,ba]
其中，vB为IMU的速度，bg和ba分别代表陀螺仪和加速度计的偏置。
构建由点线特征的重投影误差和IMU误差所共同组成的联合误差函数：
其中，为IMU误差项：
其中，和分别为视觉与惯性集合，和分别为点和线的集合。eR、ev和ep分别为IMU的旋转、速度和位移误差，eb＝bk-bj为IMU偏置误差。ΣI和ΣR分别为IMU预积分的信息矩阵和随机游走误差。
优选的，上述步骤3中，IMU的状态向量为：
x＝[p v q bg ba]T
其中，p是位移，v速度，四元数q是世界坐标系中的旋转，bg和ba分别为陀螺仪和加速度计的偏差。
IMU观测模型可以用由真值、噪声和偏差组成的原始IMU数据表示为：
其中，和分别为在IMU机体坐标系下的角速度和加速度测量值，ωb和aw为角速度和加速度在IMU机体坐标系和世界坐标系下的真值，bg和ba分别为陀螺仪和加速度计的偏差，ng和na分别为陀螺仪和加速度计的噪声，四元数qbw为从世界坐标系到IMU机体坐标系的旋转。
建立IMU运动学模型为：
由IMU观测模型和运动学模型计算可得到第i时刻到第j时刻的IMU测量积分：
从IMU积分模型中可得到IMU预积分：
通过两时刻间的估计值与测量值差值可以计算预积分的误差：
其中，rp、rq和rv分别为位移、旋转和速度的误差，rba和rbg分别为IMU的加速度计和陀螺仪偏置误差。
优选的，上述步骤4中，引入结合点线特征的新型词汇模型，在回环检测和重定位时对所有被存储在关键帧数据库中的关键帧进行相似度计算。将训练好的ORB点特征和LSD线特征的二进制描述子放入同一个视觉词典中并分为点和线两组，通过对点和线特征描述符进行分层K-均值聚类构造了一个全新的点线词汇树结构。
将多个地图构成地图集，并支持地图复用和地图融合。每个插入的关键帧在重定位时都要在地图集中经过多次验证，以确保检测是否存在重复区域的准确性。如果匹配到的关键帧在当前活动地图中，则认为检测到了回环并执行全局BA以优化位姿和地图；若匹配到的关键帧在非活动地图中，则对多个地图的数据进行关联并将当前活动地图与匹配到的地图合并。由于地图融合较为耗时，将其分为几个步骤：首先，定义一个由匹配到的关键帧及其共视关键帧组成的融合窗口；然后将匹配后的地图融合到新的活动地图中，剔除冗余的地图点和地图线；接着对焊接窗口中的关键帧进行局部BA优化，优化后的关键帧可立即用于跟踪；最后，执行位姿图优化将校正扩散到其余的已合并地图中。
为验证本发明公开的方法在点特征缺乏环境或快速移动情况下的追踪鲁棒性和定位准确性，对所设计算法在无人机数据集EuRoc中对其进行实验。实验运行平台配置为：2.6GHz的Intel Core i7-6700HQ处理器，内存为8GB，操作系统为Ubuntu16.04。本发明采用PL-VIO作为对比基准，以绝对轨迹误差的绝对均方根误差作为定量评价指标。
EuRoc数据集一共有11个序列，MH01-MH03、V101以及V201是简单序列，环境中的光照条件较好且无人机以相对较慢的速度飞行；MH02、V102以及V202为中等难度序列，其中包含光线较差的环境以及偶尔的运动模糊；而MH04、MH05、V103以及V203则是最有挑战性的困难序列，无人机以相对较快的速度飞过照明较差的环境，存在较多缺乏纹理的环境以及无人机方向转变。
表1 PL-VIO与本发明算法的RMSE.ATE对比
从表中可以看出，在光线较差以及较多运动模糊的MH04、MH05、V103以及V203序列中，本发明所提出SLAM算法均有大幅提升。并且在V102序列中，PL-VIO丢失了跟踪，而本发明所提出SLAM算法则仍然具有较小的轨迹误差。
实施例表明，本发明提出的用于电力巡检与作业的视觉惯性SLAM方法可以在点特征缺乏环境或快速移动情况下实现稳定和准确的跟踪，同时建立包含三维点线的全局完整地图，保证了无人机在电力巡检时定位与建图的及时性与可靠性。
以上所述，仅为本发明的具体实施方式实例，本发明的保护范围并不局限于此。熟悉该技术领域的技术人员在本发明揭露的技术范围内，可轻易找到变化或替换方式，这些都应涵盖在本发明的保护范围之内。为此，本发明的保护范围应以所述权利要求的保护范围为准。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种惯性测量的冗余系统及标定方法