当前位置: 首页 > 专利查询>东南大学专利>正文

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

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

A visual SLAM method based on SINS/GPS and odometer aided

The invention discloses an auxiliary SINS/GPS and visual SLAM based on mileage method comprises the following steps: when the GPS signal is available, GPS and strapdown inertial navigation system output information for data fusion, get the attitude velocity and position information; when the GPS signal is not available, the odometry data the fusion and SINS output information, get posture, speed and position information; shooting environment images using the binocular camera, the feature extraction and feature matching; using the obtained prior attitude, velocity and position information and environmental characteristics of construction and map positioning, visual SLAM algorithm. The invention utilizes SINS, GPS and odometer to assist visual SLAM, and can realize the positioning and map construction in two outdoor environments and indoor environments, and has wide application range, and can improve the positioning accuracy and robustness.

【技术实现步骤摘要】
一种基于SINS/GPS和里程计辅助的视觉SLAM方法
本专利技术涉及移动机器人自动导航
,尤其是一种基于SINS/GPS和里程计辅助的视觉SLAM方法。
技术介绍
同时定位与地图构建(Simultaneouslocalizationandmapping,SLAM)问题是移动机器人研究领域的基本问题之一,是移动机器人在未知环境中实现自主导航与自主控制的关键。解决SLAM问题的方法有很多,随着视觉传感器的发展、成本的降低以及图像处理技术的提高,视觉SLAM方法得到了广泛的应用,视觉传感器也逐步取代其他传感器称为解决SLAM问题中获取外部环境信息的主要传感器。然而,随着SLAM方法应用的广泛化,其应用的环境也越来越多,单一的传感器已经无法在复杂环境下解决SLAM问题。因此,出现了多传感器协同解决SLAM问题。目前,应用比较多的是GPS辅助视觉SLAM方法或里程计辅助视觉SLAM方法。然而,GPS信号在有遮挡环境下或室内环境下很容易失效,而里程计的误差较大,无法适应较复杂的环境。一旦一种传感器失效,则定位导航的精度将受到很大的影响。因此,出现一种能够适应多种环境、室内室外均可应用,且精度相对较高的SLAM方法是很有必要的。
技术实现思路
专利技术目的:为克服现有技术中存在的不足,本专利技术提供了一种可以适应多种环境且能够提高定位、制图精度的SLAM方法。技术方案:为解决上述技术问题,本专利技术提供了一种基于SINS/GPS和里程计辅助的视觉SLAM方法,包括如下步骤:步骤1:当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、位置信息;步骤2:当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置信息;步骤3:利用双目摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹配;步骤4:利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地图构建。进一步的,所述步骤1中当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、位置信息,具体为:步骤1.1:选择导航坐标系n为地理坐标系,其xyz分被指向“东北天”方向;选择载体坐标系b的原点在载体质心,其xyz分被指向载体的“右前上”;选取系统状态向量为:其中,分别为东向和北向速度误差;φu、φe、φn分别为天向、东向和北向平台误差角;δL、δλ、δh分别为载体纬度、经度和高度误差;分别为b系下xyz三轴的陀螺常值误差;分别为b系下xy方向上的加速度计常值误差;SINS/GPS系统的状态方程表示为:其中,φ=[φuφeφn]T,RM、RN分被为卯酉圈和子午圈半径,为计算出的速度,为计算出的速度误差,为计算得到的数学平台旋转角速度,为地球旋转角速度,为计算得到的数学平台相对地球的旋转角速度,为对应的计算误差;分别为b系下陀螺仪和加速度计的随机误差;Cω、分别为:步骤1.2:选择载体的速度作为观测量:SINS/GPS系统的观测方程表示为:其中,为载体SINS解算出的速度;为GPS输出的速度;ν为零均值高斯白噪声过程;Hk为观测矩阵,且:Hk=[I2×202×10];步骤1.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤1.1和步骤1.2分别建立的SINS/GPS系统的状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记为:利用容积卡尔曼滤波方法,对SINS/GPS系统进行数据融合,其过程如下:(1)选定滤波初值:(2)计算2n个sigma点与对应权值:(3)对k-1时刻状态估计误差方差阵Pk-1进行Cholesky分解:(4)求取2n个容积点并通过非线性状态方程进行传播:(5)求取状态一步预测值与误差方阵一步预测值:(6)计算量测预测值:(7)计算量测预测误差方差阵:(8)计算状态和量测间互协方差阵为(9)计算增益矩:(10)计算状态值估计值与状态估计误差方差阵:估计出状态向量后,通过闭环反馈,得到SINS/GPS系统输出的位置、速度、姿态信息;当GPS无信号或者系统停止运行时数据融合结束,否则,(2)~(10)循环执行。进一步的,所述步骤2中当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置信息,具体为:步骤2.1:选取SINS/OD系统状态向量为:其中,δαψ为里程计航向安装误差角;δαθ为里程计俯仰安装误差角;δKD为里程计的标度误差;分别为东向和北向速度误差;φu、φe、φn分别为天向、东向和北向平台误差角;δL、δλ、δh分别为载体纬度、经度和高度误差;分别为b系下xyz三轴的陀螺常值误差;分别为b系下xy方向上的加速度计常值误差;SINS/OD系统的状态方程表示为:步骤2.2:选择载体的速度作为观测量:SINS/OD系统的观测方程表示为:其中,为载体SINS解算出的速度;为里程计输出的速度;为b系下的真实速度;为里程计在b系下输出误差向量;ν为零均值高斯白噪声过程;δαψ和δαθ一般较小,近似表示为:其中,KD为里程计的标度系数;PD为里程计的输出脉冲;步骤2.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤2.1和步骤2.2分别建立的SINS/OD状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记为:利用容积卡尔曼滤波(CKF)方法,对SINS/OD系统进行数据融合,其过程如下:(1)选定滤波初值:(2)计算2n个sigma点与对应权值:(3)对k-1时刻状态估计误差方差阵Pk-1进行Cholesky分解:(4)求取2n个容积点并通过非线性状态方程进行传播:(5)求取状态一步预测值与误差方阵一步预测值:(6)将一步预测误差方差Pk/k-1阵进行Cholesky分解:(7)求取2n个容积点并通过非线性量测方程进行传播:(8)计算量测预测值:(9)计算量测预测误差方差阵:(10)计算状态和量测间互协方差阵为(11)计算增益矩:(12)计算状态值估计值与状态估计误差方差阵:估计出状态向量后,通过闭环反馈,得到SINS/OD系统输出的位置、速度、姿态信息;当GPS有信号或者系统停止运行时数据融合结束,否则,(2)~(11)循环执行。进一步的,所述步骤3中利用摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹配,具体为:步骤3.1:由双目摄像机拍摄得到左右两张图片;对每张图片,利用高斯卷积核对图像进行线性尺度变换,构建尺度空间:其中,I(x,y)为二维图像,G(x,y,σ)为尺度可变的高斯函数,具体表达式如下:图像尺度空间L(x,y,σ)是通过图像I(x,y)与具有可变核的高斯滤波函数G(x,y,σ)进行卷积形成的;用图像的高斯金子塔来表示,金子塔的层数为:log(width)/log(2)-2,width为图像宽度;每层放置4张经不同高斯核卷积核处理过的图像;步骤3.2:对同一层内两张相邻的经过高斯卷积的图片做差,得到高斯差分金字塔(DOG):在三维尺度空间D(x,y,σ)中,搜索每个点的26个领域,如果该点为局部极值点,则将该点保存为候选关键点;对D(x,y,σ)进行泰勒展开:其中:去除对比度低的关键点,即:的点;计算Hessian矩阵H:设矩阵H的两个特征值分别为λ1和λ2,且:λ1=rλ2;则:设置r本文档来自技高网...
一种基于SINS/GPS和里程计辅助的视觉SLAM方法

