CN114001733A - 一种基于地图的一致性高效视觉惯性定位算法 - Google Patents

一种基于地图的一致性高效视觉惯性定位算法 Download PDF

Info

Publication number
CN114001733A
CN114001733A CN202111266643.7A CN202111266643A CN114001733A CN 114001733 A CN114001733 A CN 114001733A CN 202111266643 A CN202111266643 A CN 202111266643A CN 114001733 A CN114001733 A CN 114001733A
Authority
CN
China
Prior art keywords
map
state
module
algorithm
local
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.)
Granted
Application number
CN202111266643.7A
Other languages
English (en)
Other versions
CN114001733B (zh
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202111266643.7A priority Critical patent/CN114001733B/zh
Publication of CN114001733A publication Critical patent/CN114001733A/zh
Application granted granted Critical
Publication of CN114001733B publication Critical patent/CN114001733B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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

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)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于地图的一致性高效视觉惯性定位算法，算法通过以下***来实现，***包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块；本发明能够将地图的不确定性考虑进去，通过实时地估计里程计坐标系和地图坐标系的相对位姿，完成基于地图的一致性定位，通过利用Schmidt滤波，在维护机器人状态与地图信息之间协方差的同时，保证了算法的计算效率，使***的计算量随着地图信息量的增多而仅呈线性增长，为了保证***的正确的能观性，该算法提出了“第一次估计雅克比”(FEJ)算法，提出了误差补偿算法，该算法在低频地图匹配的大场景中非常有必要。

Description

