一种鲁棒立体视觉惯性预积分SLAM方法技术

本发明专利技术公开了一种鲁棒立体视觉惯性预积分SLAM方法,属于视觉导航技术领域。本方法利用惯性传感器测量值进行预积分预测载体导航信息，并与立体视觉传感器的数据进行融合判断图像匹配质量，并根据判断结果融合预积分信息输出不同的结果，且可以对构出的地图进行复用，消除累积误差。本发明专利技术能够有效解决不良环境中视觉SLAM匹配错误与失败的问题，获得高鲁棒性以及高精度的载体导航信息，并可以构建精准地图进行复用，具有良好的推广前景。

A Robust Inertial Preintegration SLAM Method for Stereo Vision

【技术实现步骤摘要】
一种鲁棒立体视觉惯性预积分SLAM方法
本专利技术涉及一种鲁棒立体视觉惯性预积分SLAM方法,属于视觉导航

技术介绍
实时定位与构图(SLAM，SimultaneousLocalizationandMapping)技术是机器人自主导航
中的一大研究热点，也是机器人在工程实践应用中必要的关键技术。视觉传感器可以感知外界的光线，无需自身进行信号发射，具有高自主性，视觉SLAM在机器人SLAM中具有广阔的发展前景与较大的潜质。目前大多视觉SLAM都对外界环境具有较高的要求，需要利用环境信息进行视觉解算，并且在整个SLAM过程中，环境需要一直保持一个可控的状态，否则会导致视觉解算失败进而产生导航信息输出失败而对机器人造成危害。而现实环境中往往存在着明暗变化以及运动造成的成像噪声。机器人SLAM方法中，对于外界环境变化十分敏感，其变化会对直接导致精度下降，造成较大的误差甚至系统崩溃。
技术实现思路
本专利技术提出了一种鲁棒立体视觉惯性预积分SLAM方法,能够在复杂环境下获得高鲁棒性的视觉SLAM系统。本专利技术为解决其技术问题采用如下技术方案：一种鲁棒立体视觉惯性预积分SLAM方法，包括如下步骤：步骤1：周期采集i时刻加速度计数据陀螺仪数据和立体视觉传感器数据S(i)；步骤2：利用惯性传感器测量数据和预积分，预测i时刻载体的导航信息得到预积分预测值；步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量gb(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据S(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；步骤5：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载体位姿；步骤6：输出载体导航信息，更新地图，并跳转至步骤1。步骤2中所述利用惯性传感器测量数据和预积分的具体过程如下：i时刻得到惯性传感器加计输出与陀螺仪输出其中包含从i-1时刻到i时刻的惯性传感器加计测量数据和陀螺仪量测数据k∈(t(i-1),(t(i)-t(i-1))/Δt)，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点，Δt为离散周期；测量模型为：ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，为k时刻导航坐标系到机体坐标系的旋转矩阵，fn(k)为k时刻的加速度计量测在导航系中的真实值，为k时刻的加速度计量测的真实值；对ba(k)与bg(k)有：bg(k)＝ηbgηba为加速度计白噪声，ηbg为陀螺仪白噪声；预积分的过程为：ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，为i-1时刻从导航系至机体系的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，Δti-1,i为从i-1时刻到i时刻的时间周期，ΔRi-1,k为从i-1时刻到k时刻的旋转矩阵，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置，整体预积分的过程将于i时刻的全局位姿无关。步骤4中预积分预测值辅助图像匹配的方法为：1)获取i时刻视觉信息S(i)，其中包含每个特征点信息，利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点在导航坐标系中的坐标为利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点在导航坐标系中的坐标，为特征点在导航系下X值，为特征点在导航系下Y值，为特征点在导航系下Z值，为特征点在相机系下X值，为特征点在相机系下Y值，为特征点在相机系下Z值，为像素点在机体系下x值，为像素点在机体系下y值，为像素点在机体系下z值，为i-1时刻从机体系到导航系的旋转矩阵，为相机系转换至机体系的旋转矩阵，为i-1时刻到i时刻的转换矩阵，其中m代表视觉传感器数据S(i)中第m个特征点；2)设置距离阈值L1，建立以在i时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(i)中每个特征点坐标值处于此圆球内的特征点其中，j＝1,2,L,numinumi为以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(i)中特征点的数量；通过计算与的汉明距离以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行随机采样一致性方法剔除外点，得到最终匹配的特征点对。步骤4中位姿优化求解的方法具体为：检测出回环后，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置速度姿态及陀螺仪误差加速度计误差为导航系下回环检测成功的姿态，为导航系下回环检测成功的位置，优化函数为：其中||rp-Hpχ||2为边缘化先验约束，Σm为信息矩阵，为地图中的位置投影至当前像素坐标系中的像素坐标，ΕIMU(n,n+1)为惯性的误差，eb＝b(q+1)-b(q)ΣI为信息矩阵，用于描述惯性误差之间的关系，回环误差可以表示为eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，nloop为成功与回环匹配特征点的数量，为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，为q+1时刻导航系下速度，为q时刻导航系下速度，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)，加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，w为导航系下，为q时刻导航系下位置，为q+1时刻导航系下位置，Δpq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)，加速度计误差为ba(q)下从q时刻至q+1时刻位置变化，δbg(q)为q时刻陀螺仪误差变化量，δba(q)为q时刻加速度计误差变化量，ΣR为惯性误差协方差矩阵，ploop为回环位置矢量，π(·)为视觉投影；当其中一帧为匹配失败帧时，则其中Εproj(m,m+1)＝0，完成出现匹配失败时的回环优化；使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，输出优化目标χ,步骤5中所述图像匹配质量判断模型具有两种判断条件：条件1：当前图像与地图拥有α个以上的匹配点，即：为当前图像与地图匹配成功的特征点数量；条件2：将预积分预测与视觉求解位姿的残差同预积分的预测值进行比较，其差值小于预积分预测值的δ，即Εjudge＜δ需同时满足条件1与条件2的情况下即满足图像匹配质量判断模型，其余情况均为不满足；其中α和δ为常量，α＝0.85*nS(i)，nS(k)为i时刻视觉传感器数据S(i)包含的特征点数量，n为两帧间惯性量测数据的数量，与与传惯性感器特性相关。步骤5中纯惯性误差进行位姿本文档来自技高网...

