CN106709223B

CN106709223B - 基于惯性引导采样的视觉imu方向估计方法

Info

Publication number: CN106709223B
Application number: CN201510455818.7A
Authority: CN
Inventors: 梁炜; 张吟龙; 谈金东; 张晓玲; 李杨
Original assignee: Shenyang Institute of Automation of CAS
Current assignee: Shenyang Institute of Automation of CAS
Priority date: 2015-07-29
Filing date: 2015-07-29
Publication date: 2019-01-22
Anticipated expiration: 2035-07-29
Also published as: CN106709223A

Abstract

本发明提出了一种基于惯性引导采样的视觉IMU方向估计方法，即利用IMU的方向估计信息，引导视觉方向估计中匹配点对的采样过程和误匹配点的移除过程。包括基于增益自适应互补滤波器的IMU方向估计、基于尺度旋转不变的特征检测以及基于视觉IMU融合的方向估计三个步骤。本发明方法采用增益自适应互补滤波器，能够在初始迭代过程中剔除明显的误匹配点对，提高了方向估计的准确性；将IMU的姿态估计信息作为初值引入到视觉方向估计中，通过迭代移除误匹配点对，加快了方向估计的进程，有效避免了采用随机初始值带来的运算量大的问题。本发明方法适用性广、鲁棒性好、准确率高，可广泛应用在人体康复训练的动作捕捉过程中。

Description

基于惯性引导采样的视觉IMU方向估计方法

技术领域

本发明涉及计算机视觉技术以及数据处理和信息融合技术，具体是一种基于惯性引导采样的视觉IMU方向估计方法。

背景技术

人体动作捕捉系统获得的高分辨率和大容量的数据可以被用来研究疾病的起因，并有助于疾病的预防和治疗，广泛应用于生物医疗领域，例如，临床的步态分析、门诊病人的身体复原、腿部关节复原、老年人活动监测和评估以及视觉障碍人群的恢复。随着MEMS传感器的成本越来越低、体积越来越小，出现了加速计、陀螺计和磁力计集成的惯性测量单元(Inertial Measurement Unit，IMU)。现有的人体动作捕捉系统就是通过在人体各个部位固定数量不等的IMU来实现的。考虑到传感器模块的配置以及人体的限制，人体的姿势数据可以通过信号融合来获得。IMU优势在于不依赖于某个特定的基础条件，也不受视线的限制；然而，1)方向估测容易受本地磁场的影响，而地球磁场却容易受到周围电子产品或者是铁制材料的影响；2)位置和方向测量是根据集成的双惯性传感器获得的，该方法在长时间的对人体进行跟踪的时候会产生低频漂移以及错误积累的问题，无法对人体动作进行准确的定量评估。上述两个缺点使得IMU不利于其在医疗领域的推广应用。

为了克服IMU的局限性，有学者提出了利用可穿戴的摄像机对IMU的漂移进行补偿的视觉IMU方向估计方法，通过采集摄像机对周围环境的观测图像序列，得到基于视觉的方向估计值，再通过卡尔曼滤波的方法与IMU进行融合，从而实现更为准确的方向估计。然而这一补偿方法在图像处理过程中存在相邻帧特征点匹配耗时和误匹配的问题，大大制约了运算速度和计算精度。

发明内容

针对现有IMU方向估计方法存在精度低和长时间误差累积的问题，以及基于视觉的方向估计方法存在匹配耗时和误匹配的问题，本发明提出了一种基于惯性引导采样的视觉IMU方向估计方法，充分利用IMU的方向估计信息，引导视觉方向估计中匹配点对的采样过程和误匹配点的移除过程，保证了视觉IMU方向估计的精度，提高了视觉IMU方向估计的运算速度。

本发明为实现上述目的所采用的技术方案是：一种基于惯性引导采样的视觉IMU方向估计方法，包括以下步骤：

步骤1：基于增益自适应互补滤波器的IMU方向估计；

步骤2：基于尺度旋转不变的特征检测方法，检测与匹配特征点检测与匹配；

步骤3：基于视觉IMU融合的方向估计。

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

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

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

步骤1.3：基于互补滤波算法的方向估计；