一种基于地图的一致性高效视觉惯性定位算法
技术领域
本发明涉及机器人定位技术，具体地说，涉及一种基于地图的一致性高效视觉惯性定位算法。
背景技术
精确的定位算法对于移动机器人来说是非常重要的。移动机器人的运动导航离不开地图。这就要求***能够实时地获取机器人在地图中的位姿。同时，借助事先建好的地图信息，来约束里程计产生的累积漂移。
目前绝大多数的基于地图的定位算法都是认为地图信息是完美的，没有不确定性的。同时，通过构造代价函数，利用非线性优化的方法来获取定位信息。这类方法的主要缺陷在于，错误地将不完全精准的地图看成的精准的。这就导致***过度信任地图信息，进而使***的估计过于乐观，产生不一致的问题。此外，基于非线性优化的算法对计算资源有较高的要求，这使得算法难以部署在轻量化的平台上。
相比之下，基于滤波的算法可以天然地将地图的不确定度考虑进去。而且，相比于非线性优化的算法，基于滤波的算法对计算资源的要求低，适合部署在轻量化的平台上。但是基于滤波的算法需要将地图信息加入到状态变量中。如果地图的信息过于的庞大，这就会导致滤波算法的状态变量的维度过大，大大提升了计算量。另一方面，如何尽可能地减少地图的数据存储量也是一个需要考虑的问题。
发明内容
为了克服现有技术的不足，本发明公开了一种基于地图的一致性高效视觉惯性定位算法，是通过以下技术方案来实现的：
本发明公开了一种基于地图的一致性高效视觉惯性定位算法，算法通过以下***来实现，***包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，局部里程计用于接收相机和IMU的数据，实时获取***在局部参考系下的状态，获得相应的状态变量的值及其协方差；地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考和地图参考系的相对变换。
作为进一步地改进，本发明所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的观测融合状态更新模块。
作为进一步地改进，本发明所述的IMU用于为***提供实时的旋转角速度和线加速度；状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将***的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给观测模块，特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给观测融合状态更新模块；观测融合状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用扩展卡尔曼滤波(Extended Kalman Filter，EKF)来更新状态变量与协方差。
作为进一步地改进，本发明所述的状态变量及其对应的协方差需要包含以下变量：
1)***当前时刻本体的状态，包括由旋转矩阵和平移向量组成的位姿，线速度；
2)IMU传感器的关于角速度与线加速度的偏差(bias)；
3)***过去N个时刻的本***姿，N为所设定的滑动窗口的大小；
4)局部里程计参考系和地图参考系之间的位姿变换GTL，该变量需要在基于地图的定位模块中的初始化局部与地图参考系之间的相对位姿GTL模块加入到状态变量中；
5)地图中关键帧的位姿，该变量在基于地图的定位模块中将地图关键帧加入状态向量模块加入到状态变量中。
作为进一步地改进，本发明所述的地图特征匹配模块用于通过当前相机与地图的信息，获得与当前相机匹配到的地图特征点以及能观测到这些地图特征点的地图关键帧，即匹配的特征对与地图关键帧，匹配的特征对与地图关键帧信号传递至基于地图的定位模块。
作为进一步地改进，本发明所述的基于地图的定位模块包括以下模块：
初始化局部与地图参考系之间的相对位姿GTL模块，将地图关键帧加入状态向量模块和利用地图信息进行状态更新模块；初始化局部与地图参考系之间的相对位姿GTL模块用于计算GTL，并将其加入到***的状态向量中，获得增广的***状态变量和协方差；将地图关键帧加入状态向量模块用于将地图关键帧的位姿和协方差加入***的状态变量和***的协方差中，从而天然地考虑了地图的不确定度，进而提升***的一致性，通过该模块我们获得了增广的状态变量和协方差；利用地图信息进行状态更新模块在满足1)考虑地图不确定度，2)维护***的正确能观性，3)***的所需的运算量低的同时，利用地图信息更新***状态变量，获得更高精度的状态变量以及***在地图参考系下的位姿。
作为进一步地改进，本发明所述的初始化局部与地图参考系之间的相对位姿GTL模块，通过接收到的与地图匹配的特征对和地图关键帧，利用PnP算法求解地图关键帧和相机当前帧之间的相对位姿，并利用地图关键帧位姿以及当前时刻***的位姿解算出初始化局部与地图参考系之间的相对位姿GTL，完成初始化；地图关键帧加入状态向量模块具体为：当地图帧与当前帧有特征匹配对的时候，且GTL已经初始化，直接将地图帧加到状态向量模块中，当地图帧与当前帧有特征匹配对的时候，且GTL还没有初始化，则通过初始化局部与地图参考系之间的相对位姿GTL模块完成初始化，然后将用于初始化的地图关键帧位姿信号传递到地图关键帧加入状态向量模块。
作为进一步地改进，本发明所述的利用地图信息进行状态更新模块包括四个部分：
a)基于地图的观测方程：将匹配到的地图特征点，分别投影到当前帧，地图关键帧；
在计算重投影误差的时候，需要求解观测方程关于地图点的雅克比矩阵。通过计算该雅克比矩阵的左零空间，将该左零空间乘到观测方程上，从而消去地图点特征对应的误差项，并且隐式地考虑了地图特征点的不确定性；
b)基于Schmidt滤波的状态更新：
利用Schmidt滤波算法来进行状态更新，用于在考虑地图信息不确定性的同时保持滤波算法的计算效率；对于得到的观测方程
Figure BDA0003327190140000041
按照如下步骤进行Schmidt状态更新：
Figure BDA0003327190140000042
Figure BDA0003327190140000043
Figure BDA0003327190140000044
Figure BDA0003327190140000045
Figure BDA0003327190140000046
Figure BDA0003327190140000047
其中，下标k表示当前时刻，下标k|k-1表示状态传播模块输出的值，
Figure BDA0003327190140000048
为观测方程关于***状态向量的雅克比，分为两部分，
Figure BDA0003327190140000049
和
Figure BDA00033271901400000410
分别是与XA和xN相关的雅可比矩阵，其中xA和xN表示需要实时更新的状态部分和地图关键帧部分，Rk为观测噪声协方差，Pk|k-1是状态传播后的状态协方差，根据xA和xN，将其按块划分成：
Figure BDA00033271901400000411
Figure BDA00033271901400000412
为最终更新的机器人的状态，
Figure BDA00033271901400000413
为地图关键帧部分的状态，该部分保持不变；
c)能观性保持：
为了保持正确的能观性，进而保证***的一致性，提出了“第一次估计雅克比”算法(FEJ)用于基于地图的定位，“”一次估计雅克比”算法(FEJ)具体为：
在状态传播计算雅克比矩阵Φk时，利用估计值
Figure BDA0003327190140000051
来计算，而不是用
Figure BDA0003327190140000052
在Schmidt状态更新计算雅克比矩阵
Figure BDA0003327190140000053
时，利用
Figure BDA0003327190140000054
和第一次估计的GTL来计算；
d)误差补偿：
当地图点重投影误差
Figure BDA0003327190140000055
大于一定阈值时，触发误差补偿机制；
通过PnP算法重新计算GTL，替换原本的GTL，将新的***状态记为Xr；观测方程替换为如下等式：
Figure BDA0003327190140000056
其中
Figure BDA0003327190140000057
为用FEJ算法计算的雅克比矩阵。
本发明的有益效果如下：
1)能够将地图的不确定性考虑进去，通过实时地估计里程计坐标系和地图坐标系的相对位姿，完成基于地图的一致性定位；
2)通过利用Schmidt滤波，在维护机器人状态与地图信息之间协方差的同时，保证了算法的计算效率，使***的计算量随着地图信息量的增多而仅呈线性增长；
3)为了保证***的正确的能观性，该算法提出了“第一次估计雅克比”(FEJ)算法；
4)提出了误差补偿算法，该算法在低频地图匹配的大场景中非常有必要。
附图说明
图1是本发明算法的流程示意图；
图2是基于地图的观测示意图。
具体实施方式
本发明公开了一种基于地图的一致性高效视觉惯性定位算法，图1是本发明算法的流程示意图；所述的算法通过以下***来实现，包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，所述的局部里程计用于接收相机和IMU的数据，实时获取***在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；所述的基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考和地图参考系的相对变换。
图2.是基于地图的观测示意图，整个***涉及五类坐标系。
1)局部里程计坐标系L。局部里程计所获得的机器人位姿都是表示在该系下的。
2)惯性测量单元(IMU)/机器人本体坐标系I。IMU提供的角速度和线速度都表示在该系下。下角标k表示时刻k。
3)与IMU固连的相机坐标系C。
4)地图坐标系G。地图信息主要由关键帧和特征点组成。其中关键帧的位姿都表述在地图坐标系下。
5)地图关键帧坐标系KF。
一、***状态向量
在介绍***的各个模块之前，先介绍一下***的状态向量，即***需要实时估计的变量，***的状态向量
Figure BDA0003327190140000061
由两大部分组成，第一部分
xA由三部分组成
Figure BDA0003327190140000062
分别是机器人本体状态
Figure BDA0003327190140000063
N个机器人历史位姿
Figure BDA0003327190140000064
以及局部里程计的坐标系和地图坐标系之间的相对位姿变换，
Figure BDA0003327190140000065
第二部分xN存储着地图M个地图关键帧信息，
Figure BDA0003327190140000066
上述的符号意义分别为：下标k代表任意时刻k。上标
Figure BDA0003327190140000067
代表矩阵的转置。
Figure BDA0003327190140000071
是机器人本体坐标系I与局部里程计坐标系L之间的旋转姿态，表示为四元数q，其对应的旋转矩阵为
Figure BDA0003327190140000072
该旋转矩阵将在L下的三维向量旋转到I下。
Figure BDA0003327190140000073
代表I在L下的位置。
Figure BDA0003327190140000074
代表机器人在L下的线速度。
Figure BDA0003327190140000075
和
Figure BDA0003327190140000076
分别代表惯性测量单元(IMU)的角速度偏差和线加速度偏差。
二、局部里程计
图1是本发明算法的流程示意图，k时刻局部里程计的输入为IMU获取的机器人本体的角速度
Figure BDA0003327190140000077
和线速度
Figure BDA0003327190140000078
以及相机获取的图像。
1)状态传播
k时刻的IMU的数据用于状态传播(预测)，通过IMU的动力学模型，可以得到对于***在k+1时刻的状态预测：
Figure BDA0003327190140000079
其中，下标k+1|k表示通过IMU数据得到的k+1时刻的预测的状态，即状态传播的输出的状态变量，f代表IMU的动力学方程，
Figure BDA00033271901400000710
和
Figure BDA00033271901400000711
是IMU线加速度和角速度的观测噪声，该噪声被构建为零均值的高斯白噪声，通过上式，可以获得对于Xk+1|k的误差状态协方差，即状态传播模块输出的协方差：
Figure BDA00033271901400000712
其中Pk|k和Qk|k分别表示k时刻的状态协方差与噪声协方差。Φk和Gk分别表示(1)式关于***状态Xk和噪声
Figure BDA00033271901400000713
的雅可比矩阵。
2)观测融合状态更新
相机的图像用于观测方程部分。通过追踪图像流的特征点，即图1中特征跟踪模块，可以得到一系列特征点{f}，以及观测到这些特征的一系列克隆的历史位姿{TC}。对于每一个被
Figure BDA00033271901400000714
观测到的特征fi，有如下的观测方程：
Figure BDA00033271901400000715
其中
Figure BDA0003327190140000081
为fi在图像中的二维像素坐标，fi是fi的三维坐标，
Figure BDA0003327190140000082
为图像的观测噪声，h为观测方程，该方程将fi通过位姿
Figure BDA0003327190140000083
投影到图像Cj上。
通过对(3)式在当前估计状态进行一阶泰勒展开，可以得到：
Figure BDA0003327190140000084
Figure BDA0003327190140000085
其中，
Figure BDA0003327190140000086
代表估计的值，
Figure BDA0003327190140000087
代表真值与估计值之间的误差。
Figure BDA0003327190140000088
和
Figure BDA0003327190140000089
分别为(3)式关于估计的状态和观测的特征的雅克比矩阵。通过计算
Figure BDA00033271901400000810
的左零空间N，(4)式可以化为：
Figure BDA00033271901400000811
Figure BDA00033271901400000812
其中
Figure BDA00033271901400000813
通过式(7)，进行EKF状态更新，输出更新后的状态变量和协方差。
三、地图特征匹配
如图1所示，要进行基于地图的定位，需要获得当前相机与地图信息的匹配关系，即地图特征匹配模块。事先构建的地图会包含特征描述子，如BRIEF，ORB，SIFT等。通过这些特征描述子，可以获得与当前相机匹配到的地图特征点以及地图关键帧，即匹配的特征对。这些匹配信息会用于基于地图的定位算法中。
四、基于地图的定位
通过地图特征匹配，可以获取地图帧与当前帧的特征匹配对，图2是基于地图的观测示意图，如图二中的KF1，KF2的黑点与Ck的白点所示，当地图帧与当前帧有特征匹配对的时候，将地图帧加到状态向量中。
1)GTL初始化：如果获得了地图帧与当前帧的特征匹配对，且GTL还没有初始化，通过PnP算法，获得地图帧和当前帧的相对位姿，即图二中
Figure BDA00033271901400000814
进而求得地图坐标系与局部坐标系之间的相对位姿GTL。然后将这个相对位姿GTL加入到状态估计量中。
2)利用地图信息进行状态更新
这部分为本发明的主要创新部分，由四部分组成，核心问题是在考虑地图不确定度的同时，维护***的正确能观性，进而保证***的一致性，同时，要使***的所需的运算量低。
c)基于地图的观测方程：将匹配到的地图特征点
Figure BDA0003327190140000091
分别投影到当前帧C，地图关键帧KF1和KF2：
Figure BDA0003327190140000092
Figure BDA0003327190140000093
Figure BDA0003327190140000094
其中1zf，2zf，3zf分别表示
Figure BDA0003327190140000095
在当前帧C，地图关键帧KF1和KF2的二维像素观测，1g，2g，3g分别为对应的投影方程。1nf，2nf，3nf分别为对应的观测噪声。
将(8)-(10)式在估计值处进行一阶泰勒展开，可以得到如下的观测方程：
Figure BDA0003327190140000096
Figure BDA0003327190140000097
Figure BDA0003327190140000098
其中
Figure BDA0003327190140000099
分别为观测方程关于xA，xN，
Figure BDA00033271901400000910
在其估计值处的雅克比矩阵。
由于不将地图点加到状态向量中，所以需要隐式地将地图点的不确定性考虑进去，从而保证***的一致性。出于节省计算量和地图存储量的角度考虑，不将地图特征点加入到状态向量中，而是将地图的关键帧加入到状态中(地图关键帧的数量要远小于地图特征点的数量)。但是，地图特征点也是有不确定度的，如果不考虑这些点的不确定度，也会使***不一致，从而使估计的状态精度变差。为此，我们需要隐式地考虑地图的不确定度具体做法为，将(11)-(13)式通过矩阵堆叠操作合并成一个等式后，按照与(6)式类似的操作，可以通过计算
Figure BDA0003327190140000101
的左零空间来消去地图特征点
Figure BDA0003327190140000102
对应的误差项。最终得到如下的紧凑的观测方程表达式：
Figure BDA0003327190140000103
其中
Figure BDA0003327190140000104
n*分别为化简后的观测误差，观测方程雅克比矩阵，观测噪声。
d)基于Schmidt滤波的状态更新：
为了在考虑地图信息不确定性的同时保持滤波算法的计算效率，利用Schmidt滤波算法来进行状态更新，为了降低***的计算量，不同于普通的EKF状态更新，引入Schmidt滤波状态更新，使***的计算量与地图的大小成线性关系，同时还考虑了地图和状态之间的互协方差，从而保证了***的一致性。具体而言，利用(14)式，按照如下步骤进行Schmidt状态更新：
Figure BDA0003327190140000105
Figure BDA0003327190140000106
Figure BDA0003327190140000107
Figure BDA0003327190140000108
Figure BDA0003327190140000109
Figure BDA00033271901400001010
其中，
Figure BDA00033271901400001011
和
Figure BDA00033271901400001012
分别是与xA和xN相关的雅可比矩阵。Rk为观测噪声协方差。Pk|k-1是状态传播后的状态协方差，根据xA和xN，可以将其按块划分成：
Figure BDA0003327190140000111
Figure BDA0003327190140000112
为最终更新的机器人的状态。
Figure BDA0003327190140000113
为地图关键帧部分的状态，该部分保持不变。
e)能观性保持：
为了保持正确的能观性，进而保证***的一致性，提出了“第一次估计雅克比”算法(FEJ)用于基于地图的定位，不同于传统的SLAM的能观性，基于地图定位的能观性没有前人分析过，通过分析基于地图定位的能观性，提出了保持***正确能观性，进而保证***一致性的算法FEJ。具体而言，
在状态传播计算雅克比矩阵Φk时，利用估计值
Figure BDA0003327190140000114
来计算，而不是用
Figure BDA0003327190140000115
在Schmidt状态更新计算雅克比矩阵
Figure BDA0003327190140000116
时，利用
Figure BDA0003327190140000117
和第一次估计的GTL来计算。
d)误差补偿：
由于地图并不是每时每刻都能与当前帧产生关联，所以基于地图的定位相比于局部里程计定位来说是一个低频的定位。然而局部里程计是有不可避免的漂移问题的，这就导致了如下问题：当有一段时间没有地图定位约束后，下一次再进行基于地图的定位，有较大的误差，为此该发明提出了误差补偿方法，误差补偿主要为了在提升定位精度的同时，保证***的正确能观性。具体而言，当地图点重投影误差
Figure BDA0003327190140000118
大于一定阈值时，触发误差补偿机制。
通过PnP算法重新计算GTL，替换原本的GTL，将新的***状态记为Xr。
观测方程替换为如下等式
Figure BDA0003327190140000119
其中
Figure BDA00033271901400001110
为用FEJ算法计算的雅克比矩阵。
为了验证该发明的有效性，对比了纯里程计OpenVINS[1],基于优化的算法VINS-Fusion[2][3],同时做了消融实验，来验证该发明提出的每个模块的有效性。该发明涉及的算法记为MVIL。SM表示当前帧与地图单帧匹配上，MM为当前帧与地图多帧匹配上。FEJ为使用FEJ算法。ECU为使用误差补偿算法。实验在三个数据集上进行。MH02-MH05为室内的无人机数据集。Urban39为公路上的自动驾驶数据集，YQ2-YQ4位校园移动机器人数据集。实验结果如下表所示。
Figure BDA0003327190140000121
由于室内无人机数据集场景较小，所以不进行ECU实验。通过上表可以看出来，的算法在用了MM+FEJ+ECU时效果最好。VINS-Fuison在Urban39和YQ2-YQ4都无法正常运行。
显然，本发明不限于以上实施例，还可以有许多变形，本领域的普通技术人员能从本发明公开的内容直接导出或联想到的所有变形，均应认为是本发明的保护范围。

