CN106709222A - 基于单目视觉的imu漂移补偿方法 - Google Patents

Abstract

针对目前惯性测量单元(Inertial Measurement Unit，IMU)方向估计存在误差累积、无法适用于长时间方向估计的问题，本发明提出一种基于单目视觉的IMU漂移补偿方法，包括基于IMU的方向估计、单目摄像头的方向估计以及视觉辅助IMU的误差补偿三个步骤。本发明方法在相邻两帧图像间的特征点对集合中，采用外级线约束方法得到基于视觉的方向估计值，并将其引入到IMU的校准补偿中，解决IMU长时间误差累积的问题。本发明方法适用于静态和动态误差补偿，具有准确、稳定、零漂移的优势。

Description

基于单目视觉的IMU漂移补偿方法

技术领域

本发明涉及计算机视觉技术和图像处理技术，具体地说是一种基于单目视觉的IMU漂移补偿方法。

背景技术

惯性测量单元(Inertial Measurement Unit，IMU)是集成有三轴加速计、三轴陀螺仪、三轴磁力计的MEMS惯性元件，用于测量物体三轴角速度、三轴加速度和三轴地磁分量。IMU具有体积小、重量轻、可随身携带、穿戴方便等特点，广泛用于载体方向估计和人体关节的动作捕捉。

基于IMU的方向估计具有运算量小、动态特性好的优势，适用于短时间内的方向估计。但由于MEMS传感器本身由于漂移而造成误差累积，使得IMU难以实现长期的方向估计。以往基于优化的IMU方向估计方法和基于统计学的IMU方向估计方法虽然能够抑制噪声对方向估计的影响，但存在如下问题：①在基于加速计的方向估计中，两类方法都是基于一种假设：IMU采集到的三轴加速度仅包含有重力加速度分量，默认的载体自身的加速度分量为零，即载体自身只能做平缓、慢速的动作；②在基于室内环境的磁力计的方向估计中，IMU输出的三轴地磁分量容易受到周边铁磁材料的影响而造成误差；③在基于陀螺仪的方向估计中，陀螺仪输出的三轴角速度值存在非定常的随机误差、偏移误差和尺度偏差，即便经过补偿，仍然无法解决MEMS器件自身的长期误差累积问题。上述缺陷是制约IMU用于长时间方向估计的瓶颈，因此亟需一种鲁棒、准确的方向估计方法解决IMU误差累积的问题，实现长时间的方向估计。

发明内容

针对目前IMU方向估计存在误差累积、无法适用于长时间方向估计的问题，本发明提出一种基于单目视觉的IMU漂移补偿方法，在相邻两帧图像间的特征点对集合中，采用外级线约束方法得到基于视觉的方向估计值，并将其引入到IMU的校准补偿中，解决IMU长时间误差累积的问题。

本发明为实现上述目的所采用的技术方案是：一种基于单目视觉的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包括以下步骤：

步骤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处于静止状态，判定当前时刻单目摄像头的方向估计值与上一时刻单目摄像头的方向估计一致。

所述像素点偏差平方和SSD通过下式得到：

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

所述步骤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

本发明具有以下优点及有益效果：采用外级线约束方法得出基于视觉的方向估计值，并将其引入到IMU的校准补偿中，解决了IMU长时间误差累计的问题，具有准确、稳定、零漂移的优点。

附图说明

图1为本发明方法的整体流程图；

图2为IMU方向估计流程图；

图3为单目视觉方向估计流程图；

图4为IMU误差补偿的系统框图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明方法包括三部分(如图1所示)：

(1)基于IMU的方向估计；

(2)单目摄像头的方向估计；

(3)视觉辅助IMU的误差补偿。

具体实现过程如下所述。

步骤1：基于IMU的方向估计(如图2所示)。

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

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

步骤1.2：加速计与磁力计的方向估计

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

其中，和分别表示加速计和磁力计估计值的误差。

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

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

增益自适应互补滤波算法[Tian Y,Wei H,Tan J.An adaptive-gain complementary filter for real-time human motion tracking with marg sensors in free-living environments[J].Neural Systems and Rehabilitation Engineering,IEEE Transactions on,2013,21(2):254-264]。以上介绍的基于IMU增益自适应互补滤波算法的方向估计中，由于存在陀螺仪的偏差和随机误差、地磁分量受到周围铁磁材料的干扰、加速度计采集的加速度信息除了有可能包含有重力以外的载体加速度分量之外还存在偏差和随机误差，以上三个缺陷是制约IMU长时间方向估计的主要因素。为了抑制IMU方向估计的误差，需要引入其他类型传感器辅助实现IMU的漂移补偿。

步骤2：单目摄像头的方向，估计利用单目视觉的方向估计信息实现IMU的漂移补偿(如图3所示)。

步骤2.1：求取像素点偏差平方和(SSD)。

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

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

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

步骤2.2.1：采用鲁棒性好的特征点检测与描述方法SURF方法(Speeded Up Robust Features,SURF)[Bay H,Tuytelaars T,Van Gool L.Surf:Speeded up robust features[M].Computer vision–ECCV 2006.Springer Berlin Heidelberg,2006:404-417]，生成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：利用奇异值分解(Singular Value Decomposition,SVD)分解，求得旋转矩阵R_{3 × 3}，此处得到的旋转矩阵R_{3 × 3}表示单目摄像头的方向估计值。

步骤3：视觉辅助IMU的误差补偿，利用单目视觉的方向估计信息实现IMU的漂移补偿(如图4所示)。

步骤3.1：如果SSD大于阈值τ，则判断载体处于运动状态。

步骤3.1.1：将基于视觉估计出的旋转矩阵R_{3 × 3}转化成对应的欧拉角转化过程如下：

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

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

欧拉角对应的四元数为：

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

步骤3.2：如果SSD小于阈值τ，则判断载体处于静止状态。

步骤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 。

Claims (5)

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

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

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

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

2.根据权利要求1所述的基于单目视觉的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表示自适应的增益系数，表示最终的方向估计信息。

3.根据权利要求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处于静止状态，判定当前时刻单目摄像头的方向估计值与上一时刻单目摄像头的方向估计一致。

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

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

5.根据权利要求1所述的基于单目视觉的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。