步骤1.4：IMU姿态估计。

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

步骤1.1.1：求解t+Δt时刻描述IMU方向的四元数的变化率

其中，四元数中的q₁、q₂、q₃分量分别对应旋转轴在X、Y、Z方向的投影，q₀分量对应IMU围绕旋转轴旋转的角度；符号表示四元数乘积，表示t时刻IMU陀螺仪的四元数估计值，ω_t+Δt表示t+Δt时刻陀螺仪的输出值。

步骤1.1.2：求解t+Δt时刻四元数的估计值

步骤1.1.3：归一化处理四元数估计值，得出t+Δt时刻由陀螺仪估计出的四元数值

所述步骤1.2通过高斯牛顿的迭代优化算法计算加速计与磁力计的方向估计值首先确定初始时刻的加速度值z_α和磁场强度值z_m，再根据t时刻加速计和磁力计的输出值α(t)和m(t)，以最小化估计值误差为目标，建立目标函数：

其中，和分别表示由加速计和磁力计估计值的误差，计算过程为：

其中，表示的共轭四元数。

所述步骤1.3联合t+Δt时刻陀螺仪的方向估计值和以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法求解IMU坐标系下IMU的方向信息

其中，k_t表示自适应的增益系数，表示t时刻加速计、磁力计、陀螺仪融合后的方向估计信息，表示t时刻加速计与磁力计的方向估计值。

所述步骤1.4采用运动力学中的扭转表示和指数映射算法，初步估计IMU的姿态信息，具体包括以下步骤：

步骤1.4.1：根据IMU估计出的四元数得到与四元数相对应的旋转轴ω＝(cosω_x，cosω_y，cosω_z)和绕轴旋转的角度θ，求解过程是：θ＝2cos^-1q₀

步骤1.4.2：根据扭转与指数映射理论，将(ω，θ)其映射为坐标系间的旋转和平移。已知初始时刻的位置g_global，n(0)与坐标系间的转换关系(R，T)后，计算n时刻IMU的位置g_global，n(θ)，其映射关系如下所示：

其中，表示与ω相对应的扭转参数，v表示绕轴旋转的角度。

其中，q_g表示初始时刻坐标系原点与载体之间的向量。

所述步骤2包含以下步骤：

步骤2.1：基于SURF算法的特征点检测；

步骤2.2：相邻帧间图像特征点的匹配。

所述步骤2.2具体为：

通过计算特征点描述子之间的欧氏距离实现相邻帧间特征点的匹配，表示P 与P₁的特征点描述子之间的欧式距离，表示P与P₂的特征点描述子之间的距离，如果与的比值小于预定义的阈值δ_D，即则接受这一对匹配点；其中，对于图像中的某个特征点P，其特征点描述子记为D_P＝[x₁，x₂，…x₆₄]^T；待匹配图像中与P 点对应的第一近邻点和第二近邻点分别记为P₁和P₂；所述第一近邻点和第二近邻点分别指待匹配图像的特征点中，与P点描述子欧氏距离最近的描述子的特征点和次近的描述子的特征点；P₁和P₂的特征点描述子分别记为P点与P₁和P₂的特征点描述子之间的欧式距离和可表示为分别表示为：

所述步骤3包含以下步骤：

步骤3.1：期望步骤得到本次迭代的姿态信息；

步骤3.2：最大化步骤确定本次迭代中的内点集合；

反复迭代步骤3.1和步骤3.2，得到最终的参数估计集合θ^*＝{R^*，T^*，O_n ^*}，其中，R^*即为可穿戴视觉IMU的方向估计，θ^*表示载体姿态与匹配点对集合，T^*表示最终解算出的载体的平移分量，O_n ^*表示最终做出的匹配点对集合。

所述步骤3.1包含以下步骤：

步骤3.1.1：利用步骤2中得到的内点集合，通过外级线约束方程和最小二乘法估计本质矩阵E；

步骤3.1.2：对本质矩阵E做奇异值分解，估算出载体的姿态信息：旋转分量R和平移分量T；

步骤3.1.3：建立后验贝叶斯估计模型：

