一种通用视觉SLAM方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114111818A(43)申请公布日2022.03.01(21)申请号202111490931.0(22)申请日2021.12.08(71)申请人太原供水设计研究院有限公司地址030000山西省太原市杏花岭区北大街109号5层501、502申请人太原理工大学(72)发明人付世沫常青王耀力(74)专利代理机构西安亿诺专利代理有限公司61220代理人贺珊(51)Int.Cl.G01C21/32(2006.01)权利要求书6页说明书13页附图3页(54)发明名称一种通用视觉SLAM方法(57)摘要本发明涉及一种通用视觉SLAM方法，方法如下：首先，双目相机与IMU以紧耦合方式进行数据融合得到姿态信息；其次，当卫星定位系统信号不可用时，以IMU预测姿态信息，作为预测状态；当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态；然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；最后，以新的姿态信息和双目相机生成的图像特征深度图构建地图，完成SLAM算法。本发明提供了一种IMU与卫星定位系统辅助的通用视觉SLAM方法，适于搭载在人类背负系统以及无里程计的机器人系统，在室内外均可使用，满足复杂场景下需求。CN114111818ACN114111818A权利要求书1/6页1.一种通用视觉SLAM方法，其特征在于：方法如下：首先，双目相机与IMU以紧耦合方式进行数据融合得到姿态信息；其次，当卫星定位系统信号不可用时，以IMU预测姿态信息，作为预测状态；当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态；然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；最后，以新的姿态信息和双目相机生成的图像特征深度图构建地图，完成SLAM算法。2.根据权利要求1所述通用视觉SLAM方法，其特征在于：方法如下：首先，利用双目相机得到图像数据，进行视觉特征获取与匹配，并构建视觉重投影误差；同时，对惯性测量单元IMU数据进行预积分并构建IMU残差；然后将视觉重投影误差与IMU残差二者结合进行视觉惯性相邻帧紧耦合优化，得到初步测量的姿态信息作为观测状态；其次，当卫星定位系统信号不可用时，仅采用IMU进行状态预测，作为预测状态；当卫星定位系统信号可用时，卫星定位系统与IMU通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合，通过异常检测步骤来判断得到的数据是否可靠，如果异常则舍弃，否则作为预测状态；然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；最后，利用双目相机生成图像特征深度图，再根据新的姿态信息与图像特征深度图做地图构建，完成SLAM算法。3.根据权利要求2所述通用视觉SLAM方法，其特征在于：所述双目相机与IMU以紧耦合方式进行数据融合的具体过程为：首先，通过IMU获取载体的角速度与加速度数据，预积分处理，通过预积分结果，并构建残差函数；双目相机获取图像数据；然后对图像数据进行特征提取与匹配，通过视觉重投影误差构建残差函数；联合构建视觉惯性相邻帧的紧耦合优化残差函数；定义双目相机测量噪声为Nc，IMU观测噪声为NIMU，二者服从高斯分布；则联合优化残差函数如式1所示式中：εT表示视觉惯性相邻帧的紧耦合优化残差函数；||||2表示2范数的平方；p表示位置变量；q表示姿态变量；v表示速度变量；ba表示IMU的加速度计偏差，bg表示IMU的陀螺仪偏差；其中，视觉重投影误差Nc部分：Nc＝ei‑ui(2)式中：ei为双目相机测量模型，ui为齐次像素坐标，表示测量值；2CN114111818A权利要求书2/6页T式中：K为双目相机内参矩阵，Wi为齐次空间坐标，exp((p，δq)∧)为双目相机位姿(p，δq)在李群下的指数映射，李群下的指数运算的逆运算即对数运算，用log()表示；si为双目相机深度，∧表示将向量转化为反对称矩阵；δq为姿态差变量，为姿态变量q的更新量；零初始状态时，δq与q等价；IMU残差NIMU部分：i时刻IMU测量值式中：IMU测量出的由第i时刻到第j时刻各变量的变化量分别表示为：表示位置变量p的测量值变化量；表示姿态变量q的测量值变化量；表示速度变量v的测量值变化量；表示第i时刻IMU测量出的加速度偏差，表示第i时刻IMU测量出的陀螺仪偏差；i时刻IMU估计值为yi，yi通过预积分处理，如下IMU残差为估计值变化量与测量值变化量之差，则IMU残差部分如下所示式中：Δ表示残差，为位置估计值变化量与位置测量值变化量的残差、为姿态估计值变化量与姿态测量值变化量的残差、为速度估计值变化量与速