一种鲁棒立体视觉惯性预积分SLAM方法与流程

本发明涉及一种鲁棒立体视觉惯性预积分slam方法,属于视觉导航技术领域。

背景技术：

实时定位与构图(slam，simultaneouslocalizationandmapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人在工程实践应用中必要的关键技术。视觉传感器可以感知外界的光线，无需自身进行信号发射，具有高自主性，视觉slam在机器人slam中具有广阔的发展前景与较大的潜质。

目前大多视觉slam都对外界环境具有较高的要求，需要利用环境信息进行视觉解算，并且在整个slam过程中，环境需要一直保持一个可控的状态，否则会导致视觉解算失败进而产生导航信息输出失败而对机器人造成危害。而现实环境中往往存在着明暗变化以及运动造成的成像噪声。机器人slam方法中，对于外界环境变化十分敏感，其变化会对直接导致精度下降，造成较大的误差甚至系统崩溃。

技术实现要素：

本发明提出了一种鲁棒立体视觉惯性预积分slam方法,能够在复杂环境下获得高鲁棒性的视觉slam系统。

本发明为解决其技术问题采用如下技术方案：

一种鲁棒立体视觉惯性预积分slam方法，包括如下步骤：

步骤1：周期采集i时刻加速度计数据陀螺仪数据和立体视觉传感器数据s(i)；

步骤2：利用惯性传感器测量数据和预积分，预测i时刻载体的导航信息得到预积分预测值；

步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量g^b(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时刻陀螺仪的随机噪声，gⁿ为导航系下的重力，为k时刻导航坐标系到机体坐标系的旋转矩阵，fⁿ(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,numⁱ

numⁱ为以在k时刻预测的导航坐标系下的坐标值为圆心，l1为半径的圆中包含的m(i)中特征点的数量；

通过计算与的汉明距离以最小的汉明距离的两倍为阈值l1，剔除高于两倍阈值l1的特征点匹配，再进行随机采样一致性方法剔除外点，得到最终匹配的特征点对。

步骤4中位姿优化求解的方法具体为：

检测出回环后，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置速度姿态及陀螺仪误差加速度计误差为导航系下回环检测成功的姿态，为导航系下回环检测成功的位置，优化函数为：

其中||rp-hpχ||²为边缘化先验约束，σ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中纯惯性误差进行位姿优化求解载体位姿具体执行方法如下：

优化目标为为i时刻从机体系到导航系的转换矩阵，为i时刻在导航系中的位置，为i时刻在导航系中的速度，为i时刻陀螺仪误差，为i时刻加速度计误差，优化方程如下：

使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，，输出优化目标为优化后的姿态，为优化后的位置，为优化后的速度，为优化后的陀螺仪误差，为优化后的加速度计误差，将旋转矩阵转换为四元数，输出导航信息。

本发明的有益效果如下：

能够有效解决不良环境中视觉slam匹配错误与失败的问题，获得高鲁棒性以及高精度的载体导航信息，并可以构建精准地图进行复用，具有良好的推广前景。

附图说明

图1是本发明的实施例所提供的一种鲁棒立体视觉惯性预积分slam方法的原理框图。

图2是基于本发明的鲁棒立体视觉惯性预积分slam方法实际导航定位轨迹图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。

本实施例提供一种鲁棒立体视觉惯性预积分slam方法，包括惯性传感器和立体视觉传感器相对固定装载于一载体，所述惯性传感器包括加速度计和陀螺仪。参考图1，所述slam系统的工作方法包括以下步骤：

步骤1：周期采集i时刻加速度计数据陀螺仪数据和立体视觉传感器数据s(i)；

所述视觉传感器为立双目摄像机或颜色和深度(rgb-d)摄像机，步骤1中采集i时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在相机坐标系下的三维坐标值。

步骤2：利用惯性传感器测量数据和预积分预测i时刻载体的导航信息得到预积分预测值；

其中，载体的导航信息包括姿态四元数、三维位置与三维速度信息，四元数姿态角包括横滚角、俯仰角和偏航角；具体定义如下：横滚角为载体绕导航坐标系的y轴方向的转动的角度，俯仰角为载体绕导航坐标系的x轴方向转动的角度，偏航角为载体绕导航坐标系的z轴方向转动的角度。其旋转方向均满足右手定则，三维位置与三维速度为载体在速度与位置导航系各轴中的投影；

坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系b，其中x轴、y轴与z轴分别与当前时刻载体的右向、前向和天向重合；以当前时刻相机的感光元件位置为原点构建相机坐标系c，其中x轴、y轴与z轴分别与当前时刻载体的右向、地向和前向重合；以当前时刻相机成像的左上角为原点构建像素坐标系，其中x轴与y轴分别与当前图像的右向和地向重合；以初始时刻载体的位置为原点构建导航坐标系n，其中x轴、y轴与z轴分别与初始时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建地理坐标系i，其中x轴、y轴与z轴分别与初始时刻载体的右向、前向和当地水平面的天向重合。

获得载体导航信息预积分预测值的方法为：

i时刻得到惯性传感器加计输出与陀螺仪输出其中包含从i-1时刻到i时刻的惯性传感器测量数据和k∈(t(i-1),(t(i)-t(i-1))/δt)，δt为离散周期，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点。测量模型为：

ba(k)与bg(k)分别为k时刻的加速度计与陀螺仪的量测偏差，ηa(k)与ηg(k)分别为k时刻的加速度计与陀螺仪的随机噪声，gⁿ为导航系下的重力，为k时刻导航坐标系到机体坐标系的旋转矩阵，fⁿ(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时刻从导航系至机体系的旋转矩阵，δri-1,k为从i-1时刻到k时刻的旋转矩阵，vi为i时刻在导航系中的速度，vi-1为i-1时刻在导航系中的速度，δti-1,i为从i-1时刻到i时刻的时间周期，pi为i时刻在导航系中的位置，pi-1为i-1时刻在导航系中的位置。整体预积分的过程将于i时刻的全局位姿无关。

其中：

其中，为任一三维向量，‖‖表示被限制在其它任意一个连续的2π范围内，为一向量反对称矩阵，r³为代数空间，so(3)为李群空间，r为任意旋转矩阵，为旋转角度，a为旋转轴。

为流形空间李群与原点正切空间李代数的转换关系。

其中：ω为任意向量，w为任意旋转矩阵，ω^∧为ω的反对称矩阵，ωi,i＝1,2,3为任意数字，w^∨为取逆反对称矩阵。

步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量g^b(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；具体初始化方法为：

1)获取i时刻视觉信息，并提取当前定向加速段测试的特征与旋转二进制鲁棒独立基础特征点(orientedfeaturesfromacceleratedsegmenttestandrotatedbinaryrobustindependentelementaryfeatures，orb)或二进制鲁棒独立基础特征点(binaryrobustindependentelementaryfeatures，brief)特征点得到视觉传感器数据s(i)，特征点包含关键点、描述子与像素坐标

fpm(i)＝[fuml(i)fvml(i)fumr(i)]^t

fuml(i)为特征点点在双目中左像素坐标系下x坐标值，fvml(i)为在双目中左像素坐标系下y坐标值，fumr(i)为右相机像素坐标系下的x轴上的值，深度相机可以将其虚拟投影至右相机中，其中m代表第m个特征点；

2)如果当前i＝0，则以当前时刻的机体系作为导航系，并将视觉传感器数据s(i)投影至导航系，构成地图m(i)；如果当前i＞0，将i时刻视觉传感器数据s(i)与i-1时刻视觉传感器数据s(i-1)使用暴力(brute)匹配方法进行特征点匹配。

3)利用非线性优化方法，在得到匹配点对后，将i时刻的特征点位置与对应匹配的i-1时刻视觉传感器数据s(i-1)的特征点像素位置之差的绝对值之和作为优化目标，优化最佳位姿变化，为k时刻s(i)的像素坐标，公式如下：

其中，为与匹配成功的m(k)中特征点在导航坐标系下的坐标值，为其在相机坐标系中的位置，n为匹配成功的数量，r,t为从i时刻到i-1时刻的旋转矩阵以及位置的变化量，ρ为核函数，rcb为从机体系至相机系的旋转矩阵，pcb为从机体系至相机系的平移矩阵，为i时刻从导航系到机体系的旋转矩阵，pi为i时刻从导航系到机体系的平移矩阵。

其中fx为相机焦距x比例，fy为相机焦距y比例，cx为光心在像素坐标系中x偏移，cy为光心在像素坐标系中y偏移，b为基线长度，x为相机系中x坐标，y为相机系中y坐标，z为相机系中z坐标，对于rgb-d相机，例如kinect与华硕xtion大概为8cm。

使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出变换矩阵ti-1,i，

得到使用视觉信息优化求解的位姿，其中：其中rij,i,j∈1,2,3为旋转参数，ti.i∈1,2,3为平移参数，01×3为0矩阵，i1×1为单位矩阵。

4)利用连续两个关键帧之间的图像位姿变换，去估计陀螺仪的偏差，关键帧的选取准则为匹配成功的特征点少于90％或者少于100个则选取当前帧作为新的关键帧，初始关键帧q＝i＝0。通过优化预积分预测与图像估计的旋转差值，计算陀螺仪的初始偏差：

其中，连续之间关键帧加速度计与陀螺仪的偏差变化可以忽略，bg(q)＝bg(q+1)＝…＝bg(q+n-1)，n为关键帧的数量，bg(q)为q时刻陀螺的偏差估计值，bg(q+1)为q+1时刻陀螺的偏差估计值，bg(q+n-1)为第q+n-1帧的陀螺仪误差，为陀螺仪误差为bg(q)下的从q+1时刻至q时刻的旋转矩阵，δrij为从j时刻到i时刻的旋转矩阵，为为视觉估计的从机体系到导航系旋转矩阵，为视觉估计的旋转矩阵，rcb为机体系转换为相机系的旋转矩阵，为q+1时刻从机体系到导航系的旋转矩阵，为q时刻从机体系到导航系的旋转矩阵。优化该函数可以得出初始化陀螺仪偏差。

5)利用位置预测值估计重力在导航系中的分量：

