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

Abstract

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

Description

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

技术领域

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

背景技术

同时定位与地图构建(Simultaneous localization and mapping，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， R_M、R_N分被为卯酉圈和子午圈半径，为计算出的速度，为计算出的速度误差，为计算得到的数学平台旋转角速度，为地球旋转角速度，为计算得到的数学平台相对地球的旋转角速度，为对应的计算误差；分别为b系下陀螺仪和加速度计的随机误差；C_ω、分别为：

步骤1.2：选择载体的速度作为观测量：SINS/GPS***的观测方程表示为：

其中，为载体SINS解算出的速度；为GPS输出的速度；ν为零均值高斯白噪声过程；H_k为观测矩阵，且：H_k＝[I_2×2 0_2×10]；

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

利用容积卡尔曼滤波方法，对SINS/GPS***进行数据融合，其过程如下：

(1)选定滤波初值：

(2)计算2n个sigma点与对应权值：

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

(4)求取2n个容积点并通过非线性状态方程进行传播：

(5)求取状态一步预测值与误差方阵一步预测值：

(6)计算量测预测值：

(7)计算量测预测误差方差阵：

(8)计算状态和量测间互协方差阵为

(9)计算增益矩：

(10)计算状态值估计值与状态估计误差方差阵：

估计出状态向量后，通过闭环反馈，得到SINS/GPS***输出的位置、速度、姿态信息；当GPS无信号或者***停止运行时数据融合结束，否则，(2)～(10)循环执行。

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

步骤2.1：选取SINS/OD***状态向量为：

其中，δα_ψ为里程计航向安装误差角；δα_θ为里程计俯仰安装误差角；δK_D为里程计的标度误差；分别为东向和北向速度误差；φ_u、φ_e、φ_n分别为天向、东向和北向平台误差角；δL、δλ、δh分别为载体纬度、经度和高度误差；分别为b系下xyz三轴的陀螺常值误差；分别为b系下xy方向上的加速度计常值误差；

SINS/OD***的状态方程表示为：

步骤2.2：选择载体的速度作为观测量：SINS/OD***的观测方程表示为：

其中，为载体SINS解算出的速度；为里程计输出的速度；为b系下的真实速度；为里程计在b系下输出误差向量；ν为零均值高斯白噪声过程；

δα_ψ和δα_θ一般较小，近似表示为：

其中，K_D为里程计的标度系数；P_D为里程计的输出脉冲；

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

利用容积卡尔曼滤波(CKF)方法，对SINS/OD***进行数据融合，其过程如下：

(1)选定滤波初值：

(2)计算2n个sigma点与对应权值：

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

(4)求取2n个容积点并通过非线性状态方程进行传播：

(5)求取状态一步预测值与误差方阵一步预测值：

(6)将一步预测误差方差P_k/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的两个特征值分别为λ₁和λ₂，且：λ₁＝rλ₂；则：

设置r＝10，如果则该关键点位于边缘部分，应当去除，反之，保留关键点；

所保留下的关键点即为提取出的特征点；

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

梯度方向为：

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

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

其中，l_i和r_i分别为左右图像上特征点的描述符；distance越小，说明特征点越相似；

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

步骤3.5：通过三角测距原理求取特征点在载体坐标系内的位置(x^b，y^b，z^b)：

其中，B为左右摄像机投影中心间的距离，x_left和x_right分别为空间点在左右摄像机图像上的投影横坐标，f为摄像机的焦距，视差Disparity＝x_left-x_right。

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

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

其中，代表在世界坐标系w下，载体的位置坐标，初始值均为零；代表在世界坐标系w下，特征点的位置坐标；M为特征点的数量；

组合导航***的状态方程表示为：

其中，V＝[ν_e ν_n ν_u]^T代表由SINS/GPS***或SINS/OD***计算得到的“东北天”方向上的速度；ΔT代表采样时间间隔；

步骤4.2：选择特征点在载体坐标系内的位置：为观测向量；其中，m为同一时间特征点的数量。

***的观测方程可表示为：

令：则：

其中，ψ、γ、θ分别为SINS/GPS***或SINS/OD***计算得到载体航向角、横摇角和纵摇角；

步骤4.3：利用标准卡尔曼滤波方法进行***数据融合，其过程如下：

(2)选定滤波初值：

(3)状态更新：

(4)量测更新

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

P_k＝(I-K_kH_k)P_k/k-1

估计出状态向量后，通过闭环反馈，完成定位以及地图的制作；***停止运行时，数据融合结束。

本发明对于现有技术具有以下优点：

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

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

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

附图说明

图1是本发明的***框图；

图2是本发明SINS/GPS子***的示意图；

图3是本发明SINS/OD子***的示意图；

图4是本发明中特征提取与特征匹配的流程图；

图5是本发明在GPS信号良好时的定位效果图；

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

具体实施方式

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

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

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

SINS/GPS子***进行数据融合过程如下：

选取***状态向量为：

建立SINS/GPS***的状态方程：

择载体的速度作为观测量：建立SINS/GPS***的观测方程：

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

利用容积卡尔曼滤波(CKF)方法，对上述SINS/GPS***进行数据融合：

1)选定滤波初值：

