CN114111818A - 一种通用视觉slam方法 - Google Patents

一种通用视觉slam方法 Download PDF

Info

Publication number
CN114111818A
CN114111818A CN202111490931.0A CN202111490931A CN114111818A CN 114111818 A CN114111818 A CN 114111818A CN 202111490931 A CN202111490931 A CN 202111490931A CN 114111818 A CN114111818 A CN 114111818A
Authority
CN
China
Prior art keywords
imu
state
formula
time
attitude information
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.)
Pending
Application number
CN202111490931.0A
Other languages
English (en)
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.)
Taiyuan Water Supply Design And Research Institute Co ltd
Taiyuan University of Technology
Original Assignee
Taiyuan Water Supply Design And Research Institute Co ltd
Taiyuan University of Technology
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 Taiyuan Water Supply Design And Research Institute Co ltd, Taiyuan University of Technology filed Critical Taiyuan Water Supply Design And Research Institute Co ltd
Priority to CN202111490931.0A priority Critical patent/CN114111818A/zh
Publication of CN114111818A publication Critical patent/CN114111818A/zh
Pending legal-status Critical Current

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

Abstract

本发明涉及一种通用视觉SLAM方法，方法如下：首先，双目相机与IMU以紧耦合方式进行数据融合得到姿态信息；其次，当卫星定位系统信号不可用时，以IMU预测姿态信息，作为预测状态；当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态；然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；最后，以新的姿态信息和双目相机生成的图像特征深度图构建地图，完成SLAM算法。本发明提供了一种IMU与卫星定位系统辅助的通用视觉SLAM方法，适于搭载在人类背负系统以及无里程计的机器人系统，在室内外均可使用，满足复杂场景下需求。

Description

