CN108827287B - 一种复杂环境下的鲁棒视觉slam系统 - Google Patents

一种复杂环境下的鲁棒视觉slam系统 Download PDF

Info

Publication number
CN108827287B
CN108827287B CN201810315050.7A CN201810315050A CN108827287B CN 108827287 B CN108827287 B CN 108827287B CN 201810315050 A CN201810315050 A CN 201810315050A CN 108827287 B CN108827287 B CN 108827287B
Authority
CN
China
Prior art keywords
moment
axis
carrier
navigation
time
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
CN201810315050.7A
Other languages
English (en)
Other versions
CN108827287A (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 CN201810315050.7A priority Critical patent/CN108827287B/zh
Publication of CN108827287A publication Critical patent/CN108827287A/zh
Application granted granted Critical
Publication of CN108827287B publication Critical patent/CN108827287B/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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders

Landscapes

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

Abstract

本发明公开了一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：包括固定于一载体上的惯性传感器和视觉传感器，所述惯性传感器包括加速度计和陀螺仪，利用惯性传感器预测载体导航信息，并与视觉传感器的数据进行匹配判断图像匹配质量，并根据判断结果输出不同的结果。本发明提供的一种复杂环境下的鲁棒视觉SLAM系统的优点在于：能够有效解决不良环境中视觉SLAM失败的问题，获得高鲁棒性以及高精度的视觉载体导航信息，并构建精准地图，具有良好的推广前景。

Description

一种复杂环境下的鲁棒视觉SLAM系统
技术领域
本发明涉及视觉导航技术领域，尤其涉及一种复杂环境下的鲁棒视觉SLAM系统。
背景技术
实时定位与构图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人实际应用中的关键技术。视觉传感器可以直接的感知外界，具有高自主性，视觉SLAM方法是机器人SLAM方法中应用主流且具有极大发展潜质的方法。
目前大多视觉SLAM技术都需要较好的光照以及高纹理环境，并且在整个SLAM过程中，环境不能出现恶劣情况，否则丢失后就无法找回从而继续进行SLAM过程。而实际应用环境大多存在着明暗变化以及运动造成的模糊情况。机器人SLAM方法中，通常利用视觉传感器获取的图像点云数据提取特征点进行匹配来求解位姿，而视觉传感器受到的环境变化会直接影响匹配的效果，造成较大的误差甚至SLAM无法继续进行。
发明内容
本发明所要解决的技术问题在于提供一种能够在复杂环境下获得高鲁棒性的视觉SLAM系统。
本发明是通过以下技术方案解决上述技术问题的：
一种复杂环境下的鲁棒视觉SLAM系统，包括固定于一载体上的惯性传感器和视觉传感器，所述惯性传感器包括加速度计和陀螺仪，所述SLAM系统的工作方法包括以下步骤：
步骤1：周期采集k时刻加速度计数据
Figure GDA0002958119160000011
陀螺仪数据
Figure GDA0002958119160000012
和视觉传感器数据S(k)；
步骤2：利用惯性传感器数据
Figure GDA0002958119160000013
和
Figure GDA0002958119160000014
递推预测k时刻载体的导航信息得到预测值；
步骤3：使用视觉SLAM位姿匹配的方法以k时刻的预测值作为图像匹配的位姿起点匹配视觉传感器数据S(k)，求解通过图像匹配得到的载体位姿变化；
步骤4：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，输出载体导航信息的预测值，并跳转至步骤1；图像匹配质量满足条件时，进行视觉SLAM位姿计算求解载体位姿；
步骤5：将载体导航信息的预测值与解得的载体位姿信息进行卡尔曼滤波，对导航信息进行校正并输出；
步骤6：更新地图，并跳转至步骤1。
优选地，载体的导航信息包括四元数姿态角、三维位置和速度信息，四元数姿态角包括横滚角、俯仰角和航向角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，顺时针方向为正，俯仰角为载体绕导航坐标系的X轴方向转动的角度，逆时针方向为正，航向角为载体绕导航坐标系的Z轴方向转动的角度，逆时针方向为正；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合。
优选地，所述视觉传感器为双目摄像机或RGB-D摄像机，步骤1中采集k时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在机体坐标系下的真实世界坐标值。
优选地，步骤2中获得载体导航信息预测值的方法为：
将k-1时刻载体的导航信息定义为ξ(k-1)，则载体k时刻导航信息
Figure GDA0002958119160000021
的预测方法为：
Figure GDA0002958119160000022
Figure GDA0002958119160000023
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、
Figure GDA0002958119160000024
和
Figure GDA0002958119160000025
分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量，
Figure GDA0002958119160000031
Figure GDA0002958119160000032
和
Figure GDA0002958119160000033
分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
Figure GDA0002958119160000034
其中，
Figure GDA0002958119160000035
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
Figure GDA0002958119160000036
通过下式计算：
Figure GDA0002958119160000037
其中
Figure GDA0002958119160000038
通过下式计算：
Figure GDA0002958119160000039
其中，
Figure GDA00029581191600000310
为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，
Figure GDA00029581191600000311
Figure GDA00029581191600000312
为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
Figure GDA00029581191600000313
其中，
Figure GDA0002958119160000041
Figure GDA0002958119160000042
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
Figure GDA0002958119160000043
Figure GDA0002958119160000044
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
Figure GDA0002958119160000045
Figure GDA0002958119160000046
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure GDA0002958119160000047
Figure GDA0002958119160000048
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure GDA0002958119160000049
为机体系到导航系之间的姿态矩阵，通过下式计算：
Figure GDA00029581191600000410
Figure GDA00029581191600000411
3)位置预测公式如下：
Figure GDA00029581191600000412
其中，k时刻的位置
Figure GDA00029581191600000413
Figure GDA00029581191600000414
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
Figure GDA0002958119160000051
Figure GDA0002958119160000052
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏
Figure GDA0002958119160000053
和陀螺仪零偏
Figure GDA0002958119160000054
预测公式如下：
Figure GDA0002958119160000055
Figure GDA0002958119160000056
其中，
Figure GDA0002958119160000057
为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；
Figure GDA0002958119160000058
为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
优选地，步骤3中计算预测值与图像匹配结果得到的载体位姿变化量的方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点
Figure GDA0002958119160000059
包含关键点、描述子与机体坐标系下的坐标
Figure GDA00029581191600000510
Figure GDA00029581191600000511
为特征点
Figure GDA00029581191600000512
点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息
Figure GDA00029581191600000513
预测k时刻的视觉传感器数据S(k)中每个特征点
Figure GDA00029581191600000514
在导航坐标系中的坐标
Figure GDA00029581191600000515
Figure GDA00029581191600000516
Figure GDA00029581191600000517
为特征点
Figure GDA00029581191600000518
在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以
Figure GDA0002958119160000061
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
Figure GDA0002958119160000062
其中，j＝1,2,…,numk
numk为以
Figure GDA0002958119160000064
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算
Figure GDA0002958119160000065
与
Figure GDA0002958119160000066
的汉明距离
Figure GDA0002958119160000067
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，
Figure GDA0002958119160000068
为k时刻预测的导航坐标系下的坐标值，
Figure GDA0002958119160000069
为
Figure GDA00029581191600000610
匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
Figure GDA00029581191600000611
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
Figure GDA00029581191600000612
Figure GDA00029581191600000613
上式中
Figure GDA00029581191600000614
表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
优选地，步骤4中图像匹配质量判断模型具体为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度
Figure GDA0002958119160000071
和
Figure GDA0002958119160000072
分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
Figure GDA0002958119160000073
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure GDA0002958119160000074
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
Figure GDA0002958119160000075
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足。
其中，α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
Figure GDA0002958119160000076
优选地，步骤4中进行图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
优选地，步骤4中求解载体位姿的方法为：
使用
Figure GDA0002958119160000081
进行累乘得到
Figure GDA0002958119160000082
Figure GDA0002958119160000083
上式中
Figure GDA0002958119160000084
表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
令iγn(k)、iθn(k)、iψn(k)、
Figure GDA0002958119160000085
和
Figure GDA0002958119160000086
分别为使用SLAM求得的k时刻载体机体系相对于导航系的横滚角、俯仰角、航向角、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
则姿态角计算公式如下：
[iγn(k) iθn(k) iψn(k)]T＝[arctan(R23/R33)-arcsin(R13)arctan(R12/R11)]T
载体位置计算公式如下：
Figure GDA0002958119160000087
得到SLAM输出的位姿Visual(k)：
Figure GDA0002958119160000088
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
Figure GDA0002958119160000089
优选地，步骤5中通过卡尔曼滤波进行导航信息校正的方法为：
1)计算一步预测均方误差P(k|k-1)：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
其中，
Figure GDA0002958119160000091
Figure GDA0002958119160000092
Figure GDA0002958119160000093
I3×3为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
Figure GDA0002958119160000094
G为滤波器k-1时刻的滤波器噪声系数矩阵；
Figure GDA0002958119160000095
W为k-1时刻状态噪声，εωx、εωy和εωz分别为
Figure GDA0002958119160000096
和
Figure GDA0002958119160000097
的模型噪声，εfx、εfy和εfz分别为
Figure GDA0002958119160000098
和
Figure GDA0002958119160000099
的模型噪声，
Figure GDA00029581191600000910
和
Figure GDA00029581191600000911
分别为
Figure GDA00029581191600000912
Figure GDA0002958119160000101
的噪声标准差，
Figure GDA0002958119160000102
和
Figure GDA0002958119160000103
分别为
Figure GDA0002958119160000104
的噪声标准差；
2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
其中，
Figure GDA0002958119160000105
其中，
Figure GDA0002958119160000106
Figure GDA0002958119160000107
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
Figure GDA0002958119160000108
为k时刻的量测噪声，diag表示矩阵对角化，其中
Figure GDA0002958119160000109
分别为
Figure GDA00029581191600001010
iψn的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
Figure GDA00029581191600001011
Figure GDA00029581191600001012
其中，
Figure GDA00029581191600001013
Figure GDA00029581191600001014
为k时刻状态量的估计值，
Figure GDA00029581191600001015
为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
Figure GDA00029581191600001016
为k时刻的量测值；
4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
优选地，步骤6所述的更新地图的方法为：
先判断是否为初始帧，如果为初始帧，则直接将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标全部载入地图中；
如果为非初始帧，将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标载入地图中，并处理与视觉传感器数据S(k-1)匹配成功的点对形成的冗余数据，存在匹配点对的导航坐标系坐标为所有包含该特征点的图像数据中包含的导航坐标系坐标值的平均。
本发明提供的一种复杂环境下的鲁棒视觉SLAM系统的优点在于：能够有效解决不良环境中视觉SLAM失败的问题，获得高鲁棒性以及高精度的视觉载体导航信息，并构建精准地图，具有良好的推广前景。
附图说明
图1是本发明的实施例所提供的一种复杂环境下的鲁棒视觉SLAM系统的原理框图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。
本实施例提供一种复杂环境下的鲁棒视觉SLAM系统，包括固定于一载体上的惯性传感器和视觉传感器，所述惯性传感器包括加速度计和陀螺仪。参考图1，所述SLAM系统的工作方法包括以下步骤：
步骤1：周期采集k时刻加速度计数据
Figure GDA0002958119160000111
陀螺仪数据
Figure GDA0002958119160000112
和视觉传感器数据S(k)；
所述视觉传感器为为双目摄像机或RGB-D摄像机，上述步骤中采集k时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在机体坐标系下的真实世界坐标值。
步骤2：利用惯性传感器数据
Figure GDA0002958119160000121
和
Figure GDA0002958119160000122
递推预测k时刻载体的导航信息得到预测值；
其中，载体的导航信息包括四元数姿态角、三维位置和速度信息，四元数姿态角包括横滚角、俯仰角和航向角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，顺时针方向为正，俯仰角为载体绕导航坐标系的X轴方向转动的角度，逆时针方向为正，航向角为载体绕导航坐标系的Z轴方向转动的角度，逆时针方向为正；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合。
获得载体导航信息预测值的具体方法为：
将k-1时刻载体的导航信息定义为ξ(k-1)，则载体k时刻导航信息
Figure GDA0002958119160000123
的预测方法为：
Figure GDA0002958119160000124
Figure GDA0002958119160000125
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、
Figure GDA0002958119160000126
和
Figure GDA0002958119160000127
分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量，
Figure GDA0002958119160000128
Figure GDA0002958119160000129
和
Figure GDA00029581191600001210
分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
Figure GDA00029581191600001211
其中，
Figure GDA0002958119160000131
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
Figure GDA0002958119160000132
通过下式计算：
Figure GDA0002958119160000133
其中
Figure GDA0002958119160000134
通过下式计算：
Figure GDA0002958119160000135
其中，
Figure GDA0002958119160000136
为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，
Figure GDA0002958119160000137
Figure GDA0002958119160000138
为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
Figure GDA0002958119160000139
其中，
Figure GDA00029581191600001310
Figure GDA00029581191600001311
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
Figure GDA00029581191600001312
Figure GDA00029581191600001313
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
Figure GDA0002958119160000141
Figure GDA0002958119160000142
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure GDA0002958119160000143
Figure GDA0002958119160000144
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure GDA0002958119160000145
为机体系到导航系之间的姿态矩阵，通过下式计算：
Figure GDA0002958119160000146
Figure GDA0002958119160000147
3)位置预测公式如下：
Figure GDA0002958119160000148
其中，k时刻的位置
Figure GDA0002958119160000149
Figure GDA00029581191600001410
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
Figure GDA00029581191600001411
Figure GDA00029581191600001412
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏
Figure GDA00029581191600001413
和陀螺仪零偏
Figure GDA00029581191600001414
预测公式如下：
Figure GDA00029581191600001415
Figure GDA00029581191600001416
其中，
Figure GDA0002958119160000151
为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；
Figure GDA0002958119160000152
为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
步骤3：使用视觉SLAM位姿匹配的方法以k时刻的预测值作为图像匹配的位姿起点匹配视觉传感器数据S(k)，求解通过图像匹配得到的载体位姿变化；具体计算方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点
Figure GDA0002958119160000153
包含关键点、描述子与机体坐标系下的坐标
Figure GDA0002958119160000154
Figure GDA0002958119160000155
为特征点
Figure GDA0002958119160000156
点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息
Figure GDA0002958119160000157
预测k时刻的视觉传感器数据S(k)中每个特征点
Figure GDA0002958119160000158
在导航坐标系中的坐标
Figure GDA0002958119160000159
Figure GDA00029581191600001510
Figure GDA00029581191600001511
为特征点
Figure GDA00029581191600001512
在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以
Figure GDA00029581191600001513
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
Figure GDA00029581191600001514
其中，j＝1,2,…,numk
numk为以
Figure GDA00029581191600001515
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算
Figure GDA0002958119160000161
与
Figure GDA0002958119160000162
的汉明距离
Figure GDA0002958119160000163
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，
Figure GDA0002958119160000164
为k时刻预测的导航坐标系下的坐标值，
Figure GDA0002958119160000165
为
Figure GDA0002958119160000166
匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
Figure GDA0002958119160000167
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
Figure GDA0002958119160000168
Figure GDA0002958119160000169
上式中
Figure GDA00029581191600001610
表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
步骤4：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，输出载体导航信息的预测值，并跳转至步骤1；图像匹配质量满足条件时，进行视觉SLAM位姿计算求解载体位姿；
其中图像匹配质量判断模型为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度
Figure GDA00029581191600001611
和
Figure GDA00029581191600001612
分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
Figure GDA0002958119160000171
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure GDA0002958119160000172
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
Figure GDA0002958119160000173
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足。
其中α和δ为根据传感器误差特性与环境特性确定的常量，具体为：α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
Figure GDA0002958119160000174
图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
求解载体位姿的方法为：
使用
Figure GDA0002958119160000181
进行累乘得到
Figure GDA0002958119160000182
Figure GDA0002958119160000183
上式中
Figure GDA0002958119160000184
表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
令iγn(k)、iθn(k)、iψn(k)、
Figure GDA0002958119160000185
和
Figure GDA0002958119160000186
分别为使用SLAM求得的k时刻载体机体系相对于导航系的横滚角、俯仰角、航向角、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
则姿态角计算公式如下：
[iγn(k) iθn(k) iψn(k)]T＝[arctan(R23/R33)-arcsin(R13)arctan(R12/R11)]T
载体位置计算公式如下：
Figure GDA0002958119160000187
得到SLAM输出的位姿Visual(k)：
Figure GDA0002958119160000188
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
Figure GDA0002958119160000189
步骤5：将载体导航信息的预测值与解得的载体位姿信息进行卡尔曼滤波，对导航信息进行校正并输出；具体方法为：
1)计算一步预测均方误差P(k|k-1)：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
其中，
Figure GDA0002958119160000191
Figure GDA0002958119160000192
Figure GDA0002958119160000193
I3×3为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
Figure GDA0002958119160000194
G为滤波器k-1时刻的滤波器噪声系数矩阵；
Figure GDA0002958119160000195
W为k-1时刻状态噪声，εωx、εωy和εωz分别为
Figure GDA0002958119160000196
和
Figure GDA0002958119160000197
的模型噪声，εfx、εfy和εfz分别为
Figure GDA0002958119160000198
和
Figure GDA0002958119160000199
的模型噪声，
Figure GDA00029581191600001910
和
Figure GDA00029581191600001911
分别为
Figure GDA00029581191600001912
Figure GDA00029581191600001913
的噪声标准差，
Figure GDA00029581191600001914
和
Figure GDA00029581191600001915
分别为
Figure GDA00029581191600001916
的噪声标准差；
2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
其中，
Figure GDA0002958119160000201
其中，
Figure GDA0002958119160000202
Figure GDA0002958119160000203
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
Figure GDA0002958119160000204
为k时刻的量测噪声，diag表示矩阵对角化，其中
Figure GDA0002958119160000205
分别为
Figure GDA0002958119160000206
iψn的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
Figure GDA0002958119160000207
Figure GDA0002958119160000208
其中，
Figure GDA0002958119160000209
Figure GDA00029581191600002010
为k时刻状态量的估计值，
Figure GDA00029581191600002011
为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
Figure GDA00029581191600002012
为k时刻的量测值；
4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
步骤6：更新地图，并跳转至步骤1；
更新地图的方法为：先判断是否为初始帧，如果为初始帧，则直接将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标全部载入地图中；
如果为非初始帧，将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标载入地图中，并处理与视觉传感器数据S(k-1)匹配成功的点对形成的冗余数据，存在匹配点对的导航坐标系坐标为所有包含该特征点的图像数据中包含的导航坐标系坐标值的平均。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，在不脱离本发明的精神和原则的前提下，本领域普通技术人员对本发明所做的任何修改、等同替换、改进等，均应落入本发明权利要求书确定的保护范围之内。

Claims (6)

1.一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：包括固定于一载体上的惯性传感器和视觉传感器，所述惯性传感器包括加速度计和陀螺仪，所述SLAM系统的工作方法包括以下步骤：
步骤1：周期采集k-1时刻加速度计数据
Figure FDA0002958119150000011
陀螺仪数据
Figure FDA0002958119150000012
和视觉传感器数据S(k)；
步骤2：利用惯性传感器数据
Figure FDA0002958119150000013
和
Figure FDA0002958119150000014
递推预测k时刻载体的导航信息得到预测值；
步骤3：使用视觉SLAM位姿匹配的方法以k时刻的预测值作为图像匹配的位姿起点匹配视觉传感器数据S(k)，求解通过图像匹配得到的载体位姿变化；
步骤4：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，输出载体导航信息的预测值，并跳转至步骤1；图像匹配质量满足条件时，进行视觉SLAM位姿计算求解载体位姿；
步骤5：将载体导航信息的预测值与解得的载体位姿信息进行卡尔曼滤波，对导航信息进行校正并输出；
步骤6：更新地图，并跳转至步骤1；
载体的导航信息包括四元数姿态角、三维位置和速度信息，四元数姿态角包括横滚角、俯仰角和航向角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，顺时针方向为正，俯仰角为载体绕导航坐标系的X轴方向转动的角度，逆时针方向为正，航向角为载体绕导航坐标系的Z轴方向转动的角度，逆时针方向为正；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合；
所述视觉传感器为双目摄像机或RGB-D摄像机，步骤1中采集k时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在机体坐标系下的真实世界坐标值；
步骤2中获得载体导航信息预测值的方法为：
将k-1时刻载体的导航信息定义为ξ(k-1)，则载体k时刻导航信息
Figure FDA0002958119150000021
的预测方法为：
Figure FDA0002958119150000022
Figure FDA0002958119150000023
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、
Figure FDA0002958119150000024
和
Figure FDA0002958119150000025
分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量，
Figure FDA0002958119150000026
Figure FDA0002958119150000027
和
Figure FDA0002958119150000028
分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
Figure FDA0002958119150000029
其中，
Figure FDA00029581191500000210
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
Figure FDA00029581191500000211
通过下式计算：
Figure FDA00029581191500000212
其中
Figure FDA00029581191500000213
通过下式计算：
Figure FDA0002958119150000031
其中，
Figure FDA0002958119150000032
为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，
Figure FDA0002958119150000033
Figure FDA0002958119150000034
为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
Figure FDA0002958119150000035
其中，
Figure FDA0002958119150000036
Figure FDA0002958119150000037
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
Figure FDA0002958119150000038
Figure FDA0002958119150000039
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
Figure FDA00029581191500000310
Figure FDA00029581191500000311
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure FDA00029581191500000312
Figure FDA00029581191500000313
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
Figure FDA00029581191500000314
为机体系到导航系之间的姿态矩阵，通过下式计算：
Figure FDA0002958119150000041
Figure FDA0002958119150000042
3)位置预测公式如下：
Figure FDA0002958119150000043
其中，k时刻的位置
Figure FDA0002958119150000044
Figure FDA0002958119150000045
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
Figure FDA0002958119150000046
Figure FDA0002958119150000047
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏
Figure FDA0002958119150000048
和陀螺仪零偏
Figure FDA0002958119150000049
预测公式如下：
Figure FDA00029581191500000410
Figure FDA00029581191500000411
其中，
Figure FDA00029581191500000412
为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；
Figure FDA00029581191500000413
为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
步骤3中计算预测值与图像匹配结果得到的载体位姿变化量的方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点
Figure FDA00029581191500000414
包含关键点、描述子与机体坐标系下的坐标
Figure FDA00029581191500000415
Figure FDA00029581191500000416
为特征点
Figure FDA00029581191500000417
点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息
Figure FDA0002958119150000051
预测k时刻的视觉传感器数据S(k)中每个特征点
Figure FDA0002958119150000052
在导航坐标系中的坐标
Figure FDA0002958119150000053
Figure FDA0002958119150000054
Figure FDA0002958119150000055
为特征点
Figure FDA0002958119150000056
在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以
Figure FDA0002958119150000057
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
Figure FDA0002958119150000058
其中，j＝1,2,…,numk
numk为以
Figure FDA0002958119150000059
在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算
Figure FDA00029581191500000510
与
Figure FDA00029581191500000511
的汉明距离
Figure FDA00029581191500000512
以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，
Figure FDA00029581191500000513
为k时刻预测的导航坐标系下的坐标值，
Figure FDA00029581191500000514
为
Figure FDA00029581191500000515
匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
Figure FDA00029581191500000516
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
Figure FDA0002958119150000061
Figure FDA0002958119150000062
上式中
Figure FDA0002958119150000063
表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
2.根据权利要求1所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中图像匹配质量判断模型具体为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度
Figure FDA0002958119150000064
和
Figure FDA0002958119150000065
分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
Figure FDA0002958119150000066
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
Figure FDA0002958119150000067
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
Figure FDA0002958119150000068
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足；
其中，α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
Figure FDA0002958119150000071
3.根据权利要求1所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中进行图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
4.根据权利要求1所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中求解载体位姿的方法为：
使用
Figure FDA0002958119150000072
进行累乘得到
Figure FDA0002958119150000073
Figure FDA0002958119150000074
上式中
Figure FDA0002958119150000075
表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素；
令iγn(k)、iθn(k)、iψn(k)、
Figure FDA0002958119150000076
和
Figure FDA0002958119150000077
分别为使用SLAM求得的k时刻载体机体系相对于导航系的横滚角、俯仰角、航向角、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
则姿态角计算公式如下：
[iγn(k) iθn(k) iψn(k)]T＝[arctan(R23/R33) -arcsin(R13) arctan(R12/R11)]T
载体位置计算公式如下：
Figure FDA0002958119150000081
得到SLAM输出的位姿Visual(k)：
Figure FDA0002958119150000082
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
Figure FDA0002958119150000083
5.根据权利要求4所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤5中通过卡尔曼滤波进行导航信息校正的方法为：
1)计算一步预测均方误差P(k|k-1)：
P(k|k-1)＝A(k,k-1)P(k-1|k-1)A(k,k-1)T+G(k-1)W(k-1)G(k-1)T
其中，
Figure FDA0002958119150000084
Figure FDA0002958119150000085
Figure FDA0002958119150000086
I3×3为3×3的单位矩阵，I4×4为4×4的单位矩阵，03×3为3×3的零矩阵，03×4为3×4的零矩阵，A(k,k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵；P(k-1|k-1)为k-1时刻的状态估计均方差，P(k|k-1)为k-1时刻到k时刻的一步预测均方差；
Figure FDA0002958119150000091
G为滤波器k-1时刻的滤波器噪声系数矩阵；
Figure FDA0002958119150000092
W为k-1时刻状态噪声，εωx、εωy和εωz分别为
Figure FDA0002958119150000093
和
Figure FDA0002958119150000094
的模型噪声，εfx、εfy和εfz分别为
Figure FDA0002958119150000095
和
Figure FDA0002958119150000096
的模型噪声，
Figure FDA0002958119150000097
和
Figure FDA0002958119150000098
分别为
Figure FDA0002958119150000099
Figure FDA00029581191500000910
的噪声标准差，
Figure FDA00029581191500000911
和
Figure FDA00029581191500000912
分别为
Figure FDA00029581191500000913
的噪声标准差；
2)计算k时刻扩展卡尔曼滤波器滤波增益K(k)：
K(k)＝P(k|k-1)H(k)T[H(k)P(k|k-1)H(k)T+R(k)]-1
其中，
Figure FDA00029581191500000914
其中，
Figure FDA00029581191500000915
Figure FDA00029581191500000916
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
Figure FDA00029581191500000917
为k时刻的量测噪声，diag表示矩阵对角化，其中
Figure FDA00029581191500000918
分别为
Figure FDA0002958119150000101
iψn的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
Figure FDA0002958119150000102
Figure FDA0002958119150000103
其中，
Figure FDA0002958119150000104
Figure FDA0002958119150000105
为k时刻状态量的估计值，
Figure FDA0002958119150000106
为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
Figure FDA0002958119150000107
为k时刻的量测值；
4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
6.根据权利要求5所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤6所述的更新地图的方法为：
先判断是否为初始帧，如果为初始帧，则直接将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标全部载入地图中；
如果为非初始帧，将视觉传感器数据S(k)中的每个特征点以及求出的导航坐标系的坐标载入地图中，并处理与视觉传感器数据S(k-1)匹配成功的点对形成的冗余数据，存在匹配点对的导航坐标系坐标为所有包含该特征点的图像数据中包含的导航坐标系坐标值的平均。
CN201810315050.7A 2018-04-10 2018-04-10 一种复杂环境下的鲁棒视觉slam系统 Active CN108827287B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810315050.7A CN108827287B (zh) 2018-04-10 2018-04-10 一种复杂环境下的鲁棒视觉slam系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810315050.7A CN108827287B (zh) 2018-04-10 2018-04-10 一种复杂环境下的鲁棒视觉slam系统

Publications (2)

Publication Number Publication Date
CN108827287A CN108827287A (zh) 2018-11-16
CN108827287B true CN108827287B (zh) 2021-12-21

Family

ID=64155338

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810315050.7A Active CN108827287B (zh) 2018-04-10 2018-04-10 一种复杂环境下的鲁棒视觉slam系统

Country Status (1)

Country Link
CN (1) CN108827287B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110207693B (zh) * 2019-05-21 2021-05-11 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110887486B (zh) * 2019-10-18 2022-05-20 南京航空航天大学 一种基于激光线辅助的无人机视觉导航定位方法
CN112197741B (zh) * 2020-12-04 2021-02-26 华南理工大学 基于扩展卡尔曼滤波的无人机slam技术测量倾斜角系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104833352A (zh) * 2015-01-29 2015-08-12 西北工业大学 多介质复杂环境下高精度视觉/惯性组合导航方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
CN107193279A (zh) * 2017-05-09 2017-09-22 复旦大学 基于单目视觉和imu信息的机器人定位与地图构建系统
CN107796391A (zh) * 2017-10-27 2018-03-13 哈尔滨工程大学 一种捷联惯性导航系统/视觉里程计组合导航方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104833352A (zh) * 2015-01-29 2015-08-12 西北工业大学 多介质复杂环境下高精度视觉/惯性组合导航方法
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
CN106679648A (zh) * 2016-12-08 2017-05-17 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN107193279A (zh) * 2017-05-09 2017-09-22 复旦大学 基于单目视觉和imu信息的机器人定位与地图构建系统
CN107796391A (zh) * 2017-10-27 2018-03-13 哈尔滨工程大学 一种捷联惯性导航系统/视觉里程计组合导航方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Keyframe-based Visual-Inertial SLAM using Nonlinear Optimization;Leutenegger .et al;《The International Journal of Robotics Research》;20141215;第34卷(第3期);第314-334页 *

Also Published As

Publication number Publication date
CN108827287A (zh) 2018-11-16

Similar Documents

Publication Publication Date Title
CN111561923B (zh) 基于多传感器融合的slam制图方法、系统
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN111210463B (zh) 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN112240768A (zh) 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
CN112837352B (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN112577493B (zh) 一种基于遥感地图辅助的无人机自主定位方法及系统
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN115406447B (zh) 拒止环境下基于视觉惯性的四旋翼无人机自主定位方法
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN112284381B (zh) 视觉惯性实时初始化对准方法及系统
CN114593735A (zh) 一种位姿预测方法及装置
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN114842224A (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