(2)计算2n个sigma点与对应权值：

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

(4)求取2n个容积点并通过非线性状态方程进行传播：

(5)求取状态一步预测值与误差方阵一步预测值：

(6)计算量测预测值：

(7)计算量测预测误差方差阵：

(8)计算状态和量测间互协方差阵为

(9)计算增益矩：

(10)计算状态值估计值与状态估计误差方差阵：

SINS/OD子***进行数据融合过程如下：

选取SINS/OD***状态向量为：

建立SINS/OD***的状态方程：

选择载体的速度作为观测量：建立SINS/OD***的观测方程：

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

利用容积卡尔曼滤波(CKF))方法，对上述SINS/OD***进行数据融合：

(1)选定滤波初值：

(2)计算2n个sigma点与对应权值：

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

(4)求取2n个容积点并通过非线性状态方程进行传播：

(5)求取状态一步预测值与误差方阵一步预测值：

(6)将一步预测误差方差P_k/k-1阵进行Cholesky分解：

(7)求取2n个容积点并通过非线性量测方程进行传播：

(8)计算量测预测值：

(9)计算量测预测误差方差阵：

(10)计算状态和量测间互协方差阵为

(11)计算增益矩：

(12)计算状态值估计值与状态估计误差方差阵：

如图4所示是本发明特征提取与特征匹配的流程图，其过程如下：

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

其中，I(x,y)为二维图像，G(x,y,σ)为尺度可变的高斯函数，具体表达式如下：

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

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

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

对D(x,y,σ)进行泰勒展开：

其中：

去除对比度低的关键点，即：的点；

计算Hessian矩阵H：

设矩阵H的两个特征值分别为λ₁和λ₂，且：λ₁＝rλ₂；则：

设置r＝10，如果则该关键点位于边缘部分，应当去除，反之，保留关键点；

所保留下的关键点就是提取出的特征点；

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

梯度方向为：

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

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

其中，l_i和r_i分别为左右图像上特征点的描述符。distance越小，说明特征点越相似；

5：通过三角测距原理求取特征点在载体坐标系内的位置(x^b，y^b，z^b)：

其中，B为左右摄像机投影中心间的距离，x_left和x_right分别为空间点在左右摄像机图像上的投影横坐标，f为摄像机的焦距，视差Disparity＝x_left-x_right。

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

选取***的状态向量为：

建立***的状态方程：

选择特征点在载体坐标系内的位置：为观测向量，建立***的观测方程：

利用标准卡尔曼滤波方法进行***数据融合：

(2)选定滤波初值：

(1)状态更新：

(2)量测更新

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

P_k＝(I-K_kH_k)P_k/k-1

使用如下的实例说明本发明的有益效果：

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

以上结果表明，GPS信号可用和GPS信号不可用在两种情况下，本发明提供的方法都可以以较高精度完成定位与制图。

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

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

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：选择导航坐标系n为地理坐标系，其xyz分被指向“东北天”方向；选择载体坐标系b的原点在载体质心，其xyz分被指向载体的“右前上”；选取***状态向量为：

$x=\left[\begin{array}{ccccccccccccc}{\mathrm{\δv}}_e^n& {\mathrm{\δv}}_n^n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\▿}_x^b& {\▿}_y^b& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$

其中，分别为东向和北向速度误差；φ_u、φ_e、φ_n分别为天向、东向和北向平台误差角；δL、δλ、δh分别为载体纬度、经度和高度误差；分别为b系下xyz三轴的陀螺常值误差；分别为b系下xy方向上的加速度计常值误差；

