CN113532431A - 一种用于电力巡检与作业的视觉惯性slam方法 - Google Patents

一种用于电力巡检与作业的视觉惯性slam方法 Download PDF

Info

Publication number
CN113532431A
CN113532431A CN202110801543.3A CN202110801543A CN113532431A CN 113532431 A CN113532431 A CN 113532431A CN 202110801543 A CN202110801543 A CN 202110801543A CN 113532431 A CN113532431 A CN 113532431A
Authority
CN
China
Prior art keywords
imu
map
coordinate system
point
line
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110801543.3A
Other languages
English (en)
Inventor
张秋雁
杨忠
方千慧
曾鹏
许昌亮
张弛
朱傥
张海永
黄浩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guizhou Power Grid Co Ltd
Original Assignee
Guizhou Power Grid Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guizhou Power Grid Co Ltd filed Critical Guizhou Power Grid Co Ltd
Priority to CN202110801543.3A priority Critical patent/CN113532431A/zh
Publication of CN113532431A publication Critical patent/CN113532431A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种用于电力巡检与作业的视觉惯性SLAM方法，首先同时提取双目图像中的ORB点特征和LSD线特征并联合IMU测量信息进行位姿估计以及地图跟踪；然后构建由点线特征的重投影误差和IMU误差共同组成的联合误差函数并对位姿进行迭代优化；接着将追踪丢失分为短期与长期，短期丢失时利用IMU预积分信息进行位姿预测，长期丢失则重新建立局部地图；最后使用新型点线词袋模型对地图中所有关键帧进行匹配并用于回环检测和地图融合。本发明的方法可以在无人机电力巡检与作业时针对场景中丰富的线特征实现稳定和准确的跟踪，同时建立包含三维点线的全局完整地图，保证了无人机在定位与建图任务中的及时性与可靠性。

Description

