CN110246194A - 一种像机与惯性测量单元旋转关系快速标定方法 - Google Patents

Abstract

本发明为一种像机与惯性测量单元旋转关系快速标定方法。针对像机旋转和平移运动条件下、像机纯旋转运动条件下、像机纯旋转运动和焦距未知条件下，提出三种求解像机和惯性测量单元旋转关系的最小配置解，提高RANSAC算法剔除图像匹配点对中野值的效率，确定图像匹配内点集。再利用图像匹配内点集对旋转关系进行非线性优化，得到最终的标定结果。本发明不依赖已知结构的标定物和其他特殊的设备，高效率高精度地标定出像机和惯性测量单元之间的旋转关系，适用于手机和无人机等计算能力有限的设备，具有重要的研究意义和广泛的应用前景。

Description

一种像机与惯性测量单元旋转关系快速标定方法

技术领域

本发明涉及一种智能手机、无人机、无人车等设备的像机与惯性测量单元标定方法，具体是一种利用地面单应约束标定像机坐标系与惯性测量单元坐标系之间旋转关系的方法。

背景技术

智能手机、无人机、无人车等设备中通常装备了像机和惯性测量单元(Inertialmeasurement unit,IMU)等传感器，可利用惯性测量单元为像机提供旋转角信息，实现像机和惯性测量单元测量数据的融合。例如利用惯性测量单元提供像机的角度信息，已成功应用于基于运动恢复结构(Structure from Motion,SFM)和同步定位与地图创建(Simultaneous Localization and Mapping,SLAM)等领域。但是融合像机和惯性测量单元数据的基础是标定出像机坐标系与惯性测量单元坐标系之间的旋转关系，而且标定的精度直接决定这两个传感器测量数据融合的精度。因此像机与惯性测量单元旋转关系的标定对两者测量数据的融合有着重要意义。

考虑到IMU可直接输出当前状态相对于IMU参考坐标系的角度信息，可以通过各时刻IMU输出的角度信息和相应的像机位姿来标定像机与惯性测量单元之间的相对位姿关系。通常可利用手眼定标方法来标定像机与惯性测量单元，手眼定标方程可描述为：AX＝XB，其中A表示像机的相对位姿变化，B表示IMU相应的相对位姿变化，X表示像机坐标系和惯性测量单元坐标系之间的相对位姿关系，由旋转关系和平移关系组成。研究表明相对位姿关系X可通过两次以上绕不平行旋转轴的运动确定。手眼定标方法主要分为二类：第一类方法分两步求解旋转和平移关系，或仅求解旋转关系。这类方法首先求解像机坐标系和惯性测量单元坐标系之间的旋转关系，然后将旋转关系的求解结果代入手眼定标方程，求解出像机坐标系和惯性测量单元坐标系之间的平移关系。第二类方法则是同时求解像机坐标系和惯性测量单元坐标系之间的旋转关系和平移关系。这二类方法需要提前根据图像计算出像机的姿态，这些像机姿态可通过标定物或者SFM方法获得。由于未采取剔除像机姿态中野值的相关机制，手眼定标方法求解相对位姿关系的精度必定受到野值的严重影响。

发明内容

本发明要解决的技术问题是：利用图像匹配点对的单应约束高效率高精度地标定像机与惯性测量单元的旋转关系，避免事先计算像机姿态，提高RANSAC算法剔除图像匹配点对中野值的效率。

本发明的技术解决方案是：通过像机对地面拍摄图像，并记录惯性测量单元相应的角度信息，利用单应约束方便地求解出像机坐标系和惯性测量单元坐标系之间的旋转关系。本发明的标定流程如图1所示，首先对像机拍摄的地面图像进行特征点提取与匹配，获取图像匹配点集；然后针对像机旋转和平移运动条件下、像机纯旋转运动条件下、像机纯旋转运动和焦距未知条件下的像机和惯性测量单元标定问题，分别提出了相应的最小配置解，并结合RANSAC(RANdom SAmple Consensus,随机抽样一致性算法)剔除图像匹配点集中的野值，确定图像匹配内点集；最后利用图像匹配内点集对旋转关系进行非线性优化，得到最终的标定结果。本发明通过最小配置解能够明显提高标定效率，标定速度快，并且直接最小化图像对所有图像匹配内点集的单应转换误差，具有明显的几何意义。