SINS/GPS***的状态方程表示为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=C_{\mathrm{\ω}}^{-1}\[\left(I-C_n^{n^{\′}}\right){\widetilde{\mathrm{\ω}}}_{in}^n+{\mathrm{\δ\ω}}_{in}^n-C_b^{n^{\′}}{\mathrm{\ϵ}}^b\]+w_g^n\\ \mathrm{\δ}{\stackrel{\·}{v}}^n=\left(I-C_{n^{\′}}^n\right)f^{n^{\′}}-\left(2{\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n\right)\×{\tilde{v}}^n-\left(2{\widetilde{\mathrm{\ω}}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n\right)\×\mathrm{\δ}{\tilde{v}}^n+C_{n^{\′}}^n{C}_b^{n^{\′}}{\▿}^b+w_a^n\\ \stackrel{\·}{\mathrm{\δ}}L=\frac{{\mathrm{\δv}}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_E\sec L}{R_N+h}+\frac{v_E\sec L\tan L}{R_N+h}\mathrm{\δ}L\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\\ {\stackrel{\·}{\▿}}^b=0\end{array}\right.$

其中，φ＝[φ_u φ_e φ_n]^T， R_M、R_N分被为卯酉圈和子午圈半径，为计算出的速度，为计算出的速度误差，为计算得到的数学平台旋转角速度，为地球旋转角速度，为计算得到的数学平台相对地球的旋转角速度，为对应的计算误差；分别为b系下陀螺仪和加速度计的随机误差；C_ω、分别为：

$C_n^{n^{\′}}=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_u\_{\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_u+{\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& \_{\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_e\\ \_{\mathrm{cos\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_e{\mathrm{cos\φ}}_u& {\mathrm{sin\φ}}_e\\ {\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_u+{\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{sin\φ}}_n{\mathrm{sin\φ}}_u\_{\mathrm{cos\φ}}_n{\mathrm{sin\φ}}_e{\mathrm{sin\φ}}_u& {\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_e\end{array}\right]$

$C_{\mathrm{\ω}}=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_n& 0& -{\mathrm{sin\φ}}_n{\mathrm{cos\φ}}_e\\ 0& 1& {\mathrm{sin\φ}}_e\\ {\mathrm{sin\φ}}_n& 0& {\mathrm{cos\φ}}_n{\mathrm{cos\φ}}_e\end{array}\right]$

步骤1.2：选择载体的速度作为观测量：SINS/GPS***的观测方程表示为：

$z={\tilde{v}}_s^n-{\tilde{v}}_G^n=H_{k}x+v$

其中，为载体SINS解算出的速度；为GPS输出的速度；ν为零均值高斯白噪声过程；H_k为观测矩阵，且：H_k＝[I_2×2 0_2×10]；

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

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\ω}}_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

利用容积卡尔曼滤波方法，对SINS/GPS***进行数据融合，其过程如下：

(1)选定滤波初值：

$\begin{array}{cc}{\hat{x}}_0=E\left(x_0\right)& P_0=E(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\end{array}$

(2)计算2n个sigma点与对应权值：

$\begin{array}{ccc}{\mathrm{\ξ}}_i=\sqrt{n}{\[1\]}_i,& w_i=1/2n& i=1,2,\mathrm{...},2n\end{array}$

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

$P_{k-1}=S_{k-1}{S}_{k-1}^T$

(4)求取2n个容积点并通过非线性状态方程进行传播：

$\begin{array}{cc}{\mathrm{\χ}}_{k-1}^i=S_{k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1}& i=1,2,\mathrm{...},2n\end{array}$

$\begin{array}{cc}{\mathrm{\χ}}_{k/k-1}^{*i}=f\left({\mathrm{\χ}}_{k/k-1}^i\right)& i=1,2,\mathrm{...},2n\end{array}$

(5)求取状态一步预测值与误差方阵一步预测值：

${\hat{x}}_{k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\χ}}_{k/k-1}^{*i}$

$P_{k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\χ}}_{k/k-1}^{*i}{\mathrm{\χ}}_{k/k-1}^{*iT}-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+Q_{k-1}$

(6)计算量测预测值：

${\hat{z}}_{k|k-1}=H_k{\hat{x}}_{k|k-1}$

(7)计算量测预测误差方差阵：

$P_{zz,k|k-1}=H_k{P}_{k|k-1}{H}_k^T$

(8)计算状态和量测间互协方差阵为

$P_{xz,k|k-1}=P_{k|k-1}{H}_k^T$

(9)计算增益矩：

$K_k=P_{xz,k/k-1}/P_{zz,k/k-1}^{-1}$

(10)计算状态值估计值与状态估计误差方差阵：

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k({\hat{z}}_k-{\hat{z}}_{k/k-1})$

$P_k=P_{k/k-1}-K_k{P}_{zz,k/k-1}{K}_k^T$

估计出状态向量后，通过闭环反馈，得到SINS/GPS***输出的位置、速度、姿态信息；当GPS无信号或者***停止运行时数据融合结束，否则，(2)～(10)循环执行。

3.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法，其特征在于，所述步骤2中当GPS信号不可用时，将里程计与SINS输出信息进行数据融合，得到姿态、速度、位置信息，具体为：

步骤2.1：选取SINS/OD***状态向量为：

$x=\left[\begin{array}{cccccccccccccccc}{\mathrm{\δv}}_e^n& {\mathrm{\δv}}_n^n& {\mathrm{\φ}}_e& {\mathrm{\φ}}_n& {\mathrm{\φ}}_u& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\▿}_x^b& {\▿}_y^b& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b& {\mathrm{\δ\α}}_{\mathrm{\ψ}}& {\mathrm{\δK}}_D& {\mathrm{\δ\α}}_{\mathrm{\θ}}\end{array}\right]$

其中，δα_ψ为里程计航向安装误差角；δα_θ为里程计俯仰安装误差角；δK_D为里程计的标度误差；分别为东向和北向速度误差；φ_u、φ_e、φ_n分别为天向、东向和北向平台误差角；δL、δλ、δh分别为载体纬度、经度和高度误差；分别为b系下xyz三轴的陀螺常值误差；分别为b系下xy方向上的加速度计常值误差；

SINS/OD***的状态方程表示为：

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\φ}}=C_{\mathrm{\ω}}^{-1}\[\left(I-C_n^{n^{\′}}\right){\widetilde{\mathrm{\ω}}}_{in}^n+{\mathrm{\δ\ω}}_{in}^n-C_b^{n^{\′}}{\mathrm{\ϵ}}^b\]+w_g^n\\ \mathrm{\δ}{\stackrel{\·}{v}}^n=\left(I-C_{n^{\′}}^n\right)f^{n^{\′}}-\left(2{\mathrm{\δ\ω}}_{ie}^n+{\mathrm{\δ\ω}}_{en}^n\right)\×{\tilde{v}}^n-\left(2{\widetilde{\mathrm{\ω}}}_{ie}^n+{\widetilde{\mathrm{\ω}}}_{en}^n\right)\×\mathrm{\δ}{\tilde{v}}^n+C_{n^{\′}}^n{C}_b^{n^{\′}}{\▿}^b+w_a^n\\ \mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_E\sec L}{R_N+h}+\frac{v_E\sec L\tan L}{R_N+h}\mathrm{\δ}L\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\\ {\stackrel{\·}{\▿}}^b=0\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\α}}}_{\mathrm{\ψ}}=0\\ \mathrm{\δ}{\stackrel{\·}{K}}_D=0\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\α}}}_{\mathrm{\θ}}=0\end{array}\right.$