一种用于电力巡检与作业的视觉惯性SLAM方法
技术领域
本发明涉及一种用于电力巡检与作业的视觉惯性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可以表示为一个六维向量：
Figure BDA0003164799790000031
其中，
Figure BDA0003164799790000032
和
Figure BDA0003164799790000033
是P1和P2的非齐次坐标形式，w1和w2表示齐次因子，n和v分别表示直线l的法向量和方向向量。
直线l在世界坐标系Lw和相机坐标系Lc之间的转换关系可表示为：
Figure BDA0003164799790000034
其中，Rcw和tcw分别表示由世界坐标系到相机坐标系的旋转矩阵和平移向量,nc和vc是相机坐标系中的普吕克坐标向量表示。
对普吕克坐标中的矩阵[n|v]进行QR分解：
Figure BDA0003164799790000035
由三角函数矩阵将(||n||,||v||)参数化可以得到矩阵W：
Figure BDA0003164799790000036
则正交表示法可以表示为：
(U W)∈SO(3)×SO(2)
其中，U为从相机坐标系到线性坐标系的旋转矩阵，SO(3)是由三维空间的旋转矩阵所组成的特殊正交群，SO(2)是由二维空间的旋转矩阵所组成的特殊正交群。
正交表示到普吕克坐标的转换关系为：
Figure BDA0003164799790000037
其中，ui表示矩阵U的列向量。
优选的，上述步骤2中，点特征的重投影误差由投影点与观测点之间的距离
Figure BDA0003164799790000041
定义，而线特征的重投影误差则通过投影线段的两个端点到观测线段的距离
Figure BDA0003164799790000042
来计算：
Figure BDA0003164799790000043
Figure BDA0003164799790000044
其中，pci,j为空间点Pw在图像帧上的投影，poi,j为空间点Pw的观测位置，π(·)为世界坐标系中的点Pw到图像平面上的像素η＝[u v]T的映射，πh(·)是π(·)其对应的齐次表示，
Figure BDA0003164799790000045
是相机的外参矩阵即第i帧的相机位姿，Rw∈SO(3)和
Figure BDA0003164799790000046
分别为相机姿态在世界坐标系中的旋转矩阵和平移向量表示，li,k为垂直于由线特征的两个端点和相机光心所形成平面的单位法向量，表示为：
Figure BDA0003164799790000047
其中，mci,k和nci,k是空间线Lw在第i帧图像帧上的投影线段lc的2D端点。
滑动窗口中包含点、线以及IMU的全状态向量χ为：
χ＝[Rw,pw,Pw,Mw,k,Nw,k,vB,bg,ba]
其中，vB为IMU的速度，bg和ba分别代表陀螺仪和加速度计的偏置。
构建由点线特征的重投影误差和IMU误差所共同组成的联合误差函数：
Figure BDA0003164799790000048
其中，
Figure BDA0003164799790000049
为IMU误差项：
Figure BDA00031647997900000410
其中，
Figure BDA0003164799790000051
和
Figure BDA0003164799790000052
分别为视觉与惯性集合，
Figure BDA0003164799790000053
和
Figure BDA0003164799790000054
分别为点和线的集合。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数据表示为：
Figure BDA0003164799790000055
Figure BDA0003164799790000056
其中，
Figure BDA0003164799790000057
和
Figure BDA0003164799790000058
分别为在IMU机体坐标系下的角速度和加速度测量值，ωb和aw为角速度和加速度在IMU机体坐标系和世界坐标系下的真值，bg和ba分别为陀螺仪和加速度计的偏差，ng和na分别为陀螺仪和加速度计的噪声，四元数qbw为从世界坐标系到IMU机体坐标系的旋转。
建立IMU运动学模型为：
Figure BDA0003164799790000059
Figure BDA00031647997900000510
Figure BDA00031647997900000511
由IMU观测模型和运动学模型计算可得到第i时刻到第j时刻的IMU测量积分：
Figure BDA00031647997900000512
Figure BDA00031647997900000513
Figure BDA0003164799790000061
从IMU积分模型中可得到IMU预积分：
Figure BDA0003164799790000062
Figure BDA0003164799790000063
Figure BDA0003164799790000064
通过两时刻间的估计值与测量值差值可以计算预积分的误差：
Figure BDA0003164799790000065
其中，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可以表示为一个六维向量：
Figure BDA0003164799790000081
其中，
Figure BDA0003164799790000082
和
Figure BDA0003164799790000083
是P1和P2的非齐次坐标形式，w1和w2表示齐次因子，n和v分别表示直线l的法向量和方向向量。
直线l在世界坐标系Lw和相机坐标系Lc之间的转换关系可表示为：
Figure BDA0003164799790000084
其中，Rcw和tcw分别表示由世界坐标系到相机坐标系的旋转矩阵和平移向量,nc和vc是相机坐标系中的普吕克坐标向量表示。
对普吕克坐标中的矩阵[n|v]进行QR分解：
Figure BDA0003164799790000091
由三角函数矩阵将(||n||,||v||)参数化可以得到矩阵W：
Figure BDA0003164799790000092
则正交表示法可以表示为：
(U W)∈SO(3)×SO(2)
其中，U为从相机坐标系到线性坐标系的旋转矩阵，SO(3)是由三维空间的旋转矩阵所组成的特殊正交群，SO(2)是由二维空间的旋转矩阵所组成的特殊正交群。
正交表示到普吕克坐标的转换关系为：
Figure BDA0003164799790000093
其中，ui表示矩阵U的列向量。
优选的，上述步骤2中，点特征的重投影误差由投影点与观测点之间的距离
Figure BDA0003164799790000094
定义，而线特征的重投影误差则通过投影线段的两个端点到观测线段的距离
Figure BDA0003164799790000095
来计算：
Figure BDA0003164799790000096
Figure BDA0003164799790000097
其中，pci,j为空间点Pw在图像帧上的投影，poi,j为空间点Pw的观测位置，π(·)为世界坐标系中的点Pw到图像平面上的像素η＝[u v]T的映射，πh(·)是π(·)其对应的齐次表示，
Figure BDA0003164799790000101
是相机的外参矩阵即第i帧的相机位姿，Rw∈SO(3)和
Figure BDA0003164799790000102
分别为相机姿态在世界坐标系中的旋转矩阵和平移向量表示，li,k为垂直于由线特征的两个端点和相机光心所形成平面的单位法向量，表示为：
Figure BDA0003164799790000103
其中，mci,k和nci,k是空间线Lw在第i帧图像帧上的投影线段lc的2D端点。
滑动窗口中包含点、线以及IMU的全状态向量χ为：
χ＝[Rw,pw,Pw,Mw,k,Nw,k,vB,bg,ba]
其中，vB为IMU的速度，bg和ba分别代表陀螺仪和加速度计的偏置。
构建由点线特征的重投影误差和IMU误差所共同组成的联合误差函数：
Figure BDA0003164799790000104
其中，
Figure BDA0003164799790000105
为IMU误差项：
Figure BDA0003164799790000106
其中，
Figure BDA0003164799790000107
和
Figure BDA0003164799790000108
分别为视觉与惯性集合，
Figure BDA0003164799790000109
和
Figure BDA00031647997900001010
分别为点和线的集合。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数据表示为：
Figure BDA0003164799790000111
Figure BDA0003164799790000112
其中，
Figure BDA0003164799790000113
和
Figure BDA0003164799790000114
分别为在IMU机体坐标系下的角速度和加速度测量值，ωb和aw为角速度和加速度在IMU机体坐标系和世界坐标系下的真值，bg和ba分别为陀螺仪和加速度计的偏差，ng和na分别为陀螺仪和加速度计的噪声，四元数qbw为从世界坐标系到IMU机体坐标系的旋转。
建立IMU运动学模型为：
Figure BDA0003164799790000115
Figure BDA0003164799790000116
Figure BDA0003164799790000117
由IMU观测模型和运动学模型计算可得到第i时刻到第j时刻的IMU测量积分：
Figure BDA0003164799790000118
Figure BDA0003164799790000119
Figure BDA00031647997900001110
从IMU积分模型中可得到IMU预积分：
Figure BDA0003164799790000121
Figure BDA0003164799790000122
Figure BDA0003164799790000123
通过两时刻间的估计值与测量值差值可以计算预积分的误差：
Figure BDA0003164799790000124
其中，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对比
Figure BDA0003164799790000131
从表中可以看出，在光线较差以及较多运动模糊的MH04、MH05、V103以及V203序列中，本发明所提出SLAM算法均有大幅提升。并且在V102序列中，PL-VIO丢失了跟踪，而本发明所提出SLAM算法则仍然具有较小的轨迹误差。
实施例表明，本发明提出的用于电力巡检与作业的视觉惯性SLAM方法可以在点特征缺乏环境或快速移动情况下实现稳定和准确的跟踪，同时建立包含三维点线的全局完整地图，保证了无人机在电力巡检时定位与建图的及时性与可靠性。
以上所述，仅为本发明的具体实施方式实例，本发明的保护范围并不局限于此。熟悉该技术领域的技术人员在本发明揭露的技术范围内，可轻易找到变化或替换方式，这些都应涵盖在本发明的保护范围之内。为此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (8)