Claims (8)

1.一种基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的算法通过以下***来实现，所述的***包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，所述的局部里程计用于接收相机和IMU的数据，实时获取***在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；所述的基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考和地图参考系的相对变换。
2.根据权利要求1所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的观测融合状态更新模块。
3.根据权利要求2所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的IMU用于为***提供实时的旋转角速度和线加速度；所述的状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将***的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给观测模块，所述的特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给观测融合状态更新模块；所述的观测融合状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用扩展卡尔曼滤波(Extended KalmanFilter，EKF)来更新状态变量与协方差，整个过程与多状态约束卡尔曼滤波(MSCKF)一致。
4.根据权利要求3所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的状态变量及其对应的协方差需要包含以下变量：
1)***当前时刻本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度；
2)IMU传感器的关于角速度与线加速度的偏差(bias)；
3)***过去N个时刻的本***姿，N为所设定的滑动窗口的大小；
4)局部里程计参考系和地图参考系之间的位姿变换GTL，该变量需要在基于地图的定位模块中的初始化局部与地图参考系之间的相对位姿GTL模块加入到状态变量中；
5)地图中关键帧的位姿，该变量在基于地图的定位模块中将地图关键帧加入状态向量模块加入到状态变量中。
5.根据权利要求1或2或3或4所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的地图特征匹配模块用于通过当前相机与地图的信息，获得与当前相机匹配到的地图特征点以及能观测到这些地图特征点的地图关键帧，即匹配的特征对与地图关键帧，所述的匹配的特征对与地图关键帧信号传递至基于地图的定位模块。
6.根据权利要求5所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的基于地图的定位模块包括以下模块：
初始化局部与地图参考系之间的相对位姿GTL模块，将地图关键帧加入状态向量模块和利用地图信息进行状态更新模块；所述的初始化局部与地图参考系之间的相对位姿GTL模块用于计算GTL，并将其加入到***的状态向量中，获得增广的***状态变量和协方差；将地图关键帧加入状态向量模块用于将地图关键帧的位姿和协方差加入***的状态变量和***的协方差中，从而天然地考虑了地图的不确定度，进而提升***的一致性，通过该模块我们获得了增广的状态变量和协方差；所述的利用地图信息进行状态更新模块在满足1)考虑地图不确定度，2)维护***的正确能观性，3)***的所需的运算量低的同时，利用地图信息更新***状态变量，获得更高精度的状态变量以及***在地图参考系下的位姿。
7.根据权利要求6所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的初始化局部与地图参考系之间的相对位姿GTL模块，通过接收到的与地图匹配的特征对和地图关键帧，利用PnP算法求解地图关键帧和相机当前帧之间的相对位姿，并利用地图关键帧位姿以及当前时刻***的位姿解算出初始化局部与地图参考系之间的相对位姿GTL，完成初始化；所述的地图关键帧加入状态向量模块具体为：当地图帧与当前帧有特征匹配对的时候，且GTL已经初始化，直接将地图帧加到状态向量模块中，当地图帧与当前帧有特征匹配对的时候，且GTL还没有初始化，则通过初始化局部与地图参考系之间的相对位姿GTL模块完成初始化，然后将用于初始化的地图关键帧位姿信号传递到地图关键帧加入状态向量模块。
8.根据权利要求6所述的基于地图的一致性高效视觉惯性定位算法，其特征在于，所述的利用地图信息进行状态更新模块包括四个部分：
a)基于地图的观测方程：将匹配到的地图特征点，分别投影到当前帧，地图关键帧；
在计算重投影误差的时候，需要求解观测方程关于地图点的雅克比矩阵。通过计算该雅克比矩阵的左零空间，将该左零空间乘到观测方程上，从而消去地图点特征对应的误差项，并且隐式地考虑了地图特征点的不确定性；
b)基于Schmidt滤波的状态更新：
利用Schmidt滤波算法来进行状态更新，用于在考虑地图信息不确定性的同时保持滤波算法的计算效率；对于得到的观测方程
Figure FDA0003327190130000031
按照如下步骤进行Schmidt状态更新：
Figure FDA0003327190130000032
Figure FDA0003327190130000033
Figure FDA0003327190130000034
Figure FDA0003327190130000035
Figure FDA0003327190130000036
Figure FDA0003327190130000041
其中，下标k表示当前时刻，下标k|k-1表示状态传播模块输出的值，
Figure FDA0003327190130000042
为重投影误差，
Figure FDA0003327190130000043
为观测方程关于***状态向量的雅克比，分为两部分，
Figure FDA0003327190130000044
和
Figure FDA0003327190130000045
分别是与xA和XN相关的雅可比矩阵，其中xA和xN表示需要实时更新的状态部分和地图关键帧部分，Rk为观测噪声n*的协方差，Pk|k-1是状态传播后的状态协方差，根据xA和xN，将其按块划分成：
Figure FDA0003327190130000046
Figure FDA0003327190130000047
为最终更新的机器人的状态，
Figure FDA0003327190130000048
为地图关键帧部分的状态，该部分保持不变，上标^表示估计值；
c)能观性保持：
为了保持正确的能观性，进而保证***的一致性，提出了“第一次估计雅克比”算法(FEJ)用于基于地图的定位，所述的“第一次估计雅克比”算法(FEJ)具体为：
在状态传播计算雅克比矩阵Φk时，利用估计值
Figure FDA0003327190130000049
来计算，而不是用
Figure FDA00033271901300000410
在Schmidt状态更新计算雅克比矩阵
Figure FDA00033271901300000411
时，利用
Figure FDA00033271901300000412
和第一次估计的GTL来计算；
d)误差补偿：
当地图点重投影误差
Figure FDA00033271901300000413
大于一定阈值时，触发误差补偿机制；
通过PnP算法重新计算GTL，替换原本的GTL，将新的***状态记为Xr；观测方程替换为如下等式：
Figure FDA00033271901300000414
其中HxFEJ为用FEJ算法计算的雅克比矩阵。
CN202111266643.7A 2021-10-28 2021-10-28 一种基于地图的一致性高效视觉惯性定位算法 Active CN114001733B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111266643.7A CN114001733B (zh) 2021-10-28 2021-10-28 一种基于地图的一致性高效视觉惯性定位算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111266643.7A CN114001733B (zh) 2021-10-28 2021-10-28 一种基于地图的一致性高效视觉惯性定位算法

Publications (2)

Publication Number Publication Date
CN114001733A true CN114001733A (zh) 2022-02-01
CN114001733B CN114001733B (zh) 2024-03-15

Family

ID=79924774

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111266643.7A Active CN114001733B (zh) 2021-10-28 2021-10-28 一种基于地图的一致性高效视觉惯性定位算法

Country Status (1)

Country Link
CN (1) CN114001733B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114623817A (zh) * 2022-02-21 2022-06-14 武汉大学 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN114964212A (zh) * 2022-06-02 2022-08-30 广东工业大学 面向未知空间探索的多机协同融合定位与建图方法
CN114993338A (zh) * 2022-03-24 2022-09-02 浙江大学 基于多段独立地图序列的一致性高效视觉惯性里程计算法
WO2023207230A1 (zh) * 2022-04-27 2023-11-02 浙江大学 一种面向视觉惯性定位的基于地图的一致高效滤波算法
WO2024037295A1 (zh) * 2022-08-16 2024-02-22 北京三快在线科技有限公司 定位

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190154449A1 (en) * 2016-07-22 2019-05-23 Regents Of The University Of Minnesota Square-root multi-state constraint kalman filter for vision-aided inertial navigation system
US20190178646A1 (en) * 2017-12-08 2019-06-13 Regents Of The University Of Minnesota Square root inverse schmidt-kalman filters for vision-aided inertial navigation and mapping
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111811506A (zh) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及***
CN113532420A (zh) * 2021-06-29 2021-10-22 浙江大学 一种融合点线特征的视觉惯性里程计方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190154449A1 (en) * 2016-07-22 2019-05-23 Regents Of The University Of Minnesota Square-root multi-state constraint kalman filter for vision-aided inertial navigation system
US20190178646A1 (en) * 2017-12-08 2019-06-13 Regents Of The University Of Minnesota Square root inverse schmidt-kalman filters for vision-aided inertial navigation and mapping
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及***
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111811506A (zh) * 2020-09-15 2020-10-23 中国人民解放军国防科技大学 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及***
CN113532420A (zh) * 2021-06-29 2021-10-22 浙江大学 一种融合点线特征的视觉惯性里程计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
董伯麟;柴旭;: "基于IMU/视觉融合的导航定位算法研究", 压电与声光, no. 05 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114623817A (zh) * 2022-02-21 2022-06-14 武汉大学 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
WO2023155258A1 (zh) * 2022-02-21 2023-08-24 武汉大学 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN114623817B (zh) * 2022-02-21 2024-04-26 武汉大学 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN114993338A (zh) * 2022-03-24 2022-09-02 浙江大学 基于多段独立地图序列的一致性高效视觉惯性里程计算法
CN114993338B (zh) * 2022-03-24 2024-03-15 浙江大学 基于多段独立地图序列的一致性高效视觉惯性里程计算法
WO2023207230A1 (zh) * 2022-04-27 2023-11-02 浙江大学 一种面向视觉惯性定位的基于地图的一致高效滤波算法
CN114964212A (zh) * 2022-06-02 2022-08-30 广东工业大学 面向未知空间探索的多机协同融合定位与建图方法
WO2024037295A1 (zh) * 2022-08-16 2024-02-22 北京三快在线科技有限公司 定位

Also Published As

Publication number Publication date
CN114001733B (zh) 2024-03-15

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
US11519729B2 (en) Vision-aided inertial navigation
CN109211241B (zh) 基于视觉slam的无人机自主定位方法
Heo et al. EKF-based visual inertial navigation using sliding window nonlinear optimization
CN110068335B (zh) 一种gps拒止环境下无人机集群实时定位方法及***
Mourikis et al. A multi-state constraint Kalman filter for vision-aided inertial navigation
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及***
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法
Mourikis et al. A dual-layer estimator architecture for long-term localization
US20220051031A1 (en) Moving object tracking method and apparatus
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
WO2019191288A1 (en) Direct sparse visual-inertial odometry using dynamic marginalization
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与***
CN114111818A (zh) 一种通用视觉slam方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN114993338B (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
WO2023207230A1 (zh) 一种面向视觉惯性定位的基于地图的一致高效滤波算法
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN113034538B (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
GR01 Patent grant
GR01 Patent grant