一种面向视觉惯性定位的基于地图的一致高效滤波算法制造技术

本发明专利技术公开了一种面向视觉惯性定位的基于地图的一致高效滤波算法，通过以下系统来实现，系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，局部里程计用于接收相机和IMU的数据；地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对。本发明专利技术将维护的里程计相关变量以及局部里程计参考系和事先建好的地图参考系之间的相对变换表示在该李群上，进而得到了一种新的不变卡尔曼滤波算法，保证了算法在不考虑地图不确定性时的能维护系统的正确能观性。性时的能维护系统的正确能观性。性时的能维护系统的正确能观性。

【技术实现步骤摘要】
一种面向视觉惯性定位的基于地图的一致高效滤波算法


[0001]本专利技术涉及机器人定位技术，具体地说，涉及一种面向视觉惯性定位的基于地图的一致高效滤波算法。

技术介绍

[0002]定位技术是智能机器人的一个必不可少的基础模块。近年来，相机和惯性测量单元(IMU)被广泛应用于定位技术中，催生出了一大批优秀的视觉惯性里程计 (VIO)算法。但是VIO的定位精度会随着算法运行时间的推移而不可避免地产生漂移。这就需要通过引入事先建好的地图来消除累积误差。在定位算法融合事先建好的地图信息时，系统需要保证一致、有效地融合这些地图信息，才能有效地使用地图信息来消除里程计的累积误差。
[0003]目前定位算法的框架分为两大类，一类是初始化局部里程计参考系与事先建好的地图参考系之间的相对变换后，将地图参考系下进行实时定位；另一类是同时维护局部的里程计以及局部里程计参考系和事先建好的地图参考系之间的相对变换，从而间接得到在地图参考系下的位置。第一类算法的局限在于对于视觉惯性定位而言，事先建好的地图必须是在惯性系下的，这限制了定位算法的应用。对于第二类算法，虽然没有了第一类算法的限制，但是这类框架存在的潜在问题——可能会破坏了系统的能观性，使原本不可观的信息变得可观，从而引入了虚假的观测信息，降低了定位精度甚至会使算法发散。
[0004]此外，绝大多数算法出于计算效率或者算法框架的限制，没有考虑地图的不确定性，认为地图是绝对准确的。这就会使得定位算法过度信赖不准确的地图，使定位效果大打折扣。即便有一些算法在上述框架中考虑了地图的不确定性，但是他们破坏了系统的能观性，使原本不可观的信息变得可观，从而引入了虚假的观测信息，降低了定位精度甚至会使算法发散。
[0005]如何在考虑地图不确定性的同时，保证系统的正确能观性，从而提升基于地图定位算法的精度和可靠性是需要解决的问题。与此同时，还需要保证系统的计算量保持在较低的水平。

技术实现思路