1.一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：该方法包括以下步骤：
步骤1，同时提取双目图像中的ORB点特征和LSD线特征并联合IMU测量信息进行位姿估计以及地图跟踪；
步骤2，构建由点线特征的重投影误差和IMU误差共同组成的联合误差函数，对位姿进行迭代优化；
步骤3，将追踪丢失分为短期与长期，短期丢失时利用IMU预积分信息进行位姿预测，长期丢失则重新建立局部地图；
步骤4，使用新型点线词汇模型对地图中所有关键帧进行匹配，用于回环检测和地图融合。
2.根据权利要求1所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：所述步骤1中，设直线l经过点P1和P2，它们的齐次坐标分别表示为P1＝(x1,y1,z1,w1)和P2＝(x2,y2,z2,w2)，则l表示为一个六维向量L：
Figure FDA0003164799780000011
其中，
Figure FDA0003164799780000012
和
Figure FDA0003164799780000013
是P1和P2的非齐次坐标形式，w1和w2表示齐次因子，n和v分别表示直线l的法向量和方向向量；
直线l在世界坐标系Lw和相机坐标系Lc之间的转换关系表示为：
Figure FDA0003164799780000014
其中，Rcw和tcw分别表示由世界坐标系到相机坐标系的旋转矩阵和平移向量，nc和vc是相机坐标系中的普吕克坐标向量表示；
对普吕克坐标中的矩阵[n|v]进行QR分解：
Figure FDA0003164799780000021
由三角函数矩阵将(||n||,||v||)参数化得到矩阵W：
Figure FDA0003164799780000022
则正交表示法表示为：
(U W)∈SO(3)×SO(2)
其中，U为从相机坐标系到线性坐标系的旋转矩阵，SO(3)是由三维空间的旋转矩阵所组成的特殊正交群，SO(2)是由二维空间的旋转矩阵所组成的特殊正交群；
正交表示到普吕克坐标的转换关系为：
Figure FDA0003164799780000023
其中，ui表示矩阵U的列向量。
3.根据权利要求1所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：所述步骤2中，点特征的重投影误差由投影点与观测点之间的距离
Figure FDA0003164799780000024
定义，而线特征的重投影误差则通过投影线段的两个端点到观测线段的距离
Figure FDA0003164799780000025
来计算：
Figure FDA0003164799780000026
Figure FDA0003164799780000027
其中，pci,j为空间点Pw在图像帧上的投影，poi,j为空间点Pw的观测位置，π(·)为世界坐标系中的点Pw到图像平面上的像素η＝[u v]T的映射，πh(·)是π(·)其对应的齐次表示，
Figure FDA0003164799780000028
是相机的外参矩阵即第i帧的相机位姿，Rw∈SO(3)和
Figure FDA0003164799780000031
分别为相机姿态在世界坐标系中的旋转矩阵和平移向量表示，li,k为垂直于由线特征的两个端点和相机光心所形成平面的单位法向量，表示为：
Figure FDA0003164799780000032
其中，mci,k和nci,k是空间线Lw在第i帧图像帧上的投影线段lc的2D端点；
滑动窗口中包含点、线以及IMU的全状态向量χ为：
χ＝[Rw,pw,Pw,Mw,k,Nw,k,vB,bg,ba]
其中vB为IMU的速度，bg和ba分别代表陀螺仪和加速度计的偏置；
构建由点线特征的重投影误差和IMU误差所共同组成的联合误差函数：
Figure FDA0003164799780000033
其中，
Figure FDA0003164799780000034
为IMU误差项：
Figure FDA0003164799780000035
其中，
Figure FDA0003164799780000036
和
Figure FDA0003164799780000037
分别为视觉与惯性集合，
Figure FDA0003164799780000038
和
Figure FDA0003164799780000039
分别为点和线的集合；eR、ev和ep分别为IMU的旋转、速度和位移误差，eb＝bk-bj为IMU偏置误差；ΣI和ΣR分别为IMU预积分的信息矩阵和随机游走误差。
4.根据权利要求1所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：所述步骤3中的IMU的状态向量为：
x＝[p v q bg ba]T
其中，p是位移，v是速度，四元数q是世界坐标系中的旋转，bg和ba分别为陀螺仪和加速度计的偏差；
IMU观测模型用由真值、噪声和偏差组成的原始IMU数据表示为：
Figure FDA0003164799780000041
Figure FDA0003164799780000042
其中，
Figure FDA0003164799780000043
和
Figure FDA0003164799780000044
分别为在IMU机体坐标系下的角速度和加速度测量值，ωb和aw为角速度和加速度在IMU机体坐标系和世界坐标系下的真值，bg和ba分别为陀螺仪和加速度计的偏差，ng和na分别为陀螺仪和加速度计的噪声，四元数qbw为从世界坐标系到IMU机体坐标系的旋转；
建立IMU运动学模型为：
Figure FDA0003164799780000045
Figure FDA0003164799780000046
Figure FDA0003164799780000047
由IMU观测模型和运动学模型计算得到第i时刻到第j时刻的IMU测量积分：
Figure FDA0003164799780000048
Figure FDA0003164799780000049
Figure FDA00031647997800000410
从IMU积分模型中得到IMU预积分：
Figure FDA00031647997800000411
Figure FDA00031647997800000412
Figure FDA00031647997800000413
通过两时刻间的估计值与测量值差值计算预积分的误差：
Figure FDA0003164799780000051
其中，rp、rq和rv分别为位移、旋转和速度的误差，rba和rbg分别为IMU的加速度计和陀螺仪偏置误差。
5.根据权利要求1所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：所述步骤4中引入的结合点线特征的新型词汇模型，在回环检测和重定位时对所有被存储在关键帧数据库中的关键帧进行相似度计算，将训练好的ORB点特征和LSD线特征的二进制描述子放入同一个视觉词典中并分为点和线两组，通过对点和线特征描述符进行分层K-均值聚类构造了一个全新的点线词汇树结构。
6.根据权利要求5所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：将多个地图构成地图集，并支持地图复用和地图融合，每个插入的关键帧在重定位时都要在地图集中经过多次验证，如果匹配到的关键帧在当前活动地图中，则认为检测到了回环并执行全局BA以优化位姿和地图；若匹配到的关键帧在非活动地图中，则对多个地图的数据进行关联并将当前活动地图与匹配到的地图合并。
7.根据权利要求5所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：地图合并方法分为四个步骤：
1)定义一个由匹配到的关键帧及其共视关键帧组成的融合窗；
2)将匹配后的地图融合到新的活动地图中；
3)对焊接窗口中的关键帧进行局部BA优化，优化后的关键帧立即用于跟踪；
4)执行位姿图优化并将校正扩散到其余的已合并地图中。
8.根据权利要求7所述的一种用于电力巡检与作业的视觉惯性SLAM方法，其特征在于：步骤2)中剔除冗余的地图点和地图线。
CN202110801543.3A 2021-07-15 2021-07-15 一种用于电力巡检与作业的视觉惯性slam方法 Pending CN113532431A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110801543.3A CN113532431A (zh) 2021-07-15 2021-07-15 一种用于电力巡检与作业的视觉惯性slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110801543.3A CN113532431A (zh) 2021-07-15 2021-07-15 一种用于电力巡检与作业的视觉惯性slam方法

