基于迭代误差状态的卡尔曼滤波方法及系统技术方案

本发明专利技术提供一种基于迭代误差状态的卡尔曼滤波方法及系统，属于

【技术实现步骤摘要】
基于迭代误差状态的卡尔曼滤波方法及系统


[0001]本专利技术涉及
SLAM
系统
，尤其涉及一种基于迭代误差状态的卡尔曼滤波方法及系统
。

技术介绍

[0002]随着视觉惯性
SLAM(Simultaneous Localization And Mapping
，
SLAM
，同步定位与建图
)
以及视觉激光
SLAM
系统的发展，很多学者发现
VI
‑
SLAM
算法在特殊环境中的定位精度很难继续突破，因此基于视觉激光惯性
SLAM
系统的多传感器融合成为趋势
。
[0003]对于视觉激光惯性的松耦合算法包括
VLOAM
，
LIMO
，
VIL
‑
SLAM
，
DV
‑
LOAM
和
Super Odometry
等典型算法
。VLOAM
算法中的
VO(Visual Odometry
，视觉里程计
)
通过激光雷达获取一部分视觉特征点的深度并且
VO
的输出位姿变换后作为先验残差，可以完全被
IMU(Inertial Measurement Unit
，惯性测量单元
)
里程计所代替
。VLOAM
算法的核心是以
LOAM<br/>算法为主的激光里程计，带来的优点是对场景要求较低并且可以重建地图
。LIMO
算法框架主要包括特征提取
、
特征预处理
、
帧间运动估计
、
尺度估计
、BA
和回环检测，整体上就是一个完整的
VSLAM
的算法框架，区别较大的地方就是采用激光进行尺度估计
。VIL
‑
SLAM
依然是将惯性
/
视觉作为里程计进行运动估计的作用，激光雷达主要用于构建地图，闭环优化模块中执行视觉闭环检测和回环约束估计，其进一步通过激光雷达得到的稀疏点云配准对齐来增强优化目标
。DV
‑
LOAM
是直接利用了直接
VO
模块的高效率和激光雷达扫描到地图的配准方法的高精度来提升系统的定位性能的
。
特别之处是在后端模块中提出
PGLS
‑
LCD
描述子来提升回环检测的精度
。Super Odometry
提供了一种简单的方高精度多模态传感器融合框架，主要是它结合了松耦合与紧耦合的优点，将
IMU
里程计作为中心来进行运动预测，视觉惯性里程计和激光惯性里程计在约束
IMU
偏差之前提供初始位姿
。
同时该算法建立了一种八叉树的存储方式替代静态
KD
‑
Tree
来管理空间
3D
点
。
但是上述松耦合算法总体来说定位精度和鲁棒性不高
。

技术实现思路

