摄像头与惯性传感器组合定位定姿系统的标定方法

摘要

本发明为摄像头与惯性传感器组合定位定姿系统的标定方法，包括：标定摄像头的内参数矩阵；对于一个尺寸已知的标定物从不同角度拍摄多幅图像，记录在拍摄每幅图像时惯性传感器输出的横滚角和俯仰角；定义世界坐标系、摄像机坐标系、惯性传感器坐标系和地磁坐标系；基于每幅图像中标定物的图像信息和空间信息，计算该时刻世界坐标系到摄像机坐标系的旋转矩阵；将摄取图像两两组合，对于每个组合均建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵的方程组并进行求解，计算出惯性传感器坐标系到摄像机坐标系的旋转矩阵；对于每幅图像建立一个关于地磁坐标系到世界坐标系旋转矩阵的方程组并进行求解，计算出地磁坐标系到世界坐标系的旋转矩阵。

说明书

技术领域

本发明属于摄像机技术领域，涉及摄像头与惯性传感器组合定位定姿系统的标定技术。

背景技术

摄像头与惯性传感器组合定位定姿系统，一般由一个摄像头与一个惯性传感器组成，其功能是同步提供摄像头的位置和姿态信息，其中惯性传感器用来提供三轴方向角等信息进而辅助摄像头进行定位定姿。此类系统在机器人自主导航、虚拟现实、增强现实等领域有着广泛的应用。

摄像头与惯性传感器组合定位定姿系统涉及世界坐标系、摄像机坐标系、地磁坐标系和惯性传感器坐标系等四个不同的坐标系，其中世界坐标系与地磁坐标系之间的旋转变换关系以及惯性传感器坐标系与摄像机坐标系之间的旋转变换关系是固定不变的。因此如何准确地标定出世界坐标系与地磁坐标系之间的旋转变换关系以及惯性传感器坐标系与摄像机坐标系之间的旋转变换关系是一个非常重要的问题。由于该问题中待标定的参数较多，且中低档惯性传感器输出的三轴方向角(横滚角、俯仰角、航向角)中航向角的精度较低不适合用作精确标定，因此目前文献中几乎没有成熟的摄像头与惯性传感器组合定位定姿系统自动精确标定方法。

发明内容

本发明的目的在于提供一种鲁棒、实用且精度高的摄像头与惯性传感器组合定位定姿系统标定方法。

为实现上述目的，本发明提出的摄像头与惯性传感器组合定位定姿系统标定方法解决技术问题的技术方案包括以下步骤：

步骤S1：标定摄像头的内参数矩阵；

步骤S2：对于一个尺寸已知的标定物从不同角度拍摄多幅图像，同时记录在拍摄每幅图像时惯性传感器输出的横滚角和俯仰角信息；

步骤S3：定义世界坐标系、摄像机坐标系、惯性传感器坐标系和地磁坐标系；

步骤S4：基于每幅图像中标定物的图像信息和空间信息，计算该时刻世界坐标系到摄像机坐标系的旋转矩阵；

步骤S5：将所拍摄图像两两组合生成多个图像对，对于每个图像对建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的方程组：

$R_{w-c,\mathrm{ij}}\times R_{m-c}\times \left(\begin{array}{c}-\sin {\beta }_j\\ \sin {\alpha }_j\cos {\beta }_j\\ \cos {\alpha }_j\cos {\beta }_j\end{array}\right)=R_{m-c}\times \left(\begin{array}{c}-\sin {\beta }_i\\ \sin {\alpha }_i\cos {\beta }_i\\ \cos {\alpha }_i\cos {\beta }_i\end{array}\right),$

式中，R_w-c，ij表示摄像机坐标系在i、j两时刻之间的旋转矩阵，α_i，α_j分别表示惯性传感器在i、j时刻的横滚角，β_i，β_j分别表示惯性传感器在i、j时刻的俯仰角，则对于多个图像对，利用惯性传感器输出的横滚角和俯仰角，可建立关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组；

步骤S6：将关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c；

步骤7：对于每幅图像建立一个关于地磁坐标系到世界坐标系旋转矩阵R_e-w的方程组：

$\left(\begin{array}{ccc}\cos {\beta }_i& \sin {\alpha }_i\sin {\beta }_i& \cos {\alpha }_i\sin {\beta }_i\\ 0& \cos {\alpha }_i& -\sin {\alpha }_i\\ -\sin {\beta }_i& \sin {\alpha }_i\cos {\beta }_i& \cos {\alpha }_i\cos {\beta }_i\end{array}\right)\times R_{m-c}^{-1}\times R_{w-c,i}\times R_{e-w}=\left(\begin{array}{ccc}\cos {\gamma }_i& \sin {\gamma }_i& 0\\ -\sin {\gamma }_i& \cos {\gamma }_i& 0\\ 0& 0& 1\end{array}\right),$