其中，(η₁，η₂，…η_n)表示相邻帧图像间的匹配的特征点对，θ表示视觉IMU姿态估计值和匹配点对的集合，定义为：

θ＝{R，T，O_n}

其中，O_n＝(o₁，o₂，…o_n)表示匹配点对的集合，元素o_i服从伯努利分布，用于判断第i个匹配点对是否正确；o_i＝1表示样本点为内点；o_i＝0表示样本点为外点。

所述步骤3.2具体为：判断迭代样本点：

P{o_i|{R，T}}＝max{P(o_i＝1|{R，T})，P(o_i＝0|{R，T})}

其中，P(o_i＝1|{R，T})和P(o_i＝0|{R，T})分别表示为：

其中，表示Sampson误差，S和σ为预先设定的窗口大小和方差，取决于具体应用；y_n表示第n幅图像中的第i个特征点的坐标，f(x_n)表示第n幅图像中的第i个特征点所对应的外级线，o_i取值为1或0由P(o_i＝1|{R，T})和P(o_i＝0|{R，T})的大小决定：如果P(o_i＝1|{R，T})≥P(o_i＝0|{R，T})，则o_i取值为1；反之，o_i取值为0。

本发明具有以下优点：

1.本发明方法采用增益自适应互补滤波器，能够在初始迭代过程中剔除明显的误匹配点对，提高了方向估计的准确性；

2.本发明将IMU的姿态估计信息作为初值引入到视觉方向估计中，通过迭代移除误匹配点对，加快了方向估计的进程，有效避免了采用随机初始值带来的运算量大的问题。

附图说明

图1为发明方法原理图。

具体实施方式

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

本发明主要分为三部分，图1所示为本发明方法原理图，具体实现过程如下所述。

步骤1：基于增益自适应互补滤波器的IMU方向估计。

IMU包含三个主要的传感器：三轴陀螺仪、三轴加速计和三轴磁力计。IMU的姿态估计包括三类传感器的方向估计，并对其估计值进行融合。

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

步骤1.1.1：求解t+Δt时刻描述IMU方向的四元数的变化率(也称导数)公式如下：

步骤1.1.2：求解t+Δt时刻四元数的估计值

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

通过高斯牛顿的迭代优化算法[Boyd S，Vandenberghe L.Convex optimization[M].Cambridge universitypress，2004]计算加速计与磁力计的方向估计值首先确定初始时刻的加速度值z_α和磁场强度值z_m，再根据t时刻加速计和磁力计的输出值α(t)和m(t)，以最小化估计值误差为目标，建立目标函数：

其中，表示的共轭四元数。

步骤1.3：基于互补滤波算法的方向估计。

联合t+Δt时刻陀螺仪的方向估计值和以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法[TianY，Wei H，Tan J.An adaptive-gain complementary filter for real-time human motion tracking with margsensors in free-living environments[J].Neural Systems and RehabilitationEngineering，IEEE Transactions on，2013，21(2)：254-264]求解IMU坐标系下IMU的方向信息

其中k_t表示自适应的增益系数，表示t时刻加速计、磁力计、陀螺仪融合后的方向估计信息，表示t时刻加速计与磁力计的方向估计值。

步骤1.4：IMU姿态估计。

采用运动力学中的扭转表示和指数映射算法，初步估计IMU的姿态信息，具体过程包括：

其中表示与ω相对应的扭转参数，v表示绕轴旋转的角度，q_g表示初始时刻坐标系原点与载体之间的向量。

步骤2：基于尺度旋转不变的特征检测方法，检测与匹配特征点检测与匹配。

步骤2.1：基于SURF(Speeded Up Robust Features，SURF)[Bay H，Tuytelaars T，Van Gool L.Surf：Speeded up robust features[M].Computer vision-ECCV2006.Springer Berlin Heidelberg，2006：404-417]的特征点检测；

步骤2.2：相邻帧间图像特征点的匹配。

