CN106780699A - 一种基于sins/gps和里程计辅助的视觉slam方法 - Google Patents

一种基于sins/gps和里程计辅助的视觉slam方法 Download PDF

Info

Publication number
CN106780699A
CN106780699A CN201710012804.7A CN201710012804A CN106780699A CN 106780699 A CN106780699 A CN 106780699A CN 201710012804 A CN201710012804 A CN 201710012804A CN 106780699 A CN106780699 A CN 106780699A
Authority
CN
China
Prior art keywords
phi
delta
sin
sins
cos
Prior art date
Application number
CN201710012804.7A
Other languages
English (en)
Inventor
徐晓苏
杨博
代维
Original Assignee
东南大学
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 东南大学 filed Critical 东南大学
Priority to CN201710012804.7A priority Critical patent/CN106780699A/zh
Publication of CN106780699A publication Critical patent/CN106780699A/zh

Links

Classifications

    • GPHYSICS
    • G06COMPUTING; CALCULATING; COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/003D [Three Dimensional] image rendering
    • G06T15/005General purpose rendering architectures
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30Map- or contour-matching
    • G01C21/32Structuring or formatting of map data
    • GPHYSICS
    • G06COMPUTING; CALCULATING; COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29Geographical information databases

Abstract

本发明公开了一种基于SINS/GPS和里程计辅助的视觉SLAM方法,包括如下步骤:当全球卫星定位系统信号可用时,将GPS与捷联惯性导航系统输出信息进行数据融合,得到姿态、速度、位置等信息;当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置等信息;利用双目摄像机拍摄得到环境图片,对其进行特征提取和特征匹配;利用上述得到的先验姿态、速度、位置信息和环境特征实现定位与地图构建,完成视觉SLAM算法。本发明利用SINS、GPS和里程计辅助视觉SLAM,能够实现室外和室内两种环境下的定位与地图构建,应用范围广泛,且能够提高定位的精度和鲁棒性。

Description

一种基于SINS/GPS和里程计辅助的视觉SLAM方法

技术领域

[0001] 本发明涉及移动机器人自动导航技术领域,尤其是一种基于SINS/GPS和里程计辅 助的视觉SLAM方法。

背景技术

[0002] 同时定位与地图构建(Simultaneous localization and mapping,SLAM)问题是 移动机器人研究领域的基本问题之一,是移动机器人在未知环境中实现自主导航与自主控 制的关键。

[0003] 解决SLAM问题的方法有很多,随着视觉传感器的发展、成本的降低以及图像处理 技术的提高,视觉SLAM方法得到了广泛的应用,视觉传感器也逐步取代其他传感器称为解 决SLAM问题中获取外部环境信息的主要传感器。

[0004] 然而,随着SLAM方法应用的广泛化,其应用的环境也越来越多,单一的传感器已经 无法在复杂环境下解决SLAM问题。因此,出现了多传感器协同解决SLAM问题。目前,应用比 较多的是GPS辅助视觉SLAM方法或里程计辅助视觉SLAM方法。然而,GPS信号在有遮挡环境 下或室内环境下很容易失效,而里程计的误差较大,无法适应较复杂的环境。一旦一种传感 器失效,则定位导航的精度将受到很大的影响。因此,出现一种能够适应多种环境、室内室 外均可应用,且精度相对较高的SLAM方法是很有必要的。

发明内容

[0005] 发明目的:为克服现有技术中存在的不足,本发明提供了一种可以适应多种环境 且能够提高定位、制图精度的SLAM方法。

[0006] 技术方案:为解决上述技术问题,本发明提供了一种基于SINS/GPS和里程计辅助 的视觉SLAM方法,包括如下步骤:

[0007] 步骤1:当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、 位置信息;

[0008] 步骤2:当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、 速度、位置信息;

[0009] 步骤3:利用双目摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹 配;

[0010] 步骤4:利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地 图构建。

[0011] 进一步的,所述步骤1中当GPS信号可用时,将GPS与SINS输出信息进行数据融合, 得到姿态、速度、位置信息,具体为:

[0012] 步骤1.1:选择导航坐标系η为地理坐标系,其xyz分被指向“东北天”方向;选择载 体坐标系b的原点在载体质心,其xyz分被指向载体的“右前上”;选取系统状态向量为:

[0013]

Figure CN106780699AD00091

