CN104021554B - 基于部分传感器信息的相机‑惯性传感器标定方法 - Google Patents

Abstract

本发明公开了一种基于部分传感器信息的相机‑惯性传感器标定方法，首先建立基于部分惯性传感器信息的标定方程，然后通过先恢复惯性传感器缺失的信息，再求解相机‑惯性传感器之间的相对坐标变换，对上述基于部分惯性传感器信息的标定方程进行求解，并对标定结果进行了优化。解决了在惯性传感器输出信息不完全条件下相机‑惯性传感器标定问题，并且在实际标定环境中，在有观测误差的情形下，设计的优化算法可以最优化最终标定结果，获得一个很好的标定精度。

Description

基于部分传感器信息的相机-惯性传感器标定方法

技术领域

本发明属于计算机视觉中相机-惯性传感器标定领域，应用于同步定位与地图创建(simultaneous localization and mapping，SLAM)技术中，具体涉及一种在惯性传感器的输出信息不完全情形下的相机和惯性传感器之间坐标系变换的标定方法。

背景技术

同步定位与地图创建指的是移动物体在不确定自身位置的情况下，通过自身的传感器感知环境，在未知环境中创建一个与环境相一致的地图，并同时确定自身在地图中的位置和姿态。近年来随着计算机视觉等技术的进一步发展，基于单目视觉的同步定位与地图创建技术逐渐成为研究热点。考虑到单个感知器的缺点，即没有约束误差的方法，导致累积误差变大，研究者越来越多的关注多传感器融合的同步定位与地图创建方法。其中最常见的就是惯性传感器。为了进行数据融合，需要对相机-惯性传感器之间的相对坐标变换关系进行标定。

传统的相机-惯性传感器标定中，相机和传感器的输出数据都是完整的。目前市场上成本低廉的中低档的惯性传感器可以输出的精度较高的三轴方向角中横滚角(Roll角)和俯仰角(Pitch角)，然而输出的偏航角(Yaw角)的误差相对较大。如果直接应用这种惯性传感器输出的误差较大的Yaw角信息辅助单目视觉SLAM系统，不但无法有效减小系统的累积误差，而且会进一步因引入较大的Yaw角误差而增大系统误差。鉴于此，有研究者提出了一种基于Roll角和Pitch角的实时单目视觉SLAM方法。在这种相机-惯性传感器标定中，惯性传感器的输出信息并不完全，因此，需要基于不完全惯性传感器信息进行相机-惯性传感器标定。

发明内容

本发明的目的是在惯性传感器输出信息不完全条件下进行相机-惯性传感器标定，并提供在实际中有观测误差情形下的标定方法。

本发明采用如下技术方案：

基于部分传感器信息的相机-惯性传感器标定方法，包括以下步骤：

1)将相机-惯性传感器系统在两个不同的位姿进行多次测量，得到至少四组观测值；

2)对于每组观测值，恢复惯性传感器坐标系在两个不同位姿之间的坐标变换R_B；

3)对于每两组不同的观测值，结合每组观测值分别恢复出的R_B，用传统的手眼标定算法进行标定，得到4个所要求解的相机-惯性传感器之间的相对坐标变换R的候选值；

4)重复上述标定过程，得到多组候选结果，比较得到的多组候选结果，通过选出每组候选结果中都一样的值来确定每两组的正确标定结果(正确的解在每组候选结果中都是一致的)；

5)根据正确标定结果，恢复出每两组观测值中相应的α的真实值(至此，惯性传感器缺失的信息已经恢复，而且最终标定结果也得到)，完成基于部分传感器信息的相机-惯性传感器的标定。

具体地，包括以下几个方面：

1、基于部分传感器信息的标定方程

在三维空间中，相机定义了相机坐标系，惯性传感器定义了惯性传感器坐标系。在系统中，相机和惯性传感器是固定在一起的，它们之间的相对坐标变换关系是固定的，为了进行数据融合，这个坐标变换关系需要标定出来。传统的相机-惯性传感器标定问题在理论上化为求解一个矩阵方程：

R_AR＝RR_B

这个方程称为标定方程，在标定时，需要将相机-惯性传感器系统在两个不同位姿进行观测。其中，R_A是相机在两个不同位姿之间的坐标变换，可以通过相机标定方法算出，是已知的三维正交矩阵；R_B是惯性传感器坐标系在两个不同位姿之间的坐标变换，可以从惯性传感器的数据计算出来，也是已知的三维正交矩阵。R则是所要求解的相机-惯性传感器之间的相对坐标变换，是未知的三维正交矩阵。