步骤2.2：选择载体的速度作为观测量：SINS/OD***的观测方程表示为：

$z={\tilde{v}}_s^n-C_b^n{\tilde{v}}_D^b={\mathrm{\δv}}^n-\[I-{\left(C_n^{n^{\′}}\right)}^T\]C_b^{n^{\′}}(v_D^b+{\mathrm{\δv}}_D^b)+v$

其中，为载体SINS解算出的速度；为里程计输出的速度；为b系下的真实速度；为里程计在b系下输出误差向量；ν为零均值高斯白噪声过程；

δα_ψ和δα_θ一般较小，近似表示为：

${\mathrm{\δ\ν}}_D^b=\left[\begin{array}{c}\mathrm{\δ}{\mathrm{\α}}_{\mathrm{\ψ}}\\ {\mathrm{\δK}}_D\\ {\mathrm{\δ\α}}_{\mathrm{\θ}}\end{array}\right]K_D{P}_D$

其中，K_D为里程计的标度系数；P_D为里程计的输出脉冲；

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

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+{\mathrm{\ω}}_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

利用容积卡尔曼滤波(CKF)方法，对SINS/OD***进行数据融合，其过程如下：

(1)选定滤波初值：

$\begin{array}{cc}{\hat{x}}_0=E\left(x_0\right)& P_0=E(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\end{array}$

(2)计算2n个sigma点与对应权值：

$\begin{array}{ccc}{\mathrm{\ξ}}_i=\sqrt{n}{\[1\]}_i,& w_i=1/2n& i=1,2,\mathrm{...},2n\end{array}$

(3)对k-1时刻状态估计误差方差阵P_k-1进行Cholesky分解：

$P_{k-1}=S_{k-1}{S}_{k-1}^T$

(4)求取2n个容积点并通过非线性状态方程进行传播：

$\begin{array}{cc}{\mathrm{\χ}}_{k-1}^i=S_{k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1}& i=1,2,\mathrm{...},2n\end{array}$

$\begin{array}{cc}{\mathrm{\χ}}_{k/k-1}^{*i}=f\left({\mathrm{\χ}}_{k/k-1}^i\right)& i=1,2,\mathrm{...},2n\end{array}$

(5)求取状态一步预测值与误差方阵一步预测值：

${\hat{x}}_{k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\χ}}_{k/k-1}^{*i}$

