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

权利要求书

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

该方法包括如下步骤：

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

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

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

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

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

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

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

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

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

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

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

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

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

所述步骤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时刻状态向量的最优解为：