在基于部分惯性传感器信息进行标定时，R_B就不是已知矩阵。根据Euler角表示法，用惯性传感器输出的Roll角和Pitch角来重构R_B，得到基于部分惯性传感器信息的标定方程：

R_AR＝R·Rot(Z，γ₁)Rot(Y，β₁)Rot(X，α₁)·(Rot(Z，γ₂)Rot(Y，β₂)Rot(X，α₂))^-1

其中β₁，γ₁，β₂，γ₂分别为惯性传感器在两个位姿输出的Roll角和Pitch角，是已知的值，α₁，α₂为惯性传感器在两个位姿的Yaw角，是未知的值，Rot(n，θ)表示绕n轴旋转θ角度的正交变换矩阵。

2、理论标定方法

本发明首先在理论上对上述基于部分惯性传感器信息的标定方程进行求解，得到其解析解。核心思路是先恢复惯性传感器缺失的信息，即先恢复R_B然后再求解R。

直观上，能对惯性传感器缺失的信息进行恢复，是因为惯性传感器和相机的相对位置是固定的，惯性传感器缺失的信息能通过相机的信息来弥补。理论操作上，是利用了矩阵的正交性和相似性的代数约束，恢复出缺失的信息。

约定记号：Y_i＝Rot(X，α_i)，P_i＝Rot(Y，β_i)，O_i＝Rot(Z，γ_i)，则P_i，O_i是已知矩阵，T_i未知。可以将R_B写成：

式中的上标T表示矩阵的转置，其中：

K＝P₂ ^TO₂ ^TO₁P₁

Y＝Y₁Y₂W

K＝[k_ij]是已知三维矩阵，它的元素为k_ij，经过化简，Y可以写成：

其中，α＝a₁-α₂，这样R_B中只有一个未知参数α，恢复α的值即恢复了矩阵R_B。根据矩阵相似理论知：(～表示矩阵相似关系)，则相似矩阵具有相同的迹(矩阵的迹是矩阵对角线上元素之和)：

这样就可以求得α的两个候选值从而通过计算和来恢复标定方程中的R_B，问题化为传统的标定问题，用手眼标定方法即可完成理论上的标定。关于手眼标定方法，相关研究已经成熟，这里不再赘述。

规定每组测量值为系统在两个不同位姿的测量值{R_A，β1_，γ₁，β₂γ₂}，根据手眼标定内容，需要两组测量值才能标定出R。实际标定中，每组观测值会恢复出α的两个候选值其中一个是正确的，这样，两组观测值标定出4个R的候选值，其中一个是正确的。为了得到正确的标定结果，需再进行一次标定，得到另外4个R的候选值，比较这两组的R的候选值，两组候选值中一样的值就是正确的标定结果，再回溯回去，检测是哪个α的候选值得到的正确标定结果，这个α的候选值就是相应的正确的α值。即完成理论上的标定。

3、优化方法

在现实标定过程中，存在着观测误差，若只用理论标定方法来进行标定，所得到的标定结果的误差必然很大。为了在实际中存在观测误差情况下得到比较精确的标定结果，本发明采用了优化方法来最优化标定结果。

考虑m组观测值：{R_A ⁱ，β₁ ⁱ，γ₁ ⁱ，β₂ ⁱγ₂ ⁱ}，i＝1，2，…，m，定义残差矩阵：

Zⁱ(R，αⁱ)＝R_A ⁱR-RR_B ⁱ(αⁱ)

将矩阵Zⁱ(R，αⁱ)按照其列排成一个9维的列向量zⁱ(R，αⁱ)，这样就可以写出优化的代价函数：

用四元数q来表示矩阵R(罗德里格斯公式)，则代价函数中的自变量可写为：

x＝(q，α¹，α²，…，α^m)

则标定问题可以化为最优化问题：

这是一个最小二乘问题，典型方法是Gauss-Newton优化算法，一个改进的方法是Levenberg-Marquardt(LM)算法，比Gauss-Newton算法更加鲁棒。

本发明的技术效果：

