CN111780754B - 基于稀疏直接法的视觉惯性里程计位姿估计方法 - Google Patents
基于稀疏直接法的视觉惯性里程计位姿估计方法 Download PDFInfo
- Publication number
- CN111780754B CN111780754B CN202010595397.9A CN202010595397A CN111780754B CN 111780754 B CN111780754 B CN 111780754B CN 202010595397 A CN202010595397 A CN 202010595397A CN 111780754 B CN111780754 B CN 111780754B
- Authority
- CN
- China
- Prior art keywords
- image
- moment
- imu
- key frame
- pose
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 230000000007 visual effect Effects 0.000 title claims abstract description 38
- 238000005457 optimization Methods 0.000 claims abstract description 21
- 238000005259 measurement Methods 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims abstract description 13
- 238000004364 calculation method Methods 0.000 claims description 14
- 238000012546 transfer Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 5
- 239000000126 substance Substances 0.000 claims description 3
- 238000013519 translation Methods 0.000 abstract description 8
- 238000005286 illumination Methods 0.000 abstract description 5
- 230000010354 integration Effects 0.000 description 10
- 230000001133 acceleration Effects 0.000 description 8
- 230000003287 optical effect Effects 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 5
- 230000005484 gravity Effects 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于稀疏直接法的视觉惯性里程计位姿估计方法，首先给载体一个较大的旋转和平移，对安装在载体上的VO和IMU进行初始化；构建N层图像金字塔，对整个图像金字塔的灰度值进行调整；采用基于滑动窗口的非线性最小二乘法，维护一个具有若干个关键帧的滑动窗口；对于每一个图像帧，首先根据其位姿估计判断是否为关键帧，若是关键帧，则将当前帧加入滑动窗口进行非线性优化，同时将窗口内对窗口贡献最小的图像帧边缘化，计算更新先验信息矩阵和先验残差。若不是关键帧，则直接将当前帧移出滑动窗口，维持原先滑动窗口中的状态变量。本发明使VIO***对光照具有更好的鲁棒性。
Description
技术领域
本发明属于同时定位与地图构建(SLAM)技术领域。
背景技术
惯性测量单元(IMU)以较高的频率测量载体的三轴角速度和加速度并解算得到姿态，可以提供短时高精度的运动估计，但是会受到零偏、温度、振动等因素影响，存在累积误差问题。视觉里程计(VO)通过读取到的图像信息可以得到较为准确的相机旋转量和相机平移量，在静止时不会产生漂移，但是容易受到图像遮挡、光照环境等影响导致跟踪丢失。由于视觉传感器和惯性传感器具有很好的互补性，所以将视觉和惯性传感器融合得到视觉惯性里程计(VIO)。VIO是一种特殊的SLAM***，在很多领域都有着广泛的应用，如机器人、无人驾驶、智能家居等。
目前视觉传感器和惯性传感器的融合技术还在不断发展和完善中。现有的经典VIO算法有MSCKF算法、OKVIS算法、ORBSLAM算法和VINS-MONO算法等，其视觉部分都是采用特征点法或光流法得到视觉观测量，惯性部分采用流形预积分的方式得到惯性观测量，将视觉传感器和惯性传感器对齐，并在后端通过卡尔曼滤波或非线性优化的方法得到最优的位姿估计值。因为IMU是一个相对独立的传感器，所以整个***的鲁棒性主要取决于前端VO的鲁棒性。常用的VO位姿估计方法包括特征点法和光流法。特征点法需要在每一幅图像中都提取大量的特征点，在相邻的图像帧之间进行特征点匹配，再根据匹配好的特征点对进行位姿估计。由于在每一帧都要提取大量的特征点并进行匹配，所以***的计算量比较大，并且非常依赖于环境信息，在采集到的图像没有很好的纹理特征时几乎无法工作。光流法先在第一帧图像中提取一定数量的特征点，然后根据灰度不变的假设跟踪这些特征点，得到这些特征点在下一帧图像中的位置。相比于特征点法，光流法节省了后续特征点提取和特征点匹配的时间，具有更好的实时性，但是同样依赖于环境信息，并且在处理边缘上的特征点时，容易出现跟踪失败的问题，无法很好地利用到物体边缘提供的信息。所以现有的VIO方法的视觉前端容易出现特征点误匹配和图像跟丢的问题，导致VIO位姿估计失败，***没有很好的鲁棒性。此外，无论是特征点法或是光流法，都没有利用到图像特征点之间的约束，所以会出现误匹配的问题，影响后续的位姿估计精度。
发明内容
发明目的：为解决现有技术存在计算精度低等问题，本发明提供了一种基于稀疏直接法的视觉惯性里程计位姿估计方法。
技术方案：本发明提供了一种基于稀疏直接法的视觉惯性里程计位姿估计方法，具体包括如下步骤：
步骤一：在载体上安装俯视单目相机VO和惯性传感器IMU；对VO和IMU采集到的数据进行时间同步；
步骤二：对VO和IMU进行联合初始化，并同步VO和IMU的轨迹；
步骤三：对第k+1时刻VO采集到的图像建立N层图像金字塔，由上往下依次对图像金字塔的每一层进行光度补偿，从而更新该第k+1时刻图像；
步骤四：建立滑动窗口，将更新后的第k+1时刻图像放入窗口，根据该图像，计算第k+1时刻时VO的位姿，若该位姿与上一个关键帧对应的VO位姿之间的差值大于等于预设的阈值，则计算第y个关键帧中每个像素点的梯度值，并由大到小排列，选择前300个像素点作为第y个关键帧的路标点，y＝1，2，...，M,M为滑动窗口中关键帧的总个数，根据每一个关键帧中所有路标点的梯度值计算每一个关键帧对滑动窗口的贡献值，将贡献值最小的关键帧边缘化，同时将更新后的第k+1时刻图像作为一个关键帧；基于更新后的第k+1时刻图像与上一个关键帧之间的VO视觉残差、更新后的第k+1时刻图像与上一个关键帧之间的IMU惯性残差以及上一次优化计算得到的先验残差，建立基于窗口的非线性最小二乘函数，并求解该非线性最小二乘函数，得到第k+1时刻里程计位姿的最优估计值，将被边缘化的图像帧对应的测量信息转换为先验信息约束加入到滑动窗口，得到用于下一次优化计算的先验残差，并继续将下一个时刻的图像放入滑动窗口中进行计算；若第k+1时刻的VO位姿与上一个关键帧对应的VO位姿之间的差值小于预设的阈值，则删除第k+1时刻图像，并继续将下一个时刻的图像放入滑动窗口中进行计算。
进一步的，所述步骤二中同步VO和IMU的轨迹包括将IMU坐标系转换到VO坐标系下，具体为：
步骤2.1:计算相邻时刻之间IMU测得旋转积分
相邻时刻之间VO测量到的旋转
并建立如下几何约束条件：
其中，qbc为VO和IMU之间的旋转外参；
步骤2.2：根据qbc，计算得到在VO坐标系下从k时刻到k+1时刻IMU的旋转
为：
进一步的，所述步骤三具体为：
步骤3.1：对第k+1时刻VO采集到的原始图像建立一个N层图像金字塔，该图像金字塔的分辨率由下往上逐渐降低，最底层为分辨率最高的原始图像；
步骤3.2：在该图像金字塔的顶层图像的中心选择一块大小为J×L的矩形区域作为模板，计算该模板的平均灰度值，将该平均灰度值作为第k+1时刻图像金字塔的灰度值，得到第k+1时刻图像金字塔与第k时刻图像金字塔的灰度值的差值，将该差值作为第k+1时刻图像金字塔中每一层图像的光度补偿值，从而对图像金字塔中每一层图像进行更新。
进一步的，所述步骤四中，计算第k+1时刻VO的位姿具体为：将更新后的第k+1时刻图像中每个像素点的梯度值由大到小排列，选择前300个点作为该图像的路标点，根据这些路标点的最小化光度误差计算第k+1时刻VO位姿的李代数，并通过稀疏直接法求解该李代数，得到第k+1时刻VO的位姿。
进一步的，所述步骤四中，VO视觉残差为第k+1时刻图像中所有路标点灰度值之和与上一个关键帧中所有路标点灰度值之和的差。
进一步的，所述步骤四中IMU惯性残差为，对第k+1时刻图像与上一个关键帧之间IMU测得的数据进行预积分，从而得到IMU误差传递函数，利用IMU的误差传递函数处理预积分量中的噪声，从而得到IMU惯性残差。
进一步的，所述步骤四中采用图优化模型构建非线性最小二乘函数：
其中，χ为里程计的状态变量，χ*为里程计的状态变量的最优估计值，bp为上一次优化计算的先验残差，Λp为上一次优化计算的先验信息矩阵，rB(·)为IMU的惯性残差，
为第g+1个关键帧和第g个关键帧之间IMU的观测量，B为滑动窗口内所有关键帧的集合，I(ug+1,g+1)为第g+1个关键帧中所有路标点的灰度值之和，I(ug,g)为第g个关键帧中所有路标点灰度值之和，ug+1表示第g+1个关键帧中所有路标点的坐标，ug表示第g个关键帧中所有路标点的坐标，
为IMU协方差；∑c为VO的权重矩阵。。
进一步的，所述步骤四中采用高斯-牛顿迭代法或列文伯格-马夸尔特迭代法求解非线性最小二乘函数。
有益效果：本发明相比于特征点法和光流法，不需要提取特征点和计算描述子，在减小了计算量的同时克服了视觉传感器对图像特征的依赖性，使VIO***在白墙、玻璃等特征较少的场景下也能得到较好的定位结果；由于稀疏直接法不局限于点和点之间的匹配，而是直接考虑整个相机的运动，所以也不存在误匹配的问题，避免了在后端优化过程中对误匹配信息的处理；另外，本发明引入了光度补偿模块，通过图像金字塔顶层的中心模板灰度值的变化量对整个图像的灰度值进行更新，在没有过多增加***计算负荷的情况下对图像进行了光度补偿，使相机可以在高动态亮度变化的场景中得到灰度值较为平稳的图像，使VIO***的视觉前端对光照具有更好的鲁棒性。
附图说明
图1是本发明的***框图。
图2是本发明VO和IMU联合初始化的流程图。
图3是本发明采用的图优化模型。
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
本发明的流程图框图如图1。***主要分为前端、初始化和后端三大部分。前端负责采集并处理视觉传感器和惯性传感器的测量值，得到初步的位姿估计结果。初始化模块通过视觉传感器和惯性传感器之间的旋转约束和平移约束，计算得到旋转外参数、陀螺仪偏置、重力加速度和尺度因子参数。后端维护了一个具有若干个关键帧的滑动窗口，每当新的关键帧进来时，从窗口中选择一个对***贡献值最小的关键帧进行边缘化，被边缘化的图像测量信息将会转换为先验残差。这样只对滑动窗口内的图像帧进行非线性优化，将计算量限制在一定范围内，保证了VIO***的实时性，本发明具体为：
步骤一：在传感器进行数据采集时将时间戳加在惯性测量数据和视觉测量数据上，在后续处理数据时，通过时间戳的标记实现视觉和惯性传感器的时间同步。
步骤二：如图2所示，对VIO***进行联合初始化，使视觉测量轨迹和惯性测量轨迹对齐：
首先估计VO和IMU之间的旋转外参数qbc。考虑相邻的k时刻和k+1时刻，它们之间的IMU旋转积分为
视觉测量到的旋转为
那么根据几何约束条件，有：
其中，
符号表示四元数乘法。如此就可以计算得到旋转外参数qbc；根据估计出的旋转外参数，可以计算出k时刻VO到k+1时刻IMU的旋转为：
接下来，根据VO和IMU之间的旋转约束估计IMU陀螺仪的偏置。因为IMU陀螺仪受到偏置和高斯白噪声的影响，所以根据IMU测量值直接算出的旋转积分
是有很大误差的。通过最小化视觉旋转
和惯性旋转
的差就可以得到IMU陀螺仪的偏置bgyro，表示为：
其中，argmin(·)表示取得最小值；[·]xyz表示四元数取虚部；C表示所有图像帧的集合。
接下来利用VO和IMU之间的平移约束来初始化重力加速度、初始速度和尺度因子，需要初始化的状态变量表示为：
其中，χI为待估计的初始化变量；
表示载体在IMU坐标系下每个时刻的速度；
为初始重力加速度，s为相机的尺度因子，通过尺度因子可以把相机轨迹从非米制单位恢复到米制单位，使相机轨迹能和惯性轨迹对齐。通过最小二乘法可以解出初始化状态变量的初始估计值，但是并没有重力加速度的模长限制。所以要重新将重力加速度建模为两自由度的变量，再重新对χI进行优化，从而完成VIO***的初始化过程。
步骤三：根据相机采集到的第K+1时刻的图像建立一个4层、倍率为2的图像金字塔，最底层为分辨率最高的原始图像，越上层的图像分辨率越低。比如采集到了640*480的原始图像，则四层金字塔从下到上的图像分辨率分别为640*480,320*240,160*120,80*60。在通过直接法解算相机位姿时，首先从金字塔的最顶层进行计算，再将顶层计算得到的粗略估计值传递给下一层，如此直到得到在原始图像层的位姿估计值。通过金字塔的方式，可以避免因为运动过大而导致跟踪丢失的问题。
步骤四：进行光度补偿。首先在图像金字塔的顶层选择一块大小为J×L的矩形区域作为模板，计算相邻两帧之间的灰度差
为：
其中，I(m,s1,k+1)为第k+1时刻的模板中横坐标为m，纵坐标为s1对应的坐标点的原始灰度值；
为为第k时刻的模板中横坐标为m，纵坐标为s1对应的坐标点的光度补偿后的灰度值；
根据光度补偿值对从顶层到底层对整个图像金字塔进行更新，使图像的光照始终保持在较为稳定的状态。利用金字塔顶层对图像进行光度补偿，可以使***对光照有更好的鲁棒性，在明暗变化剧烈的环境中也可以得到灰度值趋于平稳的图像，从而很好的估计载体的运动。此外，因为只利用了图像金字塔顶层的一块矩形区域计算光度补偿值，***增加的计算负荷很小，保证了***的实时性。
步骤五：根据计算图像中所有像素点的梯度值，根据所有像素点的梯度值，选择其中梯度值最大的300个点作为路标点。根据这些路标点的最小化光度误差计算相机位姿的李代数ξ
其中，ek为第k时刻图像的光度误差，通过ek可以得到视觉部分对应的优化残差，这样就可以根据稀疏直接法求解出相机位姿，T为空间转置。
步骤六：对两个图像帧之间的所有惯性测量数据进行预积分，计算平移、速度和旋转预积分量，分别为：
其中，
分别表示i时刻到j时刻的平移、速度和旋转预积分量；t表示时间；
表示t时刻到i时刻的旋转；
分别表示在IMU坐标系下IMU加速度和角速度的测量值。通过预积分量就可以推导出j时刻载体的位置、速度和旋转，分别表示为：
其中，
为j时刻IMU通过预积分计算得到的世界坐标系下的载***置、速度和旋转量；
为i时刻的载***置、速度和旋转量；gw为世界坐标系下的重力加速度；Δt为积分时间。如此就得到了基于IMU的运动约束。
由于在IMU预积分的过程中也同时累加了每个IMU测量值的误差，所以要计算协方差的传递，IMU的误差传递可以近似化为线性方程：
其中，
分别为j时刻、i时刻的位置误差；
分别为i时刻、j时刻的旋转误差；
分别为i时刻、j时刻的速度误差；
分别为i时刻、时刻IMU加速度计的偏置；
分别为i时刻、j时刻IMU陀螺仪的偏置；F矩阵为状态转移矩阵；G矩阵为噪声输入矩阵；N1为过程噪声序列。通过IMU的误差传递方程，可以得到在相邻关键帧之间IMU预积分量的噪声分布，并根据IMU预积分量得到IMU相应的优化残差
步骤七：建立一个具有7个关键帧的滑动窗口。根据步骤5中得到的李代数，利用稀疏直接法求解李代数，得到第k+1时刻图像，也即当前帧的位姿，若当前帧与上一个关键帧的位姿变化超过预设的值时，将当前帧加入滑动窗口进行优化。同时，根据滑动窗口中每一个关键帧中的路标点(计算第y个关键帧中每个像素点的梯度值，并由大到小排列，选择前300个像素点作为第y个关键帧的路标点，y＝1,2,…,m,M为关键帧的总个数)的灰度值计算每一个关键帧对滑动窗口的贡献值，比较原始窗口中7个关键帧的贡献值，将贡献值最小的关键帧边缘化。被边缘化的图像帧，其测量信息转换为先验信息约束加入到滑动窗口，得到先验残差项。若当前帧与上一个关键帧的位姿变化没有超过预设的值时，直接将当前帧移出滑动窗口，不再对其进行优化。
步骤八：构建基于滑动窗口的非线性最小二乘问题。分别计算滑动窗口中基于边缘化的先验残差、基于光度误差模型的视觉残差和基于IMU残差模型的惯性残差，根据残差值对窗口内的所有状态量进行优化，得到VIO***的最优轨迹。
构建基于滑动窗口的非线性最小二乘问题。采用的图优化模型如图3，图中的顶点表示待优化的状态变量，图中的边代表由观测值得到的残差。路标点和关键帧之间的连线表示VO的视觉约束条件，IMU预积分和关键帧之间的连线表示IMU的惯性约束条件(先验信息残差；rB(·)为IMU的残差模型；
为惯性观测量；)。假设有n1个位姿和m1个路标点，那么滑动窗口中的待优化的状态变量χ表示为：
χ＝[x0,x1,…,xn1,xbc,λ0,λ1,…,λm1]
其中，x0,x1,…,xn1为位姿估计值，它是一个15维的状态向量，包括载体的位置估计值、速度估计值、姿态估计值和IMU陀螺仪的偏置与IMU加速度计的偏置；xbc为VO和IMU传感器之间的外参，包括旋转外参数和平移外参数；λ0,λ1,…,λm1为路标点的逆深度值。
根据非线性最小二乘方法，找到状态变量χ的最优估计值，使得***残差和最小。***残差主要有三部分：基于边缘化的先验残差、基于光度误差模型的视觉残差和基于IMU残差模型的惯性残差，构建的最小二乘问题表示为：
其中，χ为里程计的状态变量，χ*为里程计的状态变量的最优估计值，bp为上一次优化计算的先验残差；Λp为上一次优化计算的先验信息矩阵；rB(·)为IMU的惯性残差，
为第t个关键帧和第t+1个关键帧之间IMU观测量,B为滑动窗口内所有关键帧的集合，I(ut+1,t+1)-I(ut,t)为第t+1个关键帧和第t个关键帧之间的灰度差，ut+1表示第t+1个关键帧所有路标点的坐标，ut示第t个关键帧所有路标点的坐标，
为IMU协方差；∑c为VO的权重矩阵。
通过高斯-牛顿(Gauss Newton)迭代法或列文伯格-马夸尔特(Levenberg-Marquardt)迭代法求解构建的最小二乘问题，得到VIO***的位姿最优估计值。
本发明基于稀疏直接法的视觉惯性里程计位姿估计方法的技术方案为：将视觉传感器和惯性传感器安装在移动机器人上，开始定位时，首先给机器人一个较大的旋转和平移，使机器人根据这段时间的观测数据对传感器外参、陀螺仪偏置、当地重力加速度、速度等参数进行初始化。建立4层的图像金字塔，在分辨率最低的顶层图像中心取一块中心模板，作为光度补偿的参考模块，通过中心模板图像灰度值的变化对整个图像的灰度值进行调整，使每一帧的图像亮度平均保持在一定范围内。对于补偿后的图像金字塔，自上而下的寻找相机的位姿最优估计值，计算得到基于视觉的光度残差。把相邻图像帧之间的IMU数据进行预积分，并计算IMU的协方差矩阵，计算得到基于惯性的预积分残差。视觉前端和惯性前端将处理好的测量结果传递给后端进行优化。后端采用基于滑动窗口的非线性最小二乘法，维护一个具有若干个关键帧的滑动窗口。对于每一个图像帧，首先根据其位姿估计判断是否为关键帧，若是关键帧，则将当前帧加入滑动窗口进行非线性优化，同时将窗口内对窗口贡献最小的图像帧边缘化，计算更新先验信息矩阵和先验残差。若不是关键帧，则直接将当前帧移出滑动窗口，维持原先滑动窗口中的状态变量。最后通过迭代的方法求解建立的非线性最小二乘问题，对滑动窗口内的所有状态变量进行优化，得到机器人的实时位姿估计。
上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。
Claims (7)
1.基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，具体包括如下步骤：
步骤一：在载体上安装俯视单目相机VO和惯性传感器IMU；对VO和IMU采集到的数据进行时间同步；
步骤二：对VO和IMU进行联合初始化，并同步VO和IMU的轨迹；
步骤三：对第k+1时刻VO采集到的图像建立N层图像金字塔，由上往下依次对图像金字塔的每一层进行光度补偿，从而更新该第k+1时刻图像；
步骤四：建立滑动窗口，将更新后的第k+1时刻图像放入窗口，根据更新后的第k+1时刻图像，计算在第k+1时刻时VO的位姿，若该位姿与上一个关键帧对应的VO位姿之间的差值大于等于预设的阈值，则计算窗口中第y个关键帧中每个像素点的梯度值，并由大到小排列，选择前300个像素点作为第y个关键帧的路标点，y＝1,2,…,M,M为滑动窗口中关键帧的总个数，根据每一个关键帧中所有路标点的梯度值计算每一个关键帧对滑动窗口的贡献值，将贡献值最小的关键帧边缘化，同时将更新后的第k+1时刻图像作为一个关键帧；基于更新后的第k+1时刻图像与上一个关键帧之间的VO视觉残差、更新后的第k+1时刻图像与上一个关键帧之间的IMU惯性残差以及上一次优化计算得到的先验残差，建立基于窗口的非线性最小二乘函数，并求解该非线性最小二乘函数，得到第k+1时刻里程计位姿的最优估计值，将被边缘化的图像帧对应的测量信息转换为先验信息约束加入到滑动窗口，得到用于下一次优化计算的先验残差，并继续将下一个时刻的图像放入滑动窗口中进行计算；若第k+1时刻的VO位姿与上一个关键帧对应的VO位姿之间的差值小于预设的阈值，则删除第k+1时刻图像，并继续将下一个时刻的图像放入滑动窗口中进行计算；
所述步骤三具体为：
步骤3.1：对第k+1时刻VO采集到的原始图像建立一个N层图像金字塔，该图像金字塔的分辨率由下往上逐渐降低，最底层为分辨率最高的原始图像；
步骤3.2：在该图像金字塔的顶层图像的中心选择一块大小为J×L的矩形区域作为模板，计算该模板的平均灰度值，将该平均灰度值作为第k+1时刻图像金字塔的灰度值，得到第k+1时刻图像金字塔与第k时刻图像金字塔的灰度值的差值，将该差值作为第k+1时刻图像金字塔中每一层图像的光度补偿值，从而对图像金字塔中每一层图像进行更新。
2.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤二中同步VO和IMU的轨迹包括将IMU坐标系转换到VO坐标系下，具体为：
步骤2.1:计算相邻时刻之间IMU测得旋转积分
相邻时刻之间VO测量到的旋转
并建立如下几何约束条件：
其中，
表示四元数乘法，qbc为VO和IMU之间的旋转外参；
步骤2.2：根据qbc，计算得到在VO坐标系下从k时刻到k+1时刻IMU的旋转
为：
3.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中，计算第k+1时刻VO的位姿具体为：将更新后的第k+1时刻图像中每个像素点的梯度值由大到小排列，选择前300个点作为该图像的路标点，根据这些路标点的最小化光度误差计算第k+1时刻VO位姿的李代数，并通过稀疏直接法求解该李代数，得到第k+1时刻VO的位姿。
4.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中，VO视觉残差为第k+1时刻图像中所有路标点灰度值之和与上一个关键帧中所有路标点灰度值之和的差值。
5.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中IMU惯性残差为，对第k+1时刻图像与上一个关键帧之间IMU测得的数据进行预积分，从而得到IMU误差传递函数，利用IMU的误差传递函数处理预积分量中的噪声，从而得到IMU惯性残差。
6.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中采用图优化模型构建非线性最小二乘函数：
其中，χ为里程计的状态变量，χ*为里程计的状态变量的最优估计值，bp为上一次优化计算的先验残差，Λp为上一次优化计算的先验信息矩阵，rB(·)为IMU的惯性残差，
为第g+1个关键帧和第g个关键帧之间IMU的观测量，B为滑动窗口内所有关键帧的集合，I(ug+1,g+1)为第g+1个关键帧中所有路标点的灰度值之和，I(ug,g)为第g个关键帧中所有路标点灰度值之和，ug+1表示第g+1个关键帧中所有路标点的坐标，ug表示第g个关键帧中所有路标点的坐标，
为IMU协方差；∑c为VO的权重矩阵。
7.根据权利要求1所述的基于稀疏直接法的视觉惯性里程计位姿估计方法，其特征在于，所述步骤四中采用高斯-牛顿迭代法或列文伯格-马夸尔特迭代法求解非线性最小二乘函数。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010595397.9A CN111780754B (zh) | 2020-06-23 | 2020-06-23 | 基于稀疏直接法的视觉惯性里程计位姿估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010595397.9A CN111780754B (zh) | 2020-06-23 | 2020-06-23 | 基于稀疏直接法的视觉惯性里程计位姿估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111780754A CN111780754A (zh) | 2020-10-16 |
| CN111780754B true CN111780754B (zh) | 2022-05-20 |
Family
ID=72760906
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010595397.9A Active CN111780754B (zh) | 2020-06-23 | 2020-06-23 | 基于稀疏直接法的视觉惯性里程计位姿估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111780754B (zh) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112325872B (zh) * | 2020-10-27 | 2022-09-30 | 上海懒书智能科技有限公司 | 一种基于多传感器耦合的移动设备的定位方法 |
| CN112556692B (zh) * | 2020-11-27 | 2023-01-31 | 绍兴市北大信息技术科创中心 | 一种基于注意力机制的视觉和惯性里程计方法和*** |
| CN112633122B (zh) * | 2020-12-17 | 2024-01-23 | 厦门大学 | 一种单目vio***的前端里程计算法及*** |
| CN112697142B (zh) * | 2020-12-21 | 2023-03-10 | 南京航空航天大学 | 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 |
| CN112862768B (zh) * | 2021-01-28 | 2022-08-02 | 重庆邮电大学 | 一种基于点线特征的自适应单目vio初始化方法 |
| CN113108771B (zh) * | 2021-03-05 | 2022-08-16 | 华南理工大学 | 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法 |
| CN113155140B (zh) * | 2021-03-31 | 2022-08-02 | 上海交通大学 | 用于室外特征稀疏环境下的机器人slam方法及*** |
| CN113514058A (zh) * | 2021-04-23 | 2021-10-19 | 北京华捷艾米科技有限公司 | 融合msckf和图优化的视觉slam定位方法及装置 |
| CN113450411B (zh) * | 2021-06-30 | 2023-02-28 | 电子科技大学 | 一种基于方差分量估计理论的实时自身位姿计算方法 |
| CN113587916B (zh) * | 2021-07-27 | 2023-10-03 | 北京信息科技大学 | 实时稀疏视觉里程计、导航方法以及*** |
| CN114623817B (zh) * | 2022-02-21 | 2024-04-26 | 武汉大学 | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 |
| CN116758161A (zh) * | 2023-06-26 | 2023-09-15 | 北京道仪数慧科技有限公司 | 移动端空间数据生成方法及空间感知移动端 |
| CN117576218B (zh) * | 2024-01-17 | 2024-03-29 | 南开大学 | 一种自适应的视觉惯导里程计输出方法 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10254118B2 (en) * | 2013-02-21 | 2019-04-09 | Regents Of The University Of Minnesota | Extrinsic parameter calibration of a vision-aided inertial navigation system |
| CN103927745A (zh) * | 2014-03-28 | 2014-07-16 | 北京中海新图科技有限公司 | 面向可穿戴设备的跟踪与匹配并行计算方法 |
| US9658070B2 (en) * | 2014-07-11 | 2017-05-23 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
| CN107687850B (zh) * | 2017-07-26 | 2021-04-23 | 哈尔滨工业大学深圳研究生院 | 一种基于视觉和惯性测量单元的无人飞行器位姿估计方法 |
| CN108492316A (zh) * | 2018-02-13 | 2018-09-04 | 视辰信息科技(上海)有限公司 | 一种终端的定位方法和装置 |
| WO2019191288A1 (en) * | 2018-03-27 | 2019-10-03 | Artisense Corporation | Direct sparse visual-inertial odometry using dynamic marginalization |
| CN109993113B (zh) * | 2019-03-29 | 2023-05-02 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
-
2020
- 2020-06-23 CN CN202010595397.9A patent/CN111780754B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111780754A (zh) | 2020-10-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111780754B (zh) | 基于稀疏直接法的视觉惯性里程计位姿估计方法 | |
| CN109676604B (zh) | 机器人曲面运动定位方法及其运动定位*** | |
| CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
| CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
| CN108242079B (zh) | 一种基于多特征视觉里程计和图优化模型的vslam方法 | |
| CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
| Qin et al. | Relocalization, global optimization and map merging for monocular visual-inertial SLAM | |
| CN111780781B (zh) | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 | |
| CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
| CN109522832B (zh) | 基于点云片段匹配约束和轨迹漂移优化的回环检测方法 | |
| CN112749665A (zh) | 一种基于图像边缘特征的视觉惯性slam方法 | |
| CN110517324A (zh) | 基于变分贝叶斯自适应算法的双目vio实现方法 | |
| CN111156997A (zh) | 一种基于相机内参在线标定的视觉/惯性组合导航方法 | |
| CN111161337A (zh) | 一种动态环境下的陪护机器人同步定位与构图方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN111862316A (zh) | 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法 | |
| CN112683305B (zh) | 一种基于点线特征的视觉-惯性里程计状态估计方法 | |
| CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
| CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN114777775A (zh) | 一种多传感器融合的定位方法及*** | |
| CN108827287B (zh) | 一种复杂环境下的鲁棒视觉slam*** | |
| CN113362377B (zh) | 一种基于单目相机的vo加权优化方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |