CN109376785A - 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 - Google Patents

Abstract

本发明公开了一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法具体如下：在载体上安装单目相机与惯性测量单元，运用ROS中消息过滤器实现单目相机和惯性测量单元的时间戳同步，计算前后两帧图像之间的位姿变化，并计算其相应时间内的惯性测量单元解算得到的位置，速度，旋转等变化信息，将惯性测量单元得到的位置、速度与旋转等作为系统的状态变量，视觉传感器得到的位姿变化信息作为观测量建立系统方程。并通过一次迭代扩展卡尔曼滤波的方法对两种传感器获得的信息进行融合，实现载体的实时状态估计与导航。本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点。

Description

基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

技术领域

本发明涉及导航技术领域，特别是涉及基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法。

背景技术

近年来，导航相关的仪器设备有着较为突破的发展，设备的性能和精度有着大幅度的提升，但单一传感器实现的的导航方法仍具有某些性能上的局限性。为了满足高性能的导航需求，组合导航方法近些年来得到了广泛的重视和发展。组合导航方法将多种导航传感器组合在一起，利用多种传感器测得信息对其各自局限性进行相互补偿以实现高精度导航并且增强系统的鲁棒性。

惯性导航是一门综合类技术，是现代科学发展到一定阶段的产物。惯性导航中主要利用了IMU作为传感器进行数据采集，一般IMU中会包含一个三轴的陀螺仪和加速度计，陀螺仪用于测量角速率，加速度计用于测量三轴方向上的加速度。在已知IMU初始位置、速度和姿态的前提下，利用航位推算的方法可以实现实时估算IMU位置、速度和姿态。纯惯性的导航方法只在初始时刻附近有较好的精度，这是因为IMU采集到的数据包含陀螺和加速度计的漂移使得纯惯性导航的精度会随时间发散。

单目相机以其结构简单、标定简单与价格低廉的优点而得到广泛的应用，但是单目相机由于只能在同一时刻获得一张图片而无法从图像中直接获得像素的深度信息。除此之外，单目相机还有着尺度不确定性的特点，该特点也是导致单目测量产生误差的主要原因。通过加入IMU数据可以解决单目相机初始化尺度的问题。

基于滤波的视觉和惯性的组合导航方法按照是否把图像状态信息加入到状态向量可以分为紧耦合和松耦合两种方式。紧耦合方式由于将图像特征信息加入到状态向量中使得计算量大大增加，其计算复杂度与图像特征数量程线性关系，且可扩展性差。松耦合虽然精度上略差于紧耦合，但其计算复杂度却远小于紧耦合。其次，松耦合具有较好的可扩展性和帧间计算复杂度不变的优点。本文采用了一次迭代扩展卡尔曼滤波的方法实现单目相机与惯性传感器的松耦合，该方法相较于扩展卡尔曼滤波有着更高的精度，且在计算复杂度上比无极卡尔曼滤波方法更为简单。

发明内容

为了解决以上问题，本发明提供一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点，为达此目的，本发明提供基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法包括如下步骤：

步骤1：对IMU以及单目相机采集到的信息进行时间戳同步；

步骤2：计算单目相机连续两帧图像间的位姿变化；

步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化；

步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合。

作为本发明进一步改进，所述步骤1中对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：

在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。

作为本发明进一步改进，所述步骤2中计算单目相机连续两帧图像间的位姿变化，具体方法为：

1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；

2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；

3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿。

作为本发明进一步改进，所述步骤3中解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：

1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；

2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化。

作为本发明进一步改进，所述步骤4中建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：

1)建立惯性传感器模型：

假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：

ω＝ω_m-b_ω-n_ω

a＝a_m-b_a-n_a

其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：

2)选择状态变量

系统的状态变量表示如下，其中分别表示IMU得到的从世界坐标系，选取东-北-天坐标系为世界坐标系，到IMU坐标系之间的位置、速度和旋转四元数变化，b_ω和b_a为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，和分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量；

由此状态变量可以得到以下微分方程：

其中g表示世界坐标系下的重力矢量，ω＝[ω_x ω_y ω_z]^T表示角速度变化

Ω(ω)为ω四元数微分方程的乘法阵，表示向量ω的反对称矩阵，为IMU坐标系到世界坐标系的方向余弦矩阵，假设尺度漂移非常缓慢，所以可以得到由以上求导结果和滤波器的状态估计噪声模型可以得到：

3)建立状态偏差方程

通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：

所以利用可以得到22维的系统状态偏差变量：

根据连续时间系统状态偏差变量建立其微分方程如下：

其中使用以上方程可以得到如下线性化连续时间状态偏差方程：

其中n为系统噪声，其表示为对上述线性化连续时间误差状态方程进行离散化：

取其前两项截断后可以得到：

其中：

F₄＝-F₁

通过连续系统噪声协方差矩阵可以得到该离散系统的噪声协方差矩阵：

可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：

4)建立系统观测模型

上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵，利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量；

假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用n_p表示位置观测噪声，使用n_q表示旋转观测噪声，可以得到系统的观测噪声为n_m＝[n_p n_q]^T，并以此得到系统的观测噪声协方差矩阵为R；

通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：

位置观测量偏差如下：

根据误差四元数的定义：

通过单目相机测得的旋转观测量可表示为：

利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：

系统的观测量模型为：

Z＝[Z_p Z_q]^T＝h(X_k,k)

将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：

5)迭代扩展卡尔曼滤波更新状态

由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得H_k+1|k，所以可以得到：

有了上述推导，一次迭代扩展卡尔曼滤波描述如下：

计算滤波增益，下标0表示未进行迭代之前的值：

计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：

可以得到系统在k+1时刻状态变量的次优解为：

状态方差协方差矩阵更新如下：

利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解X_k+1|k+1,0处进行泰勒展开并取一阶近似得：

一次迭代后的滤波增益为：

一次迭代后得到的k+1时刻状态偏差向量的最优解为：

一次迭代后的状态方差协方差矩阵更新如下：

所以k+1时刻状态向量的最优解为：

本发明使用一次迭代扩展卡尔曼滤波对单目相机与惯性传感器进行数据融合，该方案相较于标准扩展卡尔曼滤波在精度上有了提升，虽然在计算复杂度上有所增加，但仍比无迹卡尔曼滤波计算复杂度简单。在系统状态方程的建立中未加入图像特征点信息大大减少了计算，并且实现了图像帧间计算复杂度不变的功能。

附图说明

图1是本发明一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法流程图；

图2是本发明单目相机与IMU采集数据时间戳同步示意图；

图3是本发明单目相机位姿估计的示意图；

图4是本发明惯性导航IMU解算位置、速度和姿态变化流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，本发明可以在长时间实时定位与导航过程中保持较高的精度，且具有帧间计算复杂度不变的优点。

如图1-4所示，一种基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，该方法包含如下步骤：

步骤1：对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：

普通单目相机的采样频率为30HZ左右，而IMU采样频率较高可以达到几百甚至上千赫兹。在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配。如图2所示，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。

步骤2：计算单目相机连续两帧图像间的位姿变化，具体方法为：

1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；

2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；

3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿。

步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：

1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；

2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化。

步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：

1)建立惯性传感器模型：

假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：

ω＝ω_m-b_ω-n_ω

a＝a_m-b_a-n_a

其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：

2)选择状态变量

系统的状态变量表示如下，其中分别表示IMU得到的从世界坐标系(选取东-北-天坐标系为世界坐标系)到IMU坐标系之间的位置、速度和旋转四元数变化，b_ω和b_a为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，和分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量。

由此状态变量可以得到以下微分方程：

其中g表示世界坐标系下的重力矢量，ω＝[ω_x ω_y ω_z]^T表示角速度变化，