Publications (1)

Publication Number Publication Date
CN113532431A true CN113532431A (zh) 2021-10-22

Family

ID=78128129

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110801543.3A Pending CN113532431A (zh) 2021-07-15 2021-07-15 一种用于电力巡检与作业的视觉惯性slam方法

Country Status (1)

Country Link
CN (1) CN113532431A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115578620A (zh) * 2022-10-28 2023-01-06 北京理工大学 一种点线面多维特征-可见光融合slam方法
CN117291984A (zh) * 2023-11-22 2023-12-26 武汉理工大学 一种基于位姿约束的多帧描述符匹配重定位方法及系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106909877A (zh) * 2016-12-13 2017-06-30 浙江大学 一种基于点线综合特征的视觉同时建图与定位方法
CN106940186A (zh) * 2017-02-16 2017-07-11 华中科技大学 一种机器人自主定位与导航方法及系统
CN107369183A (zh) * 2017-07-17 2017-11-21 广东工业大学 面向mar的基于图优化slam的跟踪注册方法及系统
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110717927A (zh) * 2019-10-10 2020-01-21 桂林电子科技大学 基于深度学习和视惯融合的室内机器人运动估计方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN111982103A (zh) * 2020-08-14 2020-11-24 北京航空航天大学 一种权值优化的点线综合视觉惯性里程计方法
CN112180903A (zh) * 2020-10-19 2021-01-05 江苏中讯通物联网技术有限公司 基于边缘计算的车辆状态实时检测系统
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112945233A (zh) * 2021-01-15 2021-06-11 北京理工大学 一种全局无漂移的自主机器人同时定位与地图构建方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106909877A (zh) * 2016-12-13 2017-06-30 浙江大学 一种基于点线综合特征的视觉同时建图与定位方法
CN106940186A (zh) * 2017-02-16 2017-07-11 华中科技大学 一种机器人自主定位与导航方法及系统
CN107369183A (zh) * 2017-07-17 2017-11-21 广东工业大学 面向mar的基于图优化slam的跟踪注册方法及系统
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110717927A (zh) * 2019-10-10 2020-01-21 桂林电子科技大学 基于深度学习和视惯融合的室内机器人运动估计方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN111982103A (zh) * 2020-08-14 2020-11-24 北京航空航天大学 一种权值优化的点线综合视觉惯性里程计方法
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112180903A (zh) * 2020-10-19 2021-01-05 江苏中讯通物联网技术有限公司 基于边缘计算的车辆状态实时检测系统
CN112945233A (zh) * 2021-01-15 2021-06-11 北京理工大学 一种全局无漂移的自主机器人同时定位与地图构建方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
CARLOS CAMPOS等: "ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM", 《IEEE TRANSACTIONS ON ROBOTICS》 *
YANQING LIU 等: "Stereo Visual-Inertial SLAM With Points and Lines", 《IEEE ACCESS》 *
施振宇: "基于视觉和IMU融合的定位算法研究" *
谢晓佳: "基于点线综合特征的双目视觉SLAM方法", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
贾晓辉 等: "基于惯性测量单元辅助的激光里程计求解方法" *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115578620A (zh) * 2022-10-28 2023-01-06 北京理工大学 一种点线面多维特征-可见光融合slam方法
CN117291984A (zh) * 2023-11-22 2023-12-26 武汉理工大学 一种基于位姿约束的多帧描述符匹配重定位方法及系统
CN117291984B (zh) * 2023-11-22 2024-02-09 武汉理工大学 一种基于位姿约束的多帧描述符匹配重定位方法及系统

