CN115031735A - 基于结构特征的单目视觉惯性里程计***的位姿估计方法 - Google Patents

基于结构特征的单目视觉惯性里程计***的位姿估计方法 Download PDF

Info

Publication number
CN115031735A
CN115031735A CN202210554427.0A CN202210554427A CN115031735A CN 115031735 A CN115031735 A CN 115031735A CN 202210554427 A CN202210554427 A CN 202210554427A CN 115031735 A CN115031735 A CN 115031735A
Authority
CN
China
Prior art keywords
image frame
sliding window
structural
newly added
frame
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
CN202210554427.0A
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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202210554427.0A priority Critical patent/CN115031735A/zh
Publication of CN115031735A publication Critical patent/CN115031735A/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/20—Instruments for performing navigational calculations
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于结构特征的单目视觉惯性里程计***的位姿估计方法，所述方法包括：获取所述最新加入滑窗的图像帧与所述前一帧的匹配的点线特征；基于结构特征提取算法提取最新加入的图像帧的结构特征；对匹配的点线特征进行正交化操作，构建世界坐标系下路标点及路标线；基于相邻图像间的约束，获取IMU预积分误差；基于所述路标点、路标线以及点线特征匹配关系获得点线特征重投影误差；对包含点线特征重投影误差、IMU预积分误差及先验误差的代价函数进行优化，确定拍摄所述最新加入的图像帧时机器人的位姿。

Description