Ω(ω)为ω四元数微分方程的乘法阵，表示向量ω的反对称矩阵，为IMU坐标系到世界坐标系的方向余弦矩阵，我们假设尺度漂移非常缓慢，所以可以得到

由以上求导结果和滤波器的状态估计噪声模型可以得到：

3)建立状态偏差方程

通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：

所以利用可以得到22维的系统状态偏差变量：

根据连续时间系统状态偏差变量建立其微分方程如下：

其中使用以上方程可以得到如下线性化连续时间状态偏差方程：

其中n为系统噪声，其表示为对上述线性化连续时间误差状态方程进行离散化：

取其前两项截断后可以得到：

其中：

F₄＝-F₁

通过连续系统噪声协方差矩阵可以得到该离散系统的噪声协方差矩阵：

可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：

4)建立系统观测模型

上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵。利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量。

假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用n_p表示位置观测噪声，使用n_q表示旋转观测噪声，可以得到系统的观测噪声为n_m＝[n_p n_q]^T，并以此得到系统的观测噪声协方差矩阵为R。

通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：

位置观测量偏差如下：

根据误差四元数的定义：

通过单目相机测得的旋转观测量可表示为：

利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：

系统的观测量模型为：

Z＝[Z_p Z_q]^T＝h(X_k,k)

将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：

5)迭代扩展卡尔曼滤波更新状态

由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得H_k+1|k，所以可以得到：

有了上述推导，一次迭代扩展卡尔曼滤波描述如下：

计算滤波增益，下标0表示未进行迭代之前的值：

计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：

可以得到系统在k+1时刻状态变量的次优解为：

状态方差协方差矩阵更新如下：

利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解X_k+1|k+1,0处进行泰勒展开并取一阶近似得：

一次迭代后的滤波增益为：

一次迭代后得到的k+1时刻状态偏差向量的最优解为：

一次迭代后的状态方差协方差矩阵更新如下：

所以k+1时刻状态向量的最优解为：

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (5)

1.基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：

该方法包括如下步骤：

步骤1：对IMU以及单目相机采集到的信息进行时间戳同步；

步骤2：计算单目相机连续两帧图像间的位姿变化；

步骤3：解算图像间IMU数据得到惯性测量的位置、速度与姿态变化；

步骤4：建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合。

2.根据权利要求1所述的基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：所述步骤1中对IMU以及单目相机采集到的信息进行时间戳同步，具体方法如下：

在机器人操作系统ROS平台上，利用ROS中消息过滤器进行传感器采集信息时间戳的匹配，每两帧图像之间有较多IMU数据，从图像的时间戳上搜索最近的IMU采集信息进行时间戳的同步。

3.根据权利要求1所述的基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：所述步骤2中计算单目相机连续两帧图像间的位姿变化，具体方法为：

1)在已知单目相机内参的前提下，提取初始时刻两帧图像作为初始帧进行初始化，提取图像的ORB特征点，利用对极约束得到对极几何关系，并计算其本质矩阵；

2)根据上述估计的本质矩阵，由奇异值分解恢复出相机的旋转和平移；

3)通过三角化的方法得到特征点的深度信息，并在之后的相机图像中利用PnP的方法解算相机的位姿。

4.根据权利要求1所述的基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：所述步骤3中解算图像间IMU数据得到惯性测量的位置、速度与姿态变化，具体方法为：

1)在已知载体初始位置、速度与姿态的前提下，利用IMU采集到的数据进行航位推算可得到载体实时位置、速度与姿态变化；

2)上述步骤1实现了IMU以及单目相机采集到的信息的时间戳同步，利用航位推算计算对应帧间IMU所测得的相对位置、速度与姿态变化。

5.根据权利要求1所述的基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法，其特征在于：所述步骤4中建立状态方程，利用一次迭代扩展卡尔曼滤波进行传感器信息融合，具体方法为：

1)建立惯性传感器模型：

