CN108257184B - 一种基于正方形点阵合作目标的相机姿态测量方法 - Google Patents

Abstract

本发明涉及一种基于正方形点阵合作目标的相机姿态测量方法，先通过确定相机内参、确定合作目标点为正方形点阵；然后通过相机对合作靶标进行拍照，并提取图像中合作目标点的像素坐标；最后通过公式解出相机相对于靶标的姿态角。该方法改进了现有的P4P相机标定算法，使得相机对不同规格目标进行姿态测量时，无需进行合作目标尺寸规格的测量和参数输入，只要合作目标点按正方形等间距分布，均可使用同一公式进行相机坐标系相对于合作目标坐标系姿态角的测量，拓展了单相机姿态测量的使用范围。

Description

一种基于正方形点阵合作目标的相机姿态测量方法

技术领域

本发明属于视觉测量、姿态检测技术领域，涉及一种使用正方形点阵合作目标进行相机姿态测量的方法。

背景技术

相机标定作为视觉测量中重要的一个环节，目前已经有广泛的研究。在一些摄影测量类的书籍中，对标定原理有比较详尽的描述。目前常用的方法是PNP标定技术，利用世界坐标已知的N个点目标，根据在这些点在相机所成图像中的对应像素坐标，解算出相机相对于世界坐标系的位置和姿态关系，表述为相机坐标系与世界坐标系的转换矩阵和平移向量。在PNP标定方法中，N通常取3以上，P3P法中，解不唯一，通常要采用其他方法判断多解的合理性，以获取正确解。N取6以上时，包含异面点，可获取唯一解，但异面点目标靶标制作困难、三维坐标测量过程复杂、误差较大。N取4时，若4点共面，利用坐标系转换矩阵的单位正交性，仍可获取唯一的位姿关系。由于共面靶标制作简单，坐标定位方便，故P4P为常用的相机标定方法，为了消除图像提取误差和计算误差，在专门的相机标定场合，将靶标共面4点扩展到多点，利用最小二乘法，提升标定精度，如棋盘格靶标、多点阵靶标等。

应用方面，相机标定技术通常用于三个方面，一是用于摄影测量的相机标定，为多相机交会测量或结构光辅助测量提供先验信息；二是用于单相机对目标的位姿测量，如机器臂上相机对被抓取物体的位姿测量；三是用于相机相对于目标的位姿测量，如飞机起飞前对地面合作目标的测量，以确定飞机初始方位角度，飞机落地前对地面合作目标的测量，以引导飞机降落等。这三个方面的测量原理是相同的，只是用途或基准坐标系的选取不同而已。

在这些测量过程中，均以合作目标上的点目标三维信息已知为前提(共面时，深度信息Z一般设为0)，必须对靶标进行精确设计和位置标定。这限制了单相机测量的应用，比如，在机器臂抓取物体时，对不同规格的被抓取物体，必须在程序中重新输入合作目标的位置参数，测量过程很不方便；在飞机初始角度测量时，由于不同型号的飞机配备的地面合作目标大小不同，也必须对单目相机配置不同的程序参数，无法进行统一批次性的生产。

发明内容

为了解决现有相机姿态测量方法均以合作目标上的点目标三维信息已知为前提，必须对靶标进行精确设计和位置标定的问题，本发明一种基于正方形点阵合作目标的相机姿态测量方法。

本发明的技术解决方案如下：

一种基于正方形点阵合作目标的相机姿态测量方法，其特殊之处在于：包括以下步骤：

1)确定相机内参，即确定相机像元尺寸e、焦距f′、光轴与探测器交点像素坐标(C_x,C_y)，计算

2)确定合作靶标上的四个合作目标点，四个合作目标点呈正方形点阵分布；

3)通过相机对合作靶标进行拍照，并提取图像中各合作目标点的像素坐标(x_i,y_i)，通过式(2)计算(k_xi,k_yi)；

其中X_Ci、Y_Ci、Z_Ci为合作目标点在相机坐标系中的坐标值，i的取值为1～4；

4)通过式(12)～式(14)，计算方位角A_Z、俯仰角A_X、横滚角A_Y；

进一步地，为了测量的方便性，在满足测量精度的前提下，本发明的像元尺寸和焦距按设计值设定，光轴与探测器交点像素坐标(C_x,C_y)按图像中心像素坐标设定。为了提高测量精度，本发明的焦距及光轴与探测器交点像素坐标(C_x,C_y)也可以进行标定。