先将相机系中的位姿转换至机体系中，利用其中，为q时刻机体系在导航系下位置，为为q时刻从相机系在导航系中的位置，为q时刻从相机系至导航系的转换矩阵，pcb为从相机系到机体系的平移矢量。

可得利用两个连续的关键帧(q,q+1),(q+1,q+2)，δpq,q+1为q时刻至q+1时刻的位移矢量，为q+1时刻从相机在导航系中的位置，为q时刻导航系中机体系的速度，δtq,q+1为q时刻至q+1时刻，gⁿ为导航系下重力向量，为q+1时刻从相机系至导航系的转换矩阵，为导航系下从机体系到相机系的位置位置矢量去除速度的影响，可得：

为q+2时刻的相机在导航系中的位置，δtq+1,q+2为q+1时刻到q+2时间，δpq+1,q+2为从q+1时刻到q+2的位置变化，为q+1时刻从相机系到导航系的旋转矩阵，通过解线性矩阵求解gⁿ，由于自由度为3，则需要3个关键帧建立三个方程组求解，得到重力分量的初值

6)利用重力初值可以进行加速度计偏差的估计以及重力分量的优化。将重力表达为：

其中，(·)×为取反对称矩阵，δθ为姿态的扰动，gⁱ为当地真实重力向量，rni为从地理系至导航系的旋转矩阵。同样将其与预积分公式进行代入。

