CN110207693B - 一种鲁棒立体视觉惯性预积分slam方法 - Google Patents

一种鲁棒立体视觉惯性预积分slam方法 Download PDF

Info

Publication number
CN110207693B
CN110207693B CN201910423422.2A CN201910423422A CN110207693B CN 110207693 B CN110207693 B CN 110207693B CN 201910423422 A CN201910423422 A CN 201910423422A CN 110207693 B CN110207693 B CN 110207693B
Authority
CN
China
Prior art keywords
time
error
moment
navigation
value
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.)
Active
Application number
CN201910423422.2A
Other languages
English (en)
Other versions
CN110207693A (zh
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201910423422.2A priority Critical patent/CN110207693B/zh
Publication of CN110207693A publication Critical patent/CN110207693A/zh
Application granted granted Critical
Publication of CN110207693B publication Critical patent/CN110207693B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种鲁棒立体视觉惯性预积分SLAM方法
技术领域
本发明涉及一种鲁棒立体视觉惯性预积分SLAM方法,属于视觉导航技术领域。
背景技术
实时定位与构图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人在工程实践应用中必要的关键技术。视觉传感器可以感知外界的光线，无需自身进行信号发射，具有高自主性，视觉SLAM在机器人SLAM中具有广阔的发展前景与较大的潜质。
目前大多视觉SLAM都对外界环境具有较高的要求，需要利用环境信息进行视觉解算，并且在整个SLAM过程中，环境需要一直保持一个可控的状态，否则会导致视觉解算失败进而产生导航信息输出失败而对机器人造成危害。而现实环境中往往存在着明暗变化以及运动造成的成像噪声。机器人SLAM方法中，对于外界环境变化十分敏感，其变化会对直接导致精度下降，造成较大的误差甚至***崩溃。
发明内容
本发明提出了一种鲁棒立体视觉惯性预积分SLAM方法,能够在复杂环境下获得高鲁棒性的视觉SLAM***。
本发明为解决其技术问题采用如下技术方案：
一种鲁棒立体视觉惯性预积分SLAM方法，包括如下步骤：
步骤1：周期采集i时刻加速度计数据
Figure BDA0002066719780000021
陀螺仪数据
Figure BDA0002066719780000022
和立体视觉传感器数据S(i)；
步骤2：利用惯性传感器测量数据
Figure BDA0002066719780000023
和
Figure BDA0002066719780000024
预积分，预测i时刻载体的导航信息得到预积分预测值；
步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量gb(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；
步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据S(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；
步骤5：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载***姿；
步骤6：输出载体导航信息，更新地图，并跳转至步骤1。
步骤2中所述利用惯性传感器测量数据
Figure BDA0002066719780000025
和
Figure BDA0002066719780000026
预积分的具体过程如下：
i时刻得到惯性传感器加计输出
Figure BDA0002066719780000027
与陀螺仪输出
Figure BDA0002066719780000028
其中包含从i-1时刻到i时刻的惯性传感器加计测量数据
Figure BDA0002066719780000031
和陀螺仪量测数据
Figure BDA0002066719780000032
k∈(t(i-1),(t(i)-t(i-1))/Δt)，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点，Δt为离散周期；测量模型为：
Figure BDA0002066719780000033
Figure BDA0002066719780000034
ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，
Figure BDA0002066719780000035
为k时刻导航坐标系到机体坐标系的旋转矩阵，fn(k)为k时刻的加速度计量测在导航系中的真实值，
Figure BDA0002066719780000036
为k时刻的加速度计量测的真实值；对ba(k)与bg(k)有：
Figure BDA0002066719780000037
bg(k)＝ηbg
ηba为加速度计白噪声，ηbg为陀螺仪白噪声；
预积分的过程为：
Figure BDA0002066719780000038
Figure BDA0002066719780000039
Figure BDA00020667197800000310
ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，
Figure BDA0002066719780000041
为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)中每个特征点
Figure BDA0002066719780000042
在导航坐标系中的坐标
Figure BDA0002066719780000043
Figure BDA0002066719780000044
Figure BDA0002066719780000045
为利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点
Figure BDA0002066719780000046
在导航坐标系中的坐标，
Figure BDA0002066719780000047
为特征点
Figure BDA0002066719780000048
在导航系下X值，
Figure BDA0002066719780000049
为特征点
Figure BDA00020667197800000410
在导航系下Y值，
Figure BDA00020667197800000411
为特征点
Figure BDA00020667197800000412
在导航系下Z值，
Figure BDA00020667197800000413
为特征点
Figure BDA00020667197800000414
在相机系下X值，
Figure BDA00020667197800000415
为特征点
Figure BDA00020667197800000416
在相机系下Y值，
Figure BDA00020667197800000417
为特征点
Figure BDA00020667197800000418
在相机系下Z值，
Figure BDA00020667197800000419
为像素点在机体系下x值，
Figure BDA00020667197800000420
为像素点在机体系下y值，
Figure BDA00020667197800000421
为像素点在机体系下z值，
Figure BDA00020667197800000422
为i-1时刻从机体系到导航系的旋转矩阵，
Figure BDA00020667197800000423
为相机系转换至机体系的旋转矩阵，
Figure BDA00020667197800000424
为i-1时刻到i时刻的转换矩阵，其中m代表视觉传感器数据S(i)中第m个特征点；
2)设置距离阈值L1，建立以
Figure BDA00020667197800000425
在i时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(i)中每个特征点坐标值处于此圆球内的特征点
Figure BDA0002066719780000051
其中，j＝1,2,L,numi
numi为以
Figure BDA0002066719780000052
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(i)中特征点的数量；
通过计算
Figure BDA0002066719780000053
与
Figure BDA0002066719780000054
的汉明距离
Figure BDA0002066719780000055
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行随机采样一致性方法剔除外点，得到最终匹配的特征点对。
步骤4中位姿优化求解的方法具体为：
检测出回环后，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置
Figure BDA0002066719780000056
速度
Figure BDA0002066719780000057
姿态
Figure BDA0002066719780000058
及陀螺仪误差
Figure BDA0002066719780000059
加速度计误差
Figure BDA00020667197800000510
Figure BDA00020667197800000511
Figure BDA00020667197800000512
为导航系下回环检测成功的姿态，
Figure BDA00020667197800000513
为导航系下回环检测成功的位置，优化函数为：
Figure BDA00020667197800000514
其中||rp-Hpχ||2为边缘化先验约束，
Figure BDA00020667197800000515
Σm为信息矩阵，
Figure BDA00020667197800000516
为地图中
Figure BDA00020667197800000517
的位置投影至当前像素坐标系中的像素坐标，ΕIMU(n,n+1)为惯性的误差，
Figure BDA0002066719780000061
Figure BDA0002066719780000062
Figure BDA0002066719780000063
Figure BDA0002066719780000064
eb＝b(q+1)-b(q)
ΣI为信息矩阵，用于描述惯性误差之间的关系，回环误差可以表示为
Figure BDA0002066719780000065
eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，nloop为成功与回环匹配特征点的数量，
Figure BDA0002066719780000066
为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，
Figure BDA0002066719780000067
为q+1时刻导航系下速度，
Figure BDA0002066719780000068
为q时刻导航系下速度，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)，加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，w为导航系下，
Figure BDA0002066719780000069
为q时刻导航系下位置，
Figure BDA00020667197800000610
为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，
Figure BDA00020667197800000611
完成出现匹配失败时的回环优化；
使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，输出优化目标χ,
Figure BDA0002066719780000071
步骤5中所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure BDA0002066719780000072
Figure BDA0002066719780000073
为当前图像与地图匹配成功的特征点数量；
条件2：将预积分预测与视觉求解位姿的残差
Figure BDA0002066719780000074
同预积分的预测值进行比较，其差值小于预积分预测值的δ，即
Εjudge＜δ
需同时满足条件1与条件2的情况下即满足图像匹配质量判断模型，其余情况均为不满足；
其中α和δ为常量，α＝0.85*nS(i)，nS(k)为i时刻视觉传感器数据S(i)包含的特征点数量，
Figure BDA0002066719780000075
n为两帧间惯性量测数据的数量，
Figure BDA0002066719780000076
与
Figure BDA0002066719780000077
与传惯性感器特性相关。
步骤5中纯惯性误差进行位姿优化求解载***姿具体执行方法如下：
优化目标为
Figure BDA0002066719780000078
Figure BDA0002066719780000079
为i时刻从机体系到导航系的转换矩阵，
Figure BDA00020667197800000710
为i时刻在导航系中的位置，
Figure BDA00020667197800000711
为i时刻在导航系中的速度，
Figure BDA00020667197800000712
为i时刻陀螺仪误差，
Figure BDA00020667197800000713
为i时刻加速度计误差，优化方程如下：
Figure BDA00020667197800000714
Figure BDA00020667197800000715
使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，，输出优化目标
Figure BDA0002066719780000081
Figure BDA0002066719780000082
为优化后的姿态，
Figure BDA0002066719780000083
为优化后的位置，
Figure BDA0002066719780000084
为优化后的速度，
Figure BDA0002066719780000085
为优化后的陀螺仪误差，
Figure BDA0002066719780000086
为优化后的加速度计误差，将旋转矩阵转换为四元数，输出导航信息。
本发明的有益效果如下：
能够有效解决不良环境中视觉SLAM匹配错误与失败的问题，获得高鲁棒性以及高精度的载体导航信息，并可以构建精准地图进行复用，具有良好的推广前景。
附图说明
图1是本发明的实施例所提供的一种鲁棒立体视觉惯性预积分SLAM方法的原理框图。
图2是基于本发明的鲁棒立体视觉惯性预积分SLAM方法实际导航定位轨迹图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。
本实施例提供一种鲁棒立体视觉惯性预积分SLAM方法，包括惯性传感器和立体视觉传感器相对固定装载于一载体，所述惯性传感器包括加速度计和陀螺仪。参考图1，所述SLAM***的工作方法包括以下步骤：
步骤1：周期采集i时刻加速度计数据
Figure BDA0002066719780000091
陀螺仪数据
Figure BDA0002066719780000092
和立体视觉传感器数据S(i)；
所述视觉传感器为立双目摄像机或颜色和深度(RGB-D)摄像机，步骤1中采集i时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在相机坐标系下的三维坐标值。
步骤2：利用惯性传感器测量数据
Figure BDA0002066719780000093
和
Figure BDA0002066719780000094
预积分预测i时刻载体的导航信息得到预积分预测值；
其中，载体的导航信息包括姿态四元数、三维位置与三维速度信息，四元数姿态角包括横滚角、俯仰角和偏航角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，俯仰角为载体绕导航坐标系的X轴方向转动的角度，偏航角为载体绕导航坐标系的Z轴方向转动的角度。其旋转方向均满足右手定则，三维位置与三维速度为载体在速度与位置导航系各轴中的投影；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系b，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以当前时刻相机的感光元件位置为原点构建相机坐标系c，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、地向和前向重合；以当前时刻相机成像的左上角为原点构建像素坐标系，其中X轴与Y轴分别与当前图像的右向和地向重合；以初始时刻载体的位置为原点构建导航坐标系n，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建地理坐标系i，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和当地水平面的天向重合。
获得载体导航信息预积分预测值的方法为：
i时刻得到惯性传感器加计输出
Figure BDA0002066719780000101
与陀螺仪输出
Figure BDA0002066719780000102
其中包含从i-1时刻到i时刻的惯性传感器测量数据
Figure BDA0002066719780000103
和
Figure BDA0002066719780000104
k∈(t(i-1),(t(i)-t(i-1))/Δt)，Δt为离散周期，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点。测量模型为：
Figure BDA0002066719780000105
Figure BDA0002066719780000106
ba(k)与bg(k)分别为k时刻的加速度计与陀螺仪的量测偏差，ηa(k)与ηg(k)分别为k时刻的加速度计与陀螺仪的随机噪声，gn为导航系下的重力，
Figure BDA0002066719780000107
为k时刻导航坐标系到机体坐标系的旋转矩阵，fn(k)为k时刻的加速度计量测在导航系中的真实值，
Figure BDA0002066719780000108
为k时刻的加速度计量测的真实值。对ba(k)与bg(k)有：
Figure BDA0002066719780000109
bg(k)＝ηbg
其中：ηba为加速度计白噪声，ηbg为陀螺仪白噪声。
预积分的过程为：
Figure BDA0002066719780000111
Figure BDA0002066719780000112
Figure BDA0002066719780000113
ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量。Ri为i时刻从机体系至导航系的旋转矩阵，
Figure BDA0002066719780000114
为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时刻的全局位姿无关。
其中：
Figure BDA0002066719780000115
Figure BDA0002066719780000116
其中，
Figure BDA0002066719780000117
为任一三维向量，‖‖表示被限制在其它任意一个连续的2π范围内，
Figure BDA0002066719780000118
为一向量反对称矩阵，R3为代数空间，SO(3)为李群空间，R为任意旋转矩阵，
Figure BDA0002066719780000119
为旋转角度，a为旋转轴。
为流形空间李群与原点正切空间李代数的转换关系。
Figure BDA0002066719780000121
其中：ω为任意向量，W为任意旋转矩阵，ω∧为ω的反对称矩阵，ωi,i＝1,2,3为任意数字，W∨为取逆反对称矩阵。
步骤3：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量gb(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；具体初始化方法为：
1)获取i时刻视觉信息，并提取当前定向加速段测试的特征与旋转二进制鲁棒独立基础特征点(Oriented Features From Accelerated Segment Test and RotatedBinary Robust Independent Elementary Features，ORB)或二进制鲁棒独立基础特征点(Binary Robust Independent Elementary Features，BRIEF)特征点得到视觉传感器数据S(i)，特征点
Figure BDA0002066719780000122
包含关键点、描述子与像素坐标
fpm(i)＝[fuml(i) fvml(i) fumr(i)]T
fuml(i)为特征点
Figure BDA0002066719780000123
点在双目中左像素坐标系下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)的特征点像素位置之差的绝对值之和作为优化目标，优化最佳位姿变化，
Figure BDA0002066719780000131
为k时刻S(i)的像素坐标，公式如下：
Figure BDA0002066719780000132
Figure BDA0002066719780000133
其中，
Figure BDA0002066719780000134
为与
Figure BDA0002066719780000135
匹配成功的M(k)中特征点在导航坐标系下的坐标值，
Figure BDA0002066719780000136
为其在相机坐标系中的位置，n为匹配成功的数量，R,t为从i时刻到i-1时刻的旋转矩阵以及位置的变化量，ρ为核函数，Rcb为从机体系至相机系的旋转矩阵，pcb为从机体系至相机系的平移矩阵，
Figure BDA0002066719780000137
为i时刻从导航系到机体系的旋转矩阵，pi为i时刻从导航系到机体系的平移矩阵。
Figure BDA0002066719780000138
其中fx为相机焦距x比例，fy为相机焦距y比例，cx为光心在像素坐标系中x偏移，cy为光心在像素坐标系中y偏移，b为基线长度，X为相机系中X坐标，Y为相机系中Y坐标，Z为相机系中Z坐标，对于RGB-D相机，例如Kinect与华硕Xtion大概为8cm。
使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出变换矩阵Ti-1,i，
Figure BDA0002066719780000141
得到使用视觉信息优化求解的位姿，其中：其中rij,i,j∈1,2,3为旋转参数，ti.i∈1,2,3为平移参数，01×3为0矩阵，I1×1为单位矩阵。
4)利用连续两个关键帧之间的图像位姿变换，去估计陀螺仪的偏差，关键帧的选取准则为匹配成功的特征点少于90％或者少于100个则选取当前帧作为新的关键帧，初始关键帧q＝i＝0。通过优化预积分预测与图像估计的旋转差值，计算陀螺仪的初始偏差：
Figure BDA0002066719780000142
其中，连续之间关键帧加速度计与陀螺仪的偏差变化可以忽略，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帧的陀螺仪误差，
Figure BDA0002066719780000143
为陀螺仪误差为bg(q)下的从q+1时刻至q时刻的旋转矩阵，ΔRij为从j时刻到i时刻的旋转矩阵，
Figure BDA0002066719780000144
为
Figure BDA0002066719780000145
为视觉估计的从机体系到导航系旋转矩阵，
Figure BDA0002066719780000146
为视觉估计的旋转矩阵，Rcb为机体系转换为相机系的旋转矩阵，
Figure BDA0002066719780000147
为q+1时刻从机体系到导航系的旋转矩阵，
Figure BDA0002066719780000148
为q时刻从机体系到导航系的旋转矩阵。优化该函数可以得出初始化陀螺仪偏差。
5)利用位置预测值估计重力在导航系中的分量：
先将相机系中的位姿转换至机体系中，利用
Figure BDA0002066719780000149
其中，
Figure BDA0002066719780000151
为q时刻机体系在导航系下位置，
Figure BDA0002066719780000152
为为q时刻从相机系在导航系中的位置，
Figure BDA0002066719780000153
为q时刻从相机系至导航系的转换矩阵，pcb为从相机系到机体系的平移矢量。
可得
Figure BDA0002066719780000154
利用两个连续的关键帧(q,q+1),(q+1,q+2)，Δpq,q+1为q时刻至q+1时刻的位移矢量，
Figure BDA0002066719780000155
为q+1时刻从相机在导航系中的位置，
Figure BDA0002066719780000156
为q时刻导航系中机体系的速度，Δtq,q+1为q时刻至q+1时刻，gn为导航系下重力向量，
Figure BDA0002066719780000157
为q+1时刻从相机系至导航系的转换矩阵，
Figure BDA0002066719780000158
为导航系下从机体系到相机系的位置位置矢量去除速度的影响，可得：
Figure BDA0002066719780000159
Figure BDA00020667197800001510
为q+2时刻的相机在导航系中的位置，Δtq+1,q+2为q+1时刻到q+2时间，Δpq+1,q+2为从q+1时刻到q+2的位置变化，
Figure BDA00020667197800001511
为q+1时刻从相机系到导航系的旋转矩阵，通过解线性矩阵求解gn，由于自由度为3，则需要3个关键帧建立三个方程组求解，得到重力分量的初值
Figure BDA00020667197800001512
6)利用重力初值可以进行加速度计偏差的估计以及重力分量的优化。将重力表达为：
Figure BDA00020667197800001513
其中，(·)×为取反对称矩阵，δθ为姿态的扰动，gi为当地真实重力向量，Rni为从地理系至导航系的旋转矩阵。同样将其与预积分公式进行代入。
Figure BDA0002066719780000161
其中C(q)，D(q)，H(q)为求解中出现的过程矩阵，ba为加速度计误差。
其中，
Figure BDA0002066719780000162
Figure BDA0002066719780000163
Figure BDA0002066719780000164
其中，Δ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)对速度进行估计，由下式可得：
Figure BDA0002066719780000165
其中
Figure BDA0002066719780000166
为q+1时刻导航系下速度，
Figure BDA0002066719780000167
为q时刻导航系下速度，
Figure BDA0002066719780000168
为q时刻到q+1时机体系下速度变化。
将旋转矩阵转换为四元数q0,q1,q2,q3，转换过程如下：
Figure BDA0002066719780000171
由此完成初始化的过程。
步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据S(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；
其中预积分预测值辅助图像匹配的方法为：
1)获取i时刻视觉信息S(i)，利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点
Figure BDA0002066719780000172
在导航坐标系中的坐标
Figure BDA0002066719780000173
Figure BDA0002066719780000174
Figure BDA0002066719780000175
为特征点
Figure BDA0002066719780000176
在导航系下X值，
Figure BDA0002066719780000177
为特征点
Figure BDA0002066719780000178
在导航系下Y值，
Figure BDA0002066719780000179
为特征点
Figure BDA00020667197800001710
在导航系下Z值，
Figure BDA00020667197800001711
为特征点
Figure BDA00020667197800001712
在相机系下X值，
Figure BDA00020667197800001713
为特征点
Figure BDA00020667197800001714
在相机系下Y值，
Figure BDA00020667197800001715
为特征点
Figure BDA00020667197800001716
在相机系下Z值，
Figure BDA00020667197800001717
为像素点在机体系下x值，
Figure BDA00020667197800001718
为像素点在机体系下y值，
Figure BDA00020667197800001719
为像素点在机体系下z值，
Figure BDA00020667197800001720
为i-1时刻从机体系到导航系的旋转矩阵，
Figure BDA00020667197800001721
为i-1时刻到i时刻的转换矩阵，其中m代表视觉传感器数据S(i)中第m个特征点；
2)设置距离阈值L1，建立以
Figure BDA0002066719780000181
在i时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(i)中每个特征点坐标值处于此圆球内的特征点
Figure BDA0002066719780000182
其中，j＝1,2,L,numi
numi为以
Figure BDA0002066719780000183
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(i)中特征点的数量；
通过计算
Figure BDA0002066719780000184
与
Figure BDA0002066719780000185
的汉明距离
Figure BDA0002066719780000186
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行随机采样一致性(RandomSampleConsensusAlgorithm,RANSAC)随机采样一致性方法剔除外点，得到最终匹配的特征点对；
回环检测与位姿优化求解的方法具体如下：
利用词袋算法将i时刻视觉传感器数据S(i)与地图数据M进行匹配，如果连续3帧存在30个以上经过RANSAC筛选的成功匹配点，则认为成功检测出回环，并在此基础上进行位姿的求解，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置
Figure BDA0002066719780000187
速度
Figure BDA0002066719780000188
姿态
Figure BDA0002066719780000189
及陀螺仪误差
Figure BDA00020667197800001810
加速度计误差
Figure BDA00020667197800001811
Figure BDA00020667197800001812
优化函数为：
Figure BDA00020667197800001813
其中rp-Hpχ2为边缘化先验约束，用于控制优化复杂度。
Figure BDA00020667197800001814
Σm为信息矩阵，用于描述特征点之间的关系。ΕIMU(n,n+1)为惯性的误差
Figure BDA0002066719780000191
Figure BDA0002066719780000192
Figure BDA0002066719780000193
Figure BDA0002066719780000194
eb＝b(q+1)-b(q)
ΣI为信息矩阵，用于描述惯性误差之间的关系，回环误差可以表示为
Figure BDA0002066719780000195
eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，nloop为成功与回环匹配特征点的数量，
Figure BDA0002066719780000196
为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，
Figure BDA0002066719780000197
为q+1时刻导航系下速度，
Figure BDA0002066719780000198
为q时刻导航系下速度，Δvq,q+1(bg(q),ba(q))为陀螺仪误差为bg(q)，加速度计误差为ba(q)下从q时刻至q+1时刻速度变化，w为导航系下，
Figure BDA0002066719780000199
为q时刻导航系下位置，
Figure BDA00020667197800001910
为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，
Figure BDA00020667197800001911
完成出现匹配失败时的回环优化。
使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出优化目标χ,
Figure BDA0002066719780000201
其中
Figure BDA0002066719780000202
为导航系下回环检测成功的姿态，
Figure BDA0002066719780000203
为导航系下回环检测成功的位置。
步骤5：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载***姿；
其中，图像匹配质量判断模型具体如下：
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure BDA0002066719780000204
其中：
Figure BDA0002066719780000205
为匹配成功特征点数量；
条件2：将预积分预测与视觉求解位姿的残差
Figure BDA0002066719780000206
同预积分的预测值进行比较，其差值小于预积分预测值的δ，即
Εjudge＜δ
需同时满足条件1与条件2的情况下即满足图像匹配质量判断模型，其余情况均为不满足。
其中α和δ为常量，α＝0.85*nS(i)，nS(i)为i时刻视觉传感器数据S(i)包含的特征点数量，
Figure BDA0002066719780000207
n为两帧间惯性量测数据的数量，
Figure BDA0002066719780000208
与
Figure BDA0002066719780000209
与传惯性感器特性相关。
当图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载***姿；
其中，联合惯性、图像误差进行位姿优化求解载***姿具体执行方法如下：
将惯性误差融合进优化方程中，进行位姿的优化求解，优化目标为
Figure BDA0002066719780000211
Figure BDA0002066719780000212
为i时刻从机体系到导航系的转换矩阵，
Figure BDA0002066719780000213
为i时刻在导航系中的位置，
Figure BDA0002066719780000214
为i时刻在导航系中的速度，
Figure BDA0002066719780000215
为i时刻陀螺仪误差，
Figure BDA0002066719780000216
为i时刻加速度计误差，优化方程如下：
Figure BDA0002066719780000217
其中
Figure BDA0002066719780000218
Figure BDA0002066719780000219
Figure BDA00020667197800002110
||rp-Hpχ||2为对地图特征点进行边缘化处理的线性雅各比矩阵，用以降低运算复杂度。
使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出优化目标
Figure BDA00020667197800002111
Figure BDA00020667197800002112
为优化后的姿态，
Figure BDA00020667197800002113
为优化后的位置，
Figure BDA00020667197800002114
为优化后的速度，
Figure BDA00020667197800002115
为优化后的陀螺仪误差，
Figure BDA00020667197800002116
为优化后的加速度计误差，将旋转矩阵转换为四元数，输出导航信息。
其中，纯惯性误差进行位姿优化求解载***姿具体执行方法如下：
因为当前帧视觉信息不在可靠，所以仅将惯性误差引入优化方程中，进行位姿的优化求解，优化目标为
Figure BDA0002066719780000221
优化方程如下：
Figure BDA0002066719780000222
Figure BDA0002066719780000223
使用图优化方法不断进行牛顿-列温伯格迭代变换矩阵使得优化目标最小，以达到最小值或者迭代30次为止，输出优化目标
Figure BDA0002066719780000224
将旋转矩阵转换为四元数，输出导航信息。
步骤6：输出载体导航信息，更新地图，并跳转至步骤1；
先判断是否为第一帧，如果为第一帧，则直接将视觉传感器数据S(i)中的每个特征点以及优化得出的导航坐标系的坐标全部载入地图中；
如果非初第一帧，将视觉传感器数据S(i)中的每个特征点以及求出的导航坐标系的坐标载入地图中，并处理与视觉传感器数据S(i-1)匹配成功的点对形成的冗余数据，存在匹配点对的导航坐标系坐标为所有包含该特征点的图像数据中包含的导航坐标系坐标值的平均。
图2所示的轨迹中，其中散点线参考轨迹，实线图为本专利轨迹，虚线图为ORB-SLAM2算法轨迹，粗体虚线为松组合轨迹图，虚线点图为紧组合轨迹图。可以看出，本发明提出的方法与参考轨迹基本重合，误差最终在0.6％，可以在出现错误的时候利用惯性预积分递推优化继续完成导航，目前最主流的松紧组合方法均以发散，相比目前最主流的松紧组合方法，本发明具有明显的优势。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，在不脱离本发明的精神和原则的前提下，本领域普通技术人员对本发明所做的任何修改、等同替换、改进等，均应落入本发明权利要求书确定的保护范围之内。

Claims (4)

1.一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：包括如下步骤：
步骤1：周期采集i时刻加速度计数据
Figure FDA0002962128080000011
陀螺仪数据
Figure FDA0002962128080000012
和立体视觉传感器数据S(i)；
步骤2：利用惯性传感器测量数据
Figure FDA0002962128080000013
和
Figure FDA0002962128080000014
预积分，预测i时刻载体的导航信息得到预积分预测值；
其中，所述利用惯性传感器测量数据
Figure FDA0002962128080000015
和
Figure FDA0002962128080000016
预积分的具体过程如下：
i时刻得到惯性传感器加计输出
Figure FDA0002962128080000017
与陀螺仪输出
Figure FDA0002962128080000018
其中包含从i-1时刻到i时刻的惯性传感器加计测量数据
Figure FDA0002962128080000019
和陀螺仪量测数据
Figure FDA00029621280800000110
k∈(t(i-1),(t(i)-t(i-1))/Δt)，k为i-1至i时刻的离散采样时刻序列，t(i-1)为以离散周期计数的起点，t(i)为以离散周期计数的终点，Δt为离散周期；测量模型为：
Figure FDA00029621280800000111
Figure FDA00029621280800000112
ba(k)为k时刻的加速度计量测偏差，bg(k)为k时刻的陀螺仪量测偏差，ηa(k)为k时刻加速度计的随机噪声，ηg(k)为k时刻陀螺仪的随机噪声，gn为导航系下的重力，
Figure FDA00029621280800000113
为k时刻导航坐标系到机体坐标系的旋转矩阵，fn(k)为k时刻的加速度计量测在导航系中的真实值，
Figure FDA00029621280800000114
为k时刻的加速度计量测的真实值；对ba(k)与bg(k)有：
Figure FDA0002962128080000021
bg(k)＝ηbg
ηba为加速度计白噪声，ηbg为陀螺仪白噪声；
预积分的过程为：
Figure FDA0002962128080000022
Figure FDA0002962128080000023
Figure FDA0002962128080000024
ΔRi-1,i为从i时刻到i-1时刻的旋转矩阵，Δvi-1,i为i-1时刻到i时刻的速度变化量，Δpi-1,i为从i-1时刻到i时刻的位置变化量，Ri为i时刻从机体系至导航系的旋转矩阵，
Figure FDA0002962128080000025
为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：判断是否初始化，如果尚未初始化则进行初始化，得到惯性传感器的误差ba(i)和bg(i)与重力分量gb(i)估计，并跳转至步骤1；如果已经初始化，则进入下一步骤；
步骤4：以i时刻的预积分预测值作为图像匹配的位姿起点，匹配视觉传感器数据S(i)，并进行回环检测，如果存在回环，则进行联合回环、惯性、图像误差的位姿优化求解，并跳转至步骤6；如果不存在回环则进入下一步骤；
其中，步骤4中预积分预测值辅助图像匹配的方法为：
1)获取i时刻视觉信息S(i)，其中包含每个特征点信息，利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点
Figure FDA0002962128080000031
在导航坐标系中的坐标
Figure FDA0002962128080000032
Figure FDA0002962128080000033
Figure FDA0002962128080000034
为利用i时刻载体预积分预测的导航信息预测i时刻的视觉传感器数据S(i)中每个特征点
Figure FDA0002962128080000035
在导航坐标系中的坐标，
Figure FDA0002962128080000036
为特征点
Figure FDA0002962128080000037
在导航系下X值，
Figure FDA0002962128080000038
为特征点
Figure FDA0002962128080000039
在导航系下Y值，
Figure FDA00029621280800000310
为特征点
Figure FDA00029621280800000311
在导航系下Z值，
Figure FDA00029621280800000312
为特征点
Figure FDA00029621280800000313
在相机系下X值，
Figure FDA00029621280800000314
为特征点
Figure FDA00029621280800000315
在相机系下Y值，
Figure FDA00029621280800000316
为特征点
Figure FDA00029621280800000317
在相机系下Z值，
Figure FDA00029621280800000318
为像素点在机体系下x值，
Figure FDA00029621280800000319
为像素点在机体系下y值，
Figure FDA00029621280800000320
为像素点在机体系下z值，
Figure FDA00029621280800000321
为i-1时刻从机体系到导航系的旋转矩阵，
Figure FDA00029621280800000322
为相机系转换至机体系的旋转矩阵，
Figure FDA00029621280800000323
为i-1时刻到i时刻的转换矩阵，其中m代表视觉传感器数据S(i)中第m个特征点；
2)设置距离阈值L1，建立以
Figure FDA00029621280800000324
在i时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(i)中每个特征点坐标值处于此圆球内的特征点
Figure FDA00029621280800000325
其中，j＝1,2,L,numi
numi为以
Figure FDA00029621280800000326
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(i)中特征点的数量；
通过计算
Figure FDA0002962128080000041
与
Figure FDA0002962128080000042
的汉明距离
Figure FDA0002962128080000043
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行随机采样一致性方法剔除外点，得到最终匹配的特征点对；
步骤5：构建图像匹配质量判断模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，进行纯惯性误差位姿优化求解；图像匹配质量满足条件时，联合惯性、图像误差进行位姿优化求解载***姿；
步骤6：输出载体导航信息，更新地图，并跳转至步骤1。
2.根据权利要求1所述的一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：步骤4中位姿优化求解的方法具体为：
检测出回环后，联合回环、惯性、图像误差，其中优化变量为从回环开始的所有关键帧位置
Figure FDA0002962128080000044
速度
Figure FDA0002962128080000045
姿态
Figure FDA0002962128080000046
及陀螺仪误差
Figure FDA0002962128080000047
加速度计误差
Figure FDA0002962128080000048
Figure FDA0002962128080000049
为导航系下回环检测成功的姿态，
Figure FDA00029621280800000410
为导航系下回环检测成功的位置，优化函数为：
Figure FDA00029621280800000411
其中||rp-Hpχ||2为边缘化先验约束，
Figure FDA00029621280800000412
Σm为信息矩阵，
Figure FDA00029621280800000413
为地图中
Figure FDA00029621280800000414
的位置投影至当前像素坐标系中的像素坐标，EIMU(n,n+1)为惯性的误差，
Figure FDA0002962128080000051
Figure FDA0002962128080000052
Figure FDA0002962128080000053
Figure FDA0002962128080000054
eb＝b(q+1)-b(q)
ΣI为信息矩阵，用于描述惯性误差之间的关系，回环误差表示为
Figure FDA0002962128080000055
eR为惯性在姿态中的误差，ev为惯性在速度中的误差，ep为惯性在位置中的误差，eb为惯性在误差转移上的变化，nloop为成功与回环匹配特征点的数量，
Figure FDA0002962128080000056
为陀螺仪误差为bg(q)下从q+1时刻至q时刻旋转矩阵，
Figure FDA0002962128080000057
为q+1时刻导航系下速度，
Figure FDA0002962128080000058
为q时刻导航系下速度，Δvq,q+1(bg(q),ba(q))是陀螺仪误差为bg(q)，加速度计误差为ba(q)的情况下，从q时刻至q+1时刻的速度变化量，
Figure FDA0002962128080000059
为q时刻导航系下位置，
Figure FDA00029621280800000510
为q+1时刻导航系下位置，Δpq,q+1(bg(q),ba(q))是陀螺仪误差为bg(q)，加速度计误差为ba(q)的情况下，从q时刻至q+1时刻位置变化量，δbg(q)为q时刻陀螺仪误差变化量，δba(q)为q时刻加速度计误差变化量，ΣR为惯性误差协方差矩阵，ploop为回环位置矢量，π(·)为视觉投影；当其中一帧为匹配失败帧时，则其中Eproj(m,m+1)＝0，
Figure FDA00029621280800000511
完成出现匹配失败时的回环优化；
使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，输出优化目标χ,
Figure FDA0002962128080000061
3.根据权利要求2所述的一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：步骤5中所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure FDA0002962128080000062
为当前图像与地图匹配成功的特征点数量；
条件2：将预积分预测与视觉求解位姿的残差
Figure FDA0002962128080000063
同预积分的预测值进行比较，其差值小于预积分预测值的θ，即
Ejudge＜θ
需同时满足条件1与条件2的情况下即满足图像匹配质量判断模型，其余情况均为不满足；
其中α和θ为常量，α＝0.85*nS(i)，nS(i)为i时刻视觉传感器数据S(i)包含的特征点数量，
Figure FDA0002962128080000064
n为两帧间惯性量测数据的数量，
Figure FDA0002962128080000065
与
Figure FDA0002962128080000066
的取值与惯性传感器性能相关，为陀螺的输出噪声与加速度计的输出噪声。
4.根据权利要求1所述的一种鲁棒立体视觉惯性预积分SLAM方法，其特征在于：步骤5中纯惯性误差进行位姿优化求解载***姿具体执行方法如下：
优化目标为
Figure FDA0002962128080000067
Figure FDA0002962128080000068
为i时刻从机体系到导航系的转换矩阵，
Figure FDA0002962128080000069
为i时刻在导航系中的位置，
Figure FDA00029621280800000610
为i时刻在导航系中的速度，
Figure FDA0002962128080000071
为i时刻陀螺仪误差，
Figure FDA0002962128080000072
为i时刻加速度计误差，优化方程如下：
Figure FDA0002962128080000073
Figure FDA0002962128080000074
使用图优化方法进行牛顿-列温伯格迭代，当达到误差收敛或者迭代30次则停止，输出优化目标
Figure FDA0002962128080000075
Figure FDA0002962128080000076
为优化后的姿态，
Figure FDA0002962128080000077
为优化后的位置，
Figure FDA0002962128080000078
为优化后的速度，
Figure FDA0002962128080000079
为优化后的陀螺仪误差，
Figure FDA00029621280800000710
为优化后的加速度计误差，将旋转矩阵转换为四元数，输出导航信息。
CN201910423422.2A 2019-05-21 2019-05-21 一种鲁棒立体视觉惯性预积分slam方法 Active CN110207693B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910423422.2A CN110207693B (zh) 2019-05-21 2019-05-21 一种鲁棒立体视觉惯性预积分slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910423422.2A CN110207693B (zh) 2019-05-21 2019-05-21 一种鲁棒立体视觉惯性预积分slam方法

Publications (2)

Publication Number Publication Date
CN110207693A CN110207693A (zh) 2019-09-06
CN110207693B true CN110207693B (zh) 2021-05-11

Family

ID=67787987

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910423422.2A Active CN110207693B (zh) 2019-05-21 2019-05-21 一种鲁棒立体视觉惯性预积分slam方法

Country Status (1)

Country Link
CN (1) CN110207693B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110986968B (zh) * 2019-10-12 2022-05-24 清华大学 三维重建中实时全局优化和错误回环判断的方法及装置
CN111156997B (zh) * 2020-03-02 2021-11-30 南京航空航天大学 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN112129287B (zh) * 2020-09-24 2022-06-10 北京华捷艾米科技有限公司 一种基于视觉惯性里程计处理的方法和相关装置
CN113516714A (zh) * 2021-07-15 2021-10-19 北京理工大学 基于imu预积分信息加速特征匹配的视觉slam方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827287A (zh) * 2018-04-10 2018-11-16 南京航空航天大学 一种复杂环境下的鲁棒视觉slam***
CN109307508A (zh) * 2018-08-29 2019-02-05 中国科学院合肥物质科学研究院 一种基于多关键帧的全景惯导slam方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9766074B2 (en) * 2008-03-28 2017-09-19 Regents Of The University Of Minnesota Vision-aided inertial navigation
US9658070B2 (en) * 2014-07-11 2017-05-23 Regents Of The University Of Minnesota Inverse sliding-window filters for vision-aided inertial navigation systems
CN105783913A (zh) * 2016-03-08 2016-07-20 中山大学 一种融合车载多传感器的slam装置及其控制方法
CN107869989B (zh) * 2017-11-06 2020-02-07 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN109520497B (zh) * 2018-10-19 2022-09-30 天津大学 基于视觉和imu的无人机自主定位方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108827287A (zh) * 2018-04-10 2018-11-16 南京航空航天大学 一种复杂环境下的鲁棒视觉slam***
CN109307508A (zh) * 2018-08-29 2019-02-05 中国科学院合肥物质科学研究院 一种基于多关键帧的全景惯导slam方法

Also Published As

Publication number Publication date
CN110207693A (zh) 2019-09-06

Similar Documents

Publication Publication Date Title
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
Qin et al. Robust initialization of monocular visual-inertial estimation on aerial robots
CN109520497B (zh) 基于视觉和imu的无人机自主定位方法
Heng et al. Self-calibration and visual slam with a multi-camera system on a micro aerial vehicle
CN110084832B (zh) 相机位姿的纠正方法、装置、***、设备和存储介质
Liu et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN110595466B (zh) 轻量级的基于深度学习的惯性辅助视觉里程计实现方法
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
WO2020140431A1 (zh) 相机位姿确定方法、装置、电子设备及存储介质
CN114623817B (zh) 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知***及感知方法
Zhang et al. Vision-aided localization for ground robots
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN113516692A (zh) 一种多传感器融合的slam方法和装置
CN105324792A (zh) 用于估计移动元件相对于参考方向的角偏差的方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam***
Hong et al. Visual inertial odometry using coupled nonlinear optimization
CN111539982B (zh) 一种移动平台中基于非线性优化的视觉惯导初始化方法
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN112731503A (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
GR01 Patent grant
GR01 Patent grant