进一步地，所述合作目标点可以是正方形的角点，也可以是按沿X和Y轴等间距分布的圆点。

本发明与现有技术相比，有益效果是：

1、本发明改进了现有的P4P相机标定算法，使得相机对不同规格目标进行姿态测量时，无需进行合作目标尺寸规格的测量和参数输入，即无需输入合作目标三维坐标，无需更改软件算法程序。只要合作目标点按正方形等间距分布，均可使用同一公式进行相机坐标系相对于合作目标坐标系姿态角的测量(或合作目标系相对于相机坐标系)，拓展了单相机姿态测量的使用范围。

附图说明

图1是相机坐标系和世界坐标系的定义图。

具体实施方式

以下结合附图从本发明的原理出发，对本发明基于正方形点阵合作目标的相机姿态测量方法进行详细说明。

如图1所示，定义相机坐标系(O_CX_CY_CZ_C)：坐标系原点在光心处，Z_C轴为光轴，X_C轴与图像x方向一致，Y_C与图像y方向一致，坐标系符合右手定则。

定义世界坐标系(OXYZ)：坐标原点在四点中心处，以四点所在平面为XY平面，即为世界坐标系Z＝0平面，X_C与X、Y_C与Y、Z_C与Z轴均成锐角，坐标系符合右手定则。

图像坐标系x方向为图像横向，与X_C相同，y方向为图像纵向，与Y_C方向相同，坐标原点为图像左上角。对物空间中一点，若该点在相机坐标系中的坐标值为(X_C,Y_C)，则成像公式可表述为：

其中，x、y为物点在图像上所成像素坐标，该坐标以图像左上点为原点，横向为x，纵向为y，(C_x,C_y)是光轴与探测器交点的像素坐标，F_x和F_y为焦距与像元尺寸的商，通常，像元尺寸为正方形，故F_x和F_y相等。

记录：

式(2)中，i的取值为1～4，(x_i,y_i)对应四个合作目标点的图像像素坐标。(X_Ci，Y_Ci，Z_Ci)为的合作目标点在相机坐标系中的坐标值。

设世界坐标系原点在相机坐标系中的坐标为(T_X,T_Y,T_Z)，则世界坐标系中的坐标向相机坐标系中的转换关系为：

R_Y、R_X、R_Z(物理含义)的各表达式如下：

A_Y、A_X、A_Z分别表示世界坐标系绕Y轴、绕X轴和绕Z轴的旋转角度，旋转顺序与R_Y、R_X、R_Z的乘积顺序有关，其物理意义结合测量方法可解释为：世界坐标系上的四个点先绕Z轴旋转方位角A_Z，再绕X轴旋转俯仰角A_X，然后绕Y轴旋转横滚角A_Y，旋转后世界坐标系与相机坐标系完全平行。

通过R_Y、R_X、R_Z表达式，计算出R的表达式为：

由于四个合作目标点成正方形分布，故可设：

将式(3)代入式(2)，并结合式(5)，有：

式(6)的矩阵形式为：

其解为：

利用转换矩阵R的单位正交性，可以解出：

根据式(4)和式(8)，可知方位角为：

同理，可求得俯仰角：

横滚角：

在式(12)、(13)和(14)中，由于分子和分母中，与k相关的因子约去，只包含了与四个合作目标点象素坐标相关的

与合作目标空间三维坐标无关。

故该方法只要约束合作目标点为正方形点阵即可解出相机相对于靶标的姿态角。

依据上述的原理，本发明基于正方形点阵合作目标的相机姿态测量方法的实施步骤为：

1、确定相机内参，即确定相机像元尺寸e、焦距f′、光轴与探测器交点像素坐标(C_x,C_y)，通常像元尺寸和焦距可按设计值设定，(C_x,C_y)可按图像中心像素坐标设定，计算

2、通过相机对合作靶标进行拍照，并提取图像中合作目标点的像素坐标(x_i,y_i)，通过式(2)计算(k_xi,k_yi)；

3、通过式(12)～式(14)，计算方位角A_Z、俯仰角A_X、横滚角A_Y；