[0006]为了克服现有技术的不足，本专利技术提出了一种基于滤波的定位算法，基于预先建好的视觉地图的视觉惯性定位技术。所提出的视觉惯性定位技术是基于一种一致高效的滤波算法实现的。本专利技术是通过以下技术方案来实现的：
[0007]本专利技术公开了一种面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，算法通过以下系统来实现，系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后
的局部里程计状态以及局部参考系和地图参考系的相对变换(增广变量)，进而计算得到机器人在地图参考系下的状态。
[0008]作为进一步地改进，本专利技术所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块。
[0009]作为进一步地改进，本专利技术所述的IMU用于为系统提供实时的旋转角速度和线加速度；状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量与协方差；获得的状态变量与协方差信号传递给基于局部特征观测的MSOC
‑
S
‑
IKF 状态更新模块。特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块；基于局部特征观测的MSOC
‑ꢀ
S
‑
IKF状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用该专利技术所提出的不变卡尔曼滤波(Invariant Kalman Filter，IKF)结合多状态约束(MS)来更新状态变量与协方差。
[0010]作为进一步地改进，本专利技术所述的状态变量及其对应的协方差需要包含以下变量：
[0011]1)系统当前时刻t本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度
[0012]2)局部参考系下的特征点
[0013]3)IMU传感器的关于角速度与线加速度的偏差(bias)，构成变量
[0014]4)系统过去s个时刻的本体位姿s为所设定的滑动窗口的大小，位姿由表示朝向的旋转矩阵和表示位置的向量组成。用于基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块；
[0015]5)局部里程计参考系和地图参考系之间的位姿变换
L
T
G
(称为增广变量)，由表示朝向的旋转矩阵
L
R
G
和表示位置的向量
L
p
G
组成，该变量需要在基于地图的定位模块中的利用PnP初始化增广变量模块加入到状态变量中；
[0016]6)地图关键帧的位姿位姿由表示朝向的旋转矩阵和表示位置的向量组成，该变量在基于地图的定位模块中将地图关键帧位姿加入系统状态模块加入到状态变量中。
[0017]作为进一步地改进，本专利技术所述的状态变量1)，2)和5)一同表征在一个新的李群空间上，状态变量4)和6)表征在李群空间SE(3)，状态变量3)表征在欧式向量空间。的具体定义为：
[0018][0019]其中diag(.)表示对角块矩阵，SO(3)的定义为：det(.)表示矩阵的行列式。SE
2+K
(3)的定义为：
[0020][0021]与李群相对应的李代数定义为：
[0022][0023][0024]基于和的定义，状态1)，2)，5)构成的状态表征为：
[0025][0026]相应的状态误差定义为：
[0027][0028][0029][0030][0031][0032][0033]状态3)的误差定义为：
[0034][0035]状态4)对应的位姿的误差定义为：
[0036][0037][0038]状态6)对应的位姿的误差定义为：
[0039][0040][0041]以上各式中的θ表示相应旋转矩阵R的误差，定义为：θ＝log(RR
‑1)，log 将李群SO(3)映射到对应的李代数so(3)。以上各式中的表示估计值。
[0042]作为进一步地改进，本专利技术所述的状态传播模块和基于局部特征观测的 MSOC
‑
S
‑
IKF状态更新模块在求解运动方程或观测方程关于状本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的算法通过以下系统来实现，所述的系统包括三个模块：局部里程计模块，地图特征匹配模块和基于地图的定位模块，所述的局部里程计用于接收相机和IMU的数据，实时获取系统在局部参考系下的状态，获得相应的状态变量的值及其协方差；所述的地图特征匹配用于检测当前时刻相机所观测到的场景与事先建好的地图场景的相似度，获得当前时刻图像特征与地图特征的特征匹配对；所述的基于地图的定位模块用于接收局部里程计的输出量以及特征匹配对，获得更新后的局部里程计状态以及局部参考系和地图参考系的相对变换，进而计算得到机器人在地图参考系下的状态。2.根据权利要求1所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的局部里程计模块包括IMU、与IMU信号连接的状态传播模块、相机、与相机信号连接的特征跟踪模块，与状态传播模块和特征跟踪模块信号连接的基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块。3.根据权利要求2所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的IMU用于为系统提供实时的旋转角速度和线加速度；所述的状态传播模块用于接收IMU提供的旋转角速度和线加速度，并用这些量来将系统的状态从上一时刻传播到当前时刻，获得当前时刻预测的状态变量及其对应的协方差；获得的状态变量与协方差信号传递给基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块；所述的特征跟踪模块用于跟踪上一时刻图像中的特征点在当前时刻图像中的位置，从而获得在当前时刻图像上跟踪到特征点，将获得的特征点信号传递给基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块；所述的基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块用于通过输入的特征点信息，结合输入的预测的状态变量与协方差，通过重投影误差的方式计算观测误差，并利用该发明所提出的不变卡尔曼滤波结合多状态约束来更新状态变量与协方差。4.根据权利要求3所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的状态变量及其对应的协方差包含以下变量：1)系统当前时刻t本体的状态：包括由旋转矩阵和平移向量组成的位姿，线速度2)局部参考系下的特征点3)IMU传感器的关于角速度与线加速度的偏差构成变量4)系统过去s个时刻的本体位姿s为所设定的滑动窗口的大小，位姿由表示朝向的旋转矩阵和表示位置的向量组成。用于基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块；5)局部里程计参考系和地图参考系之间的位姿变换
L
T
G
，由表示朝向的旋转矩阵
L
R
G
和表示位置的向量
L
p
G
组成，该变量需要在基于地图的定位模块中的利用PnP初始化增广变量模块加入到状态变量中；6)地图关键帧的位姿位姿由表示朝向的旋转矩阵
和表示位置的向量组成，该变量在基于地图的定位模块中将地图关键帧位姿加入系统状态模块加入到状态变量中。5.根据权利要求4所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的状态变量1)，2)和5)一同表征在一个新的李群空间上，状态变量4)和6)表征在李群空间SE(3)，状态变量3)表征在欧式向量空间，所述的的具体定义为：为：R
i
∈SO(3)，i＝1，
…
，M}，其中diag(.)表示对角块矩阵，SO(3)的定义为：det(.)表示矩阵的行列式，SE
2+K
(3)的定义为：与李群相对应的李代数定义为：定义为：定义为：基于和的定义，状态1)，2)，5)构成的状态表征为：相应的状态误差定义为：相应的状态误差定义为：相应的状态误差定义为：
状态3)的误差定义为：状态4)对应的位姿的误差定义为：的误差定义为：状态6)对应的位姿的误差定义为：的误差定义为：以上各式中的θ表示相应旋转矩阵R的误差，定义为：θ＝log(RR
‑1)，log将李群SO(3)映射到对应的李代数so(3)。以上各式中的表示估计值；所述的状态传播模块和基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块在求解运动方程或观测方程关于状态变量误差的雅克比时，所述的状态变量误差为以上定义的误差。6.根据权利要求2或3所述的面向视觉惯性定位的基于地图的一致高效滤波算法，其特征在于，所述的基于局部特征观测的MSOC
‑
S
‑
IKF状态更新模块在进行状态更新时是通过下列公式完成的：列公式完成的：列公式完成的：P
t|t
＝P
t|t
‑1‑
K
t
H
t
P
t|t
‑1其中H
t
为观测方程相对于状态误差的雅克比，W
t
为观测噪声的方差，r
t
为观测残差导出的“新息项”exp()为相应李群对应的指数映射，P
t|t
‑1为状态传播模块输出的方差，和P
t|t<...

【专利技术属性】
技术研发人员：王越，张铸青，熊蓉，
申请(专利权)人：浙江大学，
类型：发明
国别省市：