对于RANSAC算法，寻找最小配置解显得非常重要，因为在相同的野值比例条件下，RANSAC的随机采样次数随着最小配置解所需图像匹配点对数量的增加而成指数型增长。根据像机的运动情况和像机内参数是否已知，本发明分别提出了三种标定像机和惯性测量单元旋转关系的最小配置解：

(1)像机旋转和平移运动条件下，采用3个图像匹配点对求解像机与惯性测量单元之间的旋转关系，同时恢复出像机的运动；

(2)像机纯旋转运动或者近似纯旋转运动条件下，采用1.5个图像匹配点对求解像机与惯性测量单元之间的旋转关系。由于只使用第二个图像匹配点对引入单应约束方程中的任意一个，故该方法称为1.5个图像匹配点对方法；

(3)像机纯旋转运动和焦距未知条件下，采用2个图像匹配点对同时求解像机的焦距和像机与惯性测量单元之间的旋转关系。

本发明可以达到以下的技术效果：

1)本发明利用地面场景点提供的单应约束标定像机与惯性测量单元的旋转关系，不依赖已知结构的标定物和其他特殊的设备，简单方便；

2)本发明根据像机的运动情况和像机内参数是否已知，提出三种标定像机和惯性测量单元旋转关系的最小配置解，有效提高标定效率，标定速度快；

3)本发明直接最小化图像对中所有图像匹配内点集的单应转换误差，具有明显的几何意义，标定精度高；

4)本发明的方法计算量小，适用于手机和无人机等计算能力有限的设备，具有广泛的应用前景。

附图说明

图1基于单应约束的像机与惯性测量单元旋转关系标定流程图，

图2像机坐标系与惯性测量单元坐标系的几何关系，

图3像机焦距估计示意图。

具体实施措施

本发明提出直接利用图像匹配点对的单应约束标定像机坐标系和惯性测量单元坐标系之间旋转关系，避免事先计算像机姿态，并提出三种求解像机和惯性测量单元旋转关系的最小配置解，提高RANSAC算法剔除图像匹配点对中野值的效率。本发明不依赖已知结构的标定物和其他特殊的设备，高效率高精度地标定出像机和惯性测量单元之间的旋转关系，适用于手机和无人机等计算能力有限的设备，具有重要的研究意义和广泛的应用前景。

本发明标定像机和惯性测量单元旋转关系的过程如下:

1)像机对地面拍摄图像，进行特征点提取与匹配，并记录惯性测量单元相应的角度信息；

2)根据最小配置解求解像机和惯性测量单元之间的旋转关系，并结合RANSAC剔除图像匹配点集中的野值，确定图像匹配内点集；

3)利用图像匹配内点集进一步优化像机和惯性测量单元之间的旋转关系，得到最终的标定结果。

2像机与惯性测量单元旋转关系的标定方法

2.1像机与惯性测量单元旋转关系的基本原理

假定像机对同一平面场景拍摄图像，两个图像之间的单应关系表示为：

其中x_i＝[x_i,y_i,1]^T和x_j＝[x_j,y_j,1]^T是3D平面上点分别在图像i和j中的图像点齐次坐标。α是一个比例因子，H为图像i和j之间的单应矩阵。R和t分别表示图像i坐标系到图像j坐标系的旋转矩阵和平移向量。d是图像i坐标系到3D平面的距离。N＝[n₁,n₂,n₃]^T是3D平面在图像i坐标系的单位法向量。

如图2所示，惯性测量单元与像机固联安装，表示惯性测量单元坐标系，表示惯性测量单元参考坐标系，表示像机坐标系，表示校正后的像机坐标系，其坐标轴方向与惯性测量单元参考坐标系一致。图2左边为原始图像对，右边为校正后的图像对。

R_calib表示像机坐标系与惯性测量单元坐标系之间的旋转关系。R_imu表示从惯性测量单元坐标系转换到惯性测量单元参考坐标系的旋转矩阵，可由惯性测量单元输出的偏航角，俯仰角和滚转角合成，其中和分别表示像机拍摄图像i和j时惯性测量单元输出的角度信息。图像i和j在惯性测量单元参考坐标系中的旋转矩阵可分别表示为和因此可将图像点坐标从像机坐标系转化到校正后的像机坐标系中：