通过计算特征点描述子之间的欧氏距离实现相邻帧间特征点的匹配。对于图像中的某个特征点P，其特征点描述子记为D_P＝[x₁，x₂，…x₆₄]^T。待匹配图像中与P点对应的第一近邻点和第二近邻点分别记为P₁和P₂。所述第一近邻点和第二近邻点分别指待匹配图像的特征点中，与P点描述子欧氏距离最近的描述子的特征点和次近的描述子的特征点。P₁和P₂的特征点描述子分别记为 P点与P₁和P₂的特征点描述子之间的欧式距离和可表示为分别表示为：

如果与的比值小于预定义的阈值δ_D，即则接受这一对匹配点。依据这一匹配规则，找到所有相邻帧间的特征匹配点对，作为初始的匹配点对。

由于以上匹配方法仅考虑特征点描述子的相似程度，而图像间存在周期性的纹理和相似的斑块，仍然会造成误匹配，需要进一步把误匹配点对移除掉。

步骤3：基于视觉IMU融合的方向估计。

旋转矩阵R和平移向量T作为初始姿态信息带入到相机本质矩阵E＝T_×·R(T_×是关于平移向量T的负斜对称矩阵)的估计中。

根据外级线约束方程x′Ex＝0，来初步判断内点和外点。所述内点，是指两幅图像间正确的匹配点对；所述外点，是指两幅图像间错误的匹配点对。

视觉IMU融合的方向估计是利用两幅图像间的匹配点对和IMU的姿态估计信息来建立后验贝叶斯估计模型，通过期望最大化算法来迭代估计出方向的。具体步骤如下：

步骤3.1：期望步骤得到本次迭代的姿态信息。

步骤3.1.1：利用步骤2中得到的内点集合，通过外级线约束方程和最小二乘法估计本质矩阵E。

步骤3.1.2：对本质矩阵E做奇异值分解，估算出载体的姿态信息：旋转分量R和平移分量T。

步骤3.1.3：建立后验贝叶斯估计模型：

其中(η₁，η₂，…η_n)表示相邻帧图像间的匹配的特征点对，θ表示视觉IMU姿态估计值和匹配点对的集合，定义为：

θ＝{R，T，O_n}

其中O_n＝(o₁，o₂，…o_n)表示匹配点对的集合，元素o_i服从伯努利分布，用于判断第i个匹配点对是否正确。o_i＝1表示样本点为内点；o_i＝0表示样本点为外点。

步骤3.2：最大化步骤确定本次迭代中的内点集合。

判断迭代样本点：

P{o_i|{R，T}}＝max{P(o_i＝1|{R，T})，P(o_i＝0|{R，T})}

其中P(o_i＝1|{R，T})和P(o_i＝0|{R，T})分别表示为：

其中表示Sampson误差，S和σ为预先设定的窗口大小和方差，取决于具体应用。y_n表示第n幅图像中的第i个特征点的坐标，f(x_n)表示第n幅图像中的第i个特征点所对应的外级线。

o_i取值为1或0由P(o_i＝1|{R，T})和P(o_i＝0|{R，T})的大小决定：如果P(o_i＝1|{R，T})≥P(o_i＝0|{R，T})，则o_i取值为1；反之，o_i取值为0。

反复迭代步骤3.1和步骤3.2，得到最终的参数估计集合θ^*＝{R^*，T^*，O_n ^*}，其中θ^*表示载体姿态与匹配点对集合，T^*表示最终解算出的载体的平移分量，O_n ^*表示最终做出的匹配点对集合，R^*即为可穿戴视觉IMU的方向估计。

Claims

1.一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，包括以下步骤：

步骤1：基于增益自适应互补滤波器的IMU方向估计；

步骤2：基于尺度旋转不变的特征检测方法，来检测与匹配两幅图像间的特征点；

步骤3：基于视觉IMU融合的方向估计；

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

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

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

步骤1.3：基于互补滤波算法的方向估计；

步骤1.4：IMU姿态估计；

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

步骤1.1.1：求解t+Δt时刻描述IMU方向的四元数的变化率

步骤1.1.2：求解t+Δt时刻四元数的估计值

2.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.2通过高斯牛顿的迭代优化算法计算加速计与磁力计的方向估计值首先确定初始时刻的加速度值z_α和磁场强度值z_m，再根据t时刻加速计和磁力计的输出值α(t)和m(t)，以最小化估计值误差为目标，建立目标函数：