其中，R_w-c，i表示在i时刻世界坐标系到摄像机坐标系的旋转矩阵，R_m-c表示惯性传感器坐标系到摄像机坐标系的旋转矩阵，α_i表示惯性传感器在i时刻的横滚角，β_i表示惯性传感器在i时刻的俯仰角，γ_i表示惯性传感器在i时刻的航向角；利用上式两边对应项相等的性质以及旋转矩阵的正交性约去有关惯性传感器航向角γ_i的计算项，可得如下线性方程组：

B_iV_e-w＝b_i，

式中B_i，b_i分别为上式经过化简后的系数矩阵与常数项，V_e-w是地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化表示形式，则对于多幅图像，利用惯性传感器输出的横滚角和俯仰角，可建立关于地磁坐标系到世界坐标系旋转矩阵R_e-w的多个方程组，进而建立关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组；

步骤S8：将关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出地磁坐标系到世界坐标系的旋转矩阵。

本发明的有益效果：本发明提供的摄像头与惯性传感器组合定位定姿系统标定方法，没有像其它相关文献那样同时使用惯性传感器输出的三轴方向角(横滚角、俯仰角、航向角)以及摄像头摄取的图像信息实现系统标定，而是仅仅使用惯性传感器输出的横滚角、俯仰角以及摄像头摄取的图像信息实现系统自动标定。由于中低档惯性传感器的航向角输出精度明显低于横滚角输出精度和俯仰角输出精度，因此本发明可以有效地避免由航向角误差所引起的较大的系统标定误差，大大提高系统标定的精度。此外，本发明方法还具有简单、实用、鲁棒性好等特点。本发明使得包含摄像头与惯性传感器组合定位定姿系统的标定走向实用化、简便化。

附图说明

图1为本发明的流程图。

图2为本发明实例中的摄像头与惯性传感器组合定位定姿系统硬件示意图。

图3为本发明实例中的标定物示意图。

图4为本发明实例中的坐标系示意图。

具体实施方式

下面结合附图详细说明本发明技术方案中所涉及的各个细节问题。应指出的是，所描述的实施例仅旨在便于对本发明的理解，而对其不起任何限定作用。

为了实现本发明的方法，实施时采用一台CPU2.33G、内存2G的计算机，在计算机上采用VC编制相关程序，所用摄像头为罗技(logitech)C200，惯性传感器为MTi微型惯性传感器，标定物为黑白棋盘格板。图1为本发明方法的流程图，图2为本发明实例中的摄像头与惯性传感器组合定位定姿系统硬件示意图。

本发明的具体实施步骤如下：

1.步骤S1标定摄像头的内参数矩阵：

利用已有技术张正友摄像机标定方法标定摄像头的内参数矩阵。

2.步骤S2对于一个尺寸已知的标定物从不同角度拍摄多幅图像，并记录该时刻惯性传感器输出的横滚角(Roll)和俯仰角(Pitch)信息：

用摄像头从不同角度拍摄多幅同一标定物的图像(图3中的黑白棋盘格，每个小格子的边长已知)，记为m幅，并将这m幅图像输入计算机。同时，利用计算机读入该时刻惯性传感器输出的横滚角和俯仰角信息。目前惯性传感器一般都带有导入数据到计算机的接口，按照惯性传感器的使用说明可以很方便地完成这步。

3.步骤S3定义系统中的四个坐标系，如图4所示：

世界坐标系O_wX_wY_wZ_w：原点O_w为黑白棋盘格的某个角点，X_w轴、Y_w轴分别与黑白棋盘格的长边、短边平行，Z_w轴方向由右手定则确定。

摄像机坐标系O_cX_cY_cZ_c：原点O_c为摄像机光心，X_c轴、Y_c轴分别与图像平面的横轴、纵轴平行，Z_c轴为摄像机光轴。

惯性传感器坐标系O_mX_mY_mZ_m：惯性传感器坐标系是固定在惯性传感器上的坐标系。原点O_m为惯性传感器的重心，X_m轴、Y_m轴分别与惯性传感器的长边、短边平行，Z_m轴方向由右手定则确定。

地磁坐标系O_eX_eY_eZ_e：原点O_e为空间中任意点(这里设置为与世界坐标系原点O_w重合)，O_eX_e指向磁场北极，O_eY_e指向西方，Z_e轴垂直于地平面。惯性传感器输出的三轴方向角(横滚角、俯仰角、航向角)以地磁坐标系为基准。

由上述坐标系定义可知，世界坐标系到摄像机坐标系的旋转矩阵R_w-c、地磁坐标系到世界坐标系的旋转矩阵R_e-w、地磁坐标系到惯性传感器坐标系的旋转矩阵R_e-m以及惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c满足如下关系式：