【技术保护点】
1.一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：包括如下步骤：步骤1：周期采集i时刻加速度计数据

【技术特征摘要】
1.一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：包括如下步骤：步骤1：周期采集i时刻加速度计数据陀螺仪数据和立体视觉传感器数据S(i)；步骤2：利用惯性传感器测量数据和预积分，预测i时刻载体的导航信息得到预积分预测值；步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量gb(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据S(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；步骤5：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载体位姿；步骤6：输出载体导航信息，更新地图，并跳转至步骤1。2.根据权利要求1所述的一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：步骤2中所述利用惯性传感器测量数据和预积分的具体过程如下：i时刻得到惯性传感器加计输出与陀螺仪输出其中包含从i-1时刻到i时刻的惯性传感器加计测量数据和陀螺仪量测数据k∈(t(i-1),(t(i)-t(i-1))/Δt)，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点，Δt为离散周期；测量模型为：ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，为k时刻导航坐标系到机体坐标系的旋转矩阵，fn(k)为k时刻的加速度计量测在导航系中的真实值，为k时刻的加速度计量测的真实值；对ba(k)与bg(k)有：bg(k)＝ηbgηba为加速度计白噪声，ηbg为陀螺仪白噪声；预积分的过程为：ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，为i-1时刻从导航系至机体系的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，Δti-1,i为从i-1时刻到i时刻的时间周期，ΔRi-1,k为从i-1时刻到k时刻的旋转矩阵，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置，整体预积分的过程将于i时刻的全局位姿无关。3.根据权利要求1所述的一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：步骤4中预积分预测值辅助图像匹配的方法为：1)获取i时刻视觉信息S(i)，其中包含每个特征点信息，利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点在导航坐标系中的坐标为利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点在导航坐标系中的坐标，为特征点在导航系下X值，为特征点在导航系下Y值，为特征点在导航系下Z值，为特征点在相机系下X值，为特征点在相机系下Y值，为特征点在相机系下Z值，为像素点在机体系下x值，为像素点在机体系下y值，为像素点在机体系下z值，为i-1时刻从机体系到导航系的旋转矩阵，为相机系转换至机体系的旋转矩阵，为i-1时刻到i时刻的转换矩阵，其中m代表视觉传感器数据S(i)中第m个特征点；2)设置距离阈...

【专利技术属性】
技术研发人员：袁诚，赖际舟，吕品，何容，杨子寒，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：江苏,32