假设IMU中含有零偏误差b和高斯噪声n，所以在IMU框架下得到的真实的角速度和加速度为：

ω＝ω_m-b_ω-n_ω

a＝a_m-b_a-n_a

其中下标m表示为测量值，将以上零偏建模为动态随机过程可以得到：

2)选择状态变量

系统的状态变量表示如下，其中分别表示IMU得到的从世界坐标系到IMU坐标系之间的位置、速度和旋转四元数变化，选取东-北-天坐标系为世界坐标系，b_ω和b_a为上述IMU中陀螺和加速度计的零偏，L表示为相机的尺度因子，和分别表示从IMU坐标系到相机坐标系的旋转四元数和位置变换，由此可以得到24维的系统状态向量；

由此状态变量可以得到以下微分方程：

其中g表示世界坐标系下的重力矢量，ω＝[ω_x ω_y ω_z]^T表示角速度变化，

Ω(ω)为ω四元数微分方程的乘法阵，表示向量ω的反对称矩阵，为IMU坐标系到世界坐标系的方向余弦矩阵，假设尺度漂移非常缓慢，所以可以得到由以上求导结果和滤波器的状态估计噪声模型可以得到：

3)建立状态偏差方程

通过上述状态向量及其微分方程，可以得到系统误差状态变量，其中四元数的误差不是通过简单的算术计算得到，而是使用四元数误差，如下所示：

所以利用可以得到22维的系统状态偏差变量：

根据连续时间系统状态偏差变量建立其微分方程如下：

其中使用以上方程可以得到如下线性化连续时间状态偏差方程：

其中n为系统噪声，其表示为对上述线性化连续时间误差状态方程进行离散化：

取其前两项截断后可以得到：

其中：

F₄＝-F₁

通过连续系统噪声协方差矩阵可以得到该离散系统的噪声协方差矩阵：

可以得到系统状态偏差的一步预测和一步预测的协方差矩阵：

4)建立系统观测模型

上述根据惯性测量单元以及其相关模型建立了系统状态偏差方程，更新系统状态偏差与其协方差矩阵，利用单目视觉传感器测得的信息建立观测方程，用于矫正上述系统状态偏差方程的状态量；

假设可以得到相机变换中旋转轴和旋转幅值的偏差，使用n_p表示位置观测噪声，使用n_q表示旋转观测噪声，可以得到系统的观测噪声为n_m＝[n_p n_q]^T，并以此得到系统的观测噪声协方差矩阵为R；

通过对单目相机采集到的图片进行处理可以得到视觉解算的系统位置观测量：

位置观测量偏差如下：

根据误差四元数的定义：

通过单目相机测得的旋转观测量可表示为：

利用上述误差四元数的定义可以得到系统旋转观测量偏差如下：

系统的观测量模型为：

Z＝[Z_p Z_q]^T＝h(X_k,k)

将上述位置观测量偏差和旋转观测量偏差结合得到系统观测量偏差模型：

5)迭代扩展卡尔曼滤波更新状态

由上述状态偏差变量的一步预测可以得到该状态变量的一步预测，将系统观测方程在一步预测处进行泰勒展开并取一阶近似得H_k+1|k，所以可以得到：

有了上述推导，一次迭代扩展卡尔曼滤波描述如下：

计算滤波增益，下标0表示未进行迭代之前的值：

计算k+1时刻状态偏差向量的次优解，此处与标准离散扩展卡尔曼滤波是相同的：

可以得到系统在k+1时刻状态变量的次优解为：

状态方差协方差矩阵更新如下：

利用以上得到的次优解进行一次迭代，首先将系统的观测方程在次优解X_k+1|k+1,0处进行泰勒展开并取一阶近似得：

一次迭代后的滤波增益为：

一次迭代后得到的k+1时刻状态偏差向量的最优解为：

一次迭代后的状态方差协方差矩阵更新如下：

所以k+1时刻状态向量的最优解为：