一种通用视觉SLAM方法
技术领域
本发明涉及同时定位与地图构建领域。
背景技术
SLAM(Simultaneous localization and mapping，SLAM)同时定位与地图构建是机器人自主导航中的一个非常重要组成部分，它可用于人类背负系统以及机器人系统的导航应用中，是最终实现全自主移动机器人系统的关键技术。视觉SLAM技术是利用相机视觉传感器采集环境图像信息，用于目标定位与识别；因图像所包含的信息丰富，视觉SLAM技术已经成为各种SLAM应用组成部分。
然而，随着应用场景复杂程度的提高，单一视觉传感器在复杂场景下使用会受到诸多限制，需多传感器技术融合来解决SLAM问题。目前，多传感器融合技术，如专利CN113223161A一种基于IMU和轮速计紧耦合的鲁棒全景SLAM系统和方法、专利CN 108665540A基于双目视觉特征和IMU信息的机器人定位与地图构建系统、专利CN 110517324A基于变分贝叶斯自适应算法的双目VIO实现方法；专利CN 110986939A一种基于IMU预积分的视觉惯性里程计方法等，它们或者利用卫星定位系统辅助视觉SLAM方法，或者利用里程计辅助视觉SLAM方法，前者方法在卫星定位系统遮挡时会造成卫星定位系统定位失败；而后者的里程计辅助方法仅适合于拥有里程计的轮式机器人应用中，无法用于诸如旋翼式机器人系统、人类背负系统。因此，需要一种适于搭载在人类背负系统以及机器人系统的通用SLAM方法。
发明内容
本发明解决现有多传感器辅助视觉SLAM融合中，当卫星定位系统定位失效以及无里程计辅助时，提高定位与地图构建精度问题，提供一种可用于人类背负系统以及机器人系统通用SLAM方法与结构。与现有其他的在卫星定位系统失效时依赖于里程计辅助以提高定位与地图构建精度的视觉SLAM方法不同，如该视觉SLAM方法是通过惯性测量单元(IMU)与视觉特征跟踪二者结合进行视觉惯性优化，得到观测状态；当卫星定位系统信号不可用时，仅采用IMU进行状态预测可得到姿态信息，利用此姿态信息与图像特征深度图做地图构建，完成SLAM算法，进而成为一种适于搭载在人类背负系统以及机器人系统的通用SLAM方法。
本发明的技术方案在于。
(一)一种通用视觉SLAM方法，方法如下：
首先，双目相机与IMU以紧耦合方式进行数据融合得到姿态信息；
其次，当卫星定位系统信号不可用时，以IMU预测姿态信息，作为预测状态；
当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态；
然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；
最后，以新的姿态信息和双目相机生成的图像特征深度图构建地图，完成SLAM算法。
(二)一种通用视觉SLAM方法，方法如下：
首先，利用双目相机得到图像数据，进行视觉特征获取与匹配，并构建视觉重投影误差；同时，对惯性测量单元IMU数据进行预积分并构建IMU残差；然后将视觉重投影误差与IMU残差二者结合进行视觉惯性相邻帧紧耦合优化，得到初步测量的姿态信息作为观测状态；其次，当卫星定位系统信号不可用时，仅采用IMU进行状态预测，作为预测状态；
当卫星定位系统信号可用时，卫星定位系统与IMU通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合，通过异常检测步骤来判断得到的数据是否可靠，如果异常则舍弃，否则作为预测状态；
然后，利用EKF通过观测状态去更新预测状态得到新的姿态信息；
最后，利用双目相机生成图像特征深度图，再根据新的姿态信息与图像特征深度图做地图构建，完成SLAM算法。
(三)一种通用视觉SLAM方法，方法如下。
步骤1：首先，利用双目相机得到图像数据，进行视觉特征获取与匹配，并构建视觉重投影误差；同时，对惯性测量单元IMU数据进行预积分并构建IMU残差；然后将视觉重投影误差与IMU残差二者结合进行视觉惯性相邻帧紧耦合优化，得到初步测量的姿态信息作为观测状态；
具体过程为：
首先，通过IMU获取载体的角速度与加速度数据，预积分处理，通过预积分结果，并构建残差函数；双目相机获取图像数据；然后对图像数据进行特征提取与匹配，通过视觉重投影误差构建残差函数；联合构建视觉惯性相邻帧的紧耦合优化残差函数；
定义双目相机测量噪声为Nc，IMU观测噪声为NIMU，二者服从高斯分布；则联合优化残差函数如式1所示
Figure BDA0003399313630000021
式中：εT表示视觉惯性相邻帧的紧耦合优化残差函数；
|| ||2表示2范数的平方；p表示位置变量；q表示姿态变量；v表示速度变量；ba表示IMU的加速度计偏差，bg表示IMU的陀螺仪偏差；
其中，视觉重投影误差Nc部分：
Nc＝ei-ui (2)
式中：ei为双目相机测量模型，ui为齐次像素坐标，
Figure BDA0003399313630000031
表示测量值；
Figure BDA0003399313630000032
式中：K为双目相机内参矩阵，Wi为齐次空间坐标，
Figure BDA0003399313630000033
exp((p,δq)T∧)为双目相机位姿(p,δq)在李群下的指数映射，李群下的指数运算的逆运算即对数运算，用log()表示；si为双目相机深度，∧表示将向量转化为反对称矩阵；
δq为姿态差变量，为姿态变量q的更新量；零初始状态时，δq与q等价；
IMU残差NIMU部分：
i时刻IMU测量值
Figure BDA0003399313630000034
式中：IMU测量出的由第i时刻到第j时刻各变量的变化量分别表示为：
Figure BDA0003399313630000035
表示位置变量p的测量值变化量；
Figure BDA0003399313630000036
表示姿态变量q的测量值变化量；
Figure BDA0003399313630000037
表示速度变量v的测量值变化量；
Figure BDA0003399313630000038
表示第i时刻IMU测量出的加速度偏差，
Figure BDA0003399313630000039
表示第i时刻IMU测量出的陀螺仪偏差；
i时刻IMU估计值为yi，yi通过预积分处理，如下
Figure BDA00033993136300000310
IMU残差为估计值变化量与测量值变化量之差，则IMU残差部分如下所示
Figure BDA0003399313630000041
式中：Δ表示残差，
Figure BDA0003399313630000042
为位置估计值变化量与位置测量值
Figure BDA0003399313630000043
变化量的残差、
Figure BDA0003399313630000044
为姿态估计值变化量与姿态测量值变化量
Figure BDA0003399313630000045
的残差、
Figure BDA0003399313630000046
为速度估计值变化量与速度测量值变化量
Figure BDA0003399313630000047
的残差、δba为加速度偏差的残差、δbg为陀螺仪偏差的残差；
变量角标w表示世界坐标系，变量角标b表示IMU载体坐标系；
Figure BDA00033993136300000414
表示在李群空间中，姿态变量q估计值的变化量运算符；
Figure BDA0003399313630000048
表示将位置变量p或速度变量v，由世界坐标系到第i时刻IMU载体坐标系的映射关系；
Figure BDA0003399313630000049
分别表示将姿势变量q，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure BDA00033993136300000410
分别表示将位置变量p，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure BDA00033993136300000411
分别表示将速度变量v，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
gw表示世界坐标系下的重力加速度g；
Δt表示第i时刻与第j时刻的时间差；
Figure BDA00033993136300000412
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的加速度偏差；
Figure BDA00033993136300000413
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的陀螺仪偏差；
设双目相机与IMU的观测数据条件概率为
P(ei|(p,q))＝NN(ei,Nc) (7)
P(yi|(p，q，v,ba,bg))＝N(yi,NIMU) (8)
其中，N为高斯分布函数；ei为相机的测量模型，P为概率；
用基于贝叶斯概率图推理的因子图Factor Graph优化方法实现双目惯性相邻帧紧耦合优化，则式(1)等价为式(9)的因子图优化解；
Figure BDA0003399313630000051
εT优化结果是得到姿态信息(p,q)。
步骤2：
(1)当卫星定位系统信号不可用时，仅采用IMU进行状态预测，作为预测状态；具体过程为：
由式(1)得到姿态变量q；
姿态变量q与旋转矩阵R的关系式为式(10)
R＝exp(q∧) (10)
位置p、姿态q、速度v、加速度偏差残差δba、陀螺仪偏差残差δbg的预测方程为式(11)(12)(13)(14)(15)；
Figure BDA0003399313630000052
Figure BDA0003399313630000053
Figure BDA0003399313630000054
Figure BDA0003399313630000055
Figure BDA0003399313630000056
式中：ng,na是IMU测量噪声，rg,ra是IMU系统噪声；a是IMU测量的加速度，ω是IMU测量的角速度；
由式(11)至(15)计算得到的IMU预测姿态信息，由式(16)表示
Figure BDA0003399313630000057
(2)当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态的具体过程为：
εL＝εGPS+εT (18)
Figure BDA0003399313630000058
式中：εL表示GPS的误差优化函数εGPS与εT的松耦合联合优化函数；
Zk为k时刻观测量；Xk为k时刻系统状态量；H为测量矩阵；
以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测的姿态信息为
Xk＝AXk-1+Q (20)
式中：Xk-1为k-1时刻系统状态量；A为状态转移矩阵，Q为状态转移高斯噪声；
观测方程
Zk＝HXk+RG (21)
式中：RG是观测高斯噪声；
根据k-1时刻预测k时刻的状态：
Figure BDA0003399313630000061
式中：
Figure BDA0003399313630000062
表示由k-1时刻对k时刻的状态的预测值，
Figure BDA0003399313630000063
表示k-1时刻状态的预测值。
卡尔曼滤波增益K
Figure BDA0003399313630000064
式中：Pk,k-1为k时刻状态预测协方差，通过计算状态X对各变量的雅可比矩阵计算出Pk,k-1；Hk表示第k时刻的测量矩阵，
Figure BDA0003399313630000065
表示第k时刻测量矩阵的转置；
Figure BDA0003399313630000066
是第k时刻的噪声项；
最终k时刻的状态
Figure BDA0003399313630000067
式中：
Figure BDA0003399313630000068
表示最终k时刻的状态，即最终k时刻的预测姿态信息；
由式(20)至式(24)计算得到最终k时刻预测姿态信息，由式(25)表示
Figure BDA0003399313630000069
式中：
Figure BDA00033993136300000610
分别表示第k时刻世界坐标系下位置、姿态、速度、陀螺仪偏差、加速度偏差的预测值。
步骤3：通过异常检测步骤来判断得到的数据是否可靠，如果异常则舍弃，否则作为预测状态；具体过程为：
所述异常检测步骤的过程为：通过下式(17)计算预测残差与观测残差平方和，残差平方和增量指数式上升时，，则说明该时刻值出现异常，需要舍弃，否则作为预测信息输出
Figure BDA00033993136300000611
若预测残差与观测残差平方和正常，则以式(25)输出预测姿态信息；
若预测残差与观测残差平方和异常增大，则以式(16)输出预测姿态信息。
步骤4：利用EKF通过观测状态去更新预测状态得到新的姿态信息；具体过程为：卡尔曼滤波前对时间配准：
定义视觉里程计的相对测量模型为式(27)、式(28)
Figure BDA0003399313630000071
Figure BDA0003399313630000072
式中：Δp∈R3表示位移pvo到当前位移p的增量，Δq∈R3表示姿势qvo到当前姿势q的增量，Rvo表先前状态的旋转矩阵，Rvo＝exp(qvo∧)；
对获得的预测姿态信息进行增广预测，计算出当前的系统状态
Figure BDA0003399313630000073
式中：
Figure BDA0003399313630000074
表示增广系统状态；
将式(27)、式(28)与预测姿态信息通过扩展卡尔曼滤波进行数据融合，得到扩展卡尔曼滤波增益K2
Figure BDA0003399313630000075
式中：
Figure BDA0003399313630000076
表示由k时刻对k+1时刻的增广状态预测协方差；Hk T表示测量矩阵的转置
得到k时刻经扩展卡尔曼滤波耦合之后的包含了位姿信息的新的姿态信息如下
Figure BDA0003399313630000077
式中：
Figure BDA0003399313630000078
是由k时刻经过扩展卡尔曼滤波耦合后，对k+1时刻状态的预测值；
Figure BDA0003399313630000079
是式(27)(28)视觉里程计的相对测量模型的残差。
步骤5：最后，利用双目相机生成图像特征深度图，再根据新的姿态信息与图像特征深度图做地图构建，完成SLAM算法。具体过程为：
由双目相机生成图像特征深度图，并转化为深度数据D；通过式(30)中的新的姿态信息与数据D计算得到占据概率，构建二维占据网格地图；
深度数据D以及从开始到t时刻的增广系统状态
Figure BDA00033993136300000710
计算地图m的后验概率，表示为
Figure BDA00033993136300000711
在世界坐标系下，将增广系统状态
Figure BDA00033993136300000712
合并于深度数据D中，地图m的后验概率可以表示为p(m|D1:t)；
将地图m分成相同尺寸网格单元，设第i个网格单元占据变量为mi，则第i个网格单元被占据概率为p(mi＝1)，第i个单元网格空闲用p(mi＝0)表示，第i个网格单元后验分布为p(mi|D1:t)；当网格单元之间相互独立时，地图m表示为每一个网格单元后验分布的乘积；
Figure BDA0003399313630000081
式中：其中N为地图元个数；
当传感器有新测量结果时，地图中一部分单元网格占据概率会发生变化，则需更新地图；当第i个网格单元传感器测量值Di随时间推移条件独立，采用二进制贝叶斯滤波器解决静态环境占据概率问题；
Figure BDA0003399313630000082
式中：D1:t表示从初始时刻1到时刻t的深度数据，mi表示第i个网格单元占据变量，p(Dt|mi)表示第i个网格单元占据变量为mi时，t时刻深度数据为Dt的概率，其他以此类推。
采用log-odd率来表示占据概率，如下所示
Figure BDA0003399313630000083
则式(23)可写为:
L(mi|D1:t)＝L(mi|Dt)+L(mi|D1:t-1)-L(mi) (34)
其中，L(mi|D1:t-1)表示地图更新后第i个网格单元占据概率，L(mi|Dt)为传感器测量值在t时刻的概率，L(mi|D1:t-1)为前一时刻的占据概率，L(mi)是地图先验信息。
本发明的技术效果在于：
本发明研究室内外两种情况下的定位与地图构建，提供了一种IMU与卫星定位系统辅助的通用视觉SLAM方法，适于搭载在人类背负系统以及无里程计的机器人系统，在室内外均可使用，满足复杂场景下需求。本发明方法计算复杂度低，可实现二维占据网格地图构建，适合资源受限的嵌入式设备使用。
附图说明
图1是本发明的系统框图。
图2是视觉IMU紧耦合子系统示意图。
图3是IMU与卫星定位系统数据融合示意图。
图4是二维网格地图构建流程图。
图5是室内卫星定位系统失效时且里程计无法使用时所构建的网格地图。
图6是室外构建的网格地图。
具体实施方式
步骤1：首先，利用双目相机得到图像数据，进行视觉特征获取与匹配，并构建视觉重投影误差；同时，对惯性测量单元IMU数据进行预积分并构建IMU残差；然后将视觉重投影误差与IMU残差二者结合进行视觉惯性相邻帧紧耦合优化，得到初步测量的姿态信息作为观测状态；
具体过程为：
首先，通过IMU获取载体的角速度与加速度数据，预积分处理，通过预积分结果，并构建残差函数；双目相机获取图像数据；然后对图像数据进行特征提取与匹配，通过视觉重投影误差构建残差函数；联合构建视觉惯性相邻帧的紧耦合优化残差函数；
定义双目相机测量噪声为Nc，IMU观测噪声为NIMU，二者服从高斯分布；则联合优化残差函数如式1所示
Figure BDA0003399313630000091
式中：εT表示视觉惯性相邻帧的紧耦合优化残差函数；
|| ||2表示2范数的平方；p表示位置变量；q表示姿态变量；v表示速度变量；ba表示IMU的加速度计偏差，bg表示IMU的陀螺仪偏差；
其中，视觉重投影误差Nc部分：
Nc＝ei-ui (2)
式中：ei为双目相机测量模型，ui为齐次像素坐标，
Figure BDA0003399313630000092
表示测量值；
Figure BDA0003399313630000093
式中：K为双目相机内参矩阵，Wi为齐次空间坐标，
Figure BDA0003399313630000094
exp((p,δq)T∧)为双目相机位姿(p,δq)在李群下的指数映射，李群下的指数运算的逆运算即对数运算，用log()表示；si为双目相机深度，∧表示将向量转化为反对称矩阵；
δq为姿态差变量，为姿态变量q的更新量；零初始状态时，δq与q等价；
IMU残差NIMU部分：
i时刻IMU测量值
Figure BDA0003399313630000095
式中：IMU测量出的由第i时刻到第j时刻各变量的变化量分别表示为：
Figure BDA0003399313630000096
表示位置变量p的测量值变化量；
Figure BDA0003399313630000101
表示姿态变量q的测量值变化量；
Figure BDA0003399313630000102
表示速度变量v的测量值变化量；
Figure BDA00033993136300001016
表示第i时刻IMU测量出的加速度偏差，
Figure BDA0003399313630000103
表示第i时刻IMU测量出的陀螺仪偏差；
i时刻IMU估计值为yi，yi通过预积分处理，如下
Figure BDA0003399313630000104
IMU残差为估计值变化量与测量值变化量之差，则IMU残差部分如下所示
Figure BDA0003399313630000105
式中：Δ表示残差，
Figure BDA0003399313630000106
为位置估计值变化量与位置测量值
Figure BDA0003399313630000107
变化量的残差、
Figure BDA0003399313630000108
为姿态估计值变化量与姿态测量值变化量
Figure BDA0003399313630000109
的残差、
Figure BDA00033993136300001010
为速度估计值变化量与速度测量值变化量
Figure BDA00033993136300001011
的残差、δba为加速度偏差的残差、δbg为陀螺仪偏差的残差；
变量角标w表示世界坐标系，变量角标b表示IMU载体坐标系；
Figure BDA00033993136300001012
表示在李群空间中，姿态变量q估计值的变化量运算符；
Figure BDA00033993136300001013
表示将位置变量p或速度变量v，由世界坐标系到第i时刻IMU载体坐标系的映射关系；
Figure BDA00033993136300001014
分别表示将姿势变量q，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure BDA00033993136300001015
分别表示将位置变量p，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure BDA0003399313630000111
分别表示将速度变量v，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
gw表示世界坐标系下的重力加速度g；
Δt表示第i时刻与第j时刻的时间差；
Figure BDA0003399313630000112
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的加速度偏差；
Figure BDA0003399313630000113
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的陀螺仪偏差；
设双目相机与IMU的观测数据条件概率为
P(ei|(p,q))＝N(ei,Nc) (7)
P(yi|(p，q，v，ba,bg))＝N(yi,NIMU) (8)
其中，N为高斯分布函数；ei为相机的测量模型，P为概率；
用基于贝叶斯概率图推理的因子图Factor Graph优化方法实现双目惯性相邻帧紧耦合优化，则式(1)等价为式(9)的因子图优化解；
Figure BDA0003399313630000114
εT优化结果是得到姿态信息(p,q)。
步骤2：
(1)当卫星定位系统信号不可用时，仅采用IMU进行状态预测，作为预测状态；具体过程为：
由式(1)得到姿态变量q；
姿态变量q与旋转矩阵R的关系式为式(10)
R＝exp(q∧) (10)
位置p、姿态q、速度v、加速度偏差残差δba、陀螺仪偏差残差δbg的预测方程为式(11)(12)(13)(14)(15)；
Figure BDA0003399313630000115
Figure BDA0003399313630000116
Figure BDA0003399313630000117
Figure BDA0003399313630000118
Figure BDA0003399313630000119
式中：ng，na是IMU测量噪声，rg，ra是IMU系统噪声；a是IMU测量的加速度，ω是IMU测量的角速度；
由式(11)至(15)计算得到的IMU预测姿态信息，由式(16)表示
Figure BDA0003399313630000121
(2)当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态的具体过程为：
εL＝εGPS+εT (18)
Figure BDA0003399313630000122
式中：εL表示GPS的误差优化函数εGPS与εT的松耦合联合优化函数；
Zk为k时刻观测量；Xk为k时刻系统状态量；H为测量矩阵；
以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测的姿态信息为
Xk＝AXk-1+Q (20)
式中：Xk-1为k-1时刻系统状态量；A为状态转移矩阵，Q为状态转移高斯噪声；
观测方程
Zk＝HXk+RG (21)
式中：RG是观测高斯噪声；
根据k-1时刻预测k时刻的状态：
Figure BDA0003399313630000123
式中：
Figure BDA0003399313630000124
表示由k-1时刻对k时刻的状态的预测值，
Figure BDA0003399313630000125
表示k-1时刻状态的预测值。
卡尔曼滤波增益K
Figure BDA0003399313630000126
式中：Pk，k-1为k时刻状态预测协方差，通过计算状态X对各变量的雅可比矩阵计算出Pk,k-1；Hk表示第k时刻的测量矩阵，
Figure BDA0003399313630000127
表示第k时刻测量矩阵的转置；
Figure BDA00033993136300001212
是第k时刻的噪声项；
最终k时刻的状态
Figure BDA0003399313630000128
式中：
Figure BDA0003399313630000129
表示最终k时刻的状态，即最终k时刻的预测姿态信息；
由式(20)至式(24)计算得到最终k时刻预测姿态信息，由式(25)表示
Figure BDA00033993136300001210
式中：
Figure BDA00033993136300001211
分别表示第k时刻世界坐标系下位置、姿态、速度、陀螺仪偏差、加速度偏差的预测值。
步骤3：通过异常检测步骤来判断得到的数据是否可靠，如果异常则舍弃，否则作为预测状态；具体过程为：
所述异常检测步骤的过程为：通过下式(17)计算预测残差与观测残差平方和，残差平方和增量指数式上升时，，则说明该时刻值出现异常，需要舍弃，否则作为预测信息输出
Figure BDA0003399313630000131
若预测残差与观测残差平方和正常，则以式(25)输出预测姿态信息；
若预测残差与观测残差平方和异常增大，则以式(16)输出预测姿态信息。
步骤4：利用EKF通过观测状态去更新预测状态得到新的姿态信息；具体过程为：卡尔曼滤波前对时间配准：
定义视觉里程计的相对测量模型为式(27)、式(28)
Figure BDA0003399313630000132
Figure BDA0003399313630000133
式中：Δp∈R3表示位移pvo到当前位移p的增量，Δq∈R3表示姿势qvo到当前姿势q的增量，Rvo表先前状态的旋转矩阵，Rvo＝exp(qvo∧)；
对获得的预测姿态信息进行增广预测，计算出当前的系统状态
Figure BDA0003399313630000134
式中：
Figure BDA0003399313630000135
表示增广系统状态；
将式(27)、式(28)与预测姿态信息通过扩展卡尔曼滤波进行数据融合，得到扩展卡尔曼滤波增益K2
Figure BDA0003399313630000136
式中：
Figure BDA0003399313630000137
表示由k时刻对k+1时刻的增广状态预测协方差；Hk T表示测量矩阵的转置
得到k时刻经扩展卡尔曼滤波耦合之后的包含了位姿信息的新的姿态信息如下
Figure BDA0003399313630000138
式中：
Figure BDA0003399313630000139
是由k时刻经过扩展卡尔曼滤波耦合后，对k+1时刻状态的预测值；
Figure BDA00033993136300001310
是式(27)(28)视觉里程计的相对测量模型的残差。
步骤5：最后，利用双目相机生成图像特征深度图，再根据新的姿态信息与图像特征深度图做地图构建，完成SLAM算法。具体过程为：
由双目相机生成图像特征深度图，并转化为深度数据D；通过式(30)中的新的姿态信息与数据D计算得到占据概率，构建二维占据网格地图；
深度数据D以及从开始到t时刻的增广系统状态
Figure BDA0003399313630000141
计算地图m的后验概率，表示为
Figure BDA0003399313630000142
在世界坐标系下，将增广系统状态
Figure BDA0003399313630000143
合并于深度数据D中，地图m的后验概率可以表示为p(m|D1:t)；
将地图m分成相同尺寸网格单元，设第i个网格单元占据变量为mi，则第i个网格单元被占据概率为p(mi＝1)，第i个单元网格空闲用p(mi＝0)表示，第i个网格单元后验分布为p(mo|D1:t)；当网格单元之间相互独立时，地图m表示为每一个网格单元后验分布的乘积；
Figure BDA0003399313630000144
式中：其中N为地图元个数；
当传感器有新测量结果时，地图中一部分单元网格占据概率会发生变化，则需更新地图；当第i个网格单元传感器测量值Di随时间推移条件独立，采用二进制贝叶斯滤波器解决静态环境占据概率问题；
Figure BDA0003399313630000145
式中：D1:t表示从初始时刻1到时刻t的深度数据，mi表示第i个网格单元占据变量，p(Dt|mi)表示第i个网格单元占据变量为mi时，t时刻深度数据为Dt的概率，其他以此类推。
采用log-odd率来表示占据概率，如下所示
Figure BDA0003399313630000146
则式(23)可写为：
L(mi|D1:t)＝L(mi|Dt)+L(mi|D1:t-1)-L(mi) (34)
其中，L(mi|D1:t-1)表示地图更新后第i个网格单元占据概率，L(mi|Dt)为传感器测量值在t时刻的概率，L(mi|D1:t-1)为前一时刻的占据概率，L(mi)是地图先验信息。
图4显示了得到结果式(26)--位姿信息与深度信息构建网格地图的过程。由双目相机生成深度图像，通过深度图像转换得到深度数据D。通过式(26)
Figure BDA0003399313630000147
中的位姿(p,q)与数据D计算得到占据概率，由此构建二维网格地图。
如图5与图6所示是室内外构建的网格地图示例。其中黑色代表障碍。白色即为空闲区域。示例所用双目相机深度图的测量范围为0.8m至5m，室内地图构建时当障碍物距离障碍物比较近的时候，会导致地图产生一定误差。从真实环境室内、外地图构建效果与误差分析看，本发明环境地图构建满足实际需求，如表1对比了本文方法与最具代表性的开源实时地图构建方案RTAB-MAP，测量值和误差值在线段A、B、C处均优于开源方案。
Figure BDA0003399313630000151