其中c(q)，d(q)，h(q)为求解中出现的过程矩阵，ba为加速度计误差。

其中，

其中，δpq+1,q+2(ba(q))为当加速度偏差为ba(q)时q+1时刻到q+2时刻导航系下位置变化，δvq,q+1为q时刻到q+1时刻导航系下的速度变化，δpq,q+1(ba(q))为当加速度偏差为ba(q)，q时刻到q+1时刻下导航系下位置变化，通过解线性矩阵求解，可以得出加速度计的初始偏差和重力的进一步估计。

7)对速度进行估计，由下式可得：

其中为q+1时刻导航系下速度，为q时刻导航系下速度，为q时刻到q+1时机体系下速度变化。

将旋转矩阵转换为四元数q⁰,q¹,q²,q³，转换过程如下：

由此完成初始化的过程。

步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据s(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；

其中预积分预测值辅助图像匹配的方法为：

1)获取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,numⁱ

numⁱ为以在k时刻预测的导航坐标系下的坐标值为圆心，l1为半径的圆中包含的m(i)中特征点的数量；

通过计算与的汉明距离以最小的汉明距离的两倍为阈值l1，剔除高于两倍阈值l1的特征点匹配，再进行随机采样一致性(randomsampleconsensusalgorithm,ransac)随机采样一致性方法剔除外点，得到最终匹配的特征点对；