其中，表示的共轭四元数。

3.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.3联合t+Δt时刻陀螺仪的方向估计值以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法求解IMU坐标系下IMU的方向信息

4.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤1.4采用运动力学中的扭转表示和指数映射算法，初步估计IMU的姿态信息，具体包括以下步骤：

步骤1.4.1：根据IMU估计出的四元数得到与四元数相对应的旋转轴ω＝(cosω_x,cosω_y,cosω_z)和绕轴旋转的角度θ，求解过程是：θ＝2cos^-1q₀

步骤1.4.2：根据扭转与指数映射理论，将(ω,θ)其映射为坐标系间的旋转和平移；已知初始时刻的位置g_global,n(0)与坐标系间的转换关系(R,T)后，计算n时刻IMU的位置g_global,n(θ)，其映射关系如下所示：

其中，表示与ω相对应的扭转参数，ν表示绕轴旋转的角度；

ν＝-ω×q_g

其中，q_g表示初始时刻坐标系原点与载体之间的向量。

5.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤2包含以下步骤：

步骤2.1：基于SURF算法的特征点检测；

步骤2.2：相邻帧间图像特征点的匹配。

6.根据权利要求5所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤2.2具体为：

通过计算特征点描述子之间的欧氏距离实现相邻帧间特征点的匹配，表示P与P₁的特征点描述子之间的欧式距离，表示P与P₂的特征点描述子之间的距离，如果与的比值小于预定义的阈值δ_D，即则接受这一对匹配点；其中，对于图像中的某个特征点P，其特征点描述子记为D_P＝[x₁,x₂,…x₆₄]^T；待匹配图像中与P点对应的第一近邻点和第二近邻点分别记为P₁和P₂；所述第一近邻点和第二近邻点分别指待匹配图像的特征点中，与P点描述子欧氏距离最近的描述子的特征点和次近的描述子的特征点；P₁和P₂的特征点描述子分别记为P点与P₁和P₂的特征点描述子之间的欧式距离和可表示为分别表示为：

7.根据权利要求1所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤3包含以下步骤：

步骤3.1：期望步骤得到本次迭代的姿态信息；

步骤3.2：最大化步骤确定本次迭代中的内点集合；

反复迭代步骤3.1和步骤3.2，得到最终的参数估计集合θ^*＝{R^*,T^*,O_n ^*}，其中，R^*即为可穿戴视觉IMU的方向估计，θ^*表示载体姿态与匹配点对集合，T^*表示最终解算出的载体的平移分量，O_n ^*表示最终做出的匹配点对集合。

8.根据权利要求7所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤3.1包含以下步骤：

步骤3.1.3：建立后验贝叶斯估计模型：

其中，(η₁,η₂,…η_n)表示相邻帧图像间的匹配的特征点对，θ表示视觉IMU姿态估计值和匹配点对的集合，定义为：

θ＝{R,T,O_n}

其中，O_n＝(o₁,o₂,…o_n)表示匹配点对的集合，元素o_i服从伯努利分布，用于判断第i个匹配点对是否正确；o_i＝1表示样本点为内点；o_i＝0表示样本点为外点，其中1<i<n。

9.根据权利要求7所述的一种基于惯性引导采样的视觉IMU方向估计方法，其特征在于，所述步骤3.2具体为：判断迭代样本点：

P{o_i|{R,T}}＝max{P(o_i＝1|{R,T}),P(o_i＝0|{R,T})}

其中，P(o_i＝1|{R,T})和P(o_i＝0|{R,T})分别表示为：

其中，表示Sampson误差，S和σ为预先设定的窗口大小和方差，取决于具体应用；y_n表示第n幅图像中的第i个特征点的坐标，f(x_n)表示第n幅图像中的第i个特征点所对应的外级线，o_i取值为1或0由P(o_i＝1|{R,T})和P(o_i＝0|{R,T})的大小决定：如果P(o_i＝1|{R,T})≥P(o_i＝0|{R,T})，则o_i取值为1；反之，o_i取值为0。