校正后的图像和坐标系之间无旋转变化，仅存在一个平移向量，因此校正后图像对之间的单应关系可表示为：

其中表示3D平面在校正后图像坐标系的单位法向量。标定中像机对地面进行观测，地面在图像坐标系中的单位法向量可取：I表示3×3的单位矩阵，为校正后图像和坐标系之间的平移向量。将公式(2)和(3)代入公式(4)，并在公式两边同时乘以可得到：

图像i和j之间的平移向量t与校正后图像和之间的平移向量存在以下关系：

由于图像i到3D平面的距离d未知，图像i和j之间的平移向量t仅能被恢复到尺度大小，因此设d等于1。将公式(6)代入(5)，图像i和j之间的单应矩阵H可重新表示为：

为消除未知的比例因子α，在公式(1)的两边乘以反对称矩阵[x_j]_×，可得：

[x_j]_×Hx_i＝0 (8)

虽然公式(8)有三个方程，但由于反对称矩阵[x_j]_×的秩是2，公式(8)对单应矩阵H仅存在两个独立的约束。通常情况下，像机与惯性测量单元之间的近似安装关系R_A是已知的，例如对于智能手机而言，R_A可以通过手动测量或者设计图纸获得，其安装角度往往是0°、90°或者180°等。借助这一假设，可将像机与惯性测量单元之间的旋转矩阵进行一阶近似展开，从而达到简化多项式方程组的目的。首先用近似安装关系R_A来旋转图像i和j中的图像点：

旋转后的像机坐标系与惯性测量单元坐标系之间的旋转角均是小角度，因此可将旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵表示为：

其中是由三个小数值旋转角组成的三维向量，I_3×3表示3×3的单位矩阵。类似于公式(7)和(8)，推导出关于旋转后图像点对的单应矩阵和单应约束：

本发明主要利用公式(12)中的单应约束建立方程组求解像机坐标系与惯性测量单元坐标系之间的旋转关系R_calib。

2.2像机与惯性测量单元旋转关系的求解

(1)像机旋转和平移运动条件下的标定方法

公式(12)中仅存在6个未知参数，包括3个从旋转后像机坐标系到惯性测量单元坐标系的旋转变量和3个从旋转后图像i到旋转后图像j的平移向量根据公式(12)可知，3个图像匹配点对共给出包含6个未知参数的6个多项式方程：

通过格罗布纳基方法分析可知该方程组的次数仅为2，且最多有24个解。在RANSAC算法中，通过公式(11)将每个解合成相应的单应矩阵，用于判断旋转后图像匹配点对之间的内点，并选取获得最大内点集合的解作为输出。对于每个图像对，可计算出旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵和旋转后图像i到旋转后图像j的平移向量最后像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib可表示为：

同时，像机的运动也可以被恢复，图像i和图像j之间的旋转矩阵和平移向量表示为：

(2)像机纯旋转运动条件下的标定方法

在像机纯旋转运动条件下，图像匹配点对之间同样满足单应约束。像机纯旋转运动条件下公式(11)中的平移向量为[0,0,0]^T，则单应矩阵可表示为：

该公式包含的未知参数为3个从旋转后像机坐标系到惯性测量单元坐标系的旋转变量根据公式(12)可知，1.5个图像匹配点对共给出包含3个未知参数的3个多项式方程：

通过格罗布纳基方法分析可知该方程组的次数仅为2，且最多有8个解。本方法只需要1.5个图像匹配点对，意味着只用第二个图像匹配点对引入单应约束中的其中一个。虽然在RANSAC过程中，仍需要采样二个图像匹配点对来进行像机与惯性测量单元之间旋转关系的标定，但是仅使用1.5个图像匹配点对求解方法具有明显的优势：即第二个图像匹配点对引入单应约束中的另一个方程，可以在RANSAC每次迭代过程中排除多个解中不合理的解，只有合理的解才能同时满足第二个图像匹配点对引入的两个单应约束方程；