Similar Documents

Publication Publication Date Title
CN111739063B (zh) 一种基于多传感器融合的电力巡检机器人定位方法
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
Huang Review on LiDAR-based SLAM techniques
CN111156998A (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN109520497A (zh) 基于视觉和imu的无人机自主定位方法
CN108253963A (zh) 一种基于多传感器融合的机器人自抗扰定位方法以及定位系统
CN105783913A (zh) 一种融合车载多传感器的slam装置及其控制方法
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN112734765B (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN205426175U (zh) 一种融合车载多传感器的slam装置
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113532431A (zh) 一种用于电力巡检与作业的视觉惯性slam方法
CN110455301A (zh) 一种基于惯性测量单元的动态场景slam方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN113189613B (zh) 一种基于粒子滤波的机器人定位方法
CN112734841A (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN110533719A (zh) 基于环境视觉特征点识别技术的增强现实定位方法及装置
CN111862200B (zh) 一种煤棚内无人机定位方法
CN116878501A (zh) 一种基于多传感器融合的高精度定位与建图系统及方法
Ranft et al. 3d perception for autonomous navigation of a low-cost mav using minimal landmarks
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN110780325A (zh) 运动对象的定位方法及装置、电子设备
CN108827287A (zh) 一种复杂环境下的鲁棒视觉slam系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination