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

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

Info

Publication number
CN108827287A
CN108827287A CN201810315050.7A CN201810315050A CN108827287A CN 108827287 A CN108827287 A CN 108827287A CN 201810315050 A CN201810315050 A CN 201810315050A CN 108827287 A CN108827287 A CN 108827287A
Authority
CN
China
Prior art keywords
moment
carrier
axis
coordinate
matrix
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.)
Granted
Application number
CN201810315050.7A
Other languages
English (en)
Other versions
CN108827287B (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

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)
  • Electromagnetism (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (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时刻加速度计数据fn b b(k)、陀螺仪数据ωb nb(k)和视觉传感器数据S(k)；
步骤2：利用惯性传感器数据fn b b(k)和ωb nb(k)递推预测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时刻导航信息ξ(k)的预测方法为：
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量， 和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
其中，
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
其中，
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
3)位置预测公式如下：
其中，k时刻的位置
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏和陀螺仪零偏预测公式如下：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
优选地，步骤3中计算预测值与图像匹配结果得到的载体位姿变化量的方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点包含关键点、描述子与机体坐标系下的坐标
为特征点点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息预测k时刻的视觉传感器数据S(k)中每个特征点在导航坐标系中的坐标
为特征点在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
其中，j＝1,2,…,numk
numk为以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算与的汉明距离以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，为k时刻预测的导航坐标系下的坐标值，为匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
上式中表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
优选地，步骤4中图像匹配质量判断模型具体为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度和分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足。
其中，α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
优选地，步骤4中进行图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
优选地，步骤4中求解载体位姿的方法为：
使用进行累乘得到
上式中表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
令iγn(k)、iθn(k)、iψn(k)、和分别为使用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
载体位置计算公式如下：
得到SLAM输出的位姿M(k)：
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
优选地，步骤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
其中，
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时刻的一步预测均方差；
G为滤波器k-1时刻的滤波器噪声系数矩阵；
W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
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
其中，
其中，
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
为k时刻的量测噪声，diag表示矩阵对角化，其中 分别为的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
其中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
为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时刻加速度计数据陀螺仪数据和视觉传感器数据S(k)；
所述视觉传感器为为双目摄像机或RGB-D摄像机，上述步骤中采集k时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在机体坐标系下的真实世界坐标值。
步骤2：利用惯性传感器数据和递推预测k时刻载体的导航信息得到预测值；
其中，载体的导航信息包括四元数姿态角、三维位置和速度信息，四元数姿态角包括横滚角、俯仰角和航向角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，顺时针方向为正，俯仰角为载体绕导航坐标系的X轴方向转动的角度，逆时针方向为正，航向角为载体绕导航坐标系的Z轴方向转动的角度，逆时针方向为正；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合。
获得载体导航信息预测值的具体方法为：
将k-1时刻载体的导航信息定义为ξ(k-1)，则载体k时刻导航信息ξ(k)的预测方法为：
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量， 和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
其中，
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
其中，
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
3)位置预测公式如下：
其中，k时刻的位置
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏和陀螺仪零偏预测公式如下：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
步骤3：使用视觉SLAM位姿匹配的方法以k时刻的预测值作为图像匹配的位姿起点匹配视觉传感器数据S(k)，求解通过图像匹配得到的载体位姿变化；具体计算方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点包含关键点、描述子与机体坐标系下的坐标
为特征点点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息预测k时刻的视觉传感器数据S(k)中每个特征点在导航坐标系中的坐标
为特征点在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
其中，j＝1,2,…,numk
numk为以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算与的汉明距离以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，为k时刻预测的导航坐标系下的坐标值，为匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
上式中表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
步骤4：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，输出载体导航信息的预测值，并跳转至步骤1；图像匹配质量满足条件时，进行视觉SLAM位姿计算求解载体位姿；
其中图像匹配质量判断模型为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度和分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足。
其中α和δ为根据传感器误差特性与环境特性确定的常量，具体为：α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
求解载体位姿的方法为：
使用进行累乘得到
上式中表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
令iγn(k)、iθn(k)、iψn(k)、和分别为使用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
载体位置计算公式如下：
得到SLAM输出的位姿M(k)：
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
步骤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
其中，
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时刻的一步预测均方差；
G为滤波器k-1时刻的滤波器噪声系数矩阵；
W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
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
其中，
其中，
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
为k时刻的量测噪声，diag表示矩阵对角化，其中 分别为的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
其中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
为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 (10)

