CN117421384A - 基于共视投影匹配的视觉惯性slam***滑窗优化方法 - Google Patents

基于共视投影匹配的视觉惯性slam***滑窗优化方法 Download PDF

Info

Publication number
CN117421384A
CN117421384A CN202311384144.7A CN202311384144A CN117421384A CN 117421384 A CN117421384 A CN 117421384A CN 202311384144 A CN202311384144 A CN 202311384144A CN 117421384 A CN117421384 A CN 117421384A
Authority
CN
China
Prior art keywords
sliding window
frame image
constraint
frame
visual
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
CN202311384144.7A
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.)
Harbin University of Science and Technology
Original Assignee
Harbin University of Science and Technology
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 Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202311384144.7A priority Critical patent/CN117421384A/zh
Publication of CN117421384A publication Critical patent/CN117421384A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Databases & Information Systems (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • Remote Sensing (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)

Abstract

基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，涉及SLAM技术领域。本发明是为了解决现有视觉惯性SLAM***后端的滑动窗口优化方法精度低的问题。本发明在不改变滑动窗口优化快速性的前提下，通过共视投影匹配关系改进滑动窗口优化方法，消除了“影子”地图点，提高了地图点精度；增加了关键帧成功匹配地图点的数量，扩大了关键帧的观测视野，新增了SLAM***的视觉观测约束，从而提高了滑动窗口优化估计位姿的精度，进而提高了移动机器人的定位精度。

Description

基于共视投影匹配的视觉惯性SLAM***滑窗优化方法
技术领域
本发明属于SLAM技术领域。
背景技术
SLAM(Simultaneous Localization and Mapping，同时定位与地图构建)技术能够使移动机器人实现自主导航定位与规划，通过移动机器人自身携带的一种或多种传感器设备，感知环境信息，测量自身姿态，实现对移动机器人自身的位姿跟踪，定位其相对于周围环境的位置，同时构建出环境地图。SLAM***可分为前端和后端两部分，前端实现环境感知、位姿跟踪、回环检测等功能，后端实现位姿优化、轨迹修正、地图构建等功能。前端主要完成数据处理相关的工作，以便SLAM进行实时的显示与更新交互信息，后端主要完成消除累积误差等计算量大且耗时的优化相关工作。SLAM的后端优化方法分为滤波法和非线性优化法，滤波法根据贝叶斯理论通过预测和测量信息估计出移动机器人位姿的后验概率，从而实现对其位姿的更新，但是协方差矩阵和状态量是平方增长关系，导致数据量过大，不适合大型场景环境下使用；非线性优化方法则将移动机器人的位姿信息、测量约束关系，甚至地图点的位置信息，全部纳入到因子图模型中，通过非线性优化的方法实现对其状态估计的整体更新。
当前，非线性优化方法是SLAM后端处理的主流方案，为了实现快速修正和更新移动机器人的位姿状态的目的，SLAM后端通常采用局部优化的方式，即选择一部分关键帧进行局部位姿优化，从而加快后端优化的响应速度。滑动窗口法是经典的局部区域筛选优化方法，通过设置固定数量关键帧的滑窗，在该局部区域内对视觉与惯性测量建立约束关系模型，然后优化该模型。视觉惯性融合的SLAM后端优化常用滑动窗口法进行局部区域筛选，典型的有中国香港科技大学沈邵劼团队所著的“VINS-Mono:A Robust and VersatileMonocular Visual-Inertial State Estimator(一种鲁棒性强、通用性强的单目视觉惯性状态估计器)”，以及Leutenegger等人所著的“Keyframe-based Visual-inertial SLAMusing Nonlinear Optimization(基于关键帧的视觉惯性SLAM非线性优化)”。滑动窗口法通过舒尔补操作更新滑窗内关键帧，在保留关键帧之间约束关系的前提对滑入或滑出的关键帧进行边缘化，保证滑窗内关键帧的数量不变，从而能够使SLAM***在较快的时间内完成优化计算，然而优化的精度则不能保证，优化的精度完全由关键帧的质量和误差约束关系决定。因此，传统的视觉惯性SLAM***有待将进一步改进其后端的滑动窗口优化方法，以提高其优化精度。
发明内容
本发明是为了解决现有视觉惯性SLAM***后端的滑动窗口优化方法精度低的问题，现提供基于共视投影匹配的视觉惯性SLAM***滑窗优化方法。
本发明所述基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，包括以下步骤：
步骤一：当视觉惯性SLAM***的后端模块订阅到其前端模块发布的机器人最新帧图像时，判断滑动窗口是否已满，是则执行步骤二，否则执行步骤四；
步骤二：判断次新帧图像是否为关键帧，是则将滑动窗口内最老帧图像边缘化然后执行步骤三，否则将次新帧图像边缘化然后执行步骤三；
步骤三：根据舒尔补理论重构滑动窗口的约束关系，然后执行步骤四；
步骤四：根据最新帧图像与滑动窗口内当前最新关键帧图像的视差判断所述最新帧图像是否为关键帧，是则将滑动窗口内的地图点投影到所述最新帧图像上，所述地图点为滑动窗口内能够与所述最新帧图像共视的关键帧所观测到的地图点，然后执行步骤五，否则更新最新帧图像对应的IMU约束关系，然后返回步骤一；
步骤五：将最新帧图像上的投影点与特征点进行匹配，将未匹配到特征点的投影点删除，同时判断有相匹配投影点的特征点是否存在原地图点，所述原地图点为特征点在进行匹配之前滑动窗口内就存在能够与之对应的地图点，是则执行步骤六，否则执行步骤七；
步骤六：比较投影点对应的地图点与特征点对应的原地图点的被观测次数，仅保留被观测次数多的地图点，使得滑窗内各关键帧的视觉观测约束得到更新，然后执行步骤八；
步骤七：增加新的地图点，使得所述最新帧图像与新增地图点之间建立观测约束，然后执行步骤八；
步骤八：判断滑动窗口内关键帧数量是否已满，是则执行步骤九，否则返回步骤一；
步骤九：分别构建视觉约束因子、IMU预积分约束因子以及边缘化先验约束因子，将所述视觉约束因子、IMU预积分约束因子以及边缘化先验约束因子加入到滑窗因子图优化模型中构建目标函数；
步骤十：利用非线性优化算法对所述目标函数进行优化，更新滑动窗口内机器人的状态量，完成滑窗优化。
进一步的，上述步骤二中所述将滑动窗口内最老帧图像边缘化，包括：
将滑动窗口内最老帧图像及其对应的IMU约束关系删除，并保留最老帧图像的视觉约束关系；
步骤二中所述将次新帧图像边缘化，包括：
将次新帧图像及其视觉约束关系删除，并保留次新帧图像对应的IMU约束关系。
进一步的，上述步骤三所述根据舒尔补理论重构滑动窗口的约束关系，包括：
将滑动窗口内所有图像帧的状态变量χ以增量方程的方式表示如下：
其中，δχd为待边缘化的状态向量，δχs为保留的状态向量，Ha为δχd的协方差矩阵，Hc为δχs的协方差矩阵，表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵，bd和bs分别为δχd和的δχs常数向量；
根据舒尔补理论对状态变量χ的增量方程进行高斯消元并推导获得：
进而根据上式获得保留的状态向量δχs，实现滑动窗口约束关系的重构。
进一步的，上述步骤五中，若最新帧图像上的投影点与特征点的描述子匹配，则所述投影点与所述特征点匹配。
进一步的，上述步骤九中，所述视觉约束因子的待优化状态量χc为：
其中，和/>分别为相机在第i帧和第j帧的位置，/>和/>分别为相机在第i帧和第j帧的姿态，所述第i帧和第j帧为相邻的两个关键帧，/>和/>分别为相机相对IMU的位置和旋转量，λ为地图点的逆深度。
进一步的，上述步骤九中，所述IMU预积分约束因子的待优化变量χimu为：
其中，和/>分别为ti时刻和tj时刻IMU的速度，/>和/>分别为ti时刻和tj时刻IMU的加速度计偏置量，/>和/>分别为ti时刻和tj时刻IMU的陀螺仪偏置量，所述ti时刻和tj时刻分别为相机拍摄第i帧图像和第j帧图像的时刻。
进一步的，上述步骤九中，所述目标函数表达式如下：
其中，χ为滑动窗口内所有图像帧的状态变量且有k∈[0,N]，λm为第m个地图点的逆深度，m为关键帧图像观测到地图点的总数，N为滑动窗口内所有帧图像的总数，第k帧的状态向量和/>分别为相机在第k帧的位置、速度和姿态，ba和bω分别为IMU的加速度计偏置量和陀螺仪偏置量，/>和/>分别为相机相对IMU的位置和旋转量，eC(xi,xj)、eB(xi,xj)和eM分别表示视觉约束残差、IMU预积分约束残差和边缘化先验约束残差，xi和xj分别为第i帧图像和第j帧图像的状态向量。
进一步的，上述视觉约束残差eC(xi,xj)表达式如下：
其中，为空间内地图点P在第j帧图像的归一化平面上的投影坐标，/>为空间内地图点P第j帧图像中第一次被观测到时的观测坐标。
进一步的，上述IMU预积分约束残差eB(xi,xj)表达式为：
其中，rp、rq、rv、分别为ti时刻至tj时刻机器人的位置残差、姿态残差、速度残差、加速度计残差和陀螺仪残差，所述ti时刻和tj时刻分别为相机拍摄第i帧图像和第j帧图像的时刻。
进一步的，上述边缘化先验约束残差eM表达式为：
eM＝bp-Hpδχ1:N，
其中，δχ1:N为滑动窗口内所有关键帧的状态增量，
Ha为待边缘化的状态向量δχd的协方差矩阵，Hc为保留的状态向量δχs的协方差矩阵，表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵，bd和bs分别为δχd和的δχs常数向量。
发明的有益效果如下：
本发明通过对SLAM后端的滑动窗口优化方法进行改进，提出了一种基于共视投影匹配的视觉惯性SLAM***滑窗优化方法。
该方法通过对滑窗内共视地图点的投影匹配，解决了同一地图点在SLAM不同运行阶段或不同线程中可能被多次创建而导致重影的问题，将这些“影子”地图点融合为一个地图点，提高了地图点的观测精度，从而提高SLAM***估计移动机器人的位姿的精度。
进一步的，本发明通过对共视匹配的投影点的筛选，增加了最新关键帧成功匹配地图点的数量，扩大了关键帧的观测视野，新增了SLAM***的视觉观测约束，从而提高了滑动窗口优化估计位姿的精度，进而提高了SLAM***定位移动机器人位置的精度。
进一步的，本发明不改变滑动窗口优化区域的大小，滑动窗口可容纳待优化关键帧的数量固定不变，能够保证优化运算的快速性，从而能够保证视觉惯性SLAM运行的快速性，同时兼顾提高了SLAM对移动机器人定位和位姿估计的精度。
附图说明
图1为视觉惯性SLAM***的流程框图；
图2为具体实施方式所述基于共视投影匹配的视觉惯性SLAM***滑窗优化方法的流程图；
图3为当次新帧图像为关键帧的情况下，滑动窗口更新过程示意图；
图4为当次新帧图像不为关键帧的情况下，滑动窗口更新过程示意图；
图5为当前关键帧与其共视关键帧的共视关系示意图；
图6为滑动窗口内最新关键帧与其共视关键帧的投影匹配关系示意图；
图7为视觉惯性融合的滑动窗口因子图优化模型示意图；
图8为our-VINS-Mono在Machine Hall 01数据集上估计出的无人机飞行轨迹图；
图9为图8中的飞行轨迹在x、y、z三个坐标轴上的位移量变化与时间的关系曲线；
图10为采用绝对位姿误差指标(APE)评估图8的飞行轨迹的APE曲线与时间的变化关系图，并给出了全局轨迹的均方根误差(rmse)、误差中位数(median)、误差均值(mean)和误差标准差(std)；
图11为VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹的APE误差箱线图的对比图；
图12为VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹，在不同误差评价指标下的APE误差量的对比图；
图13为VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹，轨迹误差的密度分布与APE误差关系的对比图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其它实施例，都属于本发明保护的范围。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
参照图1至图13具体说明本实施方式，本实施方式所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，包括以下步骤：
步骤1：设置滑动窗口大小，使其能够容纳待优化的关键帧数量为N。
步骤2：利用视觉惯性SLAM***的后端模块订阅前端模块发布的移动机器人的位姿状态信息，当后端模块采集到一个新的图像帧后，判断滑动窗口是否已满，若已满，则需要更新滑动窗口，执行步骤3；若未满，则执行步骤4。
步骤3：判断最新帧图像与滑动窗口内当前最新关键帧图像的视差是否达到阈值。
若达到阈值，则次新帧图像是关键帧，此时需要将滑窗内的最老帧边缘化，即：将滑动窗口内最老帧图像及其对应的IMU约束关系删除，并保留最老帧图像的视觉约束关系，也就是将最老帧观测到的地图点约束关系寄存于滑窗内的与最老帧有共视关系的关键帧上。根据舒尔补理论重构滑动窗口的约束关系，然后执行步骤5。
若未达到阈值，则次新帧图像不是关键帧，此时则将次新帧图像边缘化，即：将滑动窗口内次新帧图像及其视觉约束关系删除，并保留次新帧图像对应的IMU约束关系。根据舒尔补理论重构滑动窗口的约束关系，然后执行步骤5。
步骤3的内容实现了对滑动窗口内关键帧的更新功能，根据次新帧是否为关键帧，决定对滑动窗口进行滑入更新或滑出更新，若次新帧为关键帧，则对滑窗内的最老帧进行边缘化，即滑出更新。如图3给出了在次新帧是关键帧的情况下滑动窗口更新示意图。若次新帧不为关键帧，则对次新帧进行边缘化，即滑入更新。如图4给出了次新帧不是关键帧的情况下的滑动窗口更新示意图。
对滑动窗口进行更新的目的是在保留关键帧约束关系的前提下，对滑动窗口区域内的关键帧进行新增和删除。选择滑入更新还是滑出更新，其本质是使滑动窗口内的关键帧具有最大的视野范围，避免这些关键帧的视野狭小且重叠。将关键帧从滑窗内删除，但仍保留该关键帧约束关系的过程称为将该关键帧从滑窗内边缘化，通常采用舒尔补方法实现。
滑动窗口内所有图像帧的状态变量χ在滑动窗口内构建最小二乘表达式，并简化为Hδχ＝b的形式，H为***协方差矩阵，δχ为状态量的增量，b为一个常数向量。将状态变量χ分为两类，分别为待边缘化的状态量δχd和需要保留的状态量δχs。将***协方差矩阵H拆分为四个分块矩阵Ha、Hc、Hb，Ha为δχd的协方差矩阵，即被边缘化的变量；Hc为δχs的协方差矩阵，即被保留的状态变量的协方差矩阵；/>表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵。
将状态变量χ以增量方程的方式表示如下：
其中，bd和bs分别为δχd和的δχs常数向量。
根据舒尔补理论，在不计算待边缘化状态量δχd的情况下，求解需要保留的状态量δχs，进行高斯消元，可得：
其中，表示Ha在Hb中的舒尔补，根据上式可以推导出δχs为：
上式中的状态量只有δχs，使得被边缘化的关键帧的约束信息得以保留，满足在不损失精度前提下将边缘化关键帧的状态量剔除。在边缘化的过程中，虽然状态变量会不断更新，但采用优化一致性的策略固定线性化点，即计算边缘化雅可比矩阵时，求导变量的值是固定的，而不是用每次迭代更新后的状态变量求雅可比，以保证***不可观的维度，避免协方差矩阵的零空间发生变化。
步骤4：判断最新帧图像是否为关键帧。
若是关键帧，称其为最新关键帧。将滑动窗口内的地图点投影到所述最新帧图像上，所述地图点为滑动窗口内能够与所述最新帧图像共视的关键帧所观测到的地图点，然后执行步骤5。
若不是关键帧，仅更新最新帧图像对应的IMU约束关系，然后返回步骤2。
步骤5：分别判断最新帧图像上的投影点是否能够与最新帧图像上的特征点(在图像平面中的二维特征点，用ORB或SIFT方法提取)匹配成功，即最新帧图像上的投影点与特征点的描述子匹配，若某个投影点在最新关键帧的特征点中找不到匹配点，那么抛弃该投影点；若某个投影点在最新关键帧上的特征点中能够找到匹配点，则判断有相匹配投影点的特征点是否存在原地图点，所述原地图点为特征点在进行匹配之前滑动窗口内就存在能够与之对应的地图点，是则执行步骤6，否则执行步骤7。
步骤6：比较投影点对应的地图点与特征点对应的原地图点的被观测次数，并保留所述被观测次数多的地图点及更新滑窗内各关键帧的视觉观测约束，删除被观测次数较少的那个地图点，即地图点替换。然后执行步骤8。
步骤7：增加新的地图点，使得所述最新帧图像与新增地图点之间建立观测约束，即地图点新增。然后执行步骤8。
对于步骤5、6、7的内容，是将与最新关键帧共视的全部关键帧能够观测到的所有地图点投影到最新关键帧上，通过投影点与特征点的关系以及特征点是否原有对应的地图点，决定对地图点的进一步处理措施。如图5所示，给出了当前关键帧与其共视关键帧的共视关系示意图。同一个地图点可能在移动机器人运动的不同阶段被观测到，也可能在SLAM***的不同线程中被创建，从而导致“影子”地图点现象。对地图点进行投影匹配，通过地图点替换处理，能够对“影子”地图点进行筛选，从而提高了地图点的精度；通过地图点新增处理，能够增加关键帧观测地图点的视野，增加关键帧成功匹配地图点的数量，从而增加了视觉观测约束关系。
根据相机视觉成像透视关系对地图点进行投影匹配，图6给出了滑动窗口内最新关键帧与其共视关键帧的投影匹配关系示意图。在投影计算中，关键帧的位姿由SLAM的跟踪模块估计得到，即使位姿准确度不高也没有关系，可以在投影点的圆形邻域内寻找候选匹配特征点，后续通过SLAM后端持续优化和修正位姿。下式给出了从三维地图点到二维图像的针孔相机模型透视投影坐标变换表达式：
上式中，[u v 1]T是成像平面上像素坐标系下的投影点齐次坐标，u和v分别为像素坐标系下投影点的横纵坐标。fx和fy分别为相机在横向方向和纵向方向的焦距，cx和cy分别为图像的中心像素坐标至图像原点像素坐标之间相差的横向和纵向像素数。Z是从相机坐标系到归一化坐标系变换时的光轴方向的深度值，R为移动机器人的旋转矩阵，t为移动机器人的平移向量，[xw yw zw 1]T是地图点在世界坐标系下的齐次坐标，xw、yw、zw分别为世界坐标系下三轴的坐标值，K是相机内参矩阵，T表示相机位姿的变换矩阵。
步骤8：判断滑窗内关键帧数量是否已满，若未满，则返回步骤2；若已满，则执行步骤9。
步骤9：根据视觉重投影透视关系，构建移动机器人的位姿与地图点之间的视觉约束因子。根据IMU(惯性测量单元)预积分理论，构建IMU预积分约束因子。根据更新滑动窗口的边缘化处理结果，构建先验约束因子。将所述视觉约束因子、IMU预积分约束因子以及边缘化先验约束因子加入到滑窗因子图优化模型中构建目标函数。图7给出了由三种约束因子构建的滑动窗口因子图优化模型的示意图。
根据滑动窗口因子图优化模型，建立紧耦合光束平差优化目标函数，通过最小化视觉约束残差、IMU预积分约束残差、边缘化先验约束残差之和，获得状态的最大后验概率估计，形式如下所示：
滑动窗口内所有帧图像的状态变量k∈[0,N]，N为滑动窗口内所有图像帧总数，λm为第m个地图点的逆深度，m为关键帧图像观测到地图点的总数，所述目标函数表达式如下：
其中，第k帧的状态向量和/>分别为相机在第k帧的位置、速度和姿态，ba和bω分别为IMU的加速度计偏置量和陀螺仪偏置量， 和/>分别为相机相对IMU的位置和旋转量，eC(xi,xj)、eB(xi,xj)和eM分别表示视觉约束残差、IMU预积分约束残差和边缘化先验约束残差，xi和xj分别为第i帧图像和第j帧图像的状态向量。
视觉约束因子基于重投影误差构建，空间中的地图点投影到每个能观测到该地图点的关键帧上，其投影点的坐标与该地图点的观测坐标应该是重合的，两类坐标的重投影误差即为视觉约束因子的残差。视觉惯性SLAM***中，视觉因子的待优化状态量χc表示为：
其中，和/>分别为相机在第i帧和第j帧的位置，/>和/>分别为相机在第i帧和第j帧的姿态，所述第i帧和第j帧为相邻的两个关键帧，/>和/>分别为相机相对IMU的位置和旋转量，λ为地图点的逆深度。
假设空间中有一地图点为P，它的索引编号为l，其在第j帧的归一化平面上的投影坐标为在第j帧的观测坐标为/>则有视觉约束残差eC(xi,xj)表达式如下：
其中，可由重投影的估计模型得到，表达式为：
上式中，表示地图点P被第i帧第一次观测到时，在第i帧的归一化平面上的观测值，/>为IMU坐标系至相机坐标系的旋转量，/>为世界坐标系至第j帧IMU坐标系的旋转量，/>为第i帧IMU坐标系至世界坐标系的旋转量，/>为相机坐标系至IMU坐标系的旋转量。
IMU预积分约束因子用于描述两个关键帧时间差之间的IMU测量信息的相对位姿变化关系。将IMU采样的i时刻和j时刻与图像的第k帧和第k+1帧对齐，则IMU预积分因子的待优化变量表示为：
其中，和/>分别为ti时刻和tj时刻IMU的速度，/>和/>分别为ti时刻和tj时刻IMU的加速度计偏置量，/>和/>分别为ti时刻和tj时刻IMU的陀螺仪偏置量，所述ti时刻和tj时刻分别为相机拍摄第i帧和第j帧的时刻。
所述tj时刻IMU的位置速度/>和姿态/>通过下式获得：
其中，Δt为第i帧和第j帧之间的时间间隔，ti≤t≤tj，为IMU坐标系至世界坐标系的旋转矩阵，gw为世界坐标系下的重力向量，δt为相邻两个IMU测量数据之间的时间间隔，/>为IMU坐标系下t时刻以第i帧时刻所在位置为参考，载体相对于ti时刻的旋转量。
和/>分别为t时刻IMU的加速度计偏置量和陀螺仪偏置量。
和/>分别表示t时刻IMU输出的加速度和角速度，/>和/>分别表示t时刻IMU测量信息的加速度噪声和角速度噪声，并假设它们均为服从高斯分布的白噪声。对上式进行变换，将位置量、速度量和旋转量的参考坐标系，由世界坐标系变换到第i帧的体坐标系下，可得：
上式中即为位置、速度、姿态的IMU预积分变量，表达式如下：
表示t时刻相对于ti时刻的机器人姿态的IMU预积分变量。
可见，第j帧时刻的状态量只与IMU的测量值有关，不会受到第i帧时刻状态量的影响，从而保证了测量值与状态量无关，避免了重复积分。则有IMU预积分约束残差eB(xi,xj)表达式如下：
其中，rp、rq、rv、分别为ti时刻至tj时刻机器人的位置残差、姿态残差、速度残差、加速度计残差和陀螺仪残差，所述ti时刻和tj时刻分别为相机拍摄第i帧图像和第j帧图像的时刻，/>是四元数乘法符号，[·]xyz表示取四元数的虚部组成三维向量，/>为从世界坐标系到ti时刻IMU坐标系的旋转量(四元数表示法)，/>为从ti时刻IMU坐标系到tj时刻IMU坐标系的旋转量(四元数表示法)。
先验约束因子是在更新滑动窗口时，剔除边缘化关键帧后，所保留下来的约束关系。以因子图优化模型构建最小二乘表达式，状态向量χ设为节点，将被边缘化的状态向量χd中的位姿信息与地图点的约束关系，以先验信息的形式加入到被保留的状态向量χs的优化关系中，避免了因更新滑窗后被边缘化的关键帧所对应的约束信息的丢失。在边缘化处理后，得到的先验矩阵Hp和对应的先验残差量bp，这与步骤3中的边缘化表达式是一致的，所述边缘化先验约束残差eM表达式为：
eM＝bp-Hpδχ1:N，
其中，δχ1:N为滑动窗口内所有关键帧的状态增量，
Ha为待边缘化的状态向量δχd的协方差矩阵，Hc为保留的状态向量δχs的协方差矩阵，表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵，bd和bs分别为δχd和的δχs常数向量。
步骤10：利用非线性优化算法对所述目标函数进行优化，例如高斯-牛顿法，Levenberg-Marquadt法等。以Levenberg-Marquadt法为例，通过为状态增量设置置信区域的方法，避免了因状态增量变化较大而导致的近似失效，避免了线性方程组的系数矩阵非奇异和病态问题，因此能够提供更稳定更准确的状态增量。将目标函数构建成为状态增量方程的形式：
(Hh+μI)Δχ＝g，
上式中，Hh为Hessian矩阵(黑塞矩阵)，μ为Lagrange乘子(拉格朗日乘子)，通过调整参数μ的大小来反映非线性二次近似模型是否准确，然后通过迭代的方式优化得到最优解。
更新滑窗内所有关键帧的位姿，完成一次滑动窗口优化，判断SLAM后端的优化线程是否被中止，若未被中止，返回步骤2；若被中止，则结束。
本实施方式在不改变滑动窗口优化快速性的前提下，通过共视投影匹配关系改进滑动窗口优化方法，消除了“影子”地图点，提高了地图点精度；增加了关键帧成功匹配地图点的数量，扩大了关键帧的观测视野，新增了SLAM***的视觉观测约束，从而提高了滑动窗口优化估计位姿的精度，进而提高了移动机器人的定位精度。
本发明的实现与验证：
本发明提出的一种基于共视投影匹配改进滑窗优化的视觉惯性SLAM方法使用C++语言编程实现，通过评估SLAM***估计出的移动机器人运动轨迹的准确度，来验证本发明的有益效果。以视觉惯性SLAM***——VINS-Mono框架为基础，将使用本发明提出的改进型滑动窗口优化方法的VINS-Mono命名为our-VINS-Mono。使用苏黎世联邦理工学院公开的EuRoc飞行数据集作为验证数据集，该数据集以微型旋翼飞行器为运载体，搭载了可见光相机传感器和惯性测量单元，用于感知周围环境信息和测量飞行器自身运动姿态。
以EuRoc飞行数据集中的Machine Hall 01数据子集为例，将VINS-Mono和our-VINS-Mono分别估计Machine Hall 01数据集中旋翼飞行器的飞行轨迹，然后采用不同的评价指标对两条轨迹的精度进行评估。图8给出了our-VINS-Mono在Machine Hall 01数据集上估计出的无人机飞行轨迹图，图9给出了图8中的飞行轨迹在x、y、z三个坐标轴上的位移量变化与时间的关系曲线；图10是采用绝对位姿误差指标(APE)评估图8的飞行轨迹的APE误差曲线与时间的变化关系图，并给出了全局轨迹的均方根误差(rmse)、误差中位数(median)、误差均值(mean)和误差标准差(std)。图11是VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹的APE误差箱线图的对比图，从图中可以看出our-VINS-Mono的APE误差中位数明显比VINS-Mono要低，误差分区整体下移。图12是VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹，在不同误差评价指标下的APE误差量的对比图，从图中可以看出our-VINS-Mono的max(最大误差值)、std、median、mean、rmse指标下的APE误差量均小于VINS-Mono，而min(最小误差)指标下的APE误差量偏大，说明误差量整体趋于中位数附近，异常值对误差整体影响较小。图13是VINS-Mono和our-VINS-Mono在Machine Hall 01数据集上估计出的飞行轨迹，轨迹误差的密度分布与APE误差关系的对比图，从图中可以看出our-VINS-Mono的误差量的分布比较集中，这与箱线图中误差集中于中位数相一致。
综上所述，本发明提出了一种基于共视投影匹配改进滑窗优化的视觉惯性SLAM方法，旨在通过对滑动窗口内关键帧的共视地图点进行投影匹配，优化地图点的位置精度，消除重影地图点，增加关键帧与地图点之间的观测约束，提高视觉惯性SLAM***的位姿估计精度，从而提高移动机器人的定位精度。在滑动窗口内，将与最新关键帧有共视关系的全部关键帧所能观测到的全部的地图点投影到最新关键帧上，判断每个投影点与特征点的匹配关系，通过地图点替换，消除“影子”地图点，避免了冗余存储；通过地图点新增，增加了关键帧与地图点成功匹配的数量，扩大了关键帧的观测视野。此外，本发明不改变滑动窗口的大小，滑窗内仍然是固定数量的关键帧，能够确保运算时间的快速性。
虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其它所述实施例中。

Claims (10)

1.基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，包括以下步骤：
步骤一：当视觉惯性SLAM***的后端模块订阅到其前端模块发布的机器人最新帧图像时，判断滑动窗口是否已满，是则执行步骤二，否则执行步骤四；
步骤二：判断次新帧图像是否为关键帧，是则将滑动窗口内最老帧图像边缘化然后执行步骤三，否则将次新帧图像边缘化然后执行步骤三；
步骤三：根据舒尔补理论重构滑动窗口的约束关系，然后执行步骤四；
步骤四：根据最新帧图像与滑动窗口内当前最新关键帧图像的视差判断所述最新帧图像是否为关键帧，是则将滑动窗口内的地图点投影到所述最新帧图像上，所述地图点为滑动窗口内能够与所述最新帧图像共视的关键帧所观测到的地图点，然后执行步骤五，否则更新最新帧图像对应的IMU约束关系，然后返回步骤一；
步骤五：将最新帧图像上的投影点与特征点进行匹配，将未匹配到特征点的投影点删除，同时判断有相匹配投影点的特征点是否存在原地图点，所述原地图点为特征点在进行匹配之前滑动窗口内就存在能够与之对应的地图点，是则执行步骤六，否则执行步骤七；
步骤六：比较投影点对应的地图点与特征点对应的原地图点的被观测次数，仅保留被观测次数多的地图点，使得滑窗内各关键帧的视觉观测约束得到更新，然后执行步骤八；
步骤七：增加新的地图点，使得所述最新帧图像与新增地图点之间建立观测约束，然后执行步骤八；
步骤八：判断滑动窗口内关键帧数量是否已满，是则执行步骤九，否则返回步骤一；
步骤九：分别构建视觉约束因子、IMU预积分约束因子以及边缘化先验约束因子，将所述视觉约束因子、IMU预积分约束因子以及边缘化先验约束因子加入到滑窗因子图优化模型中构建目标函数；
步骤十：利用非线性优化算法对所述目标函数进行优化，更新滑动窗口内机器人的状态量，完成滑窗优化。
2.根据权利要求1所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤二中所述将滑动窗口内最老帧图像边缘化，包括：
将滑动窗口内最老帧图像及其对应的IMU约束关系删除，并保留最老帧图像的视觉约束关系；
步骤二中所述将次新帧图像边缘化，包括：
将次新帧图像及其视觉约束关系删除，并保留次新帧图像对应的IMU约束关系。
3.根据权利要求1所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤三所述根据舒尔补理论重构滑动窗口的约束关系，包括：
将滑动窗口内所有图像帧的状态变量χ以增量方程的方式表示如下：
其中，δχd为待边缘化的状态向量，δχs为保留的状态向量，Ha为δχd的协方差矩阵，Hc为δχs的协方差矩阵，表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵，bd和bs分别为δχd和的δχs常数向量；
根据舒尔补理论对状态变量χ的增量方程进行高斯消元并推导获得：
进而根据上式获得保留的状态向量δχs，实现滑动窗口约束关系的重构。
4.根据权利要求1所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤五中，若最新帧图像上的投影点与特征点的描述子匹配，则所述投影点与所述特征点匹配。
5.根据权利要求1所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤九中，所述视觉约束因子的待优化状态量χc为：
其中，和/>分别为相机在第i帧和第j帧的位置，/>和/>分别为相机在第i帧和第j帧的姿态，所述第i帧和第j帧为相邻的两个关键帧，/>和/>分别为相机相对IMU的位置和旋转量，λ为地图点的逆深度。
6.根据权利要求5所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤九中，所述IMU预积分约束因子的待优化变量χimu为：
其中，和/>分别为ti时刻和tj时刻IMU的速度，/>和/>分别为ti时刻和tj时刻IMU的加速度计偏置量，/>和/>分别为ti时刻和tj时刻IMU的陀螺仪偏置量，所述ti时刻和tj时刻分别为相机拍摄第i帧图像和第j帧图像的时刻。
7.根据权利要求1所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，步骤九中，所述目标函数表达式如下：
其中，χ为滑动窗口内所有图像帧的状态变量且有k∈[0,N]，λm为第m个地图点的逆深度，m为关键帧图像观测到地图点的总数，N为滑动窗口内所有帧图像的总数，第k帧的状态向量/> 和/>分别为相机在第k帧的位置、速度和姿态，ba和bω分别为IMU的加速度计偏置量和陀螺仪偏置量， 和/>分别为相机相对IMU的位置和旋转量，eC(xi,xj)、eB(xi,xj)和eM分别表示视觉约束残差、IMU预积分约束残差和边缘化先验约束残差，xi和xj分别为第i帧图像和第j帧图像的状态向量。
8.根据权利要求7所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，所述视觉约束残差eC(xi,xj)表达式如下：
其中，为空间内地图点P在第j帧图像的归一化平面上的投影坐标，/>为空间内地图点P第j帧图像中第一次被观测到时的观测坐标。
9.根据权利要求7所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，所述IMU预积分约束残差eB(xi,xj)表达式为：
其中，rp、rq、rv、分别为ti时刻至tj时刻机器人的位置残差、姿态残差、速度残差、加速度计残差和陀螺仪残差，所述ti时刻和tj时刻分别为相机拍摄第i帧图像和第j帧图像的时刻。
10.根据权利要求7所述的基于共视投影匹配的视觉惯性SLAM***滑窗优化方法，其特征在于，所述边缘化先验约束残差eM表达式为：
eM＝bp-Hpδχ1:N，
其中，δχ1:N为滑动窗口内所有关键帧的状态增量，
Ha为待边缘化的状态向量δχd的协方差矩阵，Hc为保留的状态向量δχs的协方差矩阵，表示Hb的转置，Hb为δχd与δχs之间的协方差矩阵，bd和bs分别为δχd和的δχs常数向量。
CN202311384144.7A 2023-10-24 2023-10-24 基于共视投影匹配的视觉惯性slam***滑窗优化方法 Pending CN117421384A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311384144.7A CN117421384A (zh) 2023-10-24 2023-10-24 基于共视投影匹配的视觉惯性slam***滑窗优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311384144.7A CN117421384A (zh) 2023-10-24 2023-10-24 基于共视投影匹配的视觉惯性slam***滑窗优化方法

Publications (1)

Publication Number Publication Date
CN117421384A true CN117421384A (zh) 2024-01-19

Family

ID=89527915

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311384144.7A Pending CN117421384A (zh) 2023-10-24 2023-10-24 基于共视投影匹配的视觉惯性slam***滑窗优化方法

Country Status (1)

Country Link
CN (1) CN117421384A (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114529576A (zh) * 2022-01-04 2022-05-24 重庆邮电大学 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN115371665A (zh) * 2022-09-13 2022-11-22 哈尔滨工业大学 一种基于深度相机和惯性融合的移动机器人定位方法
CN115479602A (zh) * 2022-10-14 2022-12-16 北京航空航天大学 一种融合事件与距离的视觉惯性里程计方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114529576A (zh) * 2022-01-04 2022-05-24 重庆邮电大学 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN115371665A (zh) * 2022-09-13 2022-11-22 哈尔滨工业大学 一种基于深度相机和惯性融合的移动机器人定位方法
CN115479602A (zh) * 2022-10-14 2022-12-16 北京航空航天大学 一种融合事件与距离的视觉惯性里程计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
翟向伟: "基于视觉的空间目标自主位姿测量研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 03, 15 March 2021 (2021-03-15), pages 138 - 672 *
郭俊阳: "基于视觉惯性融合的移动机器人SLAM优化方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 02, 15 February 2023 (2023-02-15), pages 138 - 1800 *

Similar Documents

Publication Publication Date Title
CN111258313B (zh) 多传感器融合slam***及机器人
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN112902953B (zh) 一种基于slam技术的自主位姿测量方法
CN111561923B (zh) 基于多传感器融合的slam制图方法、***
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN107341814B (zh) 基于稀疏直接法的四旋翼无人机单目视觉测程方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN106595659A (zh) 城市复杂环境下多无人机视觉slam的地图融合方法
WO2019057179A1 (zh) 一种基于点线特征的视觉slam方法和装置
CN111275763A (zh) 闭环检测***、多传感器融合slam***及机器人
CN108519102B (zh) 一种基于二次投影的双目视觉里程计算方法
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN116222543B (zh) 用于机器人环境感知的多传感器融合地图构建方法及***
CN112556719A (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
Ceriani et al. Pose interpolation slam for large maps using moving 3d sensors
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
WO2024114119A1 (zh) 一种基于双目相机引导的传感器融合方法
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及***
CN114485637A (zh) 头戴式增强现实***的视觉和惯性混合位姿跟踪方法
CN114723811A (zh) 一种非结构化环境四足机器人立体视觉定位建图方法
CN116147618B (zh) 一种适用动态环境的实时状态感知方法及***
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN117115271A (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