基于结构特征的单目视觉惯性里程计***的位姿估计方法
技术领域
本发明涉及计算机视觉领域，具体涉及一种基于结构特征的单目视觉惯性里程计***的位姿估计方法。
背景技术
近年来，随着计算机视觉技术的快速发展，基于视觉的同时定位与建图技术(简称VSLAM)广泛应用于家用机器人、无人机、无人驾驶、虚拟现实等领域。不过相比于激光同时定位与建图技术，VSLAM存在着鲁棒性不足，精度较差等问题，因此将轮速计，惯性测量单元(IMU)引入到VSLAM***中成为了一个研究热点。在此过程中惯性测量单元以其低廉的价格、较轻的重量与相机互补的特性得到了广泛关注。视觉信息与惯性测量信息的融合给视觉-惯性里程计(VIO)的精度与鲁棒性带来了有效地提升。然而在一些低纹理的人造场景中，仅采用点特征作为视觉约束的VIO***由于提取不到足够的点特征而无法鲁棒的运行。然而，低纹理场景往往是人造场景，这意味着该类场景一般富含类似家具、墙壁、电器等结构化物体，这些物体往往富含线段特征。线段特征能够提供更强的几何约束，且对光照变化不敏感，然而与点特征相同，线段特征只能提供局部的约束，这将造成累计误差的存在。
与此同时，例如办公室、工厂及地下车库等低纹理人造场景往往具有比较规则的结构，即满足曼哈顿世界假设由三个相互正交的主导方向组成。由这三个相互正交的主导方向组成的结构特征可以提供全局的旋转约束，进而起到消除旋转累计误差的作用。然而现有结构特征提取算法往往十分耗时，基于这些结构特征提取算法得到的VSLAM***实时性受到了严重的影响。
发明内容
有鉴于此，本发明提供了一种基于结构特征的单目视觉惯性里程计***的位姿估计方法，能够解决在没有环境先验信息的条件下，基于机器人搭载的相机和IMU传感器获得的测量信息，于运动过程中估计自身位姿的技术问题。
为了解决上述技术问题，本发明是这样实现的。
一种基于结构特征的单目视觉惯性里程计***的位姿确定方法，包括：
步骤S1：对滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧间的IMU测量值预积分，形成相邻图像帧间的约束；滑窗包含机器人单目相机传感器获得的十帧图像；
步骤S2：获取所述最新加入的图像帧与所述前一帧的匹配的点线特征；若全局结构特征未初始化完毕，进入步骤S3；否则，进入步骤S4；
步骤S3：获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；初始化世界坐标系下的全局结构特征；
步骤S4：基于结构特征提取算法提取最新加入的图像帧的结构特征；
步骤S5：对匹配的点线特征进行正交化操作，构建世界坐标系下路标点及路标线；
步骤S6：基于相邻图像间的约束，获取IMU预积分误差；基于所述路标点、路标线以及点线特征匹配关系获得点线特征重投影误差；对包含点线特征重投影误差、IMU预积分误差及先验误差的代价函数进行优化，确定拍摄所述最新加入的图像帧时机器人的位姿。
优选地，所述步骤S2，包括：
对获取的所述点线特征进行提取与匹配，包括：
对点特征由光流法进行匹配；
对线段特征由EDLines算法提取；
对线段特征的匹配，相邻两个图像帧间，属于不同图像帧的、相匹配的线段特征符合如下规则：
两条线段特征的LBD描述子之间的距离小于第一预设阈值；
两条线段特征中点之间的距离小于第二预设阈值；
两条线段特征的角度差小于第三预设阈值；
两条线段特征的平均梯度差小于第四预设。
优选地，所述步骤S3，包括：基于点线特征匹配关系以及所述相邻图像帧间的约束，获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征。
优选地，所述基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征，包括：
步骤S301：将滑窗内的图像帧对应的结构特征MFi转化到世界坐标系下，
Figure BDA0003654332770000031
其中，
Figure BDA0003654332770000032
为MFi于世界坐标系下的表示，i表示滑动窗口内图像帧的索引，
Figure BDA0003654332770000033
Figure BDA0003654332770000034
分别为IMU坐标系相对于世界坐标系的旋转和相机坐标系相对于IMU坐标系的旋转；选择具有线段特征最多的
Figure BDA0003654332770000035
作为全局结构特征的初始值，然后计算所述全局结构特征的初始值与滑窗内其它结构特征的距离，距离计算公式如下：
Figure BDA0003654332770000036
其中，
Figure BDA0003654332770000037
为第j帧结构特征于世界坐标系下的表示，并且，距离小于0.1的结构特征记为内点；
步骤S302：若内点比例大于0.6，则对所有内点进行Slerp插值得到全局结构特征MFW；否则认定全局结构特征初始化失败，滑动窗口移动，进入步骤S1。
优选地，所述步骤S4：基于结构特征提取算法提取所述最新加入的图像帧的结构特征，包括：
步骤S41：若全局结构特征初始化成功后滑窗内无新加入图像帧，方法结束；否则，进入步骤S42；
步骤S42：获取滑窗内所述最新加入的图像帧；若滑窗内所述最新加入的图像帧为全局结构特征初始化成功的第一帧图像，进入步骤S43；若则，进入步骤S45；
步骤S43：基于等效球构建所述最新加入的图像帧的极坐标栅格，所述极坐标栅格用于存储所述滑窗内最新加入的图像帧中任意一对线段特征的响应；
步骤S44：将所述重力方向投影到所述滑窗内所述最新加入的图像帧，基于所述最新加入的图像帧的极坐标栅格及所述最新加入的图像帧的信息，生成多个结构特征假设，对所述结构特征假设进行验证；获得最新加入的图像帧对应的结构特征，基于所述结构特征获得最新加入的图像帧通过验证的结构线段；将滑窗内最新加入的图像帧更新为所述当前滑窗内最新帧的下一帧，进入步骤S41；
步骤S45：若滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧匹配成功的结构线段数量低于阈值，则进入步骤S43，否则，基于匹配成功的结构线段数量构建最小二乘问题，直接对滑窗内最新加入的图像帧的结构特征进行求解，进而获得最新加入的图像帧通过验证的结构线段；将滑窗内最新图像帧更新为所述最新图像帧的下一帧，进入步骤S41。
优选地，在所述步骤S5之后，还包括步骤S6：基于优化结果删除视觉重投影误差超过预设阈值的路标点和路标线。
有益效果：
本发明是在没有环境先验信息的情况下，基于机器人搭载的相机和IMU传感器获得的测量信息，于机器人运动过程中构建环境地图并估计机器人自身位姿的视觉定位方法。
本发明采用EDLines算法提取线段特征，在此基础上基于线特征匹配关系以及重力方向，设计了一种鲁棒且高效的结构特征提取方法，并将其引入到一个基于点线特征的单目视觉惯性里程计中，在提升了VIO***的精度与鲁棒性的同时保持了***的实时性。
具有以下技术效果：
(1)本发明是一种基于视觉信息与IMU信息融合的定位方案，视觉定位和IMU定位存在一定的互补性，前者适合快速的短时间运动，而后者适合慢速的长时间运动；视觉信息可用于估计IMU零偏，IMU测量则可为相机提供初始的位姿估计。
(2)在视觉信息与IMU信息融合的基础上，在单目视觉惯性里程计***中引入了线段特征，提升了单目视觉惯性里程计***在低纹理区域下的鲁棒性与精度。其中线段特征由EDLines线段提取算法提取，EDLines线段提取算法是一种实时性极高的线段提取算法，这保证了引入线段特征后***的实时性，线段匹配则是通过LBD描述子结合几何验证的方法进行，有效的提升了线段匹配的准确度。
(3)基于初始化获得的重力向量与前后帧图像线段匹配关系提出了一种高效鲁棒的结构特征提取算法，极大地提升了结构特征提取速度。
(4)基于结构特征的正交性将其作为一个全局的旋转约束引入到单目视觉惯性里程计***中，有效的减少了旋转累计误差的存在，提升了***的精度与鲁棒性。
(5)本发明可直接于人造场景中为机器人或其他智能设备提供位姿信息，且方法实施简单，易于推广，能够实时运行。在机器人，虚拟现实，增强现实，无人驾驶等领域都有潜在价值。
(6)本发明在没有环境先验信息的情况下，基于机器人搭载的相机和IMU传感器测量信息，以视觉定位为背景，于运动过程中构建环境地图并估计自身位姿为目的，研究点线特征和结构特征在同时定位与建图***中的应用。采用EDLines线段提取算法，有效的减少了线段特征提取时间；基于线特征匹配关系以及重力向量，提出了一种鲁棒高效的结构特征提取算法，在办公室、工厂及地下车库等满足曼哈顿世界假设的人造场景中，结构特征能够提供全局的旋转约束，从而有效地消除了旋转累计误差。得益于高效的线段特征和结构特征的提取算法，引入结构特征单目视觉惯性里程计精度、鲁棒性、实时性得到了有效的提升。通过在数据集上进行测试，证明了发明方法的可行性和有效性。本发明在许多应用领域具有潜在的价值，如自动驾驶、虚拟现实、增强现实、航空航天等。
附图说明
图1为本发明提供的基于结构特征的单目视觉惯性里程计***的位姿确定方法流程示意图；
图2为本发明提供的结构特征的单目视觉惯性里程计***的位姿确定方法架构示意图；
图3为本发明提供的结构特征提取算法采用的等效球示意图；
图4为本发明提供的在Euroc数据集MH_02_easy序列下旋转误差情况并与其它方法对比示意图；
图5为本发明提供的在Euroc数据集MH_02_easy序列下平移误差情况并与其它方法对比示意图；
图6为本发明提供的在Euroc数据集MH_04_difficult序列下旋转误差情况并与其它方法对比示意图；
图7为本发明提供的在Euroc数据集MH_04_difficult序列下平移误差情况并与其它方法对比示意图。
具体实施方式
下面结合附图和实施例，对本发明进行详细描述。
如图1-图2所示，本发明提出了一种基于结构特征的单目视觉惯性里程计***的位姿确定方法，包括：
步骤S1：对滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧间的IMU测量值预积分，形成相邻图像帧间的约束；所述滑窗包含机器人单目相机传感器获得的最新十帧图像；
步骤S2：获取所述最新加入的图像帧与所述前一帧的匹配的点线特征；若全局结构特征未初始化完毕，进入步骤S3；否则，进入步骤S4；
步骤S3：获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；初始化世界坐标系下的全局结构特征；
步骤S4：基于结构特征提取算法提取所述最新加入的图像帧的结构特征；
步骤S5：对匹配的点线特征进行正交化操作，构建世界坐标系下路标点及路标线；
步骤S6：基于相邻图像间的约束，获取IMU预积分误差；基于所述路标点、路标线以及点线特征匹配关系获得点线特征重投影误差；对包含点线特征重投影误差、IMU预积分误差及先验误差的代价函数进行优化，确定拍摄所述最新加入的图像帧时机器人的位姿。
本发明中，机器人基于所搭载的单目相机传感器获得的视觉信息与IMU传感器获得的惯性信息对机器人的运行轨迹进行估计。
进一步地，在所述步骤S5之后，还包括步骤S6：基于优化结果删除视觉重投影误差超过预设阈值的路标点和路标线。
所述步骤S1，其中：
将机器人获取的滑窗内的最新加入的图像帧与所述最新加入的图像帧的前一帧的IMU测量值预积分，形成相邻图像间的约束。
本实施例中，基于预积分理论将两个图像帧间的IMU测量积分起来，形成一个关于两帧图像间的约束，滑窗内包括多个图像帧，相邻的两个图像帧间的IMU测量进行一次预积分，得到若干个相邻图像间的约束。
所述滑窗包含机器人单目相机传感器获得的最新十帧图像。
进一步地，所述步骤S2，包括：对所述最新加入的图像帧提取点线特征；基于所述最新加入的图像帧的相邻图像帧的数据，对所述点线特征进行匹配，得到匹配成功的点线特征，所述点线特征包括点特征、线段特征；若全局结构特征未初始化完毕，进入步骤S3；否则，进入步骤S4。所述全局结构特征是结构特征于世界坐标系下的表示。
所述步骤S2，包括：
所述对所述点线特征进行提取与匹配，包括：
对点特征由光流法进行匹配。
对线段特征由具有较高实时性的EDLines算法提取。
对线段特征的匹配，通过LBD描述子与几何验证相结合的方法。相邻两个图像帧间，属于不同图像帧的、相匹配的线段特征符合如下规则：
两条线段特征的LBD描述子之间的距离小于第一预设阈值；
两条线段特征中点之间的距离小于第二预设阈值；
两条线段特征的角度差小于第三预设阈值；
两条线段特征的平均梯度差小于第四预设阈值。
进一步地，所述步骤S3，包括：基于点线特征匹配关系以及所述相邻图像帧间的约束，获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征。
所述对滑窗内每个图像帧提取结构特征，基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征，包括：将所述滑窗内所有相邻图像间的视觉测量信息与惯性测量信息对齐，得到滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计、尺度和重力向量等信息；基于穷举叠加随机采样一致的结构特征提取算法对滑窗内每帧图像提取结构特征；同时计算所述图像帧的线段特征与其对应的结构特征的三个主轴的夹角，夹角小于4°的线段属于该结构特征对应的结构线段。基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及所述结构特征初始化世界坐标系下的全局结构特征，若初始化成功，则进入步骤S4；否则移动滑窗，进入步骤S1。
进一步地，所述基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征，包括：
步骤S301：将滑窗内的图像帧对应的结构特征MFi转化到世界坐标系下，
Figure BDA0003654332770000091
其中，
Figure BDA0003654332770000092
为MFi于世界坐标系下的表示，i表示滑动窗口内图像帧的索引，
Figure BDA0003654332770000093
分别为IMU坐标系相对于世界坐标系的旋转和相机坐标系相对于IMU坐标系的旋转；选择具有线段特征最多的
Figure BDA0003654332770000094
作为全局结构特征的初始值，然后计算所述全局结构特征的初始值与滑窗内其它结构特征的距离，距离计算公式如下：
Figure BDA0003654332770000095
其中，
Figure BDA0003654332770000101
为第j帧结构特征于世界坐标系下的表示，并且，距离小于0.1的结构特征记为内点；
步骤S302：若内点比例大于0.6，则对所有内点进行Slerp插值得到全局结构特征MFW；否则认定全局结构特征初始化失败，滑动窗口移动，进入步骤S1。
大多数人造场景，例如办公室、工厂及地下车库，其结构往往是规则的，即满足曼哈顿世界假设由三个相互正交的主导方向组成。此类场景中绝大多数的线段特征与上述三个相互正交的主导方向之一平行。在此前提下，基于图像上提取的线段特征，采用穷举叠加随机采样一致算法对该帧图像上的结构特征进行提取，进而获得结构特征于世界坐标系下的表示，即全局结构特征，全局结构特征的引入可有效的减少***的累计误差。
进一步地，所述步骤S4，包括：基于匹配成功的线段特征及重力向量提取所述最新加入的图像帧的结构特征。
所述步骤S4：基于结构特征提取算法提取所述最新加入的图像帧的结构特征，包括：
步骤S41：若全局结构特征初始化成功后滑窗内无新加入图像帧，方法结束；否则，进入步骤S42；
步骤S42：获取滑窗内所述最新加入的图像帧；若滑窗内所述最新加入的图像帧为全局结构特征初始化成功的第一帧图像，进入步骤S43；若则，进入步骤S45；
步骤S43：基于等效球构建所述最新加入的图像帧的极坐标栅格，所述极坐标栅格用于存储所述滑窗内最新加入的图像帧中任意一对线段特征的响应；
步骤S44：将所述重力方向投影到所述滑窗内所述最新加入的图像帧，基于所述最新加入的图像帧的极坐标栅格及所述最新加入的图像帧的信息，生成多个结构特征假设，对所述结构特征假设进行验证；获得最新加入的图像帧对应的结构特征，在此基础上基于结构特征获得最新加入的图像帧通过验证的结构线段；将滑窗内最新加入的图像帧更新为所述当前滑窗内最新帧的下一帧，进入步骤S41；
步骤S45：若滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧匹配成功的结构线段数量低于阈值，则进入步骤S43，否则，基于匹配成功的结构线段数量构建最小二乘问题，直接对滑窗内最新加入的图像帧的结构特征进行求解，进而获得最新加入的图像帧通过验证的结构线段；将滑窗内最新图像帧更新为所述最新图像帧的下一帧，进入步骤S41。
进一步地，所述步骤S43，包括：
如图3所示，所述极坐标栅格基于等效球构建，等效球是一个球心位于相机焦点的单位球，其X轴和Y轴与所述滑窗内最新图像帧的图像平面的x轴和y轴平行，Z轴则是从所述滑窗内最新图像帧的焦点指向像主点；
所述滑窗内最新图像帧的像主点记为(x0,y0)T，且焦距为f，图像平面上任意一个像素点(x,y)T都能够通过如下公式获得其等效球坐标系下的坐标P＝[X,Y,Z]T：
Figure BDA0003654332770000111
其经纬度坐标表示为：
Figure BDA0003654332770000112
其中φ∈[0,π/2]，λ∈[0,2π]，X、Y、Z分别为x、y、z轴；
所述极坐标栅格G大小为90×360，精度为1°，对t＝1,2,3,...,90，k＝1,2,3,...,360有G(t,k)初始值为0；
选取滑窗内最新图像帧上任意一对线段特征l1,l2，求其交点p对应的经纬度坐标(φdeg,λdeg)，进而将其对应的栅格进行更新，公式如下：
G(φdeg,λdeg)＝G(φdeg,λdeg)+||l1||×||l2||×sin(2θ)
其中||l||为线段特征对应的线段的长度，θ为两个线段的夹角，φdeg为交点p对应的经度，λdeg为交点p对应的纬度，G(φdeg,λdeg)为交点p对应的极坐标栅格对应的值，将得到的所述极坐标栅格应用3×3的高斯滤波器。
本实施例中，所述极坐标栅格构建过程是记录每对线段对极坐标栅格响应的过程。
所述步骤S44：将所述重力方向投影到所述滑窗内所述最新加入的图像帧，基于所述最新加入的图像帧的极坐标栅格及所述最新加入的图像帧的信息，生成多个结构特征假设，对所述结构特征假设进行验证；获得最新加入的图像帧对应的结构特征，在此基础上基于结构特征获得最新加入的图像帧通过验证的结构线段，包括：
将初始化得到的重力向量投影到所述滑窗内最新加入的图像帧的图像平面上获得结构特征第一个主轴的初始估计VPinit，计算所述最新加入的图像帧所有线段特征与所述结构特征第一个主轴的夹角，夹角小于4°的线段属于该结构特征第一个主轴对应的结构线段聚类L1，若L1包含的线段数量大于4，则对VPinit进行优化获得结构特征第一个主轴向量的假设VP1，具体公式如下：
Figure BDA0003654332770000121
本实施例中，构建最小二乘问题对VPinit进行优化获得结构特征第一个主轴向量VP1。
其中，S∈R3×n，T为转置，R为实数域，VP为待估结构特征第一个主轴向量，sl为第l条结构线段与相机坐标系原点组成平面的法向量在相机坐标系下的表示。l为下标用于描述该结构线段为结构特征第一个主轴VP1对应的第l条结构线段，n为L1包含结构线段特征个数，K为相机内参，ljstart,ljend分别对应第l条线段起点和终点的归一化坐标。
基于结构特征的正交性，给定结构特征第一个主轴VP1＝(X1,Y1,Z1)T后，结构特征第二个主轴VP2必然位于其超圆上，因此，以精度1°对该超圆进行均匀采样获得VP2的360个假设，其中第m个假设的经度λ＝m×2π/360，纬度φ则通过如下两个约束求解：
Figure BDA0003654332770000131
X1×X2+Y1×Y2+Z1×Z2＝0
其中，X1,Y1,Z1分别为结构特征第一个主轴VP1于相机坐标系下的X,Y,Z轴坐标，X2,Y2,Z2分别为结构特征第二个主轴VP2于相机坐标系下的X,Y,Z轴坐标。
结构特征第三个主轴VP3由VP1和VP2叉乘获得；将获得的360组结构特征假设VP1,VP2,VP3用极坐标表示为(λ1,φ1),(λ2,φ2),(λ3,φ3)，然后基于极坐标栅格查询每个结构特征假设对应的线段响应：
U＝G(φ1,λ1)+G(φ2,λ2)+G(φ3,λ3)
其中，U为该结构特征假设对应的线段响应，φ1,λ1分别为结构特征第一个主轴向量VP1经度与维度坐标，φ2,λ2分别为结构特征第二个主轴向量VP2经度与维度坐标，φ3,λ3则分别为结构特征第三个主轴向量VP3经度与维度坐标，G(φ1,λ1)为结构特征第一个主轴向量VP1的线段响应，G(φ2,λ2)为结构特征第二个主轴向量VP2的线段响应，G(φ3,λ3)为结构特征第三个主轴向量VP3的线段响应；线段响应最大的结构特征假设为最优估计，得到所述最新加入的图像帧结构特征最优估计后，基于结构特征最优估计，对滑动窗口最新帧的线段特征进行分类，得到与VP1,VP2,VP3向量夹角小于4°的结构线段特征集合L1,L2,L3。
所述步骤S45：若滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧匹配成功的结构线段数量低于阈值，则进入步骤S43，否则，基于匹配成功的结构线段数量构建最小二乘问题，直接对滑窗内最新加入的图像帧的结构特征进行求解，进而获得最新加入的图像帧通过验证的结构线段；将滑窗内最新图像帧更新为所述最新图像帧的下一帧，包括：
基于滑窗内最新视频帧与所述次新帧的线段特征匹配情况，获得与滑窗内最新视频帧匹配成功的结构线段集合，并按照包含结构线段特征的数量对其由大到小进行排序得到L1p,L2p,L3p。若L2p包含的结构线数量小于6，则进入步骤S43；否则便基于匹配到的结构线段集合对滑窗内最新视频帧的结构特征进行提取。即基于结构线段集合L1p,L2p构建最小二乘问题进行求解得到与之对应的消失点估计值VP1init,VP2init，然后将两者叉乘获得VP3init，最后对VP1init,VP2init,VP3init进行施密特正交化得到该帧的结构特征三个主轴向量VP1,VP2,VP3，并在此基础上获得该帧的结构线段集合，用于对后续图像帧的处理，将滑窗内最新图像帧更新为所述滑窗内最新图像帧的下一帧。
所述步骤S5，其中：
所述匹配成功的点线特征处于相机坐标系下，基于正交化将处于相机坐标系下的通过的点线特征转换到世界坐标系，进而，构建世界坐标系下路标点及路标线。
所述步骤S6之后，还包括步骤S7：
基于优化结果，删除S5形成的重投影误差大于第五预设阈值的点线路标。目的在于减少错误匹配带来的误差。
进一步地，又一实施例说明本发明的效果。
硬件环境：计算机；相关器
软件配置：Linux；ROS Kinetic
同时定位与建图***描述为，搭载特点传感器的机器人基于带噪声的传感器测量值于运动过程中构建环境地图，同时估计自身位姿的***。
本发明在Euroc数据集上进行测试评估，为了验证结构特征提取算法的有效性，实验中加入了与采用另一种知名结构特征提取算法同时其他部分与本发明相同的单目视觉惯性里程计MF-VIO的对比，其他对比***则是近几年知名的开源***。表1则展示了基于结构特征的单目视觉惯性里程计FMF-VIO在Euroc数据集上平移和旋转的均方根误差(RMSE)，并加粗和标记了每个序列效果最好的两个结果。实验结果可以看出，与不包含线段特征与结构特征的VINS-Mono相比，本发明提出的FMF-VIO***在平移和旋转的精度都有大幅度的提升。与包含线段特征但不包含结构特征的PL-VIO相比，FMF-VIO在大多数序列上的旋转精度都有比较明显的提升；同时，由于旋转精度的提升，FMF-VIO的平移精度在大多数序列上也有所提升，只有在MH_01_easy数据集上有所下降，这是由于记录该序列时无人机存在长时间的缓慢移动使结构特征测量无法有效利用造成的。与同样利用了结构特征的MF-VIO***相比，本发明提出的FMF-VIO***与之达到了相似的精度。
图4与图5直观的展示了PL-VIO和FMF-VIO两种算法在MH_02_easy序列下旋转与平移的误差情况，。从图中可以看出，本发明提出的FMF-VIO***具有更小的平移和旋转误差。这与表1得出的结论一致。
图6与图7展示了采用不同结构特征提取算法的MF-VIO和FMF-VIO于MH_04_difficult数据集旋转和平移的误差情况。从图中可以看出，本发明提出的FMF-VIO***得到的旋转误差和平移误差与MF-VIO得到的误差相当。这也与表1中的数据一致。
实时性方面，在EuRoc数据集上对本发明提出的基于重力向量与相邻帧图像线段匹配关系的结构特征提取算法G2MF与仅基于重力向量的结构特征提取算法GMF和知名结构特征提取算法代称MF的实时性进行了对比分析。表2列出了本发明所采用算法和对比算法在每一个序列中对每帧图像提取结构特征的平均耗时。可以看出，本发明提出的算法G2MF处理单帧图像的平均耗时要远远小于开源算法MF。与仅基于重力的穷举法消失点提取算法GMF相比，本发明所提出的算法实时性也有所提升。
本实施例还在MH_05_difficult数据集上评估了运行PL-VIO与FMF-VIO的平均执行时间，该数据集是一个典型的人造场景的图像序列，具有丰富的结构线段且相机运动较为剧烈。表3展示了两个***在不同单元的执行时间。可以看出，通过将LSD算法替换为EDLines，同时采用更为鲁棒、高效的结构特征提取算法，FMF-VIO在引入结构特征的前提下，实时性相比于PL-VIO依然有较为明显的提升，能够在Euroc数据集上实时运行且速度是PL-VIO的两倍。
综上，实验结果表明，本发明提出的FMF-VIO***优于VINS-Mono、PL-VIO和MF-VIO，达到了很好的定位效果。
表1平移和旋转的均方根误差(RMSE)
Figure BDA0003654332770000161
表2消失点提取算法的实时性展示(ms)
Figure BDA0003654332770000162
表3PL-VIO与FMF-VIO不同单元每帧平均执行时间(ms)
Figure BDA0003654332770000171
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。

Claims (6)

1.一种基于结构特征的单目视觉惯性里程计***的位姿估计方法，其特征在于，包括：
步骤S1：对滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧间的IMU测量值预积分，形成相邻图像帧间的约束；滑窗包含机器人单目相机传感器获得的十帧图像；
步骤S2：获取所述最新加入的图像帧与所述前一帧的匹配的点线特征；若全局结构特征未初始化完毕，进入步骤S3；否则，进入步骤S4；
步骤S3：获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；初始化世界坐标系下的全局结构特征；
步骤S4：基于结构特征提取算法提取最新加入的图像帧的结构特征；
步骤S5：对匹配的点线特征进行正交化操作，构建世界坐标系下路标点及路标线；
步骤S6：基于相邻图像间的约束，获取IMU预积分误差；基于所述路标点、路标线以及点线特征匹配关系获得点线特征重投影误差；对包含点线特征重投影误差、IMU预积分误差及先验误差的代价函数进行优化，确定拍摄所述最新加入的图像帧时机器人的位姿。
2.如权利要求1所述的方法，其特征在于，所述步骤S2，包括：
对获取的所述点线特征进行提取与匹配，包括：
对点特征由光流法进行匹配；
对线段特征由EDLines算法提取；
对线段特征的匹配，相邻两个图像帧间，属于不同图像帧的、相匹配的线段特征符合如下规则：
两条线段特征的LBD描述子之间的距离小于第一预设阈值；
两条线段特征中点之间的距离小于第二预设阈值；
两条线段特征的角度差小于第三预设阈值；
两条线段特征的平均梯度差小于第四预设。
3.如权利要求1所述方法，其特征在于，所述步骤S3，包括：基于点线特征匹配关系以及所述相邻图像帧间的约束，获得滑窗内每帧图像对应的机器人于世界坐标系下的初始位姿估计；对滑窗内每个图像帧提取结构特征；基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征。
4.如权利要求3所述的方法，其特征在于，所述基于每帧图像对应的机器人于世界坐标系下的初始位姿估计以及结构特征初始化世界坐标系下的全局结构特征，包括：
步骤S301：将滑窗内的图像帧对应的结构特征MFi转化到世界坐标系下，
Figure FDA0003654332760000021
其中，
Figure FDA0003654332760000022
为MFi于世界坐标系下的表示，i表示滑动窗口内图像帧的索引，
Figure FDA0003654332760000023
分别为IMU坐标系相对于世界坐标系的旋转和相机坐标系相对于IMU坐标系的旋转；选择具有线段特征最多的
Figure FDA0003654332760000024
作为全局结构特征的初始值，然后计算所述全局结构特征的初始值与滑窗内其它结构特征的距离，距离计算公式如下：
Figure FDA0003654332760000025
其中，
Figure FDA0003654332760000026
为第j帧结构特征于世界坐标系下的表示，并且，距离小于0.1的结构特征记为内点；
步骤S302：若内点比例大于0.6，则对所有内点进行Slerp插值得到全局结构特征MFW；否则认定全局结构特征初始化失败，滑动窗口移动，进入步骤S1。
5.如权利要求4所述的方法，其特征在于，所述步骤S4：基于结构特征提取算法提取所述最新加入的图像帧的结构特征，包括：
步骤S41：若全局结构特征初始化成功后滑窗内无新加入图像帧，方法结束；否则，进入步骤S42；
步骤S42：获取滑窗内所述最新加入的图像帧；若滑窗内所述最新加入的图像帧为全局结构特征初始化成功的第一帧图像，进入步骤S43；若则，进入步骤S45；
步骤S43：基于等效球构建所述最新加入的图像帧的极坐标栅格，所述极坐标栅格用于存储所述滑窗内最新加入的图像帧中任意一对线段特征的响应；
步骤S44：将重力方向投影到所述滑窗内所述最新加入的图像帧，基于所述最新加入的图像帧的极坐标栅格及所述最新加入的图像帧的信息，生成多个结构特征假设，对所述结构特征假设进行验证；获得最新加入的图像帧对应的结构特征，基于所述结构特征获得最新加入的图像帧通过验证的结构线段；将滑窗内最新加入的图像帧更新为当前滑窗内最新帧的下一帧，进入步骤S41；
步骤S45：若滑窗内最新加入的图像帧与所述最新加入的图像帧的前一帧匹配成功的结构线段数量低于阈值，则进入步骤S43，否则，基于匹配成功的结构线段数量构建最小二乘问题，直接对滑窗内最新加入的图像帧的结构特征进行求解，进而获得最新加入的图像帧通过验证的结构线段；将滑窗内最新图像帧更新为所述最新图像帧的下一帧，进入步骤S41。
6.如权利要求1-5中任一项所述的方法，其特征在于，在所述步骤S5之后，还包括步骤S6：基于优化结果删除视觉重投影误差超过预设阈值的路标点和路标线。
CN202210554427.0A 2022-05-20 2022-05-20 基于结构特征的单目视觉惯性里程计***的位姿估计方法 Pending CN115031735A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210554427.0A CN115031735A (zh) 2022-05-20 2022-05-20 基于结构特征的单目视觉惯性里程计***的位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210554427.0A CN115031735A (zh) 2022-05-20 2022-05-20 基于结构特征的单目视觉惯性里程计***的位姿估计方法

Publications (1)

Publication Number Publication Date
CN115031735A true CN115031735A (zh) 2022-09-09

Family

ID=83120328

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210554427.0A Pending CN115031735A (zh) 2022-05-20 2022-05-20 基于结构特征的单目视觉惯性里程计***的位姿估计方法

Country Status (1)

Country Link
CN (1) CN115031735A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115752489A (zh) * 2022-09-20 2023-03-07 湖南云箭集团有限公司 可移动设备的定位方法、装置和电子设备

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115752489A (zh) * 2022-09-20 2023-03-07 湖南云箭集团有限公司 可移动设备的定位方法、装置和电子设备

Similar Documents

Publication Publication Date Title
CN111968129B (zh) 具有语义感知的即时定位与地图构建***及方法
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与***
Engel et al. Large-scale direct SLAM with stereo cameras
CN107564061B (zh) 一种基于图像梯度联合优化的双目视觉里程计算方法
CN111445526B (zh) 一种图像帧之间位姿的估计方法、估计装置和存储介质
CN104732518B (zh) 一种基于智能机器人地面特征的ptam改进方法
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及***
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN108051002A (zh) 基于惯性测量辅助视觉的运输车空间定位方法及***
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Li et al. Leveraging planar regularities for point line visual-inertial odometry
CN109682385A (zh) 一种基于orb特征的即时定位与地图构建的方法
CN110675453B (zh) 一种已知场景中运动目标的自定位方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN111899280A (zh) 采用深度学习和混合型位姿估计的单目视觉里程计方法
CN112767546B (zh) 移动机器人基于双目图像的视觉地图生成方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN113393524A (zh) 一种结合深度学习和轮廓点云重建的目标位姿估计方法
CN115471748A (zh) 一种面向动态环境的单目视觉slam方法
Zhao et al. KSF-SLAM: a key segmentation frame based semantic SLAM in dynamic environments
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN115031735A (zh) 基于结构特征的单目视觉惯性里程计***的位姿估计方法

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