CN116242374A - 一种基于直接法的多传感器融合的slam定位方法 - Google Patents

一种基于直接法的多传感器融合的slam定位方法 Download PDF

Info

Publication number
CN116242374A
CN116242374A CN202310470533.5A CN202310470533A CN116242374A CN 116242374 A CN116242374 A CN 116242374A CN 202310470533 A CN202310470533 A CN 202310470533A CN 116242374 A CN116242374 A CN 116242374A
Authority
CN
China
Prior art keywords
frame
imu
optimization
algorithm
factor graph
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
CN202310470533.5A
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.)
Xiamen University
Original Assignee
Xiamen University
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 Xiamen University filed Critical Xiamen University
Priority to CN202310470533.5A priority Critical patent/CN116242374A/zh
Publication of CN116242374A publication Critical patent/CN116242374A/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/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
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

一种基于直接法的多传感器融合的SLAM定位方法，涉及移动机器人定位导航领域。利用图像中梯度明显的像素点信息的直接法对相机跟踪，用IMU预积分信息作为初始位姿，给直接法的跟踪提供初始值。根据跟踪是否成功，将图像帧分为关键帧和非关键帧。构建视觉因子图，在此视觉因子图上***IMU残差项和GPS残差项构建新的融合因子图，进行融合因子图优化完成***算法的优化流程。使用基于滑窗优化的后端优化策略，固定后端姿态优化的计算复杂度，使用多传感器紧耦合融合的策略，利用古早信息弥补边缘化的缺陷并保留边缘化的优势，提高***精度。可在纯视觉的模式下工作。即使GPS信号无法正常接收，也可在视觉惯性里程计模式下继续工作。

Description