最后选取获得最大内点集合的解作为旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵再根据公式(14)计算出像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib。

(3)像机纯旋转运动和焦距未知条件下的标定方法

针对像机纯旋转运动和焦距未知的情况，提出了同时求解像机焦距和标定像机与惯性测量单元之间旋转关系的方法。实际应用中常常假定像机的焦距固定，像元为正方形，主点设在图像中心，且忽略图像畸变，此时像机内参数中只有焦距是未知的。本标定方法分为两步：先求解像机的焦距，再标定像机坐标系与惯性测量单元坐标系之间的旋转关系。

如图3所示，图像i和j之间为纯旋转运动，假定有两个图像匹配点对和图像i中x_i1和x_i2与光心连线形成的夹角为θ₁，图像j中x_j1和x_j2与光心连线形成的夹角为θ₂，在各自图像坐标系中两个图像点与光心连线形成的夹角是相等的，因此有如下公式：

cos²θ₁＝cos²θ₂ (19)

将上式用图像坐标和像机焦距进行表示，可推导出：

其中

上式中F表示像机的焦距。然后将公式(21)代入到公式(20)中，进一步扩展可得：

公式(22)是关于F²的三次多项式方程，可直接求解出焦距F。然后在已知像机内参数的条件下，直接采用像机纯旋转运动条件下的标定方法求解像机坐标系与惯性测量单元坐标系之间的旋转关系。

2.3非线性优化

根据像机内参数是否已知，非线性优化中的待优化参数将存在不同，提出了两个不同的非线性优化目标函数。针对像机做旋转和平移运动，或者纯旋转运动的情况，假定像机与惯性测量单元旋转关系标定中共有M个图像对，p表示由图像i和j组成的图像对。采用每个图像对p进行标定，均可获得像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib、图像i和j之间的平移向量t_ij和相应的N_p个图像匹配内点集。在像机纯旋转运动条件下，图像i和图像j之间的平移向量t_ij均为0。在非线性优化中，图像对之间的平移向量t_ij均固定不变，利用M个图像对中所有的图像匹配内点集来共同优化像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib。目标函数ε为最小化M个图像对中所有图像匹配内点集的单应转换误差：

式中是待优化的像机坐标系与惯性测量单元坐标系之间的旋转矩阵，可用三个欧拉角表示。其初值可设置为M个标定结果的平均值或中值。k表示每个图像对p中图像匹配内点集的索引。和分别是内点k在图像i和j上的图像坐标。表示图像对p之间的平移向量，表示在拍摄图像i和j时惯性测量单元输出的方向信息。函数将图像i中的图像坐标x_i通过单应矩阵转换到图像j中相应的图像坐标x_j。

针对像机做纯旋转运动和焦距未知条件下的情况，采用每个图像对p进行标定，均可获得像机的焦距F、像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib和相应的N_p个图像匹配内点。在非线性优化中，同样通过M个图像对中所有的图像匹配内点集来共同优化像机的焦距F和像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib。目标函数ε为最小化M个图像对中所有图像匹配内点集的单应转换误差：

式中和是待优化的像机焦距和像机坐标系与惯性测量单元坐标系之间的旋转矩阵，其初值可设置为M个标定结果的平均值或中值。参数和的定义与公式(23)中的定义一样，函数同样是将图像i中的图像坐标x_i转换到图像j中相应的图像坐标x_j。

为减少仍可能存在的野值的影响，在优化过程中采用柯西函数公式对图像匹配内点的单应转换误差进行加权。由于标定时RANSAC中内点的判断阈值是2像素，因此将柯西函数中的参数σ设置为2像素。

Claims (4)

1.一种像机与惯性测量单元旋转关系快速标定方法，利用图像匹配点对的单应约束标定像机与惯性测量单元的旋转关系，避免事先计算像机姿态，提高RANSAC算法剔除图像匹配点对中野值的效率，其特征在于，首先对像机拍摄的地面图像进行特征点提取与匹配，获取图像匹配点集；然后针对像机旋转和平移运动条件下、像机纯旋转运动条件下、像机纯旋转运动和焦距未知条件下的像机和惯性测量单元标定问题，分别提出相应的最小配置解，并结合随机抽样一致性算法剔除图像匹配点集中的野值，确定图像匹配内点集；最后利用图像匹配内点集对旋转关系进行非线性优化，得到最终的标定结果，

