CN106679648B - 一种基于遗传算法的视觉惯性组合的slam方法 - Google Patents
一种基于遗传算法的视觉惯性组合的slam方法 Download PDFInfo
- Publication number
- CN106679648B CN106679648B CN201611121973.6A CN201611121973A CN106679648B CN 106679648 B CN106679648 B CN 106679648B CN 201611121973 A CN201611121973 A CN 201611121973A CN 106679648 B CN106679648 B CN 106679648B
- Authority
- CN
- China
- Prior art keywords
- navigation
- coordinate system
- visual
- points
- robot
- 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
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于遗传算法的视觉惯性组合的SLAM方法,该方法步骤如下:统一视觉导航与惯性导航的坐标系,并对双目摄像机进行参数标定,从而根据图像的像素坐标来求解三维空间坐标;惯性导航进行独立解算;视觉导航解算;利用扩展卡尔曼滤波器对视觉导航信息与惯性导航信息融合,建立了系统滤波模型;考虑到定位的局部性,利用双目摄像机观测全局特征点路标,并基于遗传算法对地图特征进行数据关联,并将扩展后的状态向量反馈回滤波器中。本发明可以进行长时间、高精度的定位;并且由于加入了遗传算法改进地图的数据关联,使得在同步构建地图中精度提高很多。
Description
技术领域
本发明涉及导航领域,特别是涉及一种基于遗传算法的视觉惯性组合的SLAM方法。
背景技术
在陌生环境下自主移动的机器人成为目前机器人领域研究的热点问题,与传统机器人不同的是,自主式机器人不依赖人为控制,不受环境限制,在一些危险场所的紧急救援中,可以起到关键的作用。同时,机器人SLAM(simultaneous localization and mapping,即时定位与同步构图)不仅可以完成自身准确的定位,还能将环境特征提取出来,用以构建地图,在陌生环境中,机器人只需要经过有限次的行走就可以将整个环境地图构建,从而在实际情况中,有助于我们快速了解事故现场的情况。因此,研究使机器人能够高精度的定位导航以及环境地图的精确绘制具有十分重要的理论价值以及实际应用。
惯性导航是以经典力学里的牛顿第二定律为理论依据发展起来的,它通过惯性仪器陀螺仪和加速度计测量机器人的旋转角速度和加速度,通过积分求解其姿态,速度,位置信息。其稳定性高,隐蔽性好,不易受外部环境的影响,且完全依赖自身的惯性器件,具有良好的自主性。不过由于其总是通过当前时刻的信息量来推算下一时刻的信息,因此会产生一定量的累积误差。在使用惯性导航工作了一段时间后,其精度会逐渐发散。所以惯性导航不适合长时间,长距离的导航。
视觉导航是新兴的导航技术,主要是计算机视觉以及图像处理技术的高速发展。它通过摄像头拍摄图片,根据图像处理提取特征点进行运动建模,并实现三维重建。通过视觉里程计模型准确输出其位置以及航向信息。视觉传感器类似于人的眼睛,在所有传感器中探测范围最广,探测的信息最为丰富。并且由于视觉摄像头可以观测到全局特征路标,可以修正视觉里程计长时间工作的累积误差,其精度不会受时间影响。但是其易受环境条件的影响,如光照可能会影响某些摄像头。并且图像特征点提取的准确性、匹配的快速性,以及数据关联的准确性实时性都会极大的影响到视觉导航,并且视觉导航的数据更新较慢。
发明内容
发明目的:为了解决现有的机器人SLAM技术的不足,本发明提出的一种基于遗传算法的视觉惯性组合导航的SLAM方法。
技术方案:一种基于遗传算法的视觉惯性组合的SLAM方法,包括如下步骤:
1)坐标系的统一与摄像机的标定:惯导下的坐标系包括机体坐标系与导航坐标系,视觉下的坐标系包括图像坐标系、摄像机坐标系和导航坐标系,IMU与双目摄像机均捷联在机器人上,图像坐标系与摄像机坐标系的关系由摄像机内参表示,导航坐标系取作当地地理坐标系,摄像机坐标系与导航坐标系的关系表示为一个旋转矩阵R和一个平移向量通过对双目摄像机的标定确定旋转矩阵R与平移向量这些摄像机参数,进行像素点的三维重构;
2)惯性导航解算:利用捷联在机器人上的IMU进行测量,IMU上的陀螺仪负责测量运动姿态,并建立姿态矩阵,IMU上的加速度计测量机体下的加速度,并通过姿态矩阵变换至导航坐标系中,通过积分求解速度、位置;
3)视觉导航解算:利用装载机器人头部的双目摄像机进行图像的获取,对图像进行降噪处理后利用BRISK算法提取其特征点,根据步骤1)标定好的摄像机参数并通过跟踪特征点坐标变化来估计机器人的三维运动,解算其姿态、速度;
4)信息融合:利用扩展卡尔曼滤波器对视觉信息与惯导信息进行融合,以惯导模型建立滤波器状态方程,并取部分惯导输出位姿信息及视觉导航输出信息作为观测变量建立滤波器观测方程,建立系统的滤波器模型;
5)全局路标的修正以及构图:利用双目摄像机观测到的图像中提取的特征点作为特征路标,并基于遗传算法与地图已有的特征点库做数据关联,将扩展后的状态量反馈回滤波器中,同时,根据特征点的关联性判断该特征点是否应该加入地图中,并对地图中的特征点进行重定位,完成同步构图。
所述步骤1)中坐标系的统一与摄像机的标定具体步骤包括:
(1.1)建立惯性导航的机体坐标系b,并取当地的地理坐标系作为导航坐标系n,其间的关系可以表示为一个姿态矩阵双目摄像机所得二维图像平面即为图像坐标系平面c,其经过自身内参矩阵的变换可以得到摄像机坐标系w;其关系表示为:取其左上角点为原点,取当地地理坐标系为视觉中的导航坐标系;其间的关系表示为一个旋转矩阵R和一个平移向量
(1.2)摄像机的标定即确定二维图像像素点与三维坐标的对应关系;设图像坐标系下二维图像点坐标为m=[uc,vc]T,三维空间点的坐标为M=[X,Y,Z]T,根据小孔成像原理,它们的关系为sm=A[R,t]M;s称为比例系数,A是摄像机的内参,其标准形式为
设摄像机坐标系两轴的像素点分别为dx,dy;令f表示摄像机焦距;则上式表示为:
设每张图片的单应性矩阵为相机平面的坐标通过Harris角点检测再由梯度搜索得到,而导航坐标系则由黑白棋盘格表示;
建立的闭合性矩阵方程为:
标定所需的图片由双目摄像机拍摄,标定所需导航坐标系由10×10的黑白棋盘模拟;标定至少需要三张图片;
标定所得结果内参数矩阵A,旋转矩阵R,平移向量使得如下非线性模型最小:
图像个数为n(n≥3),单个图像标定点数k,取k=10;
摄像机的透镜在制作过程中会发生一定程度的畸变,其径向畸变系数和横向畸变系数分别为k1,k2,则用
进行畸变校正;
其最小二乘解为:
k=(DTD)-1DTd
其畸变校正前后坐标关系:
将标定及优化过的摄像机参数用于视觉导航中作为惯性导航中的姿态矩阵。
所述步骤2)惯性导航解算的具体步骤包括:
(2.1)利用陀螺仪输出三轴角速度ω=[ωx ωy ωz]T作为观测量,设表征姿态旋转的四元数为Q=[q0 q1 q2 q3],求解四元数的微分方程:
给定初始四元数Q0,并实时更新四元数值,建立四元数矩阵:
根据四元数与姿态矩阵的关系可以得到
θ=arcsin(C32)
惯性导航解算出含漂移的姿态信息,并随含有随时间增长的累积误差;
(2.2)加速度计测量机体坐标系下的机器人加速度fb,由陀螺仪实时计算更新的姿态角构成的姿态矩阵为即得到用于导航计算的加速度:
根据惯导系统的基本比力方程,求取其速度:
由速度积分求其位置:
(2.3)由惯性导航得出的信息量含有随时间增长的累积误差,考虑其误差量,在卡尔曼滤波中状态量含有:
所述步骤3)视觉导航解算的具体步骤包括:
(3.1)在视觉里程计中融入一部分惯导信息,捷联在机器人上的加速度计直接输出机体坐标系下的加速度值,通过:
得到其横滚角与纵摇角,用以修正视觉中的姿态信息;
(3.2)在视觉导航中,采用已标定好的双目摄像机采集图像,通过均值滤波器进行图像去噪,利用BRISK算法进行图像特征点的提取;其具体步骤如下:
(3.2.1)建立尺度空间:将一张图片按照降采样的方法,生成N张图片,称为N个尺度空间,在每一个尺度空间内,对图像进行分层,保证在每个尺度空间内的图片个数为3~5个,形成一个从大到小的金字塔的排列;尺度空间的大小关系依次表示为:
2i-1(σ,kσ,k2σ,...kn-1σ),k=21/s
(3.2.2)特征点检测:对每一个尺度空间内的每层图像的像素点进行检测,若某个像素点在其同层相邻的8个像素点以及上下层18个像素点中是极值点,则称该点为特征点;对关键点的位置进行二维二次函数插值,得到更精确的亚像素坐标位置,再对尺度方向进行一维插值,得到极值点对应的尺度,称为特征尺度;
(3.2.3)特征点描述:以某个特征点为中心,在其周围采样M个点,以δ为方差进行高斯滤波;将采样点两两组合,称为采样点对,表示为:A={(pi,pj)∈R2×R2|i<M∧j<i∧i,j∈M},计算其局部梯度:
特征点主方向为:
α=arctan2(gy,gx)
将建立好的特征点描述符用于特征点的匹配过程,BRISK算法是二进制描述特征,对N个特征点,有N(N-1)/2个特征点对,即含有N(N-1)/2个匹配距离;设xk,xk-1表示摄像机特征点集第k帧和第k-1帧的姿态,则表征旋转的四元数为:
xk=q*xk-1+ω(k)
在已匹配好的特征点中选取至少4组不共线的点,利用RANSAC算法进行迭代计算旋转矩阵R,四元数反对称矩阵的变换关系为:
而ω(k)表征这一过程的高斯白噪声;
根据视觉旋转四元数q得到的视觉姿态为:
此视觉姿态含有累积误差,用惯性姿态做一步修正;
(3.3)视觉导航中,通过已匹配好的特征点进行机器人三维运动的估计;设特征点集合为pi,pik=[uk vk 1]和pi(k-1)=[ui(k-1) vi(k-1) 1]是时刻k和k-1的特征点运动矢量;根据运动学原理,二者间的关系用一组旋转矩阵和平移矢量表示:
pik=Rmpi(k-1)+tm
由于三维反对称阵与三维向量对应,用ax表示三维向量对应的反对称阵;令:
d=1+||a||2
则旋转矩阵为:
Rm=I+2d-1ax(I+ax)
基于此向量建立的目标函数为:
平移向量可以如下求得:
令则原函数为:
使目标函数J最小的必须满足正则方程:
通过求解线性最小二乘问题,求得向量再一步求取旋转矩阵Rm,从而完成运动的估计
所述步骤4)信息融合的具体步骤包括:
(4.1)建立机器人的运动模型
(4.1.1)机器人运动加速度的描述:利用机器人当前的统计模型描述加速度的统计分布,其下一时刻加速度为有限值且在当前加速度的邻域内,用Ea(t)表示有色加速度噪声,则加速度描述为:
(4.1.2)取视觉惯性组合系统的状态变量为
分别表示机器人惯导东向、北向的位置,速度,加速度以及机器人运动的航向;组合导航系统的连续时间状态方程为:
根据运动学关系,并考虑加入加速度高斯白噪声以及航向误差并带入其状态转移矩阵表示为:
(4.1.3)设视觉惯导系统采样周期为T,对连续系统离散化:
Xk=φk,k-1Xk-1+Uk+ωk
其中:
上式为离散型视觉/惯导组合导航系统的状态方程;
(4.2)建立视觉惯导组合导航系统观测模型
(4.2.1)取惯导输出的东向位置xe,北向位置xn,视觉航向角视觉里程计输出距离s作为观测量;
引入各个观测量的观测噪声,建立的连续系统观测方程为:
(4.2.2)连续系统离散化
观测矩阵
其中:
(4.3)根据扩展卡尔曼滤波器建立的离散状态方程以及观测方程,得到视觉惯性组合系统的递推滤波方程为:
a)状态一步预测:
b)观测值修正状态估计:
c)卡尔曼滤波增益的计算:
d)误差协方差一步预测:
e)误差协方差估计:Pk=[I-KkHk]Pk,k-1
其中观测噪声Rk~(0,σ2),过程噪声
所述步骤5)全局路标的修正以及构图具体步骤包括:
(5.1)视觉双目摄像头采集图像的特征点抽象为全局路标,机器人在移动过程中,由惯导IMU和视觉VO输出的运动信息含有累积误差,在有视觉特征路标的情况下,通过观测特征路标的距离和角度,扩展原状态量:
其中:
将扩展后的状态量反馈回扩展卡尔曼滤波器中;
(5.2)机器人SLAM完成即时定位,以及通过双目摄像机观察环境中的特征,并同步构成地图;在观察环境的特征时,机器人SLAM需要判断当前的特征与已有的地图中的特征点库是否具有关联性;若有关联性则不处理,若无关联性则将此特征点作为新的环境特征加入地图中,在同步构图过程中,对原特征点进行重定位;
判断是否具有关联性以两特征点的马氏距离为评价量,基于遗传算法的数据关联的具体步骤为:
(5.2.1)独立检测:去除环境特征点中与当前观测特征点完全无关的,当前观测点与地图特征点距离的误差向量为:
其距离误差协方差为:
两点间的马氏距离表示为:
设置关联性阈值α表示关联性的置信区间,取为90%;
(5.2.2)对经过独立检测后的地图内的特征点与当前观测特征点进行数据关联,选择地图中的特征点作为本征染色体序列F;
个体可以表示为:It={bit},t=(1,2...p),b表示特征值序号;观测值序列与本征染色体序列的关联解表示为:Hir={Fi(bi)},第i个观测序列对应原染色体序列中的某个关联性特征的集合,其关联性在关联解集合Hir中定义;
以观测路标点的距离误差及其协方差作为评价准则,并设关联解是一个高斯分布,则个体适应度函数为:
在其关联解集合中,个体It的适应度函数为:
对个体进行选择操作:在一个种群中,其个体评价值为{Ft},t=(1,2,...N),则个体是否被选择的概率为:Pt=Ft/∑Ft,设置阈值R1,若P>R1表明该个体可以直接被选择进入下一代;
交叉操作:
设立阈值R2,对满足R2<Pt<R1的个体进行两两交叉,设Ia,Ib分别是待交叉的个体,则:
用新的个体进入下一代;
变异操作:
对于Pt<R2的个体进行变异操作:
其中η~N(0,1),α为分布修正参数;
(5.2.3)遗传算法终止条件:
条件1:针对不同大小,不同数量特征点的地图,设置其终止代数为δ,当迭代次数i>δ时,遗传算法终止;
条件2:当寻找到最优解时,算法终止
经过遗传算法i代后,其最优解差分很小,将其终止;
将关联好的特征点加入地图中,在即时定位的同时完成同步构图。
有益效果:本发明使用修正视觉里程计解算出机器人在移动过程中的姿态、位置,同时为了避免环境的影响,融入了惯导信息进行组合导航,在进行机器人实时精确定位的同时引入遗传算法进行地图的同步构建。首先,统一视觉导航与惯性导航的坐标系,并对双目摄像机进行参数标定,从而根据图像的像素坐标来求解三维空间坐标;其次,惯性导航进行独立解算,根据坐标系的转化,求解在导航坐标系下的姿态、位置;再次,视觉导航解算,利用双目摄像机拍摄图像,提取图像的特征点并做k-d匹配,根据特征点的运动估计机器人三维空间的运动,从而求解其姿态、位置;然后,利用扩展卡尔曼滤波器对视觉导航信息与惯性导航信息融合,建立了系统的滤波模型;最后,考虑到定位的局部性,利用双目摄像机观测全局特征点路标,并基于遗传算法对地图特征进行数据关联,并将扩展后的状态向量反馈回滤波器中。相对于现有技术,本发明可以进行长时间、高精度的定位,并且由于加入了遗传算法改进地图的数据关联,使得在同步构建地图中精度提高很多。
附图说明
图1是本发明方法流程图;
图2是本发明中摄像机坐标系与导航坐标系关系图;
图3是本发明中惯性导航解算信息的流程图;
图4是本发明中加入全局路标修正后的视觉导航流程图;
图5是本发明中两幅不同角度的图片经BRISK算法检测并k-d匹配特征点示意图;
图6a是本发明中惯导定位误差图,图6b是本发明中组合定位误差图;
图7a是本发明中基本数据关联的同步构图,图7b是本发明中基于遗传算法数据关联的同步构图。
具体实施方式
下面将结合附图,对本发明的实施案例进行详细的描述;
如图1-5所示,本发明所述的一种基于遗传算法的视觉惯性组合的SLAM方法,该方法包含以下步骤:
步骤1:坐标系的统一,将惯导下的机体坐标系和导航坐标系与视觉下的摄像机坐标系和导航坐标系统一起来。两类坐标系间的关系主要就是旋转矩阵R和平移向量
步骤2:双目视觉摄像机的标定,获得其内参矩阵以及外部旋转参数R以及平移参数t。从而将图像的二维特征点坐标和机器人三维运动坐标对应起来。并且通过摄像机的标定可以让它的镜头畸变得到校正。
步骤3:惯性导航解算,利用陀螺仪测定机器人运动角速度,通过求解四元数微分方程,得到机器人的运动姿态,并建立姿态矩阵。利用加速度计测定机器人在机体系下的加速度,通过姿态矩阵求得导航系下的加速度,并通过惯导的比力方程求解其速度,再积分求其位置。
步骤4:利用步骤2标定好的双目摄像机进行图像的采集,将图像去噪处理后利用BRISK算法对图像进行特征提取,并做特征点匹配。
步骤5:设两幅图像间的姿态旋转四元数为q,选取步骤4中至少4组已匹配好的特征点进行RANSAC求解其一致性,得到其旋转矩阵,将其扩展后得到旋转四元数。通过旋转四元数获得视觉姿态角的信息。
步骤6:利用步骤4中匹配好的特征点做三维运动估计,通过三维向量与反对称阵的关系将非线性目标函数转化为线性最小二乘求解的问题。从而得到视觉里程计的位置输出。
步骤7:将步骤3、5、6得到的视觉/惯导信息进行融合,利用扩展卡尔曼滤波器,并选取加速度、速度、位置、航向作为状态量,选取惯导位置、航向、视觉里程计位置为观测量。建立了离散型状态模型和观测模型。
步骤8:将步骤4中的特征点建立特征点路标,并作为机器人的观测量。将观测新息加入步骤7所述的状态量中,并将扩展后的状态量反馈回步骤7所述的扩展卡尔曼滤波器,建立新的模型。
步骤9:根据步骤8中的特征点路标构建环境地图,并基于遗传算法将当前观测的特征点与环境地图已有的特征点进行数据关联。将新息加入环境地图中,完成地图的同步构建。
其中,步骤1:坐标系的统一与摄像机的标定,惯导下的坐标系包括机体坐标系与导航坐标系,视觉下的坐标系包括图像坐标系、摄像机坐标系和导航坐标系,IMU(Inertialmeasurement unit,惯性测量单元)与双目摄像机均捷联在机器人上,图像坐标系与摄像机坐标系的关系由摄像机内参表示,导航坐标系取作当地地理坐标系,摄像机坐标系与导航坐标系的关系表示为一个旋转矩阵R和一个平移向量通过对双目摄像机的标定确定旋转矩阵R与平移向量这些摄像机参数,进行像素点的三维重构;具体如下:
1)建立惯性导航的机体坐标系b,并取当地的地理坐标系作为导航坐标系n,其间的关系可以表示为一个姿态矩阵双目摄像机所得二维图像平面即为图像坐标系平面c,其经过自身内参矩阵的变换可以得到摄像机坐标系w。其关系可以表示为:取其左上角点为原点,取当地地理坐标系为视觉中的导航坐标系。其间的关系可以表示为一个旋转矩阵R和一个平移向量
2)摄像机的标定即是确定二维图像像素点与三维坐标的对应关系。设在图像坐标系下二维图像点坐标为m=[uc,vc]T,三维空间点的坐标为M=[X,Y,Z]T,根据小孔成像原理,它们的关系为sm=A[R,t]M。s称为比例系数,A是摄像机的内参,其标准形式为
设摄像机坐标系两轴的像素点分别为dx,dy。令f表示摄像机焦距。则上式可以表示为:
设每张图片的单应性矩阵为相机平面的坐标通过Harris角点检测再由梯度搜索得到,而导航坐标系则由黑白棋盘格表示。
因此建立的闭合性矩阵方程为:
本次标定所需的图片由双目摄像机拍摄,标定所需导航坐标系由10×10的黑白棋盘模拟。
本次标定至少需要三张图片。
本次标定所得结果内参数矩阵A,旋转矩阵R,平移向量t使得如下非线性模型最小:
图像个数为n(n≥3),单个图像标定点数k,取k=10。
摄像机的透镜在制作过程中会发生一定程度的畸变,其径向畸变系数和横向畸变系数分别为k1,k2,则用
进行畸变校正。
其最小二乘解为:
k=(DTD)-1DTd
其畸变校正前后坐标关系:
将标定及优化过的摄像机参数用于视觉导航中,其作用相当于惯性导航中的姿态矩阵。
步骤2:惯性导航解算,利用捷联在机器人上的IMU进行测量,IMU上的陀螺仪负责测量运动姿态,并建立姿态矩阵,IMU上的加速度计测量机体下的加速度,并通过姿态矩阵变换至导航坐标系中,通过积分求解速度、位置;具体如下:
1)利用陀螺仪输出三轴角速度ω=[ωx ωy ωz]T作为观测量,设表征姿态旋转的四元数为Q=[q0 q1 q2 q3],求解四元数的微分方程:
给定初始四元数Q0,并实时更新四元数值,建立四元数矩阵:
根据四元数与姿态矩阵的关系可以得到
θ=arcsin(C32)
惯导可以独立解算出含漂移的姿态信息,并随含有随时间增长的累积误差。
2)加速度计测量机体坐标系下的机器人加速度fb,由陀螺仪实时计算更新的姿态角构成的姿态矩阵为即可得到用于导航计算的加速度:
根据惯导系统的基本比力方程,求取其速度:
由速度积分求其位置:
3)由惯导得出的信息量含有随时间增长的累积误差,考虑其误差量,在卡尔曼滤波中状态量应含有:
步骤3:视觉导航解算,利用装载机器人头部的双目摄像机进行图像的获取,对图像进行降噪处理后利用BRISK算法(Binary Robust Invariant Scalable Keypoints)提取其特征点,根据步骤1标定好的摄像机参数并通过跟踪特征点坐标变化来估计机器人的三维运动,解算其姿态、速度;具体如下:
1)在视觉里程计中融入一部分惯导信息,由于捷联在机器人上的加速度计可以直接输出机体坐标系下的加速度值,因此,可以通过:
得到其横滚角与纵摇角,用以修正视觉中的姿态信息。这种姿态角没有通过积分的运算,不含有累积误差。
2)在视觉导航中,采用已标定好的双目摄像机采集图像,通过均值滤波器进行图像去噪,利用BRISK算法进行图像特征点的提取。这是一种二进制的特征描述算子,在对有较大模糊的图像配准时效果很好。其具体步骤如下:
a.建立尺度空间:将一张图片按照降采样的方法,生成N张图片,称为N个尺度空间,在每一个尺度空间内,对图像进行分层,保证在每个尺度空间内的图片个数为3~5个,形成一个从大到小的金字塔的排列。因此尺度空间的大小关系可以依次表示为:
2i-1(σ,kσ,k2σ,...kn-1σ),k=21/s
b.特征点检测:对每一个尺度空间内的每层图像的像素点进行检测,若某个像素点在其同层相邻的8个像素点以及上下层18个像素点中是极值点,则称该点为特征点。对关键点的位置进行二维二次函数插值,得到更精确的亚像素坐标位置,再对尺度方向进行一维插值,得到极值点对应的尺度,称为特征尺度。
c.特征点描述:为避免降采样引起的混叠效应,以某个特征点为中心,在其周围采样M个点,以δ为方差进行高斯滤波。将采样点两两组合,称为采样点对,表示为:A={(pi,pj)∈R2×R2|i<M^j<i^i,j∈M},计算其局部梯度:
特征点主方向为:
α=arctan2(gy,gx)
将建立好的特征点描述符用于特征点的匹配过程,BRISK算法是二进制描述特征,对N个特征点,有N(N-1)/2个特征点对,即含有N(N-1)/2个匹配距离。设xk,xk-1表示摄像机特征点集第k帧和第k-1帧的姿态,则表征旋转的四元数为:
xk=q*xk-1+ω(k)
在已匹配好的特征点中选取至少4组不共线的点,利用RANSAC算法进行迭代计算旋转矩阵R,四元数反对称矩阵的变换关系为:
而ω(k)表征这一过程的高斯白噪声。
根据视觉旋转四元数q得到的视觉姿态为:
此视觉姿态含有累积误差,可以用如步骤3所述第1)步的惯性姿态做一步修正。
3)视觉导航中,通过已匹配好的特征点进行机器人三维运动的估计。设特征点集合为pi,pik=[uk vk 1]和pi(k-1)=[ui(k-1) vi(k-1) 1]是时刻k和k-1的特征点运动矢量。根据运动学原理,二者间的关系可以用一组旋转矩阵和平移矢量表示:
pik=Rmpi(k-1)+tm
由于三维反对称阵与三维向量对应,用ax表示三维向量对应的反对称阵。令:
d=1+||a||2
则旋转矩阵为:
Rm=I+2d-1ax(I+ax)
基于此向量建立的目标函数为:
平移向量可以如下求得:
令则原函数为:
使目标函数J最小的必须满足正则方程:
通过求解线性最小二乘问题,求得向量再一步求取旋转矩阵Rm,从而完成运动的估计
步骤4:信息融合,利用扩展卡尔曼滤波器对视觉信息与惯导信息进行融合,以惯导模型建立滤波器状态方程,并取部分惯导输出位姿信息及视觉导航输出信息作为观测变量建立滤波器观测方程,建立系统的滤波器模型;具体如下:
1)建立机器人的运动模型,其具体步骤包括:
a.机器人运动加速度的描述:利用机器人当前的统计模型描述加速度的统计分布,其下一时刻加速度为有限值且在当前加速度的邻域内,用Ea(t)表示有色加速度噪声,则加速度描述为:
b.取视觉/惯性组合系统的状态变量为
分别表示机器人东向、北向的位置,速度,加速度以及机器人运动的航向。
组合导航系统的连续时间状态方程为:
根据运动学关系,并考虑加入加速度高斯白噪声以及航向误差并带入其状态转移矩阵可表示为:
c.设视觉/惯导系统采样周期为T,对连续系统离散化:
Xk=φk,k-1Xk-1+Uk+ωk
其中:
上式即为离散型视觉/惯导组合导航系统的状态方程。
2)视觉/惯导组合导航系统观测模型建立如下:
a.取惯导输出的东向位置xe,北向位置xn,视觉航向角视觉里程计输出距离s作为观测量。
引入各个观测量的观测噪声,建立的连续系统观测方程为:
b.连续系统离散化
观测矩阵
其中:
3)根据EKF建立的离散状态方程以及观测方程,可以得到系统的递推滤波方程为:
a)状态一步预测:
b)观测值修正状态估计:
c)卡尔曼滤波增益的计算:
d)误差协方差一步预测:
e)误差协方差估计:Pk=[I-KkHk]Pk,k-1
其中观测噪声Rk~(0,σ2),过程噪声
步骤5:全局修正与同步构图,全局路标的修正以及构图:利用双目摄像机观测到的图像中提取的特征点作为特征路标,并基于遗传算法与地图已有的特征点库做数据关联,将扩展后的状态量反馈回滤波器中,同时,根据特征点的关联性判断该特征点是否应该加入地图中,并对地图中的特征点进行重定位,完成同步构图。具体如下:
1)视觉双目摄像头采集图像的特征点可以抽象为全局路标,机器人在移动过程中,由惯导IMU和视觉VO(Visual Odometry,视觉里程计)输出的运动信息含有累积误差,在有视觉特征路标的情况下,通过观测特征路标的距离和角度,可以扩展原状态量:
其中:
将扩展后的状态向量反馈回如步骤4所述的EKF滤波器中。
2)机器人SLAM除了完成即时定位的功能,还通过双目摄像机观察环境中的特征,同步的构成地图。在观察环境的特征时,需要判断当前的特征与已有的地图(特征点库)是否具有关联性。若有关联性则不处理,若无关联性则将此特征点作为新的环境特征加入地图中,并且由于视觉观测本身的误差,原地图中的特征点的位置也含有误差,在同步构图过程中,也需要对原特征点进行重定位。地图中的特征点不能太多,一个特征点需要维护至少3×3的协方差矩阵。因此数据关联在机器人SLAM同步构图中非常重要。
数据关联以两特征点的马氏距离为评价量,基于遗传算法的数据关联的具体步骤为:
a.独立检测:去除环境特征点中与当前观测特征点完全无关的,当前观测点与地图特征点距离的误差向量为:
其距离误差协方差为:
两点间的马氏距离表示为:
设置关联性阈值α表示关联性的置信区间,一般取为90%。
b.对经过独立检测后的地图内的特征点与当前观测特征点进行数据关联,选择地图中的特征点作为本征染色体序列F。
个体可以表示为:It={bit},t=(1,2...p),b表示特征值序号。因此观测值序列与本征染色体序列的关联解可表示为:Hir={Fi(bi)},第i个观测序列对应原染色体序列中的某个关联性特征的集合,因此其关联性应该在关联解集合Hir中定义。以观测路标点的距离误差及其协方差作为评价准则,并设关联解是一个高斯分布,则个体适应度函数为:
在其关联解集合中,个体It的适应度函数为:
对个体进行选择操作:在一个种群中,其个体评价值为{Ft},t=(1,2,...N),则个体是否被选择的概率为:Pt=Ft/∑Ft,设置阈值R1,若P>R1表明该个体可以直接被选择进入下一代。
交叉操作:
设立阈值R2,对满足R2<Pt<R1的个体进行两两交叉,设Ia,Ib分别是待交叉的个体,则:
用新的个体进入下一代。
变异操作:
对于Pt<R2的个体进行变异操作:
其中η~N(0,1),α为分布修正参数。
c.遗传算法终止条件:
条件1:针对不同大小,不同数量特征点的地图,设置其终止代数为δ,当迭代次数i>δ时,遗传算法终止。
条件2:当寻找到最优解时,算法终止
表明,经过遗传算法i代后,其最优解差分很小,将其终止。
将关联好的特征点加入如步骤5所述的地图中,在即时定位的同时完成同步构图。
图6a与图6b是机器人在运动过程中单独使用惯性导航,以及使用视觉/惯导组合导航的定位误差图。在给定相同的仿真初始条件的情况下,惯性导航定位误差较组合导航更大。
图7a和图7b是机器人采用基本的数据关联方法以及优化过的基于遗传算法的数据关联方法同步构图的结果。可以看出,在遗传算法加入后,地图特征点的估计位置与仿真给定的理论位置非常接近,而基本数据关联的特征点估计却发生了一定程度的偏离,显示出本发明改进算法的可靠性和优越性。
Claims (6)
1.一种基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,包括如下步骤:
1)坐标系的统一与摄像机的标定:惯导下的坐标系包括机体坐标系与导航坐标系,视觉下的坐标系包括图像坐标系、摄像机坐标系和导航坐标系,IMU与双目摄像机均捷联在机器人上,图像坐标系与摄像机坐标系的关系由摄像机内参表示,导航坐标系取作当地地理坐标系,摄像机坐标系与导航坐标系的关系表示为一个旋转矩阵R和一个平移向量通过对双目摄像机的标定确定旋转矩阵R与平移向量这些摄像机参数,进行像素点的三维重构;
2)惯性导航解算:利用捷联在机器人上的IMU进行测量,IMU上的陀螺仪负责测量运动姿态,并建立姿态矩阵,IMU上的加速度计测量机体下的加速度,并通过姿态矩阵变换至导航坐标系中,通过积分求解速度、位置;
3)视觉导航解算:利用装载机器人头部的双目摄像机进行图像的获取,对图像进行降噪处理后利用BRISK算法提取其特征点,根据步骤1)标定好的摄像机参数并通过跟踪特征点坐标变化来估计机器人的三维运动,解算其姿态、速度;
4)信息融合:利用扩展卡尔曼滤波器对视觉信息与惯导信息进行融合,以惯导模型建立滤波器状态方程,并取部分惯导输出位姿信息及视觉导航输出信息作为观测变量建立滤波器观测方程,建立系统的滤波器模型;
5)全局路标的修正以及构图:利用双目摄像机观测到的图像中提取的特征点作为特征路标,并基于遗传算法与地图已有的特征点库做数据关联,将扩展后的状态量反馈回滤波器中,同时,根据特征点的关联性判断该特征点是否应该加入地图中,并对地图中的特征点进行重定位,完成同步构图。
2.如权利要求1所述的基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,所述步骤1)中坐标系的统一与摄像机的标定具体步骤包括:
(1.1)建立惯性导航的机体坐标系b,并取当地的地理坐标系作为导航坐标系n,其间的关系可以表示为一个姿态矩阵双目摄像机所得二维图像平面即为图像坐标系平面c,其经过自身内参矩阵的变换可以得到摄像机坐标系w;其关系表示为:取其左上角点为原点,取当地地理坐标系为视觉中的导航坐标系;其间的关系表示为一个旋转矩阵R和一个平移向量
(1.2)摄像机的标定即确定二维图像像素点与三维坐标的对应关系;设图像坐标系下二维图像点坐标为m=[uc,vc]T,三维空间点的坐标为M=[X,Y,Z]T,根据小孔成像原理,它们的关系为sm=A[R,t]M;s称为比例系数,A是摄像机的内参,其标准形式为
设摄像机坐标系两轴的像素点分别为dx,dy;令f表示摄像机焦距;则上式表示为:
设每张图片的单应性矩阵为相机平面的坐标通过Harris角点检测再由梯度搜索得到,而导航坐标系则由黑白棋盘格表示;
建立的闭合性矩阵方程为:
标定所需的图片由双目摄像机拍摄,标定所需导航坐标系由10×10的黑白棋盘模拟;标定至少需要三张图片;
标定所得结果内参数矩阵A,旋转矩阵R,平移向量使得如下非线性模型最小:
图像个数为n,其中,n≥3,单个图像标定点数k,取k=10;
摄像机的透镜在制作过程中会发生一定程度的畸变,其径向畸变系数和横向畸变系数分别为k1,k2,则用:
进行畸变校正;
其最小二乘解为:
k=(DTD)-1DTd
其畸变校正前后坐标关系:
将标定及优化过的摄像机参数用于视觉导航中作为惯性导航中的姿态矩阵。
3.如权利要求1所述的基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,所述步骤2)惯性导航解算的具体步骤包括:
(2.1)利用陀螺仪输出三轴角速度ω=[ωx ωy ωz]T作为观测量,设表征姿态旋转的四元数为Q=[q0 q1 q2 q3]
给定初始四元数Q0,并实时更新四元数值,建立四元数矩阵:
根据四元数与姿态矩阵的关系可以得到
θ=arcsin(C32)
惯性导航解算出含漂移的姿态信息,并含有随时间增长的累积误差;
(2.2)加速度计测量机体坐标系下的机器人加速度fb,由陀螺仪实时计算更新的姿态角构成的姿态矩阵为即得到用于导航计算的加速度:
根据惯导系统的基本比力方程,求取其速度:
由速度积分求其位置:
(2.3)由惯性导航得出的信息量含有随时间增长的累积误差,考虑其误差量,在卡尔曼滤波中状态量含有:
4.如权利要求1所述的基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,所述步骤3)视觉导航解算的具体步骤包括:
(3.1)在视觉里程计中融入一部分惯导信息,捷联在机器人上的加速度计直接输出机体坐标系下的加速度值,通过:
得到其横滚角与纵摇角,用以修正视觉中的姿态信息;
(3.2)在视觉导航中,采用已标定好的双目摄像机采集图像,通过均值滤波器进行图像去噪,利用BRISK算法进行图像特征点的提取;其具体步骤如下:
(3.2.1)建立尺度空间:将一张图片按照降采样的方法,生成N1张图片,称为N1个尺度空间,在每一个尺度空间内,对图像进行分层,保证在每个尺度空间内的图片个数为3~5个,形成一个从大到小的金字塔的排列;尺度空间的大小关系依次表示为:
2i-1(σ,kσ,k2σ,...kn-1σ),k=21/s
(3.2.2)特征点检测:对每一个尺度空间内的每层图像的像素点进行检测,若某个像素点在其同层相邻的8个像素点以及上下层18个像素点中是极值点,则称该点为特征点;对关键点的位置进行二维二次函数插值,得到更精确的亚像素坐标位置,再对尺度方向进行一维插值,得到极值点对应的尺度,称为特征尺度;
(3.2.3)特征点描述:以某个特征点为中心,在其周围采样M个点,以δ为方差进行高斯滤波;将采样点两两组合,称为采样点对,表示为:A={(pi,pj)∈R2×R2|i<M^j<i^i,j∈M},计算其局部梯度:
特征点主方向为:
α=arctan2(gy,gx)
将建立好的特征点描述符用于特征点的匹配过程,BRISK算法是二进制描述特征,对N个特征点,有N(N-1)/2个特征点对,即含有N(N-1)/2个匹配距离;设xk,xk-1表示摄像机特征点集第k帧和第k-1帧的姿态,则表征旋转的四元数为:
xk=q*xk-1+ω(k)
在已匹配好的特征点中选取至少4组不共线的点,利用RANSAC算法进行迭代计算旋转矩阵R,四元数反对称矩阵的变换关系为:
而ω(k)表征这一过程的高斯白噪声;
根据视觉旋转四元数q得到的视觉姿态为:
此视觉姿态含有累积误差,用惯性姿态做一步修正;
(3.3)视觉导航中,通过已匹配好的特征点进行机器人三维运动的估计;设特征点集合为pi,pik=[uk vk 1]和是时刻k和k-1的特征点运动矢量;根据运动学原理,二者间的关系用一组旋转矩阵和平移矢量表示:
pik=Rmpi(k-1)+tm
由于三维反对称阵与三维向量对应,用ax表示三维向量对应的反对称阵;令:
d=1+||a||2
则旋转矩阵为:
Rm=I+2d-1ax(I+ax)
基于此向量建立的目标函数为:
平移向量可以如下求得:
令则原函数为:
使目标函数J最小的必须满足正则方程:
通过求解线性最小二乘问题,求得向量再一步求取旋转矩阵Rm,从而完成运动的估计
5.如权利要求1所述的基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,所述步骤4)信息融合的具体步骤包括:
(4.1)建立机器人的运动模型
(4.1.1)机器人运动加速度的描述:利用机器人当前的统计模型描述加速度的统计分布,其下一时刻加速度为有限值且在当前加速度的邻域内,用Ea(t)表示有色加速度噪声,则加速度描述为:
(4.1.2)取视觉惯性组合系统的状态变量为
分别表示机器人惯导东向、北向的位置,速度,加速度以及机器人运动的航向;组合导航系统的连续时间状态方程为:
根据运动学关系,并考虑加入加速度高斯白噪声以及航向误差并带入其状态转移矩阵表示为:
(4.1.3)设视觉惯性组合导航系统采样周期为T,对连续系统离散化:
Xk=φk,k-1Xk-1+Uk+ωk
其中:
上式为离散型视觉惯性组合导航系统的状态方程;
(4.2)建立视觉惯性组合导航系统观测模型
(4.2.1)取惯导输出的东向位置xe,北向位置xn,视觉航向角视觉里程计输出距离s作为观测量;
引入各个观测量的观测噪声,建立的连续系统观测方程为:
(4.2.2)连续系统离散化
观测矩阵
其中:
(4.3)根据扩展卡尔曼滤波器建立的离散状态方程以及观测方程,得到视觉惯性组合导航系统的递推滤波方程为:
a)状态一步预测:
b)观测值修正状态估计:
c)卡尔曼滤波增益的计算:
d)误差协方差一步预测:
e)误差协方差估计:Pk=[I-KkHk]Pk,k-1
其中观测噪声Rk~(0,σ2),过程噪声
6.如权利要求1所述的基于遗传算法的视觉惯性组合的SLAM方法,其特征在于,所述步骤5)全局路标的修正以及构图具体步骤包括:
(5.1)视觉双目摄像头采集图像的特征点抽象为全局路标,机器人在移动过程中,由惯导IMU和视觉VO输出的运动信息含有累积误差,在有视觉特征路标的情况下,通过观测特征路标的距离和角度,扩展原状态量:
其中:
将扩展后的状态量反馈回扩展卡尔曼滤波器中;
(5.2)机器人SLAM完成即时定位,以及通过双目摄像机观察环境中的特征,并同步构成地图;在观察环境的特征时,机器人SLAM需要判断当前的特征与已有的地图中的特征点库是否具有关联性;若有关联性则不处理,若无关联性则将此特征点作为新的环境特征加入地图中,在同步构图过程中,对原特征点进行重定位;
判断是否具有关联性以两特征点的马氏距离为评价量,基于遗传算法的数据关联的具体步骤为:
(5.2.1)独立检测:去除环境特征点中与当前观测特征点完全无关的,当前观测点与地图特征点距离的误差向量为:
其距离误差协方差为:
两点间的马氏距离表示为:
设置关联性阈值l表示关联性的置信区间,取为90%;
(5.2.2)对经过独立检测后的地图内的特征点与当前观测特征点进行数据关联,选择地图中的特征点作为本征染色体序列F;
个体可以表示为:It={bit},t=(1,2...p),b表示特征值序号;观测值序列与本征染色体序列的关联解表示为:Hir={Fi(bi)},第i个观测序列对应原染色体序列中的某个关联性特征的集合,其关联性在关联解集合Hir中定义;
以观测路标点的距离误差及其协方差作为评价准则,并设关联解是一个高斯分布,则个体适应度函数为:
在其关联解集合中,个体It的适应度函数为:
对个体进行选择操作:在一个种群中,其个体评价值为{Ft},t=(1,2,...N),则个体是否被选择的概率为:Pt=Ft/∑Ft,设置阈值R1,若P>R1表明该个体可以直接被选择进入下一代;
交叉操作:
设立阈值R2,对满足R2<Pt<R1的个体进行两两交叉,设Ia,Ib分别是待交叉的个体,则:
用新的个体进入下一代;
变异操作:
对于Pt<R2的个体进行变异操作:
其中η~N(0,1),α为分布修正参数;
(5.2.3)遗传算法终止条件:
条件1:针对不同大小,不同数量特征点的地图,设置其终止代数为δ,当迭代次数i>δ时,遗传算法终止;
条件2:当寻找到最优解时,算法终止
经过遗传算法i代后,其最优解差分很小,将其终止;
将关联好的特征点加入地图中,在即时定位的同时完成同步构图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611121973.6A CN106679648B (zh) | 2016-12-08 | 2016-12-08 | 一种基于遗传算法的视觉惯性组合的slam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201611121973.6A CN106679648B (zh) | 2016-12-08 | 2016-12-08 | 一种基于遗传算法的视觉惯性组合的slam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106679648A CN106679648A (zh) | 2017-05-17 |
CN106679648B true CN106679648B (zh) | 2019-12-10 |
Family
ID=58868786
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201611121973.6A Active CN106679648B (zh) | 2016-12-08 | 2016-12-08 | 一种基于遗传算法的视觉惯性组合的slam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106679648B (zh) |
Families Citing this family (78)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107014379B (zh) * | 2017-05-25 | 2019-09-20 | 中国矿业大学 | 一种掘进机绝对空间位姿检测装置与方法 |
CN107180215B (zh) * | 2017-05-31 | 2020-01-31 | 同济大学 | 基于库位和二维码的停车场自动建图与高精度定位方法 |
CN107369181B (zh) * | 2017-06-13 | 2020-12-22 | 华南理工大学 | 基于双处理器结构的点云数据采集及处理方法 |
CN107255642A (zh) * | 2017-06-23 | 2017-10-17 | 昆山善思光电科技有限公司 | X光无损检测设备的导航装置及其方法 |
CN107272690B (zh) * | 2017-07-07 | 2023-08-22 | 中国计量大学 | 基于双目立体视觉的惯性导引车导航方法及惯性导引车 |
CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
US11080890B2 (en) | 2017-07-28 | 2021-08-03 | Qualcomm Incorporated | Image sensor initialization in a robotic vehicle |
CN109387192B (zh) * | 2017-08-02 | 2022-08-26 | 湖南云箭格纳微信息科技有限公司 | 一种室内外连续定位方法及装置 |
CN109387219A (zh) * | 2017-08-02 | 2019-02-26 | 珊口(上海)智能科技有限公司 | 误差标定系统 |
CN107314778B (zh) * | 2017-08-04 | 2023-02-10 | 广东工业大学 | 一种相对姿态的标定方法、装置及系统 |
CN116255992A (zh) * | 2017-08-23 | 2023-06-13 | 北京图森未来科技有限公司 | 一种同时定位与建图的方法和装置 |
CN107607111A (zh) * | 2017-09-07 | 2018-01-19 | 驭势科技(北京)有限公司 | 加速度偏置估计方法和装置、视觉惯性里程计及其应用 |
WO2019051832A1 (zh) * | 2017-09-18 | 2019-03-21 | 深圳市大疆创新科技有限公司 | 可移动物体控制方法、设备及系统 |
CN107498559B (zh) * | 2017-09-26 | 2020-12-29 | 珠海市一微半导体有限公司 | 基于视觉的机器人转向的检测方法和芯片 |
CN107796391A (zh) * | 2017-10-27 | 2018-03-13 | 哈尔滨工程大学 | 一种捷联惯性导航系统/视觉里程计组合导航方法 |
CN108051836B (zh) * | 2017-11-02 | 2022-06-10 | 中兴通讯股份有限公司 | 一种定位方法、装置、服务器及系统 |
CN107869989B (zh) * | 2017-11-06 | 2020-02-07 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
CN109827607B (zh) * | 2017-11-23 | 2021-01-26 | 清华大学 | 线结构光焊缝跟踪传感器的标定方法及装置 |
CN107833245B (zh) * | 2017-11-28 | 2020-02-07 | 北京搜狐新媒体信息技术有限公司 | 基于单目视觉特征点匹配的slam方法及系统 |
CN107945220B (zh) * | 2017-11-30 | 2020-07-10 | 华中科技大学 | 一种基于双目视觉的重建方法 |
CA3031276A1 (en) | 2018-02-08 | 2019-08-08 | Ascension Technology Corporation | Compensating for distortion in an electromagnetic tracking system |
CN108444452B (zh) * | 2018-02-11 | 2020-11-17 | 武汉雄楚高晶科技有限公司 | 目标经纬度和拍摄装置的三维空间姿态的检测方法及装置 |
CN110231028B (zh) * | 2018-03-05 | 2021-11-30 | 北京京东乾石科技有限公司 | 飞行器导航方法、装置和系统 |
CN108446657B (zh) | 2018-03-28 | 2022-02-25 | 京东方科技集团股份有限公司 | 手势抖动识别方法及装置、手势识别方法 |
CN108510550A (zh) * | 2018-03-29 | 2018-09-07 | 轻客智能科技(江苏)有限公司 | 一种双目摄像头自动标定方法及装置 |
CN108680159A (zh) * | 2018-04-03 | 2018-10-19 | 中科微至智能制造科技江苏有限公司 | 一种基于数据融合的机器人定位方法 |
CN108827287B (zh) * | 2018-04-10 | 2021-12-21 | 南京航空航天大学 | 一种复杂环境下的鲁棒视觉slam系统 |
CN108759815B (zh) * | 2018-04-28 | 2022-11-15 | 温州大学激光与光电智能制造研究院 | 一种用于全局视觉定位方法中的信息融合组合导航方法 |
CN108621201B (zh) * | 2018-05-08 | 2020-11-20 | 中国人民解放军国防科技大学 | 一种足式机器人本体状态测量方法 |
CN108481327B (zh) * | 2018-05-31 | 2023-11-07 | 珠海一微半导体股份有限公司 | 一种增强视觉的定位装置、定位方法及机器人 |
CN108801248B (zh) * | 2018-06-22 | 2020-09-15 | 深圳市北斗产业互联网研究院 | 基于ukf的平面视觉惯性导航方法 |
CN108845343B (zh) * | 2018-07-03 | 2020-04-28 | 河北工业大学 | 一种基于视觉、gps与高精度地图融合的车辆定位方法 |
CN109186592B (zh) * | 2018-08-31 | 2022-05-20 | 腾讯科技(深圳)有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
CN110880189B (zh) * | 2018-09-06 | 2022-09-09 | 舜宇光学(浙江)研究院有限公司 | 联合标定方法及其联合标定装置和电子设备 |
CN109141442B (zh) * | 2018-09-07 | 2022-05-17 | 高子庆 | 基于uwb定位与图像特征匹配的导航方法和移动终端 |
CN109543694A (zh) * | 2018-09-28 | 2019-03-29 | 天津大学 | 一种基于点特征稀疏策略的视觉同步定位与地图构建方法 |
CN109341706B (zh) * | 2018-10-17 | 2020-07-03 | 张亮 | 一种面向无人驾驶汽车的多特征融合地图的制作方法 |
CN111089580B (zh) * | 2018-10-23 | 2023-02-10 | 北京自动化控制设备研究所 | 一种基于协方差交叉的无人战车同时定位与地图构建方法 |
CN109343713B (zh) * | 2018-10-31 | 2022-02-11 | 重庆子元科技有限公司 | 一种基于惯性测量单元的人体动作映射方法 |
CN111220156B (zh) * | 2018-11-25 | 2023-06-23 | 星际空间(天津)科技发展有限公司 | 一种基于城市实景的导航方法 |
CN109655058A (zh) * | 2018-12-24 | 2019-04-19 | 中国电子科技集团公司第二十研究所 | 一种惯性/视觉智能化组合导航方法 |
CN109870167A (zh) * | 2018-12-25 | 2019-06-11 | 四川嘉垭汽车科技有限公司 | 基于视觉的无人驾驶汽车的同时定位与地图创建方法 |
CN109901154B (zh) * | 2019-03-29 | 2023-03-03 | 中国人民解放军海军航空大学 | 基于递推rtht-tbd的自适应调整方法 |
CN109976344B (zh) * | 2019-03-30 | 2022-05-27 | 南京理工大学 | 巡检机器人姿态矫正方法 |
CN110081881B (zh) * | 2019-04-19 | 2022-05-10 | 成都飞机工业(集团)有限责任公司 | 一种基于无人机多传感器信息融合技术的着舰引导方法 |
CN110032201A (zh) * | 2019-04-19 | 2019-07-19 | 成都飞机工业(集团)有限责任公司 | 一种基于卡尔曼滤波的imu机载视觉姿态融合的方法 |
CN110285810B (zh) * | 2019-06-13 | 2023-07-14 | 兖矿集团有限公司 | 一种基于惯性导航数据的采煤机自主定位方法及装置 |
CN112129272B (zh) * | 2019-06-25 | 2022-04-26 | 京东方科技集团股份有限公司 | 视觉里程计的实现方法和实现装置 |
CN110487267B (zh) * | 2019-07-10 | 2021-06-04 | 湖南交工智能技术有限公司 | 一种基于vio&uwb松组合的无人机导航系统及方法 |
CN110388919B (zh) * | 2019-07-30 | 2023-05-23 | 上海云扩信息科技有限公司 | 增强现实中基于特征图和惯性测量的三维模型定位方法 |
WO2021035669A1 (zh) * | 2019-08-30 | 2021-03-04 | 深圳市大疆创新科技有限公司 | 位姿预测方法、地图构建方法、可移动平台及存储介质 |
CN110728715B (zh) * | 2019-09-06 | 2023-04-25 | 南京工程学院 | 一种智能巡检机器人摄像机角度自适应调整方法 |
CN110647609B (zh) * | 2019-09-17 | 2023-07-18 | 上海图趣信息科技有限公司 | 视觉地图定位方法及系统 |
CN112577479B (zh) * | 2019-09-27 | 2024-04-12 | 北京魔门塔科技有限公司 | 基于地图元素数据的多传感器融合车辆定位方法及装置 |
CN111148033B (zh) * | 2019-12-19 | 2021-06-18 | 广州赛特智能科技有限公司 | 一种自移动设备的辅助导航方法 |
CN113126602B (zh) * | 2019-12-30 | 2023-07-14 | 南京景曜智能科技有限公司 | 一种移动机器人的定位方法 |
WO2021160098A1 (en) * | 2020-02-13 | 2021-08-19 | Guangdong Oppo Mobile Telecommunications Corp., Ltd. | Error state kalman filter for visual slam by dynamically tuning measurement noise covariance |
CN111805535B (zh) * | 2020-06-11 | 2022-06-07 | 浙江大华技术股份有限公司 | 一种定位导航方法、装置以及计算机存储介质 |
CN111649746B (zh) * | 2020-06-11 | 2023-03-28 | 青岛卓信通智能科技有限公司 | 一种融合惯导测量和ArUco标记的定位与导航方法 |
CN111735445B (zh) * | 2020-06-23 | 2022-02-11 | 煤炭科学研究总院 | 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法 |
CN111811502B (zh) * | 2020-07-10 | 2022-07-22 | 北京航空航天大学 | 一种运动载体多源信息融合导航方法及系统 |
CN112037289A (zh) * | 2020-09-16 | 2020-12-04 | 安徽意欧斯物流机器人有限公司 | 一种基于遗传算法的离线参数标定方法 |
CN112162561A (zh) * | 2020-10-13 | 2021-01-01 | 上海美迪索科电子科技有限公司 | 一种地图构建优化方法、装置、介质及设备 |
CN112465907A (zh) * | 2020-10-29 | 2021-03-09 | 清华大学 | 一种室内视觉导航方法及系统 |
CN112461230B (zh) * | 2020-12-07 | 2023-05-09 | 优必康(青岛)科技有限公司 | 机器人重定位方法、装置、机器人和可读存储介质 |
CN113313754A (zh) * | 2020-12-23 | 2021-08-27 | 南京凌华微电子科技有限公司 | 一种手术导航中骨锯标定方法及系统 |
CN112857367B (zh) * | 2021-01-21 | 2023-10-13 | 中国煤炭科工集团太原研究院有限公司 | 一种基于机器视觉和惯性导航的掘进机位姿检测方法 |
CN113189613B (zh) * | 2021-01-25 | 2023-01-10 | 广东工业大学 | 一种基于粒子滤波的机器人定位方法 |
CN113108780A (zh) * | 2021-03-30 | 2021-07-13 | 沈奥 | 一种基于视觉惯导slam算法的无人船自主导航方法 |
CN112729294B (zh) * | 2021-04-02 | 2021-06-25 | 北京科技大学 | 适用于机器人的视觉和惯性融合的位姿估计方法及系统 |
CN113203418B (zh) * | 2021-04-20 | 2022-09-16 | 同济大学 | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 |
CN113465596B (zh) * | 2021-06-25 | 2023-05-30 | 电子科技大学 | 一种基于多传感器融合的四旋翼无人机定位方法 |
CN113486818B (zh) * | 2021-07-09 | 2022-05-20 | 吉林大学 | 基于机器视觉的满斗率预测系统和方法 |
CN114092569B (zh) * | 2022-01-19 | 2022-08-05 | 安维尔信息科技(天津)有限公司 | 一种基于多传感器融合的双目相机在线标定方法及系统 |
CN114812576A (zh) * | 2022-05-23 | 2022-07-29 | 上海钛米机器人股份有限公司 | 地图匹配方法、装置及电子设备 |
CN114942026A (zh) * | 2022-06-01 | 2022-08-26 | 四川大学 | 基于智能数据的多模态三维影像导航系统 |
CN115127554B (zh) * | 2022-08-31 | 2022-11-15 | 中国人民解放军国防科技大学 | 一种基于多源视觉辅助的无人机自主导航方法与系统 |
CN115523929B (zh) * | 2022-09-20 | 2023-05-12 | 北京四维远见信息技术有限公司 | 一种基于slam的车载组合导航方法、装置、设备及介质 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102435188B (zh) * | 2011-09-15 | 2013-10-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
CN102538781B (zh) * | 2011-12-14 | 2014-12-17 | 浙江大学 | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 |
CN102768042B (zh) * | 2012-07-11 | 2015-06-24 | 清华大学 | 一种视觉-惯性组合导航方法 |
CN104655131B (zh) * | 2015-02-06 | 2017-07-18 | 东南大学 | 基于istssrckf的惯性导航初始对准方法 |
-
2016
- 2016-12-08 CN CN201611121973.6A patent/CN106679648B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN106679648A (zh) | 2017-05-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
CN111156984B (zh) | 一种面向动态场景的单目视觉惯性slam方法 | |
CN108717712B (zh) | 一种基于地平面假设的视觉惯导slam方法 | |
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
CN106780699B (zh) | 一种基于sins/gps和里程计辅助的视觉slam方法 | |
Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
WO2019029099A1 (zh) | 基于图像梯度联合优化的双目视觉里程计算方法 | |
CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
CN108846857A (zh) | 视觉里程计的测量方法及视觉里程计 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
CN109579825A (zh) | 基于双目视觉和卷积神经网络的机器人定位系统及方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
CN112767546B (zh) | 移动机器人基于双目图像的视觉地图生成方法 | |
CN108256563B (zh) | 基于距离度量的视觉词典闭环检测方法与装置 | |
CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
CN115371665A (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN110749308B (zh) | 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法 | |
CN109544632B (zh) | 一种基于层次主题模型的语义slam对象关联方法 | |
CN110490933A (zh) | 基于单点ransac的非线性状态空间中心差分滤波器方法 |
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 |