[0004]本专利技术提供一种基于迭代误差状态的卡尔曼滤波方法及系统，用以解决现有技术中
SLAM
系统定位精度和鲁棒性不高的缺陷，提高
SLAM
系统的定位精度和鲁棒性
。
[0005]本专利技术提供一种基于迭代误差状态的卡尔曼滤波方法，包括：
[0006]确定
SLAM
系统中激光雷达估计残差
、IMU
估计误差和视觉投影误差；
[0007]对所述视觉投影误差进行优化，得到视觉投影优化误差；
[0008]基于迭代误差状态的卡尔曼滤波确定先验估计和协方差矩阵；
[0009]基于所述激光雷达估计残差
、IMU
估计误差
、
视觉投影优化误差
、
先验估计和协方差矩阵确定卡尔曼滤波后验估计；
[0010]确定卡尔曼增益；
[0011]根据所述卡尔曼增益更新所述卡尔曼滤波后验估计，直至所述卡尔曼滤波后验估
计收敛，得到
SLAM
系统的状态估计
。
[0012]可选地，确定
SLAM
系统中激光雷达估计残差包括：
[0013]获取
3D
点云中在激光雷达坐标系下的第一特征点集合；
[0014]将所述第一特征点集合转换至全局坐标系，得到第二特征点集合；
[0015]基于所述第二特征点集合进行平面拟合，得到激光雷达估计值；
[0016]基于第一特征点集合确定激光雷达测量值；
[0017]基于所述激光雷达估计值和激光雷达测量值得到激光雷达估计残差
。
[0018]可选地，确定
SLAM
系统中
IMU
估计误差，包括：
[0019]获取
IMU
测量数据和
IMU
估计状态向量；
[0020]根据所述测量数据和估计状态向量确定
IMU
先验估计误差；
[0021]根据所述
IMU
先验估计误差确定
IMU
估计误差
。
[0022]可选地，确定
SLAM
系统中视觉投影误差，包括：
[0023]获取视觉图像的特征点；
[0024]根据所述视觉图像的特征点确定
3D
路标角点；
[0025]根据所述
3D
路标角点确定视觉投影误差
。
[0026]可选地，对所述视觉投影误差进行优化，得到视觉投影优化误差，包括：
[0027]采用平方根
BA
优化算法对所述视觉投影误差进行优化，得到视觉投影优化误差
。
[0028]本专利技术还提供一种基于迭代误差状态的卡尔曼滤波系统，包括：
[0029]误差确定模块，用于确定
SLAM
系统中激光雷达估计残差
、IMU
估计误差和视觉投影误差；
[0030]误差优化模块，用于对所述视觉投影误差进行优化，得到视觉投影优化误差；
[0031]迭代误差状态卡尔曼滤波模块，用于基于迭代误差状态的卡尔曼滤波确定先验估计和协方差矩阵；
[0032]后验估计确定模块，用于基于所述激光雷达估计残差
、IMU
估计误差
、
视觉投影优化误差
、
先验估计和协方差矩阵确定卡尔曼滤波后验估计；
[0033]卡尔曼增益确定模块，用于确定卡尔曼增益；
[0034]状态估计模块，用于根据所述卡尔曼增益更新所述卡尔曼滤波后验估计，直至所述本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种基于迭代误差状态的卡尔曼滤波方法，其特征在于，包括：确定
SLAM
系统中激光雷达估计残差
、IMU
估计误差和视觉投影误差；对所述视觉投影误差进行优化，得到视觉投影优化误差；基于迭代误差状态的卡尔曼滤波确定先验估计和协方差矩阵；基于所述激光雷达估计残差
、IMU
估计误差
、
视觉投影优化误差
、
先验估计和协方差矩阵确定卡尔曼滤波后验估计；确定卡尔曼增益；根据所述卡尔曼增益更新所述卡尔曼滤波后验估计，直至所述卡尔曼滤波后验估计收敛，得到
SLAM
系统的状态估计
。2.
根据权利要求1所述的基于迭代误差状态的卡尔曼滤波方法，其特征在于，确定
SLAM
系统中激光雷达估计残差包括：获取
3D
点云中在激光雷达坐标系下的第一特征点集合；将所述第一特征点集合转换至全局坐标系，得到第二特征点集合；基于所述第二特征点集合进行平面拟合，得到激光雷达估计值；基于第一特征点集合确定激光雷达测量值；基于所述激光雷达估计值和激光雷达测量值得到激光雷达估计残差
。3.
根据权利要求1所述的基于迭代误差状态的卡尔曼滤波方法，其特征在于，确定
SLAM
系统中
IMU
估计误差，包括：获取
IMU
测量数据和
IMU
估计状态向量；根据所述测量数据和估计状态向量确定
IMU
先验估计误差；根据所述
IMU
先验估计误差确定
IMU
估计误差
。4.
根据权利要求1所述的基于迭代误差状态的卡尔曼滤波方法，其特征在于，确定
SLAM
系统中视觉投影误差，包括：获取视觉图像的特征点；根据所述视觉图像的特征点确定
3D
路标角点；根据所述
3D
路标角点确定视觉投影误差
。5.
根据权利要求1‑4任一项所述的基于迭代误差状态的卡尔曼滤波方法，其特征在于，对所述视觉投影误差进行优化，得到视觉投影优化误差，包括：采用平方根
BA
优化算法对所述视觉投影误差进行优化，得到视觉投影优化误差
。6.
一种基于迭代误差状态的卡尔曼滤波系...

【专利技术属性】
技术研发人员：梁思远，
申请(专利权)人：陕西远海探科电子科技有限公司，
类型：发明
国别省市：