一种基于直接法的多传感器融合的SLAM定位方法
技术领域
本发明涉及移动机器人定位导航领域，具体是涉及一种基于直接法的多传感器融合的SLAM定位方法。
背景技术
随着机器人应用的逐步深入，机器人自主导航能力已成为限制机器人工作能力提高的瓶颈之一，其中定位是实现机器人自主导航的基础和依据，是制约机器人广泛应用的核心技术之一。因此，同时定位与建图(SLAM)开始成为目前的研究热点之一。SLAM指的是在未知环境中能够对运动的机器人不断跟踪其运动的轨迹，同时进行建图。完成SLAM离不开各种传感器的支持，由于单个传感器定位难以保证机器人在各种环境中长期稳定工作，融合多个传感器数据在未知环境中的定位方法得到了越来越多的研究和应用。
目前的主流SLAM算法使用的传感器包括单目/双目/深度相机，IMU惯性测量单元，激光雷达，还会使用GPS信号和轮程计融合使用。另外，对于视觉SLAM又有基于稀疏分布的特征点并计算其描述子的特征点法，基于梯度信息明显的像素信息的直接法，以及基于特征点但不计算描述子的半直接法等算法。IMU和单目相机各有优缺点，但两者也是互补的关系，将其融合后可以有以下优点：解决了单目相机尺度不确定性问题；解决单目纯旋转无法估计的问题，减小视觉SLAM受纹理、光照及动态环境的影响；IMU自身的零偏估计简单，清除积分漂移；因此，进行多传感器融合，可以构造出速度更快，计算更准确，***更鲁棒的算法，这具有非常大的研究意义和实用价值。另外多传感器融合往往又分为基于滤波和基于因子图优化两种方式，紧耦合和松耦合的融合方案，这也让人们可以针对自己的设备选择合理的融合方案升级设备的性能。
发明内容
本发明的目的在于提供一种基于直接法的多传感器融合的SLAM定位方法，改进现有的单目视觉IMU融合算法中的鲁棒性和运算速度，同时添加GPS进行全局融合定位，添加优化约束，使其坐标可以对齐现实世界，也可以提高***的准确性。
本发明包括以下步骤：
1)获取单目相机的图像帧、IMU输出信息和GPS经纬高信息，将GPS经纬高信息解算得到当前的世界坐标系坐标，根据时间戳将前述信息对齐到一个结构体data中，输入***中运算；
2)进行视觉部分算法的初始化流程，利用步骤1)得到的结构体data对第一帧图像进行构建图像金字塔，通过梯度阈值筛选候选梯度点，将第一帧初始化的候选梯度点投影到第二帧中，采用图像金字塔对初始相对运动进行估计，初始化逆深度和IMU坐标系到世界坐标系的位姿变换；根据光度误差函数公式，对两帧进行优化，通过优化结果的位移大小判断是否完成初始化；
3)进行算法的图像跟踪流程，利用IMU预积分得到的两帧之间位姿作为初始位姿，基于高斯牛顿法对前后两帧图像进行基于光度误差的优化计算，跟踪相机的运动；在跟踪的过程中，根据设定的三个条件决定是否将当前帧作为新的关键帧，包括：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔；
a、若决定将当前帧作为关键帧，则对当前帧提取到的梯度点进行激活操作，即使用深度滤波器对梯度点的逆深度优化计算使其收敛，同时将关键帧添加到视觉因子图中进行滑窗优化，边缘化采用舒尔补进行；
b、若决定将当前帧作为非关键帧，则仅使用当前帧和前一帧之间的位姿估计来更新逆深度，***以“仅视觉”的模式运行；
4)进行算法的视觉-IMU-GPS数据融合流程，基于步骤3)所述视觉因子图，拷贝该因子图作为融合因子图，加入IMU预积分因子和GPS因子使用滞后边缘化进行联合优化；融合因子图同样使用滑窗优化的方法，但是边缘化的策略为推迟n个关键帧，再同视觉因子图的边缘化顺序进行滞后边缘化以达到保持滑窗大小的目的；滞后边缘化在保证精度的同时还弥补边缘化的缺陷并保留边缘化的优势，***进入“视觉-IMU-GPS”模型运行。
步骤2)具体包括以下步骤：
2.1)初始化时，对初始两帧的第一帧图像建立图像金字塔，遍历金字塔并计算各层图像的相机内参，对第1层图像分为多个网格，网格尺寸为32*32，然后计算像素块网格中的像素点x、y方向上的光强差dx、dy，得到光强差的平方和作为梯度
Figure BDA0004203822060000021
并统计各像素块梯度，选取中位数作为像素块提取候选点的梯度阈值；
2.2)在第1层图像中，遍历每个n*n大小的像素块，像素块内遍历像素点随机选取像素坐标系中的一个方向来计算块内像素的梯度dI，记录梯度最大且大于阈值的像素位置p，作为候选点；若提取到的像素点太少，则减小块的尺寸0.5n*0.5n进行递归重采样；反之增大块的尺寸为2n*2n再重采样；产生递归则以此类推。在金字塔剩余层中，同样进行选点，需要满足一下两个条件：①梯度必须大于设定阈值；②像素坐标系中像素点应是x、y、x+y和x-y四个方向中最大的点，最后对图像金字塔各层进行关联；
2.3)计算当前相机坐标系和重力对齐的变换四元数；以步骤1)中获取的和图像时间对齐的IMU信息为基础，获取一段时间的加速度数据并取平均值作为当前帧的重力方向；同时构建一个固定大小的滑窗保存各帧的重力方向，使用滑动均值滤波优化IMU重力矢量，与现实重力方向[0,0,9.8]计算得到作为当前帧重力对齐的旋转
Figure BDA0004203822060000031
2.4)对初始两帧的第二帧图像金字塔优化以完成初始化；第二帧基于第一帧的点，构建光度误差函数：
Figure BDA0004203822060000032
式中，Np是算法中使用的像素集，‖·‖是Huber范数，p表示帧i中像素点对应的空间点p在帧j上的投影，tj表示第j帧曝光时间；为使算法可应用于曝光时间未知的图像序列，使用仿射光度公式
Figure BDA0004203822060000033
将光度归一化操作；然后从图像金字塔顶层向底层开始，将单位矩阵作为初始值通过高斯牛顿法对图像金字塔各层优化，并判断位移是否足够大；若位移足够大且此步骤跟踪帧数大于5则完成初始化，将随后将第一帧设置为关键帧，将第一帧中提取的梯度点激活，将其加入视觉因子图的滑窗优化中；否则重复步骤2.1)到2.4)。
步骤3)具体包括以下步骤：
3.1)进入算法的跟踪流程：对步骤1)获取的与图像时间对齐的IMU数据进行预积分，得到两帧之间的位姿变换，预积分公式为：
Figure BDA0004203822060000034
Figure BDA0004203822060000035
Figure BDA0004203822060000036
式中，
Figure BDA0004203822060000037
和/>
Figure BDA0004203822060000038
为bk+1相对于bk的位姿变化；算法以IMU预积分得到的位姿作为直接法的初始位姿；
3.2)使用初始位姿将参考帧的点投影到当前帧中，构建光度误差函数；以光度误差最小为优化目标，使用列文伯格-马夸尔特算法从图像金字塔顶层向下对候选点开始迭代优化，即由粗到精迭代优化逆深度、光度参数和位姿；有几种不同的优化处理操作：①若光度误差大于能量阈值的60％则放大阈值重新优化；②若金字塔中某层能量大于2倍最小值，直接判断跟踪失败；③若光度误差满足条件，则跟踪成功。
3.3)根据预设的三个条件判断当前帧是否可以成为关键帧：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔；
3.4)若当前帧成为非关键帧，更新步骤2.2)的候选点的逆深度范围；根据逆深度范围得到极线搜索的范围来计算图像梯度和极线夹角的大小；若夹角过大则说明两帧方向趋于垂直，认为结果误差会很大，则不会更新候选点参数；所以夹角合适时，会在极线上按照固定步长去搜索光度误差最小的位置p1和半径大于2的第二小的位置p2，将后者除以前者得到一个比例
Figure BDA0004203822060000041
作为质量属性，质量越大则位置越优；在此基础上进行进一步细致搜索，沿着极线使用高斯牛顿法进行优化直到增量足够小，此时得到投影的最佳位置，用此最佳位置计算新的逆深度范围；
3.5)若当前帧需要成为关键帧，则首先边缘化关键帧序列中满足以下3个条件的关键帧：①与参考帧的光度参数相比变化较大的；②若关键帧大于7，且与当前帧距离最远的；③被激活的候选点少于5％的；删除被边缘化的帧的点和外点，加入当前帧到序列中，构建新关键帧和序列中所有关键帧的残差以得到误差函数；在新关键帧中构建了距离地图，当候选点满足极线搜索距离小于8，步骤3.4)的质量属性好，逆深度为正这三个条件，则选择激活这些候选点，即使用深度滤波器对这些点的逆深度进行优化，激活后可以将这些激活点加入误差函数中准备计算光度误差；
3.6)根据光度误差函数对视觉因子图的激活点开始滑窗优化，使用高斯牛顿法对位姿、光度参数、逆深度进行优化，需要对优化变量求导：
①光度参数求导：
Figure BDA0004203822060000042
Figure BDA0004203822060000043
式中，wp为Huber权重，a和b是光度仿射变换的参数。
②相对位姿求导：
Figure BDA0004203822060000051
式中，
Figure BDA0004203822060000052
分别为像素点在pj处的水平和垂直方向上的梯度，/>
Figure BDA0004203822060000053
为归一化坐标，/>
Figure BDA0004203822060000054
fx和fy为相机内参；
③逆深度求导：
Figure BDA0004203822060000055
式中，ρ为逆深度，tz为相机在z方向上的平移；
获得各个优化变量的雅克比矩阵后可以构建Hessian矩阵进行高斯牛顿法的优化得到更加准确的位姿；结束后，该算法的视觉模块能够独立运行并且为后续的多传感器融合提供基础。
所述步骤4)包括以下步骤：
4.1)基于步骤3)所述视觉因子图，拷贝该因子图并加入IMU预积分残差因子和GPS残差因子作为融合因子图，用滞后边缘化进行联合滑窗优化；
4.2)添加的IMU预积分残差因子为：
Figure BDA0004203822060000056
式中，r表示分别对应位置p、姿态q、速度v、IMU加速度计偏差ba和陀螺仪偏差bg的残差项，
Figure BDA0004203822060000057
表示在第j帧IMU坐标系相对世界坐标系的位置，/>
Figure BDA0004203822060000058
表示在第i帧IMU坐标系到世界坐标系的旋转，/>
Figure BDA0004203822060000059
表示在第i帧的加速度偏差，/>
Figure BDA00042038220600000511
表示四元数乘法，/>
Figure BDA00042038220600000510
和/>
Figure BDA0004203822060000061
表示测量的位置、姿态和速度；
4.3)添加的GPS残差因子为：
Figure BDA0004203822060000062
式中，GPS数据的时间戳k∈(i,j)，i和j是前后两帧图像时间戳，
Figure BDA0004203822060000063
表示时间戳k的GPS位置和i帧位置的差，其他同步骤4.2)所示；
4.4)算法的总残差为光度误差Epj、IMU残差项rp,,,ba,和GPS位置残差项
Figure BDA0004203822060000064
之和，融合因子图以总残差最小为优化目标进行优化；
4.5)融合因子图首先进行IMU的粗初始化步骤，即保持大小为7的滑窗，开始根据优化目标开始计算残差，若计算到残差小于设定值则令算法直接进入VIO-GPS(视觉-IMU-GPS融合)模式，否则重复此步骤；在IMU初始化后使用IMU信息更新最初的视觉边缘先验，重新线性化因子图中的变量，同时保持可以所有的视觉和大多数惯性信息；
4.6)进一步地，融合因子图滑窗优化的边缘化滞后4个关键帧，根据视觉因子图的边缘化顺序进行边缘化操作，使算法能利用古早信息对融合因子图进行更加精确的优化计算，得到更好的位姿结果。
4.7)优化一段时间后，若发现融合因子图中优化变量的逆深度收敛到一定程度后的大小相差较大，则选择抛弃掉老的关键帧，减少旧关键帧的逆深度噪声的影响，否则算法继续正常实时运行；此后，算法在多传感器融合的工作模式工作，不断根据传感器输入的数据实时地输出使用算法的对象的运动轨迹。
本发明利用图像中梯度明显的像素点信息的直接法对相机进行跟踪，用IMU预积分信息作为初始位姿，给直接法的跟踪提供初始值。根据跟踪是否成功，将图像帧分为关键帧和非关键帧。同时，构建视觉因子图，在此视觉因子图上***IMU残差项和GPS残差项构建新的融合因子图，然后开始进行融合因子图优化来完成***算法的优化流程。另外，算法也使用了基于滑窗优化的后端优化策略，固定后端姿态优化的计算复杂度，与***IMU和GPS前的视觉因子图相比，融合因子图保留n个古早信息后，按视觉因子图的边缘化顺序进行滞后边缘化，此策略能利用古早信息，弥补边缘化的缺陷并保留边缘化的优势，让因子图优化过程能够得到更理想的优化结果，因此能提高***的精度。由于算法分了视觉因子图和融合因子图两个部分，所以在视觉IMU初始化完成前或者IMU无法工作的情况下，算法仍旧能够保留一定的性能，可以在纯视觉的模式下进行工作。即使GPS信号无法正常接收，算法也可以在视觉惯性里程计(VIO)的模式下继续工作。单一传感器受影响时其他传感器能够进行互补，反映多传感器融合的定位算法在未知环境下工作的鲁棒性和可靠性，本发明具有较大的实用价值和广泛的应用场景。
与现有技术相比，本发明具有如下的优点和有益结果：
1、本发明使用基于光度误差的直接法，直接利用候选点的像素级的光度信息(梯度)，相比起特征点法省去了提取特征描述子这步相对耗时的操作，也允许在特征点不明显的场景下正常工作，实时性相对更好，算法计算效率得以提速。对于直接法必需的初始位姿，多传感器融合的方案为其提供了IMU预积分得到的位姿，相比传统方案本算法的方案更加迅速和准确。
2、本发明使用多传感器紧耦合融合的策略，紧耦合利用图像的原始信息能提高算法的准确性，同时对于融合因子图采用了滞后边缘化的方法，滞后n帧再进行边缘化的方法这使得算法能够利用古早信息完成初始化和优化，相比常规边缘化他的精度和鲁棒性都更好。
3、本发明采用一种基于光度误差信息的关键帧选取策略，通过判断因子图中逆深度收敛的激活点信息数量是否足够和曝光变化是否过大等条件来确定当前帧是否为关键帧，比特征点法的关键帧选择的常规策略更加贴近算法本身。
4、本发明采用GPS数据融合的策略，能够将算法计算得到的运动轨迹转化到东北天坐标系中，即能够对应现实世界的具***置，使得更加贴近现实，提高运动轨迹的可信度。
附图说明
图1是本发明的优化流程的视觉因子图示意图。
图2是本发明的多传感器融合优化的融合因子图示意图。
图3是本发明基于直接法的多传感器融合的SLAM定位方法的流程图。
具体实施方式
以下实施例将结合附图对本发明进行详细说明。
如图3，本发明所述基于直接法的多传感器融合的SLAM定位方法实施例，包括以下步骤：
1)获取单目相机的图像帧、IMU输出信息和GPS经纬高信息，将GPS经纬高信息解算得到当前的世界坐标系坐标，然后根据时间戳将前述信息进行对齐到一个结构体data中，并输入到***中进行运算；
2)进行视觉部分算法的初始化流程，利用步骤1)得到的结构体data对第一帧图像进行构建图像金字塔，通过梯度阈值筛选候选梯度点，然后将第一帧初始化的候选梯度点投影到第二帧中，并采用图像金字塔对初始相对运动进行估计，初始化逆深度和IMU坐标系到世界坐标系的位姿变换。根据光度误差函数公式，对两帧进行优化，通过优化结果的位移大小判断是否完成初始化。视觉因子图示意图参见图1。
所述步骤2)包括以下步骤：
2.1)初始化时，对初始两帧的第一帧图像建立图像金字塔，遍历金字塔并计算各层图像的相机内参，对第1层图像分为多个网格，网格尺寸为32*32，然后计算像素块网格中的像素点x、y方向上的光强差dx、dy，得到光强差的平方和作为梯度
Figure BDA0004203822060000081
并统计各像素块梯度，选取中位数作为像素块提取候选点的梯度阈值。
2.2)在第1层图像中，遍历每个n*n大小的像素块，像素块内遍历像素点随机选取像素坐标系中的一个方向来计算块内像素的梯度dI，记录梯度最大且大于阈值的像素位置p，作为候选点。若提取到的像素点太少，则减小块的尺寸0.5n*0.5n进行递归重采样；反之增大块的尺寸为2n*2n再重采样。产生递归则以此类推。在金字塔剩余层中，同样进行选点，需要满足一下两个条件：①梯度必须大于设定阈值；②像素坐标系中像素点应是x、y、x+y和x-y四个方向中最大的点，最后对图像金字塔各层进行关联。
2.3)计算当前相机坐标系和重力对齐的变换四元数。以步骤1)中获取的和图像时间对齐的IMU信息为基础，获取一段时间的加速度数据并取平均值作为当前帧的重力方向。同时构建一个固定大小的滑窗保存各帧的重力方向，并使用滑动均值滤波优化IMU重力矢量，与现实重力方向[0,0,9.8]计算得到作为当前帧重力对齐的旋转
Figure BDA0004203822060000082
2.4)对初始两帧的第二帧图像金字塔优化以完成初始化。第二帧基于第一帧的点，构建光度误差函数:
Figure BDA0004203822060000083
式中Np是算法中使用的像素集，‖·‖是Huber范数，p表示帧i中像素点对应的空间点p在帧j上的投影，tj表示第j帧曝光时间。为了使算法可应用于曝光时间未知的图像序列，使用了仿射光度公式
Figure BDA0004203822060000084
将光度归一化操作。然后从图像金字塔顶层向底层开始，将单位矩阵作为初始值通过高斯牛顿法对图像金字塔各层优化，并判断位移是否足够大。若位移足够大且此步骤跟踪帧数大于5则完成初始化，将随后将第一帧设置为关键帧，将第一帧中提取的梯度点激活，将其加入视觉因子图的滑窗优化中；否则重复步骤2.1)到2.4)。
3)进行算法的图像跟踪流程，利用IMU预积分得到的两帧之间位姿作为初始位姿，基于高斯牛顿法对前后两帧图像进行基于光度误差的优化计算，来跟踪相机的运动。在跟踪的过程中，根据设定的三个条件决定是否将当前帧作为新的关键帧，包括：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔。
a、若决定将当前帧作为关键帧，则对当前帧提取到的梯度点进行激活操作，即使用深度滤波器对梯度点的逆深度优化计算使其收敛，同时将关键帧添加到视觉因子图中进行滑窗优化，边缘化采用舒尔补进行；
b、若决定将当前帧作为非关键帧，则仅使用当前帧和前一帧之间的位姿估计来更新逆深度。此时，***以“仅视觉”的模式运行。所述步骤3)包括以下步骤：
3.1)进入算法的跟踪流程。对步骤1)获取的与图像时间对齐的IMU数据进行预积分，得到两帧之间的位姿变换，预积分公式为
Figure BDA0004203822060000091
Figure BDA0004203822060000092
Figure BDA0004203822060000093
式中，
Figure BDA0004203822060000094
和/>
Figure BDA0004203822060000095
为bk+1相对于bk的位姿变化。算法以IMU预积分得到的位姿作为直接法的初始位姿。
3.2)使用初始位姿将参考帧的点投影到当前帧中，并构建光度误差函数。以光度误差最小为优化目标，使用列文伯格-马夸尔特算法从图像金字塔顶层向下对候选点开始迭代优化，即由粗到精迭代优化逆深度、光度参数和位姿。此处有几种不同的优化处理操作：①若光度误差大于能量阈值的60％则放大阈值重新优化；②若金字塔中某层能量大于2倍最小值，直接判断跟踪失败；③若光度误差满足条件，则跟踪成功。
3.3)根据预设的三个条件判断当前帧是否可以成为关键帧：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔。
3.4)若当前帧成为非关键帧，更新步骤2.2)的候选点的逆深度范围。根据逆深度范围得到极线搜索的范围来计算图像梯度和极线夹角的大小。若夹角过大则说明两帧方向趋于垂直，认为结果误差会很大，则不会更新候选点参数。所以夹角合适时，会在极线上按照固定步长去搜索光度误差最小的位置p1和半径大于2的第二小的位置p2，将后者除以前者得到一个比例
Figure BDA0004203822060000101
作为质量属性，质量越大则位置越优。在此基础上进行进一步细致搜索，沿着极线使用高斯牛顿法进行优化直到增量足够小，此时得到投影的最佳位置，用此最佳位置计算新的逆深度范围。
3.5)若当前帧需要成为关键帧，则首先边缘化关键帧序列中满足以下3个条件的关键帧：①与参考帧的光度参数相比变化较大的；②若关键帧大于7，且与当前帧距离最远的；③被激活的候选点少于5％的。删除被边缘化的帧的点和外点，然后加入当前帧到序列中，并构建新关键帧和序列中所有关键帧的残差以得到误差函数。接着在新关键帧中构建了距离地图，当候选点满足极线搜索距离小于8，步骤3.4)的质量属性好，逆深度为正这三个条件，则选择激活这些候选点，即使用深度滤波器对这些点的逆深度进行优化，激活后可以将这些激活点加入误差函数中准备计算光度误差。
3.6)根据光度误差函数对视觉因子图的激活点开始滑窗优化，使用高斯牛顿法对位姿、光度参数、逆深度进行优化，需要对优化变量求导：
①光度参数求导：
Figure BDA0004203822060000102
Figure BDA0004203822060000103
式中，wp为Huber权重，a和b是光度仿射变换的参数。
②相对位姿求导：
Figure BDA0004203822060000104
式中
Figure BDA0004203822060000105
分别为像素点在pj处的水平和垂直方向上的梯度，/>
Figure BDA0004203822060000106
为归一化坐标，/>
Figure BDA0004203822060000107
fx和fy为相机内参；
③逆深度求导：
Figure BDA0004203822060000111
式中，ρ为逆深度，tz为相机在z方向上的平移；
获得各个优化变量的雅克比矩阵后可以构建Hessian矩阵进行高斯牛顿法的优化得到更加准确的位姿。步骤3)结束后，该算法的视觉模块能够独立运行并且为后续的多传感器融合提供了基础。
4)进行算法的视觉-IMU-GPS数据融合流程。基于步骤3)所述视觉因子图，拷贝该因子图作为融合因子图，加入IMU预积分因子和GPS因子使用滞后边缘化进行联合优化。融合因子图同样使用滑窗优化的方法，但是边缘化的策略为推迟n个关键帧，再同视觉因子图的边缘化顺序进行滞后边缘化以达到保持滑窗大小的目的。滞后边缘化在保证精度的同时还弥补了边缘化的缺陷并保留了边缘化的优势。此时，***进入“视觉-IMU-GPS”模型运行。多传感器融合优化的融合因子图示意图参见图2。
所述步骤4)包括以下步骤：
4.1)基于步骤3)所述视觉因子图，拷贝该因子图并加入IMU预积分残差因子和GPS残差因子作为融合因子图，用滞后边缘化进行联合滑窗优化。
4.2)添加的IMU预积分残差因子为
Figure BDA0004203822060000112
式中，r表示分别对应位置p、姿态q、速度v、IMU加速度计偏差ba和陀螺仪偏差bg的残差项，
Figure BDA0004203822060000113
表示在第j帧IMU坐标系相对世界坐标系的位置，/>
Figure BDA0004203822060000114
表示在第i帧IMU坐标系到世界坐标系的旋转，/>
Figure BDA0004203822060000115
表示在第i帧的加速度偏差，/>
Figure BDA00042038220600001110
表示四元数乘法，/>
Figure BDA0004203822060000116
和
Figure BDA0004203822060000117
表示测量的位置、姿态和速度。
4.3)添加的GPS残差因子为：
Figure BDA0004203822060000118
式中GPS数据的时间戳k∈(i,j)，i和j是前后两帧图像时间戳，
Figure BDA0004203822060000119
表示时间戳k的GPS位置和i帧位置的差，其他同步骤4.2)所示。
4.4)算法的总残差为光度误差Epj、IMU残差项rp,,,ba,和GPS位置残差项
Figure BDA0004203822060000121
之和，融合因子图以总残差最小为优化目标进行优化。
4.5)融合因子图首先进行IMU的粗初始化步骤，即保持大小为7的滑窗，开始根据优化目标开始计算残差，若计算到残差小于设定值则令算法直接进入VIO-GPS(视觉-IMU-GPS融合)模式，否则重复此步骤。在IMU初始化后使用IMU信息更新最初的视觉边缘先验，重新线性化因子图中的变量，同时保持可以所有的视觉和大多数惯性信息。
4.6)进一步地，融合因子图滑窗优化的边缘化滞后4个关键帧，再根据视觉因子图的边缘化顺序来进行边缘化操作，这使得算法能够利用古早信息来对融合因子图进行更加精确的优化计算，得到更好的位姿结果。
4.7)当优化一段时间后，若发现融合因子图中优化变量的逆深度收敛到一定程度后的大小相差较大，则选择抛弃掉老的关键帧，减少旧关键帧的逆深度噪声的影响，否则算法继续正常实时运行。此后，算法在多传感器融合的工作模式来工作，不断根据传感器输入的数据实时地输出使用算法的对象的运动轨迹。
综上所述，本发明算法使用基于光度误差的直接法，直接利用候选点的像素级的光度信息(梯度)，实时性相对更好，算法计算效率得以提速。对于直接法必需的初始位姿，多传感器紧耦合融合的方案为其提供了IMU预积分得到的位姿，减少噪声的影响，紧耦合利用图像的原始信息能提高算法的准确性，进一步地还采用GPS数据融合的策略，能够对应现实世界的具***置，提高运动轨迹的可信度。此外对于融合因子图采用滞后边缘化的方法，滞后n帧再进行边缘化的方法这使得算法能够利用古早信息完成初始化和优化，相比常规边缘化他的精度和鲁棒性都更好。另外，通过判断因子图中逆深度收敛的激活点信息数量是否足够和曝光变化是否过大等条件来确定当前帧是否为关键帧，比特征点法的关键帧选择的常规策略更加贴近算法本身，效果更好。