$R_{w-c}=R_{m-c}\times R_{e-m}\times R_{e-w}^{-1}---\left(1\right)$

4.步骤S4基于每幅图像中标定物的图像信息和空间信息，计算该时刻的世界坐标系到摄像机坐标系的旋转矩阵：

对于每幅图像I_i，提取图像中标定物黑白棋盘格的角点，即可获得黑白棋盘格角点的图像坐标。由于步骤S3中已定义世界坐标系的原点O_w为黑白棋盘格的某个角点，X_w轴、Y_w轴分别与黑白棋盘格的长边、短边平行，且黑白棋盘格中每个小格子的边长已知，因此黑白棋盘格角点在世界坐标系下的世界坐标可以直接确定。根据黑白棋盘格角点的图像坐标和世界坐标，利用已有技术直接线性变换方法(Direct Linear Transformation方法)计算对应于该幅图像的世界坐标系到摄像机坐标系的旋转矩阵R_w-c，i。

5.标定惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c：

惯性传感器的三轴方向角与旋转矩阵的变换关系是：

$R_{e-m}=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & \sin \alpha \\ 0& -\sin \alpha & \cos \alpha \end{array}\right)\times \left(\begin{array}{ccc}\cos \beta & 0& -\sin \beta \\ 0& 1& 0\\ \sin \beta & 0& \cos \beta \end{array}\right)\times \left(\begin{array}{ccc}\cos \gamma & \sin \gamma & 0\\ -\sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)---\left(2\right)$

其中，α表示惯性传感器的横滚角，β表示惯性传感器的俯仰角，γ表示惯性传感器的航向角。

步骤S5：将所拍摄图像两两组合生成多个图像对，对于每个图像对建立一个关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的方程组；ze对于多个图像对，利用惯性传感器输出的横滚角和俯仰角，可建立关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组；

对于任意两张图像I_i和I_j的相关数据，根据手眼标定基本方程可得：

R_w-c，ij×R_m-c＝R_m-c×R_e-m，ij          (3)

其中表示摄像机坐标系在i、j两时刻之间的旋转矩阵，表示惯性传感器坐标系在i、j两时刻之间的旋转矩阵，R_m-c为惯性传感器坐标系到摄像机坐标系的旋转矩阵。

由公式(2)、(3)可得一个以惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c为未知量的方程组：

$R_{w-c,\mathrm{ij}}\times R_{m-c}\times \left(\begin{array}{c}-\sin {\beta }_j\\ \sin {\alpha }_j\cos {\beta }_j\\ \cos {\alpha }_j\cos {\beta }_j\end{array}\right)=R_{m-c}\times \left(\begin{array}{c}-\sin {\beta }_i\\ \sin {\alpha }_i\cos {\beta }_i\\ \cos {\alpha }_i\cos {\beta }_i\end{array}\right)---\left(4\right)$

$\Rightarrow A_{i,j}{V}_{m-c}=0---\left(5\right)$

其中R_w-c，ij表示摄像机坐标系在i、j两时刻之间的旋转矩阵，A_i，j为等式(4)化简后的系数矩阵，V_m-c为矩阵R_m-c的向量化表示形式，α_i，α_j分别表示惯性传感器在i、j时刻的横滚角，β_i，β_j分别表示惯性传感器在i、j时刻的俯仰角。

对于拍摄的m幅图像进行两两组合，共可得种不同的图像对，对这些图像对分别按照上述方法进行计算，共可得个形如式(5)的关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的方程组，即

${\mathrm{AV}}_{m-c}=\left(\begin{array}{c}{A}_{\mathrm{1,2}}\\ A_{\mathrm{1,3}}\\ .\\ .\\ .\\ A_{m-1,m}\end{array}\right)V_{m-c}=0$

其中，$A=\left(\begin{array}{c}{A}_{\mathrm{1,2}}\\ A_{\mathrm{1,3}}\\ .\\ .\\ .\\ A_{m-1,m}\end{array}\right).$

步骤S6：将关于惯性传感器坐标系到摄像机坐标系旋转矩阵R_m-c的多个方程组转化为一个含约束的非线性优化问题，求解上述非线性优化问题即可求得惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c；

由于R_m-c为旋转矩阵，有(E₃为三阶单位阵)。因此求解旋转矩阵R_m-c的问题可转化为如下一个含约束的非线性优化问题：

$\underset{V_{m-c}}{\arg \min }{V}_{m-c}^T{A}^T{\mathrm{AV}}_{m-c},$

$s.t.R_{m-c}^T{R}_{m-c}=E_3,$

求解上述非线性优化问题即可求得惯性传感器坐标系到摄像机坐标系的旋转矩阵R_m-c。

6.标定地磁坐标系到世界坐标系的旋转矩阵R_e-w：

步骤S7：对于每幅图像I_i建立一个关于地磁坐标系到世界坐标系旋转矩阵R_e-w的方程组，由公式(1)(2)可得

$\left(\begin{array}{ccc}\cos {\beta }_i& \sin {\alpha }_i\sin {\beta }_i& \cos {\alpha }_i\sin {\beta }_i\\ 0& \cos {\alpha }_i& -\sin {\alpha }_i\\ -\sin {\beta }_i& \sin {\alpha }_i\cos {\beta }_i& \cos {\alpha }_i\cos {\beta }_i\end{array}\right)\times R_{m-c}^{-1}\times R_{w-c,i}\times R_{e-m}=\left(\begin{array}{ccc}\cos {\gamma }_i& \sin {\gamma }_i& 0\\ -\sin {\gamma }_i& \cos {\gamma }_i& 0\\ 0& 0& 1\end{array}\right)---\left(6\right)$

其中，R_w-c，i表示在i时刻世界坐标系到摄像机坐标系的旋转矩阵，R_m-c表示惯性传感器坐标系到摄像机坐标系的旋转矩阵，α_i表示惯性传感器在i时刻的横滚角，β_i表示惯性传感器在i时刻的俯仰角，γ_i表示惯性传感器在i时刻的航向角。

将步骤S4中已计算的i时刻世界坐标系到摄像机坐标系的旋转矩阵R_w-c，i、步骤S6中已标定的R_m-c和步骤S2中已记录的惯性传感器输出的α_i，β_i代入到上式中，并令V_e-w为矩阵R_e-w的向量化表示形式。利用等式两边对应项相等的性质以及旋转矩阵的正交性约去有关惯性传感器航向角γ_i的计算项，可得如下线性方程组：

B_iV_e-w＝b_i                   (7)

其中B_i，b_i分别为等式(6)经过化简后的系数矩阵与常数项，V_e-w是地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化表示形式。

对于拍摄的m幅图像，分别按照上述方法，利用惯性传感器输出的横滚角和俯仰角建立关于地磁坐标系到世界坐标系旋转矩阵R_e-w的多个方程组，进而建立形如式(7)的关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组，如下：

${\mathrm{BV}}_{e-w}=\left(\begin{array}{c}{B}_1\\ B_2\\ .\\ .\\ .\\ B_m\end{array}\right)V_{e-w}=\left(\begin{array}{c}{b}_1\\ b_2\\ .\\ .\\ .\\ b_m\end{array}\right)=b,$

其中$B=\left(\begin{array}{c}{B}_1\\ B_2\\ .\\ .\\ .\\ B_m\end{array}\right),$$b=\left(\begin{array}{c}{b}_1\\ b_2\\ .\\ .\\ .\\ b_m\end{array}\right).$

步骤S8：将关于地磁坐标系到世界坐标系旋转矩阵R_e-w的向量化V_e-w的多个线性方程组转化为一个含约束的非线性优化问题，求解所述含约束的非线性优化问题则计算出地磁坐标系到世界坐标系的旋转矩阵。

由于R_e-w为旋转矩阵，有(E₃为三阶单位阵)。因此求解地磁坐标系到世界坐标系旋转矩阵R_e-w的问题可转化为如下一个含约束的非线性优化问题：

$\underset{V_{e-w}}{\arg \min }{({\mathrm{BT}}_{e-w}-b)}^T({\mathrm{BV}}_{e-w}-b),$

$s.t.R_{e-w}^T{R}_{e-w}=E_3,$

求解上述非线性优化问题即可求得地磁坐标系到世界坐标系的旋转矩阵R_e-w的向量化V_e-w，也即求得了地磁坐标系到世界坐标系的旋转矩阵R_e-w。

图2为本发明实例中的摄像头与惯性传感器组合定位定姿系统硬件示意图。其中MTi惯性传感器和摄像头均固定在一个头盔上。

图3为本发明实例中的标定物示意图。图像尺寸为320*240，图中黑白棋盘格板为标定物，每个正方形小格子的边长为2厘米。

图4为本发明实例中的坐标系示意图。其中世界坐标系的原点以及地磁坐标系的原点均设定在黑白棋盘格左上角的角点。

应用本发明方法得到的标定结果为：

$R_{m-c}=\left(\begin{array}{ccc}-0.0093& -0.9999& -0.0133\\ -0.3640& 0.0158& -0.9313\\ 0.9313& -0.0039& -0.3641\end{array}\right),$

$R_{e-w}=\left(\begin{array}{ccc}-0.1135& 0.0114& -0.9935\\ -0.9930& -0.0355& 0.1130\\ -0.0340& 0.9993& 0.0154\end{array}\right),$

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。