本发明解决了在惯性传感器输出信息不完全条件下相机-惯性传感器标定问题，并且在实际标定环境中，在有观测误差的情形下，设计的优化算法可以最优化最终标定结果，获得一个很好的标定精度。

本发明的标定算法不仅仅应用于上述提到的基于惯性传感器的视觉SLAM，其应有前景非常广阔，比如现在流行的智能手机，上面都配有高清摄像头，还有重力传感器，重力传感器并不能输出手机完全的朝向信息，只能输出手机左右倾斜、前后倾斜信息，当手机围绕地轴方向旋转时，这个旋转信息并不能获取。因此，要标定智能手机上相机与重力传感器坐标关系，其本质也是基于部分信息的标定。

附图说明

下面结合附图，对本发明做出详细描述。

图1标定方程推导的坐标变换示意图。

图2基于部分惯性传感器信息的相机-惯性传感器的理论标定算法图。

图3基于优化方法的标定流程图。

具体实施方式：

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，可以理解的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施方案，都属于本发明保护的范围。

1、系统功能：

传统的相机-惯性传感器标定问题在理论上化为求解一个矩阵方程：

R_AR＝RR_B

这个方程称为标定方程，如附图1所示。相机和惯性传感器在两个不同的位姿进行观测，C_C1和C_m1分别是相机和惯性传感器在第一个位姿状态时的坐标系，C_C2和C_m2分别是相机和惯性传感器在第二个位姿状态时的坐标系，C_world是相机所参考的世界坐标系，C_base是惯性传感器所参考的地磁坐标系，图中各个箭头旁的字符表示从一个坐标系到箭头所指向的坐标系之间的坐标变换，通过坐标之间的变换关系可以得到如上的标定方程。

将相机-惯性传感器系统进行多次测量，得到多组观测值，根据这些观测值，计算出相机和惯性传感器之间的坐标变换关系。其中，惯性传感器输出的信息不完全，只包含Roll角和Pitch角。

2、系统输入：

m组观测值：{R_A ⁱ，β₁ ⁱ，γ₁ ⁱ，β₂ ⁱ，γ₂ ⁱ}，i＝1，2，…，m。

3、系统输出：

相机和惯性传感器之间的坐标变换关系R和每组观测值缺失的α值。

4、具体实现：

实际标定中分两步，即理论求解和优化标定结果

1)、理论求解过程如图2所示：

输入：四组观测数据，每组有{R_A，β_1，γ₁，β₂，γ₂}的形式

输出：三维正交矩阵R

开始：

1.把第1和2组观测数据作为一对观测数据组

2.考虑这对观测数据组中第一组观测数据

3.计算O_i＝Rot(Z，γ_i)，P_i＝Rot(Y，β_i)，i＝1，2

4.计算K＝P₂ ^TO₂ ^TO₁P₁

5.将写成关于α的形式：

6.解关于α的方程：得到两个解

7.对于分别计算和来恢复标定方程中的R_B

8.考虑这对观测数据组中每二组观测数据，重复步骤3～7，得到另外两个以及相应的R_B

9.根据这两组观测数据和每组观测数据分别恢复出的R_B，进行标定，得到4个候选标定结果(其中是和的组合得到的标定结果)

10.把第3和4组观测数据作为一对观测数据组，重复步骤2～9，得到另外四个候选标定结果

11.比较假设

则确定标定结果

12.恢复出四组观测数据中相应的α的真实值为

2)、优化标定结果

本发明基于优化方法的标定流程图，如图3所示。首先进行多次观测，获得m组观测值；然后按照之前介绍的理论标定方法，恢复出每组观测数据中的R_B；接下来就可以用传统的手眼标定算法标定出每组观测值的标定结果(由于观测误差的存在，这些标定结果都相对于真实的标定结果有误差)，并把这些标定结果的平均值作为优化算法的初值带入LM优化算法中，经过LM优化算法的迭代运算，得到最终的最优标定结果R。

以理论求解得到的标定结果和各个恢复的α值作为优化方法中的初值，这个初值虽然有误差，但是作为优化方法中的初值，已经非常靠近真实值，可以帮助优化算法快速的向真实值收敛，也会避免优化算法陷入局部最小值。用LM算法进行优化迭代得到最终最优标定结果。LM算法的函数调用如下：