根据像机的运动情况和像机内参数是否已知，提出了三种标定像机和惯性测量单元旋转关系的最小配置解：

(1)像机旋转和平移运动条件下，采用3个图像匹配点对求解像机与惯性测量单元之间的旋转关系，同时恢复出像机的运动；

(2)像机纯旋转运动或者近似纯旋转运动条件下，采用1.5个图像匹配点对求解像机与惯性测量单元之间的旋转关系；

(3)像机纯旋转运动和焦距未知条件下，采用2个图像匹配点对同时求解像机的焦距和像机与惯性测量单元之间的旋转关系。

2.根据权利要求1所述的一种像机与惯性测量单元旋转关系快速标定方法，其特征在于，所述像机旋转和平移运动条件下的标定方法，具体为：

公式

中仅存在6个未知参数，包括3个从旋转后像机坐标系到惯性测量单元坐标系的旋转变量和3个从旋转后图像i到旋转后图像j的平移向量

根据公式(12)知，3个图像匹配点对共给出包含6个未知参数的6个多项式方程：

通过格罗布纳基方法分析知该方程组的次数仅为2，且最多有24个解，通过公式

其中x_i＝[x_i,y_i,1]^T和x_j＝[x_j,y_j,1]^T是3D平面上点分别在图像i和j中的图像点齐次坐标，H为图像i和j之间的单应矩阵，R和t分别表示图像i坐标系到图像j坐标系的旋转矩阵和平移向量， 是旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵，表示像机拍摄图像i时惯性测量单元输出的角度信息，表示像机拍摄图像j时惯性测量单元输出的角度信息，

R_A是像机与惯性测量单元之间的近似安装关系，为已知，

将每个解合成相应的单应矩阵，用于判断旋转后图像匹配点对之间的内点，并选取获得最大内点集合的解作为输出，对于每个图像对，计算出旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵和旋转后图像i到旋转后图像j的平移向量最后像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib表示为：

同时，像机的运动也可以被恢复，图像i和图像j之间的旋转矩阵和平移向量表示为：

3.根据权利要求1所述的一种像机与惯性测量单元旋转关系快速标定方法，其特征在于，所述像机纯旋转运动条件下的标定方法，具体为：

在像机纯旋转运动条件下，图像匹配点对之间同样满足单应约束，像机纯旋转运动条件下公式

中的平移向量为[0,0,0]^T，则单应矩阵表示为：

该公式包含的未知参数为3个从旋转后像机坐标系到惯性测量单元坐标系的旋转变量根据公式

可知，1.5个图像匹配点对共给出包含3个未知参数的3个多项式方程：

通过格罗布纳基方法分析知该方程组的次数仅为2，且最多有8个解,只需要1.5个图像匹配点对，最后选取获得最大内点集合的解作为旋转后像机坐标系与惯性测量单元坐标系之间的旋转矩阵再根据公式计算出像机坐标系与惯性测量单元坐标系之间的旋转矩阵R_calib。

4.根据权利要求1所述的一种像机与惯性测量单元旋转关系快速标定方法，其特征在于，所述像机纯旋转运动和焦距未知条件下的标定方法，具体为：

分为两步：先求解像机的焦距，再标定像机坐标系与惯性测量单元坐标系之间的旋转关系，图像i和j之间为纯旋转运动，假定有两个图像匹配点对和图像i中x_i1和x_i2与光心连线形成的夹角为θ₁，图像j中x_j1和x_j2与光心连线形成的夹角为θ₂，在各自图像坐标系中两个图像点与光心连线形成的夹角是相等的，因此有如下公式：

cos²θ₁＝cos²θ₂ (19)

将上式用图像坐标和像机焦距进行表示，推导出：

其中

上式中F表示像机的焦距，将公式(21)代入到公式(20)中，进一步扩展得：

公式(22)是关于F²的三次多项式方程，直接求解出焦距F，然后在已知像机内参数的条件下，直接采用像机纯旋转运动条件下的标定方法求解像机坐标系与惯性测量单元坐标系之间的旋转关系。