$P_{k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\χ}}_{k/k-1}^{*i}{\mathrm{\χ}}_{k/k-1}^{*iT}-{\hat{x}}_{k/k-1}{\hat{x}}_{k/k-1}^T+Q_{k-1}$

(6)将一步预测误差方差P_k/k-1阵进行Cholesky分解：

$P_{k/k-1}=S_{k/k-1}{S}_{k/k-1}^T,$

(7)求取2n个容积点并通过非线性量测方程进行传播：

$\begin{array}{cc}{\widetilde{\mathrm{\χ}}}_{k-1}^i=S_{k/k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k/k-1}& i=1,2,\mathrm{...},2n\end{array}$

$\begin{array}{cc}{Z}_{k/k-1}^i=h\left({\widetilde{\mathrm{\χ}}}_{k-1}^i\right)& i=1,2,\mathrm{...},2n\end{array}$

(8)计算量测预测值：

${\hat{z}}_{k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{k/k-1}^i$

(9)计算量测预测误差方差阵：

$P_{zz,k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{Z}_{k/k-1}^i{Z}_{k/k-1}^{iT}-{\hat{z}}_{k/k-1}{\hat{z}}_{k/k-1}^T+R_k$

(10)计算状态和量测间互协方差阵为

$P_{xz,k/k-1}=\frac{1}{2n}\underset{i=1}{\overset{2n}{\Σ}}{\mathrm{\χ}}_{k/k-1}^{i*}{Z}_{k/k-1}^{iT}-{\hat{x}}_{k/k-1}{\hat{z}}_{k/k-1}^T$

(11)计算增益矩：

$K_k=P_{xz,k/k-1}/P_{zz,k/k-1}^{-1}$

(12)计算状态值估计值与状态估计误差方差阵：

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k({\hat{z}}_k-{\hat{z}}_{k/k-1})$

$P_k=P_{k/k-1}-K_k{P}_{zz,k/k-1}{K}_k^T$

估计出状态向量后，通过闭环反馈，得到SINS/OD***输出的位置、速度、姿态信息；当GPS有信号或者***停止运行时数据融合结束，否则，(2)～(11)循环执行。

4.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法，其特征在于，所述步骤3中利用摄像机拍摄得到环境图片，对环境图片进行特征提取和特征匹配，具体为：

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

$L(x,y,\mathrm{\σ})=G(x,y,\mathrm{\σ})\⊗I(x,y)$

其中，I(x,y)为二维图像，G(x,y,σ)为尺度可变的高斯函数，具体表达式如下：

$G(x,y,\mathrm{\σ})=\frac{1}{2{\mathrm{\π\σ}}^2}{e}^{\frac{-(x^2+y^2)}{2{\mathrm{\σ}}^2}}$

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

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

$D(x,y,\mathrm{\σ})=(G(x,y,k\mathrm{\σ})-G(x,y,\mathrm{\σ}))\⊗I(x,y)=L(x,y,k\mathrm{\σ})-L(x,y,\mathrm{\σ})$

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

对D(x,y,σ)进行泰勒展开：

$D\left(\hat{x}\right)=D+\frac{1}{2}\frac{\∂D^T}{\∂x}\hat{x}$

其中：

去除对比度低的关键点，即：的点；

计算Hessian矩阵H：

$H=\left[\begin{array}{cc}{D}_{xx}& D_{xy}\\ D_{xy}& D_{yy}\end{array}\right]$

设矩阵H的两个特征值分别为λ₁和λ₂，且：λ₁＝rλ₂；则：

$\frac{{\mathrm{Tr}}^2\left(H\right)}{Det\left(H\right)}=\frac{{({\mathrm{\λ}}_1+{\mathrm{\λ}}_2)}^2}{{\mathrm{\λ}}_1{\mathrm{\λ}}_2}=\frac{{(r+1)}^2}{r}$

设置r＝10，如果则该关键点位于边缘部分，应当去除，反之，保留关键点；

所保留下的关键点即为提取出的特征点；

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

$m\left(x,y\right)=\sqrt{{\left(L\left(x+1,y\right)-L\left(x-1,y\right)\right)}^2+{\left(L\left(x,y+1\right)-L\left(x,y-1\right)\right)}^2}$

梯度方向为：

$\mathrm{\θ}\left(x,y\right)={\tan }^{-1}\left[\begin{array}{c}L\left(x,y+1\right)-L\left(x,y-1\right)\\ L\left(x+1,y\right)-L\left(x-1,y\right)\end{array}\right]$

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

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

$dis\tan ce=\underset{i=1}{\overset{n}{\Σ}}{(l_i-r_i)}^2$

其中，l_i和r_i分别为左右图像上特征点的描述符；distance越小，说明特征点越相似；

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

步骤3.5：通过三角测距原理求取特征点在载体坐标系内的位置(x^b，y^b，z^b)：

$\left\{\begin{array}{c}{x}^b=f\frac{B\·x_{left}}{Disparity}\\ y^b=f\frac{B\·x_{right}}{Disparity}\\ z^b=\frac{B\·f}{Disparity}\end{array}\right.$

其中，B为左右摄像机投影中心间的距离，x_left和x_right分别为空间点在左右摄像机图像上的投影横坐标，f为摄像机的焦距，视差Disparity＝x_left-x_right。

5.如权利要求1所述的基于SINS/GPS和里程计辅助的视觉SLAM方法，其特征在于，所述步骤4中利用步骤1和步骤2得到的姿态、速度、位置信息和环境特征实现定位与地图构建，具体为：

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

$x=\left[\begin{array}{cccccccccc}{x}_0^w& y_0^w& z_0^w& x_1^w& y_1^w& z_1^w& \mathrm{...}& x_M^w& y_M^w& z_M^w\end{array}\right]$

其中，代表在世界坐标系w下，载体的位置坐标，初始值均为零；代表在世界坐标系w下，特征点的位置坐标；M为特征点的数量；

组合导航***的状态方程表示为：

$\left\{\begin{array}{c}{X}_{0,k}=X_{0,k-1}+\mathrm{\Δ}T\·V\\ \begin{array}{cc}{X}_{i,k}=X_{i,k-1}& i=\mathrm{1...}M\end{array}\end{array}\right.$

其中，V＝[ν_e ν_n ν_u]^T代表由SINS/GPS***或SINS/OD***计算得到的“东北天”方向上的速度；ΔT代表采样时间间隔；

步骤4.2：选择特征点在载体坐标系内的位置：为观测向量；其中，m为同一时间特征点的数量。

***的观测方程可表示为：

$z=C_w^b{X}_i+v$

令：则：

$H=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}\_\sin \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \_\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \_\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\ψ}\sin \mathrm{\θ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}\_\cos \mathrm{\γ}\cos \mathrm{\ψ}\sin \mathrm{\θ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

其中，ψ、γ、θ分别为SINS/GPS***或SINS/OD***计算得到载体航向角、横摇角和纵摇角；

步骤4.3：利用标准卡尔曼滤波方法进行***数据融合，其过程如下：

(2)选定滤波初值：

$\begin{array}{cc}{\hat{x}}_0=E\left(x_0\right)& P_0=E(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\end{array}$

(1)状态更新：

${\hat{x}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{x}}_{k-1}$

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

(2)量测更新

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(Z_k-H_k{\hat{x}}_{k/k-1})$

P_k＝(I-K_kH_k)P_k/k-1

估计出状态向量后，通过闭环反馈，完成定位以及地图的制作；***停止运行时，数据融合结束。