[0014] 其中,Sv=分别为东向和北向速度误差;(Κ、(Κ、φη分别为天向、东向和北向 平台误差角;SLJAJh分别为载体炜度、经度和高度误差;<、<、<分别为b系下xyz三轴 的陀螺常值误差;、▽$分别为b系下xy方向上的加速度计常值误差;

[0015] SINS/GPS系统的状态方程表示为:

[0016]

Figure CN106780699AD00101

[0017] 其中,

Figure CN106780699AD00102

Figure CN106780699AD00103

Rm、Rn分被为卯酉圈和子午圈半径,铲为计算出的速度,5铲为计算出的速 度误差,成:为计算得到的数学平台旋转角速度,C为地球旋转角速度,成!,为计算得到的数 学平台相对地球的旋转角速度,3<、为对应成、乾、^的计算误差; »<、分别为b系下陀螺仪和加速度计的随机误差;Cu、Cf分别为:

[0018]

Figure CN106780699AD00104

[0020] 步骤1.2:选择载体的速度作为观测量:2 = ;SINS/GPS系统的观测方程

Figure CN106780699AD00105

表示为·

[0021]

[0022] 其中,g为载体sins解算出的速度;K Sgps输出的速度;V为零均值高斯白噪声过 程;Hk为观测矩阵,且:Hk= [12X2 02Χ10];

[0023] 步骤1.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤1.1和步骤1.2 分别建立的SINS/GPS系统的状态方程和观测方程离散化,得到离散形式的状态方程和观测 方程,记为:

[0024]

Figure CN106780699AD00111

[0025] 利用容积卡尔曼滤波方法,对SINS/GPS系统进行数据融合,其过程如下:

[0026] (1)选定滤波初值:

Figure CN106780699AD00112

[0049] 估计出状态向量后,通过闭环反馈,得到SINS/GPS系统输出的位置、速度、姿态信 息;当GPS无信号或者系统停止运行时数据融合结束,否则,⑵〜(10)循环执行。

[0050] 进一步的,所述步骤2中当GPS信号不可用时,将里程计与SINS输出信息进行数据 融合,得到姿态、速度、位置信息,具体为:

[0051] 步骤2.1:选取SINS/0D系统状态向量为:

[0052]

Figure CN106780699AD00113

[0053] 其中,δαΦ为里程计航向安装误差角;δαθ为里程计俯仰安装误差角;SKd为里程计的 标度误差;SvBs分别为东向和北向速度误差;(Κ、(Κ、φη分别为天向、东向和北向平 台误差角;SLJAJh分别为载体炜度、经度和高度误差;<、<、<分别为b系下xyz三轴的 陀螺常值误差;、V6y分别为b系下xy方向上的加速度计常值误差;

[0054] SINS/0D系统的状态方程表示为:

[0055]

Figure CN106780699AD00121

[0056] 步骤2.2:选择载体的速度作为观测量:2 = [5<知’;;]61奶/00系统的观测方程 表示为:

[0057]

Figure CN106780699AD00122

[0058] 其中,€为载体SINS解算出的速度;%为里程计输出的速度;^为b系下的真实速 度;0^为里程计在b系下输出误差向量;V为零均值高斯白噪声过程;

[0059] δαφ和δαθ—般较小,近似表不为:

[0060]

Figure CN106780699AD00123

[0061] 其中,Kd为里程计的标度系数;Pd为里程计的输出脉冲;

[0062] 步骤2.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤2.1和步骤2.2 分别建立的SINS/0D状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记 为:

[0063]

Figure CN106780699AD00124

[0064] 利用容积卡尔曼滤波(CKF)方法,对SINS/0D系统进行数据融合,其过程如下:

[0065] (1)选定滤波初值:

Figure CN106780699AD00125

[0069] (3)对k-1时刻状态估计误差方差阵Ph进行Cholesky分解:

Figure CN106780699AD00131

[0093] 估计出状态向量后,通过闭环反馈,得到SINS/0D系统输出的位置、速度、姿态信 息;当GPS有信号或者系统停止运行时数据融合结束,否则,⑵〜(11)循环执行。

[0094] 进一步的,所述步骤3中利用摄像机拍摄得到环境图片,对环境图片进行特征提取 和特征匹配,具体为:

[0095] 步骤3.1:由双目摄像机拍摄得到左右两张图片;对每张图片,利用高斯卷积核对 图像进行线性尺度变换,构建尺度空间:

[0096]

Figure CN106780699AD00132

[0097] 其中,I (x,y)为二维图像,G (x,y,〇)为尺度可变的高斯函数,具体表达式如下:

[0098]

Figure CN106780699AD00141

[0099] 图像尺度空间L (x,y,〇)是通过图像I (x,y)与具有可变核的高斯滤波函数G (x,y, σ)进行卷积形成的;用图像的高斯金子塔来表示,金子塔的层数为:log (width) /log (2) -2, width为图像宽度;每层放置4张经不同高斯核卷积核处理过的图像;

[0100] 步骤3.2:对同一层内两张相邻的经过高斯卷积的图片做差,得到高斯差分金字塔 (DOG):

[0101] D(x,y,a) = (C Cr, ν,/ίσ)-G (χ,ν,σ))0 / (x, >·)-/_. (,v, ν,Ασ)- L (.v, ν,σ)

[0102] 在三维尺度空间D (x,y,〇)中,搜索每个点的26个领域,如果该点为局部极值点,则 将该点保存为候选关键点;

[0103] 对D(x,y,〇)进行泰勒展开:

[0104]

Figure CN106780699AD00142

[0105] 其中::;

[0106] 去除对比度低的关键点,S卩:ID(A)Is 0.03的点;

[0107] 计算 Hessian 矩阵 Η:

Figure CN106780699AD00143

[0111] 设置r=10,如I

Figure CN106780699AD00144

,则该关键点位于边缘部分,应当去除,反之, 保留关键点;

[0112] 所保留下的关键点即为提取出的特征点;

[0113] 步骤3.3:利用特征点邻域内像素的梯度方向分布特性为每个特征点定义主方向; 每个像素点的梯度幅值为:

Figure CN106780699AD00145

[0117] 利用像素点梯度创建梯度方向直方图;直方图以每10°为一个柱,共36个柱;柱所 代表的方向为像素点的梯度方向,柱的长短代表了梯度幅值;最后选择直方图的主峰值作 为该特征点的主方向,选取量值达到主峰值80%以上的局部峰值作为辅助方向;

[0118] 步骤3.4:计算以特征点为中心的8X8的窗口,并平均分为4个子窗口,计算每个子 窗口内,8个方向上的梯度幅值并累加;得到每个特征点的描述符;通过特征点之间的距离 计算公式判断两幅图片特征点之间的相似程度,从而进行特征匹配:

[0119]

Figure CN106780699AD00151

[0120] 其中,Ii和ri分别为左右图像上特征点的描述符;di stance越小,说明特征点越相 似;

[0121] 将双目摄像机在当前时刻拍摄到的2幅图片与以前时刻拍摄过的图片进行特征匹 配,判断特征点是否出现过,若出现过,则不需要进行状态、观测向量增广,则执行完步骤 3.5后,需要需要进行状态、观测向量增广;

[0122] 步骤3.5:通过三角测距原理求取特征点在载体坐标系内的位置(xb,yb,zb):

[0123]

Figure CN106780699AD00152

[0124] 其中,B为左右摄像机投影中心间的距离,Xleft和Xright分别为空间点在左右摄像机 图像上的投影横坐标,f为摄像机的焦距,视差Di spar ity = xieft-xright。

[0125] 进一步的,所述步骤4中利用步骤1和步骤2得到的姿态、速度、位置信息和环境特 征实现定位与地图构建,具体为:

[0126] 步骤4.1:选取载体初始位置的质心为原点建立世界坐标系w,w系的xyz分被指向 “东北天”方向;该坐标系与地球固联,xyz始终指向“东北天”方向,且原点位置不随载体运 动变化;选取系统的状态向量为:

[0127]

Figure CN106780699AD00153

[0128] 其中,A =[λ,;' % 4]代表在世界坐标系w下,载体的位置坐标,初始值均为零;

Figure CN106780699AD00154

弋表在世界坐标系w下,特征点的位置坐标;M为特征 点的数量;

[0129] 组合导航系统的状态方程表示为:

[0130]

Figure CN106780699AD00155

[0131] 其中,V= [ve νη %]7代表由31吧八^系统或31吧/00系统计算得到的“东北天”方 向上的速度;A T代表采样时间间隔;

[0132] 步骤4.2:选择特征点在载体坐标系内的位置:

Figure CN106780699AD00156

为观测向量;其中,m为同一时间特征点的数量。

[0133] 系统的观测方程可表示为:

[0134]

Figure CN106780699AD00157

[0135] 令:双=《,则:

[0136]

Figure CN106780699AD00161

[0137] 其中,Φ、γ、θ分别为SINS/GPS系统或SINS/0D系统计算得到载体航向角、横摇角和 纵摇角;

[0138] 步骤4.3:利用标准卡尔曼滤波方法进行系统数据融合,其过程如下:

[0139] ⑵选定滤波初值:

Figure CN106780699AD00162

[0148] 估计出状态向量后,通过闭环反馈,完成定位以及地图的制作;系统停止运行时, 数据融合结束。

[0149] 本发明对于现有技术具有以下优点:

[0150] 1.本发明针对室外情况和室内情况,分别提供了 SINS/GPS组合导航和SINS/0D组 合导航方法辅助视觉SLAM的方法,从而使得该方法在室内和室外均可以应用,克服了现有 技术应用环境单一的问题。

[0151] 2.本发明利用SINS/GPS组合导航和SINS/0D组合导航方法,可以提供精度较高的 先验姿态、速度、位置信息,从而进一步提高了视觉SLAM的定位与制图的精度。

[0152] 3.本发明可以实现载体位置的三维定位与制图,克服现有技术中大多数只在二维 平面上进行定位与制图的缺陷。

附图说明

[0153] 图1是本发明的系统框图;

[0154] 图2是本发明SINS/GPS子系统的示意图;

[0155] 图3是本发明SINS/0D子系统的示意图;

[0156] 图4是本发明中特征提取与特征匹配的流程图;

[0157] 图5是本发明在GPS信号良好时的定位效果图;

[0158] 图6是本发明在GPS信号不可用时的定位效果图。

具体实施方式

[0159] 下面结合附图对本发明作更进一步的说明。

[0160] 如图1所示是本发明的系统方案图。当GPS信号可用时,由SINS/GPS系统提供先验 的姿态、速度、位置信息;当GPS信号不可用时,由SINS/0D系统提供先验的姿态、速度、位置 信息。其中SINS为捷联惯性导航系统(Strap-down Inertial Navigation System)、0D为里 程计(Odometry)。同时,双目摄像机拍摄得到环境图片,并进行特征提取、特征匹配等工作, 最后由视觉SLAM算法计算得到位置信息并完成制图。

[0161] 如图2和3所示是本发明SINS/GPS子系统和SINS/0D子系统的方案图,两个子系统 都使用MU采集得到载体的比力信息和角速度信息,并进行捷联解算。SINS/GPS子系统通过 GPS测量得到速度信息,而SINS/0D系统,使用里程计测量得到速度信息,并将本身误差带入 滤波器中。两个子系统均采用CKF滤波器完成数据融合。

[0162] SINS/GPS子系统进行数据融合过程如下:

[0163] 选取系统状态向量为:

Figure CN106780699AD00171

[0167] 择载体的速度作为观测量:2 = 办;:],建立SINS/GPS系统的观测方程:

[0168]

Figure CN106780699AD00172

[0169] 使用四阶龙格-库塔积分方法,以采样周期T为步长将上述建立的SINS/GPS状态方 程和观测方程离散化,得到离散形式的状态方程和观测方程,简记为:

[0170]

Figure CN106780699AD00173

[0171] 利用容积卡尔曼滤波(CKF)方法,对上述SINS/GPS系统进行数据融合:

[0172] 1)选定滤波初值:

Figure CN106780699AD00174

Figure CN106780699AD00181

[0195] SINS/0D子系统进行数据融合过程如下:

[0196] 选取SINS/0D系统状态向量为:

[0197]

Figure CN106780699AD00182

[0200] 选择载体的速度作为观测量= ;建立SINS/0D系统的观测方程:

[0201]

Figure CN106780699AD00183

[0202] 使用四阶龙格-库塔积分方法,以采样周期T为步长将上述建立的SINS/0D状态方 程和观测方程离散化,得到离散形式的状态方程和观测方程:

[0203]

Figure CN106780699AD00191

[0204] 利用容积卡尔曼滤波(CKF))方法,对上述SINS/0D系统进行数据融合:

[0205] (1)选定滤波初值:

Figure CN106780699AD00192

[0232]

Figure CN106780699AD00201

[0233] 如图4所示是本发明特征提取与特征匹配的流程图,其过程如下:

[0234] 1.由双目摄像机拍摄得到左右两张图片;对每张图片,利用高斯卷积核对图像进 行线性尺度变换,构建尺度空间:

[0235] Z. (x, >·,σ) = G (x, y,σ)® I (x, y)

[0236] 其中,I (x,y)为二维图像,G (x,y,〇)为尺度可变的高斯函数,具体表达式如下:

[0237]

Figure CN106780699AD00202

[0238] 图像尺度空间L (x,y,〇)是通过图像I (x,y)与具有可变核的高斯滤波函数G (x,y, σ)进行卷积形成的;可以用图像的高斯金子塔来表示,金子塔的层数为:l〇g(width)/log (2) _2,width为图像宽度;每层放置4张经不同高斯核卷积核处理过的图像;

[0239] 2.对同一层内两张相邻的经过高斯卷积的图片做差,得到高斯差分金字塔(DOG):

[0240] D(x, V, σ) = ((7 (x, y,ka ) - G (x, v,σ)jΘ / (x, r) -L (x, v,ko)~ L (x, y,σ)

[0241] 在三维尺度空间D (x,y,σ)中,搜索每个点的26个领域,如果该点为局部极值点,则 将该点保存为候选关键点;

[0242] 对D (X,y,〇)进行泰勒展开:

[0243]

Figure CN106780699AD00203

[0244] 其中:A (λ%.V- 〇j%

[0245] 去除对比度低的关键点,BP: 0.03的点;

[0246] 计算 Hessian 矩阵 Η:

Figure CN106780699AD00204

[0250] 设置r= 1〇,如果

Figure CN106780699AD00205

则该关键点位于边缘部分,应当去除,反之, 保留关键点;

[0251] 所保留下的关键点就是提取出的特征点;

[0252] 3.利用特征点邻域内像素的梯度方向分布特性为每个特征点定义主方向;每个像 素点的梯度幅值为:

[0253]

Figure CN106780699AD00206

[0254] 梯度方向为:

[0255]

Figure CN106780699AD00211

[0256] 利用像素点梯度创建梯度方向直方图;直方图以每10°为一个柱,共36个柱;柱所 代表的方向为像素点的梯度方向,柱的长短代表了梯度幅值;最后选择直方图的主峰值作 为该特征点的主方向,选取量值达到主峰值80%以上的局部峰值作为辅助方向;

[0257] 4.计算以特征点为中心的8 X 8的窗口,并平均分为4个子窗口,计算每个子窗口 内,8个方向上的梯度幅值并累加;得到每个特征点的描述符;通过特征点之间的距离计算 公式判断两幅图片特征点之间的相似程度,从而进行特征匹配:

[0258]

Figure CN106780699AD00212

[0259] 其中,Ii和ri分别为左右图像上特征点的描述符。di stance越小,说明特征点越相 似;

[0260] 5:通过三角测距原理求取特征点在载体坐标系内的位置(xb,yb,zb):

[0261]

Figure CN106780699AD00213

[0262] 其中,B为左右摄像机投影中心间的距离,Xleft和Xright分别为空间点在左右摄像机 图像上的投影横坐标,f为摄像机的焦距,视差Di spar ity = xieft-xright。

[0263] 最后,利用上述得到的先验位置信息和环境信息完成视觉SLAM算法,实现定位与 地图构建,具体过程如下:

[0264] 选取系统的状态向量为:

Figure CN106780699AD00214

[0268] 选择特征点在载体坐标系内的位置= 分,zf… <,义,为观测向 量,建立系统的观测方程:

[0269]

Figure CN106780699AD00215

[0270] 利用标准卡尔曼滤波方法进行系统数据融合:

[0271] (2)选定滤波初值:

Figure CN106780699AD00216

Figure CN106780699AD00221

[0280] 使用如下的实例说明本发明的有益效果:

[0281] 在MATLAB环境下,分被模拟GPS信号可用和GPS信号不可用两种情况下,利用本发 明提供的基于SINS/GPS和里程计辅助的视觉SLAM方法进行定位与制图的仿真。结果如图5 和图6所示。其中,细轨迹代表真实轨迹;粗轨迹代表载体通过计算得到的定位轨迹;带有数 字的点代表换环境特征点;半椭球代表特征点的定位精度,最外围的椭球为特征点初始定 位精度,而最内部的椭球代表最终的特征点初始定位精度,即,制图的精度。可以发现,该精 度在不断提高。

[0282] 以上结果表明,GPS信号可用和GPS信号不可用在两种情况下,本发明提供的方法 都可以以$父尚精度完成定位与制图。

[0283] 本发明包括如下步骤:当全球卫星定位系统(GPS)信号可用时,将GPS与捷联惯性 导航系统(SINS)输出信息进行数据融合,得到姿态、速度、位置等信息;当GPS信号不可用 时,将里程计(OD)与SINS输出信息进行数据融合,得到姿态、速度、位置等信息;利用双目摄 像机拍摄得到环境图片,对其进行特征提取和特征匹配;利用上述得到的先验姿态、速度、 位置信息和环境特征实现定位与地图构建,完成视觉SLAM算法。本发明利用SINS、GPS和里 程计辅助视觉SLAM,能够实现室外和室内两种环境下的定位与地图构建,应用范围广泛,且 能够提高定位的精度和鲁棒性。

[0284] 以上详细描述了本发明的优选实施方式,但是,本发明并不限于上述实施方式中 的具体细节,在本发明的技术构思范围内,可以对本发明的技术方案进行多种等同变换,这 些等同变换均属于本发明的保护范围。

Claims (5)

1. 一种基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,包括如下步骤: 步骤1:当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、位置 信息; 步骤2:当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、 位置信息; 步骤3:利用双目摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹配; 步骤4:利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地图构 建。
2. 如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,所述 步骤1中当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、位置信 息,具体为: 步骤1.1:选择导航坐标系η为地理坐标系,其xyz分被指向“东北天”方向;选择载体坐 标系b的原点在载体质心,其xyz分被指向载体的“右前上”;选取系统状态向量为:
Figure CN106780699AC00021
其中,分别为东向和北向速度误差;(Κ、Φμ φη分别为天向、东向和北向平台 误差角JLJAjh分别为载体炜度、经度和高度误差;<、<、g分别为b系下xyz三轴的陀 螺常值误差;分别为b系下xy方向上的加速度计常值误差; SINS/GPS系统的状态方程表示为:
Figure CN106780699AC00022
其中,
Figure CN106780699AC00023
.
Figure CN106780699AC00024
|RM、RN分被为卯酉圈和子午圈半径,P为计算出的速度,为计算出的 速度误差,C为计算得到的数学平台旋转角速度,令[为地球旋转角速度,成;为计算得到的 数学平台相对地球的旋转角速度,&amp;<、为对应我〖、<、武的计算误差; ^、W=分别为b系下陀螺仪和加速度计的随机误差;、C„B’分别为:
Figure CN106780699AC00025
Figure CN106780699AC00031
步骤1.2:选择载体的速度作为观测量:2 = pv:; ;SINS/GPS系统的观测方程表示 为:
Figure CN106780699AC00032
其中,€为载体SINS解算出的速度;K为GPS输出的速度;V为零均值高斯白噪声过程;Hk 为观测矩阵,且:Hk= [12X2 〇2Χ10]; 步骤1.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤1.1和步骤1.2分别 建立的SINS/GPS系统的状态方程和观测方程离散化,得到离散形式的状态方程和观测方 程,记为:
Figure CN106780699AC00033
(9)计算增益矩:
Figure CN106780699AC00041
估计出状态向量后,通过闭环反馈,得到SINS/GPS系统输出的位置、速度、姿态信息;当 GPS无信号或者系统停止运行时数据融合结束,否则,(2)〜(10)循环执行。
3.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,所述 步骤2中当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位 置信息,具体为: 步骤2.1:选取SINS/OD系统状态向量为:
Figure CN106780699AC00042
其中,δαΦ为里程计航向安装误差角;δαθ为里程计俯仰安装误差角;SKd为里程计的标度 误差» 6<分别为东向和北向速度误差;Φυ、Φβ、Φ η分别为天向、东向和北向平台误差 角;δί、δλ、δΐ!分别为载体炜度、经度和高度误差;sf、: <、4分别为b系下xyz三轴的陀螺常 值误差;Vi、▽;.分别为b系下xy方向上的加速度计常值误差; SINS/OD系统的状态方程表示为:
Figure CN106780699AC00043
步骤2.2:选择载体的速度作为观测量:® = Rif ;SINS/OD系统的观测方程表示 为:
Figure CN106780699AC00044
其中,巧为载体SINS解算出的速度;巧为里程计输出的速度; <为b系下的真实速度; 为里程计在b系下输出误差向量;V为零均值高斯白噪声过程; δαΦ和δαθ—般较小,近似表示为:
Figure CN106780699AC00051
其中,Kd为里程计的标度系数;Pd为里程计的输出脉冲; 步骤2.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤2.1和步骤2.2分别 建立的SINS/OD状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记为:
Figure CN106780699AC00052
利用容积卡尔曼滤波(CKF)方法,对SINS/OD系统进行数据融合,其过程如下: (1)选定滤波初值:
Figure CN106780699AC00053
(11)计算增益矩:
Figure CN106780699AC00061
估计出状态向量后,通过闭环反馈,得到SINS/OD系统输出的位置、速度、姿态信息;当 GPS有信号或者系统停止运行时数据融合结束,否则,(2)〜(I 1)循环执行。
4.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,所述 步骤3中利用摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹配,具体为: 步骤3.1:由双目摄像机拍摄得到左右两张图片;对每张图片,利用高斯卷积核对图像 进行线性尺度变换,构建尺度空间: L(χ.,γ,σ) ^ G (λ·, j,σ)® I (,v, r) 其中,I (x,y)为二维图像,G (x,y,〇)为尺度可变的高斯函数,具体表达式如下:
Figure CN106780699AC00062
图像尺度空间L(x,y,〇)是通过图像I(x,y)与具有可变核的高斯滤波函数G(x,y,〇)进 行卷积形成的;用图像的高斯金子塔来表示,金子塔的层数为:log (width) /log (2) -2, width为图像宽度;每层放置4张经不同高斯核卷积核处理过的图像; 步骤3.2:对同一层内两张相邻的经过高斯卷积的图片做差,得到高斯差分金字塔 (DOG):
Figure CN106780699AC00063
在三维尺度空间D (x,y,〇)中,搜索每个点的26个领域,如果该点为局部极值点,则将该 点保存为候选关键点; 对D (X,y,σ)进行泰勒展开:
Figure CN106780699AC00064
去除对比度低的关键点,即:|i>(句I以.〇3的点; 计算Hessian矩阵H:
Figure CN106780699AC00065
设矩阵H的两个特征值分别为λ#Ρλ2,且:A1 = I1A2;则:
Figure CN106780699AC00066
设置r= 10,如·
Figure CN106780699AC00067
则该关键点位于边缘部分,应当去除,反之,保留 关键点; 所保留下的关键点即为提取出的特征点; 步骤3.3:利用特征点邻域内像素的梯度方向分布特性为每个特征点定义主方向;每个 像素点的梯度幅值为:
Figure CN106780699AC00071
利用像素点梯度创建梯度方向直方图;直方图以每10°为一个柱,共36个柱;柱所代表 的方向为像素点的梯度方向,柱的长短代表了梯度幅值;最后选择直方图的主峰值作为该 特征点的主方向,选取量值达到主峰值80%以上的局部峰值作为辅助方向; 步骤3.4:计算以特征点为中心的8X8的窗口,并平均分为4个子窗口,计算每个子窗口 内,8个方向上的梯度幅值并累加;得到每个特征点的描述符;通过特征点之间的距离计算 公式判断两幅图片特征点之间的相似程度,从而进行特征匹配:
Figure CN106780699AC00072
其中,Ii和ri分别为左右图像上特征点的描述符;distance越小,说明特征点越相似; 将双目摄像机在当前时刻拍摄到的2幅图片与以前时刻拍摄过的图片进行特征匹配, 判断特征点是否出现过,若出现过,则不需要进行状态、观测向量增广,则执行完步骤3.5 后,需要需要进行状态、观测向量增广; 步骤3.5:通过三角测距原理求取特征点在载体坐标系内的位置(xb,yb,zb):
Figure CN106780699AC00073
其中,B为左右摄像机投影中心间的距离,Xlef t和Xright分别为空间点在左右摄像机图像 上的投影横坐标,f为摄像机的焦距,视差Disparity = Xieft-Xright。
5.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,所述 步骤4中利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地图构建, 具体为: 步骤4.1:选取载体初始位置的质心为原点建立世界坐标系w,w系的xyz分被指向“东北 天”方向;该坐标系与地球固联,xyz始终指向“东北天”方向,且原点位置不随载体运动变 化;选取系统的状态向量为:
Figure CN106780699AC00074
其中,Xci =[< <<;]代表在世界坐标系w下,载体的位置坐标,初始值均为零; 孓4…4 W 代表在世界坐标系W下,特征点的位置坐标;M为特征 点的数量; 组合导航系统的状态方程表示为:
Figure CN106780699AC00081
其中,¥=&amp;%^]7代表由31吧六^系统或31奶/00系统计算得到的“东北天”方向上 的速度;△ T代表米样时间间隔; 步骤4.2:选择特征点在载体坐标系内的位置:2=[4,分,4〜4,_),丨,,4]7为观测 向量;其中,m为同一时间特征点的数量。 系统的观测方程可表示为:
Figure CN106780699AC00082
其中,Φ、γ、θ分别为SINS/GPS系统或SINS/0D系统计算得到载体航向角、横摇角和纵摇 角; 步骤4.3:利用标准卡尔曼滤波方法进行系统数据融合,其过程如下: ⑵选定滤波初值:
Figure CN106780699AC00083
估计出状态向量后,通过闭环反馈,完成定位以及地图的制作;系统停止运行时,数据 融合结束。
CN201710012804.7A 2017-01-09 2017-01-09 一种基于sins/gps和里程计辅助的视觉slam方法 CN106780699A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710012804.7A CN106780699A (zh) 2017-01-09 2017-01-09 一种基于sins/gps和里程计辅助的视觉slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710012804.7A CN106780699A (zh) 2017-01-09 2017-01-09 一种基于sins/gps和里程计辅助的视觉slam方法

Publications (1)

Publication Number Publication Date
CN106780699A true CN106780699A (zh) 2017-05-31

Family

ID=58950624

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710012804.7A CN106780699A (zh) 2017-01-09 2017-01-09 一种基于sins/gps和里程计辅助的视觉slam方法

Country Status (1)

Country Link
CN (1) CN106780699A (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107356252A (zh) * 2017-06-02 2017-11-17 青岛克路德机器人有限公司 一种融合视觉里程计与物理里程计的室内机器人定位方法
CN107389064A (zh) * 2017-07-27 2017-11-24 长安大学 一种基于惯性导航的无人车变道控制方法
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN107741234A (zh) * 2017-10-11 2018-02-27 深圳勇艺达机器人有限公司 一种基于视觉的离线地图构建及定位方法
CN108230247A (zh) * 2017-12-29 2018-06-29 达闼科技(北京)有限公司 基于云端的三维地图的生成方法、装置、设备及应用程序
CN108873038A (zh) * 2018-09-10 2018-11-23 芜湖盟博科技有限公司 自主泊车定位方法及定位系统
CN109029433A (zh) * 2018-06-28 2018-12-18 东南大学 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109115232A (zh) * 2017-06-22 2019-01-01 华为技术有限公司 导航的方法和装置
CN109490931A (zh) * 2018-09-03 2019-03-19 天津远度科技有限公司 飞行定位方法、装置及无人机
WO2020024845A1 (zh) * 2018-08-01 2020-02-06 京东方科技集团股份有限公司 定位方法和装置

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
US20100176987A1 (en) * 2009-01-15 2010-07-15 Takayuki Hoshizaki Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103323002A (zh) * 2012-03-19 2013-09-25 联想(北京)有限公司 即时定位与地图构建方法和装置
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN104729506A (zh) * 2015-03-27 2015-06-24 北京航空航天大学 一种视觉信息辅助的无人机自主导航定位方法
CN105411490A (zh) * 2015-10-26 2016-03-23 曾彦平 移动机器人的实时定位方法及移动机器人
CN106289285A (zh) * 2016-08-20 2017-01-04 南京理工大学 一种关联场景的机器人侦察地图及构建方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101413800A (zh) * 2008-01-18 2009-04-22 南京航空航天大学 导航/稳瞄一体化系统的导航、稳瞄方法
US20100176987A1 (en) * 2009-01-15 2010-07-15 Takayuki Hoshizaki Method and apparatus to estimate vehicle position and recognized landmark positions using GPS and camera
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN103323002A (zh) * 2012-03-19 2013-09-25 联想(北京)有限公司 即时定位与地图构建方法和装置
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN104729506A (zh) * 2015-03-27 2015-06-24 北京航空航天大学 一种视觉信息辅助的无人机自主导航定位方法
CN105411490A (zh) * 2015-10-26 2016-03-23 曾彦平 移动机器人的实时定位方法及移动机器人
CN106289285A (zh) * 2016-08-20 2017-01-04 南京理工大学 一种关联场景的机器人侦察地图及构建方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
高扬: ""可量测影像实景导航关键技术研究"", 《中国博士学位论文全文数据库 基础科学辑》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107356252A (zh) * 2017-06-02 2017-11-17 青岛克路德机器人有限公司 一种融合视觉里程计与物理里程计的室内机器人定位方法
CN107356252B (zh) * 2017-06-02 2020-06-16 青岛克路德机器人有限公司 一种融合视觉里程计与物理里程计的室内机器人定位方法
CN109115232A (zh) * 2017-06-22 2019-01-01 华为技术有限公司 导航的方法和装置
CN107478214A (zh) * 2017-07-24 2017-12-15 杨华军 一种基于多传感器融合的室内定位方法及系统
CN107389064A (zh) * 2017-07-27 2017-11-24 长安大学 一种基于惯性导航的无人车变道控制方法
CN107741234A (zh) * 2017-10-11 2018-02-27 深圳勇艺达机器人有限公司 一种基于视觉的离线地图构建及定位方法
CN108230247A (zh) * 2017-12-29 2018-06-29 达闼科技(北京)有限公司 基于云端的三维地图的生成方法、装置、设备及应用程序
CN108230247B (zh) * 2017-12-29 2019-03-15 达闼科技(北京)有限公司 基于云端的三维地图的生成方法、装置、设备及计算机可读的存储介质
CN109029433A (zh) * 2018-06-28 2018-12-18 东南大学 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
WO2020024845A1 (zh) * 2018-08-01 2020-02-06 京东方科技集团股份有限公司 定位方法和装置
CN109490931A (zh) * 2018-09-03 2019-03-19 天津远度科技有限公司 飞行定位方法、装置及无人机
CN108873038A (zh) * 2018-09-10 2018-11-23 芜湖盟博科技有限公司 自主泊车定位方法及定位系统

Similar Documents

Publication Publication Date Title
CN104061934B (zh) 基于惯性传感器的行人室内位置跟踪方法
KR20180079428A (ko) 자동 로컬리제이션을 위한 장치 및 방법
Li et al. Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera
Lovegrove et al. Spline Fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras.
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
Atia et al. Integrated indoor navigation system for ground vehicles with automatic 3-D alignment and position initialization
CN102538781B (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
US20160209846A1 (en) Visual Localization Within LIDAR Maps
CN104704384B (zh) 具体用于装置的基于视觉的定位的图像处理方法
US10247556B2 (en) Method for processing feature measurements in vision-aided inertial navigation
Lupton et al. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions
US8494225B2 (en) Navigation method and aparatus
CN105021184B (zh) 一种用于移动平台下视觉着舰导航的位姿估计系统及方法
CN104406586B (zh) 基于惯性传感器的行人导航装置和方法
Kim et al. Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system, global positioning system, odometer and vision data
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
US7233691B2 (en) Any aspect passive volumetric image processing method
CN102435188B (zh) 一种用于室内环境的单目视觉/惯性全自主导航方法
Georgy et al. Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter
CN103379619B (zh) 一种定位方法和系统
CN106708066B (zh) 基于视觉/惯导的无人机自主着陆方法
CN103411609B (zh) 一种基于在线构图的飞行器返航路线规划方法
CN102353377B (zh) 一种高空长航时无人机组合导航系统及其导航定位方法
KR100728377B1 (ko) 레이저 스캐너 및 무선인터넷을 이용한 변경된 지역시설물의 gis 실시간 업데이트 방법

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