[x，ssq，cnt]＝LMFsolve(Equation，XO，…）

其中：

Equation是代价函数；

XO是迭代的初值；

x是LM算法最后的最小二乘解；

ssq是残差平方和；

cnt是算法迭代的次数；

上述实例只是本发明的举例，尽管为说明目的公开了本发明的最佳实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换、变化和修改都是可能的。因此，本发明不应局限于最佳实施例和附图所公开的内容。

Claims (9)

1.基于部分传感器信息的相机-惯性传感器标定方法，包括以下步骤：

1)将相机-惯性传感器系统在两个不同的位姿进行多次测量，得到至少四组观测值；

2)对于每组观测值，恢复惯性传感器坐标系在两个不同位姿之间的坐标变换R_B，再对基于部分惯性传感器信息的标定方程进行求解，所述基于部分惯性传感器信息的标定方程为：

R_AR＝R·Rot（Z，γ₁)Rot(Y，β₁)Rot(X，α₁)·(Rot(Z，γ₂)Rot(Y，β₂)Rot(X，α₂))^-1

其中，R_A是相机在两个不同位姿之间的坐标变换，β₁，γ₁，β₂，γ₂分别为惯性传感器在两个位姿输出的Roll角和Pitch角，是已知的值，α₁，α₂为惯性传感器在两个位姿的Yaw角，是未知的值，Rot(n，θ)表示绕n轴旋转θ角度的正交变换矩阵；

3)对于每两组不同的观测值，结合每组观测值分别恢复出的R_B，用传统的手眼标定算法进行标定，得到4个所要求解的相机-惯性传感器之间的相对坐标变换R的候选值；

4)重复上述标定过程，得到多组候选结果，比较得到的多组候选结果，通过选出每组候选结果中都一样的值来确定每两组的正确标定结果；

5)根据正确标定结果，恢复出每两组观测值中相应的α的真实值，完成基于部分传感器信息的相机-惯性传感器的标定。

2.如权利要求1所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，每组观测值为系统在两个不同位姿的观测值{R_A，β₁，γ₁，β₂，γ₂}，其中，R_A是相机在两个不同位姿之间的坐标变换，β₁，γ₁，β₂，γ₂分别为惯性传感器在两个位姿输出的Roll角和Pitch角。

3.如权利要求1所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，对于每组观测值，通过计算和来恢复R_B，其中：为求解得到的α的两个候选值，K＝P₂ ^TO₂ ^TO₁P₁，O_i＝Rot(Z，γ_i)，P_i＝Rot(Y，β_i)，i＝1，2，Rot(n，θ)表示绕n轴旋转θ角度的正交变换矩阵。

4.如权利要求1所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，对于每两组不同的观测数据：结合每组观测数据分别恢复出的R_B，进行标定，得到4个候选标定结果其中是和的组合得到的标定结果；重复上述标定过程，得到多组候选结果，比较得到的多组候选结果，确定每两组的正确标定结果根据这个标定结果，恢复出每两组观测数据中相应的α的真实值为：

5.如权利要求1所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，每组观测值会恢复出两个α值，其中一个是正确的；两组观测值标定出4个R的候选值，其中一个是正确的。

6.如权利要求1所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，还包括：利用对标定结果进行优化，其中：i＝1，2，…，m，m表示m组观测数据，x＝(q，α¹，α²，…，α^m)，四元数q表示矩阵R，Zⁱ(R，αⁱ)＝R_A ⁱR-R R_B ⁱ(αⁱ)。

7.如权利要求6所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，将矩阵Zⁱ(R，αⁱ)按照其列排成一个9维的列向量zⁱ(R，αⁱ)，写出优化的代价函数：

$E(R,{\mathrm{\α}}^i)=\underset{i=1}{\overset{m}{\Σ}}{z}^i{(R,{\mathrm{\α}}^i)}^T{z}^i(R,{\mathrm{\α}}^i).$

8.如权利要求6所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，所述优化采用Gauss-Newton优化算法或Levenberg-Marquardt算法。

9.如权利要求8所述的基于部分传感器信息的相机-惯性传感器标定方法，其特征在于，用Levenberg-Marquardt算法进行优化迭代得到最终最优标定结果，Levenberg-Marquardt算法的函数调用如下：

[x，ssq，cnt]＝LMFsolve(Equation，XO，…)

其中：

Equation是代价函数；

X0是迭代的初值；

x是LM算法最后的最小二乘解；

ssq是残差平方和；

cnt是算法迭代的次数。