1.一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：包括固定于一载体上的惯性传感器和视觉传感器，所述惯性传感器包括加速度计和陀螺仪，所述SLAM系统的工作方法包括以下步骤：
步骤1：周期采集k时刻加速度计数据陀螺仪数据和视觉传感器数据S(k)；
步骤2：利用惯性传感器数据和递推预测k时刻载体的导航信息得到预测值；
步骤3：使用视觉SLAM位姿匹配的方法以k时刻的预测值作为图像匹配的位姿起点匹配视觉传感器数据S(k)，求解通过图像匹配得到的载体位姿变化；
步骤4：构建图像匹配质量判定模型，对图像匹配的质量进行判断；图像匹配质量不满足条件时，输出载体导航信息的预测值，并跳转至步骤1；图像匹配质量满足条件时，进行视觉SLAM位姿计算求解载体位姿；
步骤5：将载体导航信息的预测值与解得的载体位姿信息进行卡尔曼滤波，对导航信息进行校正并输出；
步骤6：更新地图，并跳转至步骤1。
2.根据权利要求1所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：载体的导航信息包括四元数姿态角、三维位置和速度信息，四元数姿态角包括横滚角、俯仰角和航向角；具体定义如下：横滚角为载体绕导航坐标系的Y轴方向的转动的角度，顺时针方向为正，俯仰角为载体绕导航坐标系的X轴方向转动的角度，逆时针方向为正，航向角为载体绕导航坐标系的Z轴方向转动的角度，逆时针方向为正；
坐标系的定义如下：以当前时刻载体的位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的右向、前向和天向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与初始时刻载体的右向、前向和天向重合。
3.根据权利要求2所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：所述视觉传感器为双目摄像机或RGB-D摄像机，步骤1中采集k时刻的视觉传感器数据为点云数据，每个像素点包含像素值以及在机体坐标系下的真实世界坐标值。
4.根据权利要求3所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤2中获得载体导航信息预测值的方法为：
将k-1时刻载体的导航信息定义为ξ(k-1)，则载体k时刻导航信息的预测方法为：
上标T表示矩阵的转置，其中，qo(k-1)、q1(k-1)、q2(k-1)、q3(k-1)、和分别为k-1时刻载体的机体系相对于导航系的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量， 和分别为k时刻预测的载体的机体系相对于导航系下的姿态四元数、X轴坐标、Y轴坐标、Z轴坐标、X轴速度分量、Y轴速度分量和Z轴速度分量；
由此得到以下结果：
1)姿态四元数预测公式如下：
其中，
为k时刻预测的姿态四元数，上标T表示矩阵的转置；
Q(k-1)＝[qo(k-1) q1(k-1) q2(k-1) q3(k-1)]T
为k-1时刻的姿态四元数；
ΔT为离散采样周期；
通过下式计算：
其中通过下式计算：
其中，为k时刻陀螺仪读取的视觉传感器载体的机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，为k时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量；
2)速度预测公式如下：
其中，
为k时刻加速度计读取的视觉传感器载体的机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；
为k时刻加速度计零偏在机体系X、Y、Z轴上的分量；
g＝[0 0 g]T
g为当地重力加速度值；
为k时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为k-1时刻机体系相对于导航系的线速度在导航系X、Y、Z轴上的分量；
为机体系到导航系之间的姿态矩阵，通过下式计算：
3)位置预测公式如下：
其中，k时刻的位置
分别为飞行器k时刻在导航系X、Y、Z轴上的位置坐标；
k-1时刻的位置
分别为飞行器k-1时刻在导航系X、Y、Z轴上的位置坐标；
4)加速度计零偏和陀螺仪零偏预测公式如下：
其中，为k-1时刻加速度计零偏在机体系X、Y、Z轴上的分量；为k-1时刻陀螺仪的零偏在机体系X、Y、Z轴上的分量。
5.根据权利要求4所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤3中计算预测值与图像匹配结果得到的载体位姿变化量的方法为：
1)获取k时刻视觉信息，并提取ORB特征点得到视觉传感器数据S(k)，特征点包含关键点、描述子与机体坐标系下的坐标
为特征点点在机体系下X、Y、Z轴值，其中i代表第i个特征点；
2)如果该时刻数据为第一帧数据或丢失后第一帧数据，则认为为初始帧，跳转至步骤4；
3)利用k时刻载体预测的导航信息预测k时刻的视觉传感器数据S(k)中每个特征点在导航坐标系中的坐标
为特征点在导航系下X、Y、Z轴值，其中i代表视觉传感器数据S中第i个特征点；
4)设置距离阈值L1，建立以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆球，利用Brute暴力匹配方法匹配k时刻地图数据M(k)中每个特征点坐标值处于此圆球内的特征点
其中，j＝1,2,…,numk
numk为以在k时刻预测的导航坐标系下的坐标值为圆心，L1为半径的圆中包含的M(k)中特征点的数量；
通过计算与的汉明距离以最小的汉明距离的两倍为阈值L1，剔除高于两倍阈值L1的特征点匹配，再进行RANSAC随机采样一致性方法剔除外点，得到最终匹配的特征点对；
5)利用非线性优化方法，在得到匹配点对后，将k时刻的特征点位置与对应匹配的k时刻地图数据M(k)的特征点位置之差的绝对值之和作为优化目标，为k时刻预测的导航坐标系下的坐标值，为匹配成功的M(k)中特征点在导航坐标系下的坐标值，n为匹配成功的数量，ζ为随机位置扰动，公式如下：
使用图优化方法不断优化变换矩阵使得优化目标最小，以达到最小值或者迭代50次为止，输出变换矩阵
上式中表示从k-1时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
6.根据权利要求5所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中图像匹配质量判断模型具体为：
根据步骤3得到当前图像与上一时刻的图像匹配点对的数量n(k)，并求出载体在k-1时刻到k时刻依据SLAM得到载体的机体系相对于导航系的速度和分别为k-1时刻到k时刻载体的导航坐标系下依据SLAM得到载体的机体系相对于导航系的右向、前向和天向速度信息，具体公式如下：
其中，ΔT为从k-1时刻到k时刻的现实时间；
所述图像匹配质量判断模型具有两种判断条件：
条件1：当前图像与地图拥有α个以上的匹配点，即：
条件2：使用SLAM求解位k时刻姿变换矩阵后除以两帧图像的时间间隔求取速度，将该速度与k时刻惯性递推预测的载体导航信息中速度进行对比，其差值小于递推速度的δ，即：
初始帧满足条件1认为满足图像匹配质量判断模型，非初始帧需同时满足条件1与条件2的情况下满足图像匹配质量判断模型，其余情况均为不满足。
其中，α＝0.8*nS(k)
nS(k)为k时刻视觉传感器数据S(k)包含的特征点数量，
7.根据权利要求5所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中进行图像匹配质量判断的具体执行方法如下：
先判断是否为初始帧，为初始帧且满足图像匹配质量判断模型，则跳转至步骤6；
若为非初始帧，根据图像匹配质量判断模型，对图像匹配的质量进行判断，如果不满足质量判定模型，从而无法利用SLAM方法求解位姿，此时使用载体导航信息的预测值作为当前时刻的载体位姿，并跳转至步骤1；
非初始帧且满足图像匹配质量判断模型的判断条件，则进行视觉SLAM位姿计算求解载体位姿。
8.根据权利要求5所述的一种复杂环境下的鲁棒视觉SLAM系统，其特征在于：步骤4中求解载体位姿的方法为：
使用进行累乘得到
上式中表示从初始时刻到k时刻变换矩阵，包含一个旋转矩阵R，其中R包含9个旋转元素，同时还包含一个平移矩阵T，其中包含三个平移元素。
令iγn(k)、iθn(k)、iψn(k)、和分别为使用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
载体位置计算公式如下：
得到SLAM输出的位姿M(k)：
取k时刻载体机体系相对于导航系的航向角、X轴坐标、Y轴坐标和Z轴坐标作为量测信息：
9.根据权利要求8所述的一种复杂环境下的鲁棒视觉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
其中，
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时刻的一步预测均方差；
G为滤波器k-1时刻的滤波器噪声系数矩阵；
W为k-1时刻状态噪声，εωx、εωy和εωz分别为和的模型噪声，εfx、εfy和εfz分别为和的模型噪声，和分别为的噪声标准差，和分别为 的噪声标准差；
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
其中，
其中，
H(k)为k时刻量测矩阵，K(k)为k时刻的滤波增益；
为k时刻的量测噪声，diag表示矩阵对角化，其中 分别为iψn的噪声；03×4为3×4的零矩阵，03×3为3×3的零矩阵，01×3为1×3的零矩阵，03×9为3×9的零矩阵，01×9为1×9的零矩阵；
3)计算k时扩展卡尔曼滤波器状态估计值
其中，
为k时刻状态量的估计值，为k-1到k时刻的状态变量一步预测值，使用步骤3的预测公式计算得到：
为k时刻的量测值；
4)计算k时刻扩展卡尔曼滤波器估计均方误差P(k|k)：
P(k|k)＝[I-K(k)H(k)]P(k|k-1)
式中，P(k|k)为k时刻估计均方误差，I为单位矩阵。
10.根据权利要求9所述的一种复杂环境下的鲁棒视觉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 true CN108827287A (zh) 2018-11-16
CN108827287B 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)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110207693A (zh) * 2019-05-21 2019-09-06 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110887486A (zh) * 2019-10-18 2020-03-17 南京航空航天大学 一种基于激光线辅助的无人机视觉导航定位方法
CN112197741A (zh) * 2020-12-04 2021-01-08 华南理工大学 基于扩展卡尔曼滤波的无人机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
LEUTENEGGER .ET AL: "Keyframe-based Visual-Inertial SLAM using Nonlinear Optimization", 《THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH》 *

Cited By (5)

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

Also Published As

Publication number Publication date
CN108827287B (zh) 2021-12-21

Similar Documents

Publication Publication Date Title
CN106708066B (zh) 基于视觉/惯导的无人机自主着陆方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN109540126A (zh) 一种基于光流法的惯性视觉组合导航方法
WO2021180128A1 (zh) 一种基于rgbd传感器和imu传感器的定位方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN111780754B (zh) 基于稀疏直接法的视觉惯性里程计位姿估计方法
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
Weiss et al. Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN107796391A (zh) 一种捷联惯性导航系统/视觉里程计组合导航方法
CN106056664A (zh) 一种基于惯性和深度视觉的实时三维场景重构系统及方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN108253963A (zh) 一种基于多传感器融合的机器人自抗扰定位方法以及定位系统
CN110398245A (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN108846857A (zh) 视觉里程计的测量方法及视觉里程计
CN110455301A (zh) 一种基于惯性测量单元的动态场景slam方法
CN107014376A (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN108827287A (zh) 一种复杂环境下的鲁棒视觉slam系统
CN108827339A (zh) 一种基于惯性辅助的高效视觉里程计
CN114234967B (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