【技术保护点】
一种基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,包括如下步骤:步骤1:当GPS信号可用时,将GPS与SINS输出信息进行数据融合,得到姿态、速度、位置信息;步骤2:当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置信息;步骤3:利用双目摄像机拍摄得到环境图片,对环境图片进行特征提取和特征匹配;步骤4:利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地图构建。

【技术特征摘要】
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:选择导航坐标系n为地理坐标系,其xyz分被指向“东北天”方向;选择载体坐标系b的原点在载体质心,其xyz分被指向载体的“右前上”;选取系统状态向量为:其中,分别为东向和北向速度误差;φu、φe、φn分别为天向、东向和北向平台误差角;δL、δλ、δh分别为载体纬度、经度和高度误差;分别为b系下xyz三轴的陀螺常值误差;分别为b系下xy方向上的加速度计常值误差;SINS/GPS系统的状态方程表示为:其中,φ=[φuφeφn]T,RM、RN分被为卯酉圈和子午圈半径,为计算出的速度,为计算出的速度误差,为计算得到的数学平台旋转角速度,为地球旋转角速度,为计算得到的数学平台相对地球的旋转角速度,为对应的计算误差;分别为b系下陀螺仪和加速度计的随机误差;Cω、分别为:步骤1.2:选择载体的速度作为观测量:SINS/GPS系统的观测方程表示为:其中,为载体SINS解算出的速度;为GPS输出的速度;ν为零均值高斯白噪声过程;Hk为观测矩阵,且:Hk=[I2×202×10];步骤1.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤1.1和步骤1.2分别建立的SINS/GPS系统的状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记为:利用容积卡尔曼滤波方法,对SINS/GPS系统进行数据融合,其过程如下:(1)选定滤波初值:(2)计算2n个sigma点与对应权值:(3)对k-1时刻状态估计误差方差阵Pk-1进行Cholesky分解:(4)求取2n个容积点并通过非线性状态方程进行传播:(5)求取状态一步预测值与误差方阵一步预测值:(6)计算量测预测值:(7)计算量测预测误差方差阵:(8)计算状态和量测间互协方差阵为(9)计算增益矩:(10)计算状态值估计值与状态估计误差方差阵:估计出状态向量后,通过闭环反馈,得到SINS/GPS系统输出的位置、速度、姿态信息;当GPS无信号或者系统停止运行时数据融合结束,否则,(2)~(10)循环执行。3.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法,其特征在于,所述步骤2中当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置信息,具体为:步骤2.1:选取SINS/OD系统状态向量为:其中,δαψ为里程计航向安装误差角;δαθ为里程计俯仰安装误差角;δKD为里程计的标度误差;分别为东向和北向速度误差;φu、φe、φn分别为天向、东向和北向平台误差角;δL、δλ、δh分别为载体纬度、经度和高度误差;分别为b系下xyz三轴的陀螺常值误差;分别为b系下xy方向上的加速度计常值误差;SINS/OD系统的状态方程表示为:步骤2.2:选择载体的速度作为观测量:SINS/OD系统的观测方程表示为:其中,为载体SINS解算出的速度;为里程计输出的速度;为b系下的真实速度;为里程计在b系下输出误差向量;ν为零均值高斯白噪声过程;δαψ和δαθ一般较小,近似表示为:其中,KD为里程计的标度系数;PD为里程计的输出脉冲;步骤2.3:使用四阶龙格-库塔积分方法,以采样周期T为步长将步骤2.1和步骤2.2分别建立的SINS/OD状态方程和观测方程离散化,得到离散形式的状态方程和观测方程,记...

【专利技术属性】
技术研发人员:徐晓苏杨博代维
申请(专利权)人:东南大学
类型:发明
国别省市:江苏,32

网友询问留言 已有0条评论
  • 还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。

1