Claims (8)

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所示
Figure FDA0003399313620000011
式中：εT表示视觉惯性相邻帧的紧耦合优化残差函数；
|| ||2表示2范数的平方；p表示位置变量；q表示姿态变量；v表示速度变量；ba表示IMU的加速度计偏差，bg表示IMU的陀螺仪偏差；
其中，视觉重投影误差Nc部分：
Nc＝ei-ui (2)
式中：ei为双目相机测量模型，ui为齐次像素坐标，
Figure FDA0003399313620000021
表示测量值；
Figure FDA0003399313620000022
式中：K为双目相机内参矩阵，Wi为齐次空间坐标，
Figure FDA0003399313620000023
exp((p，δq)T∧)为双目相机位姿(p，δq)在李群下的指数映射，李群下的指数运算的逆运算即对数运算，用log()表示；si为双目相机深度，∧表示将向量转化为反对称矩阵；
δq为姿态差变量，为姿态变量q的更新量；零初始状态时，δq与q等价；
IMU残差NIMU部分：
i时刻IMU测量值
Figure FDA0003399313620000024
式中：IMU测量出的由第i时刻到第j时刻各变量的变化量分别表示为：
Figure FDA0003399313620000025
表示位置变量p的测量值变化量；
Figure FDA0003399313620000026
表示姿态变量q的测量值变化量；
Figure FDA0003399313620000027
表示速度变量v的测量值变化量；
Figure FDA0003399313620000028
表示第i时刻IMU测量出的加速度偏差，
Figure FDA0003399313620000029
表示第i时刻IMU测量出的陀螺仪偏差；
i时刻IMU估计值为yi，yi通过预积分处理，如下
Figure FDA00033993136200000210
IMU残差为估计值变化量与测量值变化量之差，则IMU残差部分如下所示
Figure FDA00033993136200000211
Figure FDA0003399313620000031
式中：Δ表示残差，
Figure FDA0003399313620000032
为位置估计值变化量与位置测量值
Figure FDA0003399313620000033
变化量的残差、
Figure FDA0003399313620000034
为姿态估计值变化量与姿态测量值变化量
Figure FDA0003399313620000035
的残差、
Figure FDA0003399313620000036
为速度估计值变化量与速度测量值变化量
Figure FDA0003399313620000037
的残差、δba为加速度偏差的残差、δbg为陀螺仪偏差的残差；
变量角标w表示世界坐标系，变量角标b表示IMU载体坐标系；
Figure FDA0003399313620000038
表示在李群空间中，姿态变量q估计值的变化量运算符；
Figure FDA0003399313620000039
表示将位置变量p或速度变量v，由世界坐标系到第i时刻IMU载体坐标系的映射关系；
Figure FDA00033993136200000310
分别表示将姿势变量q，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure FDA00033993136200000311
分别表示将位置变量p，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
Figure FDA00033993136200000312
分别表示将速度变量v，在第i时刻、第j时刻，由IMU载体坐标系变换到世界坐标系的映射关系；
gw表示世界坐标系下的重力加速度g；
Δt表示第i时刻与第j时刻的时间差；
Figure FDA00033993136200000313
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的加速度偏差；
Figure FDA00033993136200000314
分别表示在IMU载体坐标系中对应第i时刻、第j时刻的陀螺仪偏差；
设双目相机与IMU的观测数据条件概率为
P(ei|(p，q))＝N(ei，Nc) (7)
P(yi|(p，q，v，ba，bg))＝N(yi，NIMU) (8)
其中，N为高斯分布函数；ei为相机的测量模型，P为概率；
用基于贝叶斯概率图推理的因子图Factor Graph优化方法实现双目惯性相邻帧紧耦合优化，则式(1)等价为式(9)的因子图优化解；
Figure FDA00033993136200000315
εT优化结果是得到姿态信息(p，q)。
4.根据权利要求2所述通用视觉SLAM方法，其特征在于：所述当卫星定位系统信号不可用时，以IMU预测姿态信息，作为预测状态的具体过程为：
由式(1)得到姿态变量q；
姿态变量q与旋转矩阵R的关系式为式(10)
R＝exp(q∧) (10)
位置p、姿态q、速度v、加速度偏差残差δba、陀螺仪偏差残差δbg的预测方程为式(11)(12)(13)(14)(15)；
Figure FDA0003399313620000041
Figure FDA0003399313620000042
Figure FDA0003399313620000043
Figure FDA0003399313620000044
Figure FDA0003399313620000045
式中：ng，na是IMU测量噪声，rg，ra是IMU系统噪声；a是IMU测量的加速度，ω是IMU测量的角速度；
由式(11)至(15)计算得到的IMU预测姿态信息，由式(16)表示
Figure FDA0003399313620000046
5.根据权利要求2所述通用视觉SLAM方法，其特征在于：当卫星定位系统信号可用时，以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测姿态信息，作为预测状态的具体过程为：
εL＝εGPS+εT (18)
Figure FDA0003399313620000047
式中：εL表示GPS的误差优化函数εGPS与εT的松耦合联合优化函数；
Zk为k时刻观测量；Xk为k时刻系统状态量；H为测量矩阵；
以IMU和卫星定位系统通过扩展卡尔曼滤波EKF方法以松耦合方式进行数据融合后再预测的姿态信息为
Xk＝AXk-1+Q (20)
式中：Xk-1为k-1时刻系统状态量；A为状态转移矩阵，Q为状态转移高斯噪声；
观测方程
Zk＝HXk+RG (21)
式中：RG是观测高斯噪声；
根据k-1时刻预测k时刻的状态：
Figure FDA0003399313620000051
式中：
Figure FDA0003399313620000052
表示由k-1时刻对k时刻的状态的预测值，
Figure FDA0003399313620000053
表示k-1时刻状态的预测值。
卡尔曼滤波增益K
Figure FDA0003399313620000054
式中：Pk，k-1为k时刻状态预测协方差，通过计算状态X对各变量的雅可比矩阵计算出Pk，k-1；
Hk表示第k时刻的测量矩阵，
Figure FDA0003399313620000055
表示第k时刻测量矩阵的转置；
Figure FDA0003399313620000056
是第k时刻的噪声项；
最终k时刻的状态
Figure FDA0003399313620000057
式中：
Figure FDA0003399313620000058
表示最终k时刻的状态，即最终k时刻的预测姿态信息；
由式(20)至式(24)计算得到最终k时刻预测姿态信息，由式(25)表示
Figure FDA0003399313620000059
式中：
Figure FDA00033993136200000510
分别表示第k时刻世界坐标系下位置、姿态、速度、陀螺仪偏差、加速度偏差的预测值。
6.根据权利要求5所述通用视觉SLAM方法，其特征在于：所述异常检测步骤的过程为：通过下式(17)计算预测残差与观测残差平方和，残差平方和增量呈指数式上升时，则说明该时刻值出现异常，需要舍弃，否则作为预测姿态信息输出
Figure FDA00033993136200000511
若预测残差与观测残差平方和正常，则以式(25)输出预测姿态信息；
若预测残差与观测残差平方和异常增大，则以式(16)输出预测姿态信息。
7.根据权利要求6所述通用视觉SLAM方法，其特征在于：所述利用EKF通过观测状态去更新预测状态得到新的姿态信息的具体过程为：
卡尔曼滤波前对时间配准：
定义视觉里程计的相对测量模型为式(27)、式(28)
Figure FDA00033993136200000512
Figure FDA00033993136200000513
式中：Δp∈R3表示位移pvo到当前位移p的增量，Δq∈R3表示姿势qvo到当前姿势q的增量，Rvo表先前状态的旋转矩阵，Rvo＝exp(qvo∧)；
对获得的预测姿态信息进行增广预测，计算出当前的系统状态
Figure FDA0003399313620000061
式中：
Figure FDA0003399313620000062
表示增广系统状态；
将式(27)、式(28)与预测姿态信息通过扩展卡尔曼滤波进行数据融合，得到扩展卡尔曼滤波增益K2
Figure FDA0003399313620000063
式中：
Figure FDA0003399313620000064
表示由k时刻对k+1时刻的增广状态预测协方差；Hk T表示测量矩阵的转置得到k时刻经扩展卡尔曼滤波耦合之后的包含了位姿信息的新的姿态信息如下
Figure FDA0003399313620000065
式中：
Figure FDA0003399313620000066
是由k时刻经过扩展卡尔曼滤波耦合后，对k+1时刻状态的预测值。
Figure FDA00033993136200000613
是式(27)(28)视觉里程计的相对测量模型的残差。
8.根据权利要求5所述通用视觉SLAM方法，其特征在于：所述以新的姿态信息和双目相机生成的图像特征深度图构建地图的具体步骤为：
由双目相机生成图像特征深度图，并转化为深度数据D；通过式(30)中的新的姿态信息与数据D计算得到占据概率，构建二维占据网格地图；
深度数据D以及从开始到t时刻的增广系统状态
Figure FDA0003399313620000067
计算地图m的后验概率，表示为
Figure FDA0003399313620000068
在世界坐标系下，将增广系统状态
Figure FDA0003399313620000069
合并于深度数据D中，地图m的后验概率可以表示为p(m|D1：t)；
将地图m分成相同尺寸网格单元，设第i个网格单元占据变量为mi，则第i个网格单元被占据概率为p(mi＝1)，第i个单元网格空闲用p(mi＝0)表示，第i个网格单元后验分布为p(mi|D1：t)；当网格单元之间相互独立时，地图m表示为每一个网格单元后验分布的乘积；
Figure FDA00033993136200000610
式中：其中N为地图元个数；
当传感器有新测量结果时，地图中一部分单元网格占据概率会发生变化，则需更新地图；当第i个网格单元传感器测量值Di随时间推移条件独立，采用二进制贝叶斯滤波器解决静态环境占据概率问题；
Figure FDA00033993136200000611
式中：D1：t表示从初始时刻1到时刻t的深度数据，mi表示第i个网格单元占据变量，p(Dt|mi)表示第i个网格单元占据变量为mi时，t时刻深度数据为Dt的概率，其他以此类推。
采用log-odd率来表示占据概率，如下所示
Figure FDA00033993136200000612
则式(23)可写为：
L(mi|D1：t)＝L(mi|Dt)+L(mi|D1：t-1)-L(mi) (34)
其中，L(mi|D1：t-1)表示地图更新后第i个网格单元占据概率，L(mi|Dt)为传感器测量值在t时刻的概率，L(mi|D1：t-1)为前一时刻的占据概率，L(mi)是地图先验信息。
CN202111490931.0A 2021-12-08 2021-12-08 一种通用视觉slam方法 Pending CN114111818A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111490931.0A CN114111818A (zh) 2021-12-08 2021-12-08 一种通用视觉slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111490931.0A CN114111818A (zh) 2021-12-08 2021-12-08 一种通用视觉slam方法