通过本发明的具体实施步骤可以看出，在传统的P4P相机姿态测量方法中，必须输入合作目标点在世界坐标系中的三维坐标，以矩阵的方式求解相机姿态，计算过程复杂。本发明方法直接推算出相机姿态角公式，简单的四则运算和三角函数即可计算出相机姿态角A_Z、A_X、A_Y，计算方法简单；本方法计算公式中，只包含了与合作目标点所成图像的像素坐标相关的变量(k_xi,k_yi)，不涉及到合作目标点的世界坐标系坐标，对合作目标点的约束只需要是正方形点阵分布即可，拓展了测量适用性。

以下是本发明的应用实例：

在飞机起飞前初始导航角测量试验中，采用像元尺寸为5.5um×5.5um，分辨率2048×2048，焦距35mm的相机，在约2m的高度下，对450mm×450mm和240×240mm两种规格的正方形合作靶标，以其角点为合作目标点，按本发明实施步骤，进行相机相对于合作靶标的姿态测量，其试验数据如表1所示：

表1试验数据对照表

从表中可以看出，本发明方法对不同规格尺寸的正方形点阵分布合作靶标，在不进行靶标规格参数输入的情况下，能够快速实现相机姿态角度的测量，提升了相机姿态测量对不同规格合作靶标的适应性。

其它实施例：

1、飞机起飞前初始导航角测量，其方案为在地面设置正方形合作靶标，机型不同，其对应的地面合作靶标大小不同，分别由450mm×450mm规格、240mm×240mm规格，120mm×120mm规格，在飞机底部安装相机，相机像元尺寸为5.5um×5.5um，分辨率2048×2048，焦距35mm。相机对合作靶标进行成像，提取正方形四个角点，由于其算法平台为嵌入式平台，在进行算法设计时，矩阵求逆等运算较为复杂，嵌入式平台资源不够，且对不同机型，必须设定不同的合作靶标参数，软件通用性不强。运用本发明方法后，成功解决了嵌入式平台算法设计和软件通用性不强的问题，无需进行矩阵运算，无需在算法中输入不同规格的合作靶标规格参数。

2、机械臂实现对某工件的自动抓取、分拣功能，在机械臂上安装相机，像元尺寸为5.5um×5.5um，分辨率2048×2048，焦距12mm，工件本身有正方形定位平面，通过相机对工件定位平面进行成像，提取其四个角点，通过P4P算法即能实现相机相对于工件的姿态测量，从而引导机械臂，实现对工件的无损抓取。但工件本身规格型号不同，其定位面规格由40mm×40mm至120mm×120mm不等，传统的P4P算法无法实现对所有工件的姿态测量，使用本发明方法后，解决了机械臂上相机姿态测量对不同规格的工件适应性问题。

Claims (3)

1.一种基于正方形点阵合作目标的相机姿态测量方法，其特征在于：包括以下步骤：

1)确定相机内参，即确定相机像元尺寸e、焦距f′、光轴与探测器交点像素坐标(C_x,C_y)，计算

其中：F_x和F_y为焦距与像元尺寸的商；

2)确定合作靶标上的四个合作目标点，四个合作目标点呈正方形点阵分布；

3)通过相机对合作靶标进行拍照，并提取图像中各合作目标点的像素坐标(x_i,y_i)，通过以下公式计算(k_xi,k_yi)；

其中i的取值为1～4；

4)通过式(1)～式(5)，计算方位角A_Z、俯仰角A_X、横滚角A_Y；

设世界坐标系原点在相机坐标系中的坐标为(T_X,T_Y,T_Z)，则世界坐标系中的坐标X_i、Y_i、Z_i向相机坐标系中的转换关系为：

R_Y、R_X、R_Z的各表达式如下：

A_Y、A_X、A_Z分别表示世界坐标系绕Y轴、绕X轴和绕Z轴的旋转角度；

通过R_Y、R_X、R_Z表达式，计算出R的表达式为：

2.根据权利要求1所述的基于正方形点阵合作目标的相机姿态测量方法，其特征在于：

像元尺寸和焦距按设计值设定，光轴与探测器交点像素坐标(C_x,C_y)按图像中心像素坐标设定。

3.根据权利要求1所述的基于正方形点阵合作目标的相机姿态测量方法，其特征在于：

所述合作目标点可以是正方形的角点，也可以是按沿X和Y轴等间距分布的圆点。

Images