[发明专利]基于单目视觉的IMU漂移补偿方法

权利要求书

1.一种基于单目视觉的IMU漂移补偿方法，其特征在于，包括以下步骤：

步骤1：基于IMU的方向估计；

步骤2：单目摄像头的方向估计；

步骤3：视觉辅助IMU的误差补偿；

所述步骤1包括以下步骤：

步骤1.1：计算陀螺仪的方向估计

其中表示t时刻由IMU陀螺仪估计出的四元数，由(q₀,q₁,q₂,q₃)表示，q₁、q₂、q₃分量分别对应旋转轴在X、Y、Z方向的投影，q₀分量对应IMU围绕旋转轴旋转的角度；表示陀螺仪估计出的四元数的变化率，Δt表示采样间隔；

步骤1.2：以最小化估计值误差为目标，定义目标函数：

从而得到加速计与磁力计的方向估计其中，和分别表示加速计和磁力计估计值的误差；

步骤1.3：利用增益自适应互补滤波算法，估计求解IMU坐标系下的方向信息

其中，k_t表示自适应的增益系数，表示最终的方向估计信息。

2.根据权利要求1所述的基于单目视觉的IMU漂移补偿方法，其特征在于，所述步骤2包括以下步骤：

步骤2.1：求取像素点偏差平方和SSD：针对单目摄像头采集到的相邻两帧图像，求取像素点偏差平方和SSD；

步骤2.2：如果SSD大于某一指定的阈值τ，则判断IMU处于运动状态；

步骤2.2.1：采用特征点检测与描述方法SURF方法生成64维特征点描述子；

步骤2.2.2：利用特征点对的描述子具有欧氏距离最小原则实现相邻帧图像间特征点的匹配，得到匹配点对集合：{{x₁,x′₁},{x₂,x′₂},…{x_n,x′_n}}；

步骤2.2.3：结合重采样一致性分析算法RANSAC移除特征点对中存在的误匹配点；

步骤2.2.4：利用计算机视觉中的外级线约束方程x′Ex＝0，得到本质矩阵E；

步骤2.2.5：利用奇异值分解，求得旋转矩阵R_3×3，此处得到的旋转矩阵R_3×3表示单目摄像头的方向估计值；

步骤2.3：如果SSD不大于某一指定的阈值τ，则判断IMU处于静止状态，判定当前时刻单目摄像头的方向估计值与上一时刻单目摄像头的方向估计一致。

3.根据权利要求2所述的基于单目视觉的IMU漂移补偿方法，其特征在于，所述像素点偏差平方和SSD通过下式得到：

其中，P(i,j,t)和P(i,j,t+1)分别表示像素点P(i,j)在t时刻和t+1时刻的图像像素值，M表示以像素为单位的图片长度，N表示以像素为单位的图片宽度。

4.根据权利要求2所述的基于单目视觉的IMU漂移补偿方法，其特征在于，所述步骤3具体为：

如果像素点偏差平方和SSD大于阈值τ，则判断载体处于运动状态，IMU漂移补偿的具体实现为：

步骤3.1.1：将基于视觉估计出的旋转矩阵R_3×3转化成对应的欧拉角

其中，R_{ij,1≤i,j≤3}表示旋转矩阵R_3×3中的第i行，第j列的元素；

步骤3.1.2：利用欧拉角的线性插值来补偿IMU输出四元数，欧拉角对应的四元数为：

利用欧拉角解算出的四元数来实现IMU的漂移补偿；

如果SSD不大于阈值τ，则判断载体处于静止状态，IMU漂移补偿的具体实现为：

步骤3.2.1：将IMU中陀螺仪输出的t时刻的角速度ω(t)＝(ω_x,t,ω_y,t,ω_z,t)矢量设置为：

ω_x,t＝0,ω_y,t＝0,ω_z,t＝0

步骤3.2.2：设置t时刻加速度矢量α(t)＝(α_x,t,α_y,t,α_z,t)和磁力强度矢量m(t)＝(m_x,t,m_y,t,m_z,t)为：

α_x,t＝α_x,t-1,α_y,t＝α_y,t-1,α_z,t＝α_z,t-1

m_x,t＝m_x,t-1,m_y,t＝m_y,t-1,m_z,t＝m_z,t-1。