回环检测与位姿优化求解的方法具体如下：

利用词袋算法将i时刻视觉传感器数据s(i)与地图数据m进行匹配，如果连续3帧存在30个以上经过ransac筛选的成功匹配点，则认为成功检测出回环，并在此基础上进行位姿的求解，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置速度姿态及陀螺仪误差加速度计误差优化函数为：

其中rp-hpχ²为边缘化先验约束，用于控制优化复杂度。σ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(i)为i时刻视觉传感器数据s(i)包含的特征点数量，n为两帧间惯性量测数据的数量，与与传惯性感器特性相关。

当图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载体位姿；

其中，联合惯性、图像误差进行位姿优化求解载体位姿具体执行方法如下：

将惯性误差融合进优化方程中，进行位姿的优化求解，优化目标为为i时刻从机体系到导航系的转换矩阵，为i时刻在导航系中的位置，为i时刻在导航系中的速度，为i时刻陀螺仪误差，为i时刻加速度计误差，优化方程如下：

其中

||rp-hpχ||²为对地图特征点进行边缘化处理的线性雅各比矩阵，用以降低运算复杂度。

使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出优化目标为优化后的姿态，为优化后的位置，为优化后的速度，为优化后的陀螺仪误差，为优化后的加速度计误差，将旋转矩阵转换为四元数，输出导航信息。

其中，纯惯性误差进行位姿优化求解载体位姿具体执行方法如下：

因为当前帧视觉信息不在可靠，所以仅将惯性误差引入优化方程中，进行位姿的优化求解，优化目标为优化方程如下：

使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出优化目标将旋转矩阵转换为四元数，输出导航信息。

步骤6：输出载体导航信息，更新地图，并跳转至步骤1；

先判断是否为第一帧，如果为第一帧，则直接将视觉传感器数据s(i)中的每个特征点以及优化得出的导航坐标系的坐标全部载入地图中；

如果非初第一帧，将视觉传感器数据s(i)中的每个特征点以及求出的导航坐标系的坐标载入地图中，并处理与视觉传感器数据s(i-1)匹配成功的点对形成的冗余数据，存在匹配点对的导航坐标系坐标为所有包含该特征点的图像数据中包含的导航坐标系坐标值的平均。

图2所示的轨迹中，其中散点线参考轨迹，实线图为本专利轨迹，虚线图为orb-slam2算法轨迹，粗体虚线为松组合轨迹图，虚线点图为紧组合轨迹图。可以看出，本发明提出的方法与参考轨迹基本重合，误差最终在0.6％，可以在出现错误的时候利用惯性预积分递推优化继续完成导航，目前最主流的松紧组合方法均以发散，相比目前最主流的松紧组合方法，本发明具有明显的优势。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，在不脱离本发明的精神和原则的前提下，本领域普通技术人员对本发明所做的任何修改、等同替换、改进等，均应落入本发明权利要求书确定的保护范围之内。