Publications (1)

Publication Number Publication Date
CN114111818A true CN114111818A (zh) 2022-03-01

Family

ID=80367492

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111490931.0A Pending CN114111818A (zh) 2021-12-08 2021-12-08 一种通用视觉slam方法

Country Status (1)

Country Link
CN (1) CN114111818A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116659510A (zh) * 2023-06-02 2023-08-29 海南大学 水下机器人定位与避障方法、装置及存储介质
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
WO2024045632A1 (zh) * 2022-08-31 2024-03-07 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备
CN117686158A (zh) * 2024-02-04 2024-03-12 太原供水设计研究院有限公司 一种水管检漏装置

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024045632A1 (zh) * 2022-08-31 2024-03-07 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备
CN116659510A (zh) * 2023-06-02 2023-08-29 海南大学 水下机器人定位与避障方法、装置及存储介质
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN117686158A (zh) * 2024-02-04 2024-03-12 太原供水设计研究院有限公司 一种水管检漏装置
CN117686158B (zh) * 2024-02-04 2024-04-09 太原供水设计研究院有限公司 一种水管检漏装置

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
US20240011776A9 (en) Vision-aided inertial navigation
CN114111818A (zh) 一种通用视觉slam方法
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN106780699B (zh) 一种基于sins/gps和里程计辅助的视觉slam方法
CN111220153B (zh) 基于视觉拓扑节点和惯性导航的定位方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Zhu et al. Multisensor fusion using fuzzy inference system for a visual-IMU-wheel odometry
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
Zhao et al. Vision-based tracking control of quadrotor with backstepping sliding mode control
CN113776519B (zh) 一种无光动态开放环境下agv车辆建图与自主导航避障方法
CN113220818B (zh) 一种停车场自动建图与高精度定位方法
Meier et al. Visual‐inertial curve simultaneous localization and mapping: Creating a sparse structured world without feature points
Yousuf et al. Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications
CN114897988B (zh) 一种铰链式车辆中的多相机定位方法、装置及设备
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN112652001B (zh) 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统
Cristofalo et al. Vision-based control for fast 3-d reconstruction with an aerial robot
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN114993338A (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
Zhao et al. 3D indoor map building with monte carlo localization in 2D map
Stoven-Dubois et al. Graph optimization methods for large-scale crowdsourced mapping
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