Claims (4)

1.一种基于直接法的多传感器融合的SLAM定位方法，其特征在于包括以下步骤：
1)获取单目相机的图像帧、IMU输出信息和GPS经纬高信息，将GPS经纬高信息解算得到当前的世界坐标系坐标，根据时间戳将前述信息对齐到一个结构体data中，输入***中运算；
2)进行视觉部分算法的初始化流程，利用步骤1)得到的结构体data对第一帧图像进行构建图像金字塔，通过梯度阈值筛选候选梯度点，将第一帧初始化的候选梯度点投影到第二帧中，采用图像金字塔对初始相对运动进行估计，初始化逆深度和IMU坐标系到世界坐标系的位姿变换；根据光度误差函数公式，对两帧进行优化，通过优化结果的位移大小判断是否完成初始化；
3)进行算法的图像跟踪流程，利用IMU预积分得到的两帧之间位姿作为初始位姿，基于高斯牛顿法对前后两帧图像进行基于光度误差的优化计算，跟踪相机的运动；在跟踪的过程中，根据设定的三个条件决定是否将当前帧作为新的关键帧，包括：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔；
a、若决定将当前帧作为关键帧，则对当前帧提取到的梯度点进行激活操作，即使用深度滤波器对梯度点的逆深度优化计算使其收敛，同时将关键帧添加到视觉因子图中进行滑窗优化，边缘化采用舒尔补进行；
b、若决定将当前帧作为非关键帧，则仅使用当前帧和前一帧之间的位姿估计来更新逆深度，***以“仅视觉”的模式运行；
4)进行算法的视觉-IMU-GPS数据融合流程，基于步骤3)所述视觉因子图，拷贝该因子图作为融合因子图，加入IMU预积分因子和GPS因子使用滞后边缘化进行联合优化；融合因子图同样使用滑窗优化的方法，但是边缘化的策略为推迟n个关键帧，再同视觉因子图的边缘化顺序进行滞后边缘化以达到保持滑窗大小的目的；滞后边缘化在保证精度的同时还弥补边缘化的缺陷并保留边缘化的优势，***进入“视觉-IMU-GPS融合”模式运行。
2.如权利要求1所述一种基于直接法的多传感器融合的SLAM定位方法，其特征在于在步骤2)中，所述进行视觉部分算法的初始化流程具体包括以下步骤：
2.1)初始化时，对初始两帧的第一帧图像建立图像金字塔，遍历金字塔并计算各层图像的相机内参，对第1层图像分为多个网格，网格尺寸为32×32，然后计算像素块网格中的像素点x、y方向上的光强差dx、dy，得到光强差的平方和作为梯度
Figure FDA0004203822050000021
并统计各像素块梯度，选取中位数作为像素块提取候选点的梯度阈值；
2.2)在第1层图像中，遍历每个n×n大小的像素块，像素块内遍历像素点随机选取像素坐标系中的一个方向来计算块内像素的梯度dI，记录梯度最大且大于阈值的像素位置p，作为候选点；若提取到的像素点太少，则减小块的尺寸0.5n×0.5n进行递归重采样；反之增大块的尺寸为2n×2n再重采样；产生递归则以此类推；在金字塔剩余层中，同样进行选点，需要满足一下两个条件：①梯度必须大于设定阈值；②像素坐标系中像素点应是x、y、x+y和x-y四个方向中最大的点，最后对图像金字塔各层进行关联；
2.3)计算当前相机坐标系和重力对齐的变换四元数；以步骤1)中获取的和图像时间对齐的IMU信息为基础，获取一段时间的加速度数据并取平均值作为当前帧的重力方向；同时构建一个固定大小的滑窗保存各帧的重力方向，使用滑动均值滤波优化IMU重力矢量，与现实重力方向[0,0,9.8]计算得到作为当前帧重力对齐的旋转
Figure FDA0004203822050000022
2.4)对初始两帧的第二帧图像金字塔优化以完成初始化；第二帧基于第一帧的点，构建光度误差函数：
Figure FDA0004203822050000023
式中，Np是算法中使用的像素集，‖·‖是Huber范数，p表示帧i中像素点对应的空间点p在帧j上的投影，tj表示第j帧曝光时间；为使算法能应用于曝光时间未知的图像序列，使用仿射光度公式
Figure FDA0004203822050000024
将光度归一化操作；从图像金字塔顶层向底层开始，将单位矩阵作为初始值通过高斯牛顿法对图像金字塔各层优化，判断位移是否足够大；若位移足够大且此步骤跟踪帧数大于5则完成初始化，将随后将第一帧设置为关键帧，将第一帧中提取的梯度点激活，将其加入视觉因子图的滑窗优化中；否则重复步骤2.1)到2.4)。
3.如权利要求1所述一种基于直接法的多传感器融合的SLAM定位方法，其特征在于在步骤3)中，所述进行算法的图像跟踪流程具体包括以下步骤：
3.1)进入算法的跟踪流程：对步骤1)获取的与图像时间对齐的IMU数据进行预积分，得到两帧之间的位姿变换，预积分公式为：
Figure FDA0004203822050000025
Figure FDA0004203822050000031
Figure FDA0004203822050000032
式中，
Figure FDA0004203822050000033
和/>
Figure FDA0004203822050000034
为bk+1相对于bk的位姿变化；算法以IMU预积分得到的位姿作为直接法的初始位姿；
3.2)使用初始位姿将参考帧的点投影到当前帧中，构建光度误差函数；以光度误差最小为优化目标，使用列文伯格-马夸尔特算法从图像金字塔顶层向下对候选点开始迭代优化，即由粗到精迭代优化逆深度、光度参数和位姿；有几种不同的优化处理操作：①若光度误差大于能量阈值的60％则放大阈值重新优化；②若金字塔中某层能量大于2倍最小值，直接判断跟踪失败；③若光度误差满足条件，则跟踪成功；
3.3)根据预设的三个条件判断当前帧是否可以成为关键帧：①***刚启动，存储的帧数为1；②当前帧像素的平移、旋转或光度变化比例过大；③当前帧距离上一关键帧的时间超出关键帧帧间最大时间间隔；
3.4)若当前帧成为非关键帧，更新步骤2.2)的候选点的逆深度范围；根据逆深度范围得到极线搜索的范围来计算图像梯度和极线夹角的大小；若夹角过大则说明两帧方向趋于垂直，认为结果误差会很大，则不会更新候选点参数；所以夹角合适时，会在极线上按照固定步长去搜索光度误差最小的位置p1和半径大于2的第二小的位置p2，将后者除以前者得到一个比例
Figure FDA0004203822050000035
作为质量属性，质量越大则位置越优；在此基础上进行进一步细致搜索，沿着极线使用高斯牛顿法进行优化直到增量足够小，此时得到投影的最佳位置，用此最佳位置计算新的逆深度范围；
3.5)若当前帧需要成为关键帧，则首先边缘化关键帧序列中满足以下3个条件的关键帧：①与参考帧的光度参数相比变化较大的；②若关键帧大于7，且与当前帧距离最远的；③被激活的候选点少于5％的；删除被边缘化的帧的点和外点，加入当前帧到序列中，构建新关键帧和序列中所有关键帧的残差以得到误差函数；在新关键帧中构建了距离地图，当候选点满足极线搜索距离小于8，步骤3.4)的质量属性好，逆深度为正这三个条件，则选择激活这些候选点，即使用深度滤波器对这些点的逆深度进行优化，激活后将这些激活点加入误差函数中准备计算光度误差；
3.6)根据光度误差函数对视觉因子图的激活点开始滑窗优化，使用高斯牛顿法对位姿、光度参数、逆深度进行优化，需要对优化变量求导：
①光度参数求导：
Figure FDA0004203822050000041
Figure FDA0004203822050000042
式中，wp为Huber权重，a和b是光度仿射变换的参数；
②相对位姿求导：
Figure FDA0004203822050000043
式中，
Figure FDA0004203822050000044
分别为像素点在pj处的水平和垂直方向上的梯度，/>
Figure FDA0004203822050000045
为归一化坐标，/>
Figure FDA0004203822050000046
fx和fy为相机内参；
③逆深度求导：
Figure FDA0004203822050000047
式中，ρ为逆深度，tz为相机在z方向上的平移；
获得各个优化变量的雅克比矩阵后构建Hessian矩阵进行高斯牛顿法的优化得到更准确的位姿。
4.如权利要求1所述一种基于直接法的多传感器融合的SLAM定位方法，其特征在于在步骤4)中，所述进行算法的视觉-IMU-GPS数据融合流程具体包括以下步骤：
4.1)基于步骤3)所述视觉因子图，拷贝该因子图并加入IMU预积分残差因子和GPS残差因子作为融合因子图，用滞后边缘化进行联合滑窗优化；
4.2)添加的IMU预积分残差因子为：
Figure FDA0004203822050000051
式中，r表示分别对应位置p、姿态q、速度v、IMU加速度计偏差ba和陀螺仪偏差bg的残差项，
Figure FDA0004203822050000052
表示在第j帧IMU坐标系相对世界坐标系的位置，/>
Figure FDA0004203822050000053
表示在第i帧IMU坐标系到世界坐标系的旋转，/>
Figure FDA0004203822050000054
表示在第i帧的加速度偏差，/>
Figure FDA0004203822050000055
表示四元数乘法，/>
Figure FDA0004203822050000056
和/>
Figure FDA0004203822050000057
表示测量的位置、姿态和速度；
4.3)添加的GPS残差因子为：
Figure FDA0004203822050000058
式中，GPS数据的时间戳k∈(i,j)，i和j是前后两帧图像时间戳，
Figure FDA0004203822050000059
表示时间戳k的GPS位置和i帧位置的差，其他同步骤4.2)所示；
4.4)算法的总残差为光度误差Epj、IMU残差项rp,,,ba,和GPS位置残差项rgk之和，融合因子图以总残差最小为优化目标进行优化；
4.5)融合因子图首先进行IMU的粗初始化步骤，即保持大小为7的滑窗，开始根据优化目标开始计算残差，若计算到残差小于设定值则令算法直接进入VIO-GPS模式，即视觉-IMU-GPS融合，否则重复此步骤；在IMU初始化后使用IMU信息更新最初的视觉边缘先验，重新线性化因子图中的变量，同时保持所有的视觉和大多数惯性信息；
4.6)融合因子图滑窗优化的边缘化滞后4个关键帧，根据视觉因子图的边缘化顺序进行边缘化操作，使算法能利用古早信息对融合因子图进行更加精确的优化计算，得到更好的位姿结果；
4.7)优化一段时间后，若发现融合因子图中优化变量的逆深度收敛到一定程度后的大小相差较大，则选择抛弃掉老的关键帧，减少旧关键帧的逆深度噪声的影响，否则算法继续正常实时运行；此后，算法在多传感器融合的工作模式工作，不断根据传感器输入的数据实时地输出使用算法的对象的运动轨迹。
CN202310470533.5A 2023-04-27 2023-04-27 一种基于直接法的多传感器融合的slam定位方法 Pending CN116242374A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310470533.5A CN116242374A (zh) 2023-04-27 2023-04-27 一种基于直接法的多传感器融合的slam定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310470533.5A CN116242374A (zh) 2023-04-27 2023-04-27 一种基于直接法的多传感器融合的slam定位方法

Publications (1)

Publication Number Publication Date
CN116242374A true CN116242374A (zh) 2023-06-09

Family

ID=86624542

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310470533.5A Pending CN116242374A (zh) 2023-04-27 2023-04-27 一种基于直接法的多传感器融合的slam定位方法

Country Status (1)

Country Link
CN (1) CN116242374A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116625380A (zh) * 2023-07-26 2023-08-22 广东工业大学 一种基于机器学习和slam的路径规划方法及***
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116625380A (zh) * 2023-07-26 2023-08-22 广东工业大学 一种基于机器学习和slam的路径规划方法及***
CN116625380B (zh) * 2023-07-26 2023-09-29 广东工业大学 一种基于机器学习和slam的路径规划方法及***
CN117760428A (zh) * 2024-02-22 2024-03-26 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法
CN117760428B (zh) * 2024-02-22 2024-04-30 西北工业大学 一种基于多立体视觉惯性紧耦合的自主定位方法

Similar Documents

Publication Publication Date Title
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
CN108827315B (zh) 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN110125928B (zh) 一种基于前后帧进行特征匹配的双目惯导slam***
CN110686677B (zh) 一种基于几何信息的全局定位方法
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及***
Huang Review on LiDAR-based SLAM techniques
CN108615246B (zh) 提高视觉里程计***鲁棒性和降低算法计算消耗的方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN116242374A (zh) 一种基于直接法的多传感器融合的slam定位方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN115407357B (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN112115980A (zh) 基于光流跟踪和点线特征匹配的双目视觉里程计设计方法
CN113706626A (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
Zhu et al. Fusing GNSS/INS/vision with a priori feature map for high-precision and continuous navigation
CN116448100A (zh) 一种多传感器融合的近岸无人船slam方法
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN116188417A (zh) 基于slam和图像处理的裂缝检测及其三维定位方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN112731503B (zh) 一种基于前端紧耦合的位姿估计方法及***
CN112432653A (zh) 基于点线特征的单目视觉惯性里程计方法
CN116147618B (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