一种对具有平行线特征的目标空间姿态测量方法

摘要

本发明涉及一种对具有平行线特征的目标空间姿态测量方法，现有测量方法都必须在直线上设置合作标志点、测量步骤繁复、测量仪器复杂、设备昂贵的问题。该方法包括：1)建立测量系统；2)线激光平面方程的标定；3)求取线激光照射到被测目标表面的直线的空间方向矢量；4)求取平行线特征的目标空间指向在相机坐标系中的表述；5)通过坐标系的旋转变换求解姿态角与方位角。本发明无需在目标上设置合作标志点，可直接利用被测目标物体表面所具有的平行直线特征，无需昂贵的三维坐标测量仪器，即可得到被测目标物体在空间中的三维姿态信息，测量系统架设方便、成本低、测量步骤简单。

说明书

技术领域

本发明涉及光学测量技术领域，具体涉及一种对具有平行线特征的目标空间姿态测量方法。

背景技术

在精密测量、智能自动化装配、姿态角度测量领域中，经常涉及到对待测物体空间姿态的测量，如建筑工程中钢梁的安装方向误差测量、精密仪器装配中条状结构件装配误差检测(如运动导轨)、立方体基座姿态测量等。对这类具有平行直线特征的目标空间的姿态检测，通常在两平行直线上分别设置两个标志点，采用空间坐标测量仪器，如双目测量系统、三坐标测量仪、激光跟踪仪、全站仪(发明：一种基于全站仪的空间目标的位置和朝向标定方法，申请公布号CN 105424024 A)、双经纬仪交会测量系统等，对四个标志点的三维坐标进行测量，再换算成平行直线所在平面的姿态信息。

以上测量方法中，双目测量系统需要架设两台成像仪器对直线上的标记点进行成像，并提取像素坐标，通过交会测量得到标记点的三维空间坐标；三坐标测量仪检测范围小，被测目标必须限制在测量臂活动范围之内，且需要接触测量；激光跟踪仪利用干涉测距的原理实现目标三维坐标测量，必须在标志点上架设反射靶标，激光跟踪仪发出的激光必须出射到反射靶标上，并反射回激光跟踪仪，靶标从激光跟踪仪移动到标志点的过程当中，必须保证靶标的自准直状态，如果反射靶标偏离了激光光轴则会出错，而且不能断光再续，必须重新再来，此过程中，反射靶标架设、光学调整的步骤复杂，测量过程慢；全站仪对直线上两个标志点的方位、俯仰角度和距离值进行测量，从而得到标志点的三维坐标；双经纬仪交会测量系统根据单台经纬仪对标志点的方位、俯仰角度值以及双经纬仪之间的位置、姿态关系，通过三角交会原理，计算出标志点的三维坐标，在使用前必须进行双经纬仪位置和姿态的标定；以上测量方法都必须在直线上设置合作标志点，测量步骤繁复，测量仪器复杂，设备昂贵。

发明内容

本发明的目的是解决现有测量方法都必须在直线上设置合作标志点、测量步骤繁复、测量仪器复杂、设备昂贵的问题，提供一种对具有平行线特征的目标空间姿态测量方法。

本发明的技术方案是：

一种对具有平行线特征的目标空间姿态测量方法，包括以下步骤：

1)建立测量系统；

架设相机和单线激光器，将具有平行线特征的被测目标放置到相机成像区域内，单线激光器发出的线激光投射到被测目标上，且线激光不与平行线特征平行，相机对投射到被测目标表面的线激光和目标进行成像；

2)线激光平面方程的标定；

2.1)建立坐标系；

建立相机坐标系Oc-XcYcZc，以相机光心为坐标原点Oc，沿成像镜头光轴方向为Zc轴，沿成像探测器横边为Xc轴，竖边为Yc轴；

在光轴上距坐标原点前方f′处设置图像平面，f′为镜头焦距，图像平面与探测器平面平行，图像平面上建立二维图像坐标oxy，x平行于Xc轴，y平行于Yc轴，o点为光轴与图像平面的交点，物点、像点和光心满足共线关系；

2.2)交线k₁的标准形式方程求取；

在相机和单线激光器组成的测量系统前方，相机的成像区域和线激光投射的相交区域内放置四点平面靶标，在平面靶标上设置四个已知坐标的合作目标点，通过摄影测量P4P算法，得到靶标坐标系O₁-X₁Y₁Z₁与相机坐标系Oc-XcYcZc间的旋转平移关系，即旋转矩阵R和平移向量T，表示如下：

靶标坐标系与相机坐标系间的转换关系为：

靶标平面方程在靶标坐标系中的表述为Z₁＝0，则靶标平面方程在相机坐标系中的表述为：Z₁＝r₁₆Xc+r₁₇Yc+r₁₈Zc+t_1z＝0，该平面记为π₁；

在图像平面上，通过直线提取处理算法，提取线激光在图像平面上的直线方程：A₁x+B₁y+C₁＝0，得到A₁、B₁、C₁；

则线激光在相机坐标系下的直线方程为：

求解直线与光心确定的平面，建立平面束方程，有A₁X+B₁Y+C₁+L(Z-f')＝0，代入光心坐标(0,0,0)，解得则该平面方程为：该平面记为π₂；

根据空间几何定理，两平面交线的方向矢量等于两平面法矢量的叉积，则线激光平面与该状态下靶标平面交线k₁的方向矢量为：

其中，r₁₆、r₁₇、r₁₈通过P4P算法得到，f′为镜头焦距，计算得l₁₁、m₁₁、n₁₁数值；

求取线激光平面与该状态下靶标平面交线上的一点，联立π₁、π₂：

取x₁＝0，得到其中，t_1z通过P4P算法解算得到；

将上述得到的直线方向矢量与直线上一点坐标(x₁,y₁,z₁)带入直线方程的标准形式，即得到交线k₁的标准形式方程为

2.3)交线k₂的标准形式方程求取；

任意变换靶标的姿态，靶标四点与投射的线激光在相机成像区域内，重复2.2)的计算过程得到交线k₂的方程为：

计算得l₁₂、m₁₂、n₁₂数值；

2.4)联合式(1)和式(2)，得线激光平面的法矢量为：

将得到的l₁₁、m₁₁、n¹¹和l₁₂、m₁₂、n₁₂代入式(3)，得到l,m,n的数值；

2.5)通过以上计算，通过平面上一点(x₁,y₁,z₁)与平面的法矢量

得到线激光在相机坐标系下的光平面方程：

lX+m(Y-y₁)+n(Z-z₁)＝0(4)

3)求取线激光照射到被测目标表面的直线的空间方向矢量；

将式(4)整理为一般式，得A₀X+B₀Y+C₀Z+D₀＝0，其中，A₀＝l、B₀＝m、C₀＝n、D₀＝-m×y₁-n×z₁；通过图像处理算法，提取投射到被测目标平面上的线激光在相机图像平面上的方程为：A₀'x+B₀'y+D₀'＝0，重复求取π2的解算过程，得到线激光照射到被测目标表面的直线，相机光心平面在相机坐标系下的平面方程为得A₀'、B₀'、D₀'；

两平面交线的方向矢量等于两平面法向矢量的叉积，则线激光照射到被测目标表面的直线的空间方向矢量为：

将A₀'、B₀'、D₀'和A₀、B₀、C₀代入式(5)，得l₁,m₁,n₁；

4)求取平行线特征的方向矢量在相机坐标系中的表述；

对被测目标上的平行线特征进行图像处理和目标边缘轮廓提取，被测目标长度方向边缘轮廓分别设为L₁和L₂，L₁和L₂平行，在被测目标表面建立世界坐标系O_w-X_wY_wZ_w，Y_w轴与直线L₁和L₂平行，Z_w垂直于L₁和L₂所确定的平面，X_w轴方向符合右手定则；在图像平面内，物空间直线L₁和L₂所成图像为直线L₁′和L₂′，在图像坐标系oxy中，L₁′和L₂′的平面方程表达式分别为：

A₃x+B₃y+D₃＝0(6)

A₂x+B₂y+D₂＝0(7)

在图像坐标系oxy中，在图像平面上，通过直线提取处理算法，得A₃、B₃、D₃、A₂、B₂、D₂；将得到A₃、B₃、D₃、A₂、B₂、D₂以及镜头焦距值f′代入式(13)，得到平行线特征的方向矢量在相机坐标系中的表述：

5)通过坐标系的旋转变换求解姿态角与方位角；

平行线L₁、L₂与线激光均在世界坐标系Z_w＝0平面内，故Z_w轴在相机坐标系中的方向矢量为：将l、m、n代入式(15)、式(16)和式(20)，得到目标指向在相机坐标系中三维姿态值，θ为俯仰角，γ为侧倾角，为方位角；

本发明的优点为：

1.本发明无需在目标上设置合作标志点，可直接利用被测目标物体表面所具有的平行直线特征，无需昂贵的三维坐标测量仪器，即可得到被测目标物体在空间中的三维姿态信息，测量系统架设方便、成本低、测量步骤简单。

2.本发明方法只需在目标前架设单目相机和单线激光器，配备普通计算机即可，测量设备简单、成本低，测量设备架设方便；在测量过程中，进行目标图像采集和目标图像边缘提取，图像直线方程解算，参数代入即可完成测量，测量步骤便捷；方法计算过程为加减乘除四则运算、开方运算和三角函数运算，计算简单。

3.关于现有的线激光平面方程标定方法，有毕德学等提出的基于交线的线激光光平面方程标定算法、白瑞林(发明：基于线结构光视觉传感器引导的焊接机器人系统标定方法)(申请号：201210318783.9)等提出的标定方法，均需多次(至少三次)转换线激光在标定靶板上进行透射，在本发明只需两次任意变换靶板的姿态，即可标定出线激光在单目相机坐标下的光平面方程，操作更加简单、计算量更小。

附图说明

图1为本发明坐标系定义与物像关系图；

图2为本发明双平行线成像示意图；

图3为本发明相机坐标系与世界坐标系的定义示意图；

图4为本发明平行线物、像、光心几何关系图；

图5为本发明测量系统图；

图6为本发明被测目标实物图。

附图标记：1-相机，2-单线激光器，3-被测目标。

具体实施方式

下面结合说明书附图对本发明的技术方案进行清楚、完整地描述。

本发明方法是一种普适性的三维姿态测量方法，涉及到具有平行线特征的目标空间三维姿态(俯仰角、侧倾角和方位角)的测量，如具有平行边缘的钢梁、枕木、装配件、立方体结构件，或者可在表面刻画平行线标识的目标空间姿态的测量等。

本发明对具有平行线特征的目标空间姿态测量方法的步骤如下:

如图5所示，本发明方法中将相机1与单线激光器2安装，当应用于不同的场景时，根据被测目标3对象的距离远近，可调整二者之间的基线长度，且保证线激光投射到相机成像区域的中心位置附近，此区域相机成像的畸变最小，可保证线激光投射光线的提取精度。

在由相机和单线激光器组成的测量系统(简称三姿测量系统)前方，相机的成像区域和线激光投射的相交区域，放置四点平面靶标，标定出线激光在相机坐标系下的光平面方程：A₀x+B₀y+C₀z+D₀＝0；

将具有双平行线特征的目标放置到三维姿态测量系统测量区域下，相机对投射到目标表面的线激光和目标进行成像，在计算机中，对线激光和目标上的平行线特征进行图像提取，通过计算得到线激光和双平行线在相机坐标系下的方向矢量，即式(5)和式(13)；

将上一步解算的和代入式(15)、式(16)和式(20)，可得到目标指向在相机坐标系中三维姿态值。

本发明满足多种应用需求中对目标空间姿态的测量快速性、便捷性要求，针对具有平行线轮廓、边界特征或者可在表平面刻画平行线的目标，提供一种目标空间姿态测量方法，利用相机和单线激光，其中，单线激光在相机坐标系下的平面方程事先标定给出，线激光投射到平行线特征上，相机对平行线和线激光进行拍照，对目标边缘轮廓或标志线进行图像提取，通过算法，即可实现目标空间姿态在相机三维坐标系中的数学描述。

本发明所采用的技术方案具体为：

一、线激光光平面方程的标定

相机成像模型采用小孔成像模型，相机的内部参数：等效焦距(F_x,F_y)、光轴与成像传感器阵列的交点像素坐标(C_x,C_y)、以及表征成像传感器阵列倾斜程度的物理量，通过张正友标定算法给出。接下来对线激光的光平面方程进行标定；

第一步：如图1所示,建立相机坐标系Oc-XcYcZc，以相机光心为坐标原点Oc，沿成像镜头光轴方向为Zc轴，沿成像探测器横边为Xc轴，竖边为Yc轴，Oc-XcYcZc符合右手坐标系；在光轴上距坐标原点Oc后方f′处为探测器位置(f′为镜头焦距)，物体通过镜头在探测器上成倒像，但通常相机电子学处理时，为保证观测图像符合人眼习惯，将探测器上的图像进行翻转，故在光轴上距坐标原点Oc前方f′处设置图像平面，图像平面与探测器平面平行，以符合探测器平面和图像平面的翻转关系，图像平面上建立二维图像坐标oxy，x平行于Xc轴，y平行于Yc轴，o点为光轴与图像平面的交点；在以上坐标系定义下，物点、像点和光心满足如图1中所示的共线关系。

在由相机和单线激光器组成的测量系统前方，相机的成像区域和线激光投射的相交区域，放置四点平面靶标，在平面靶标上设置四个已知坐标的合作目标点，通过摄影测量P4P算法，可得到该靶标坐标系O₁-X₁Y₁Z₁与相机坐标系Oc-XcYcZc间的旋转平移关系，即旋转矩阵R和平移向量T，表示如下：

靶标坐标系与相机坐标系间的转换关系为：

靶标平面方程在靶标坐标系中的表述为Z₁＝0，则靶标平面方程在相机坐标系中的表述为：Z₁＝r₁₆Xc+r₁₇Yc+r₁₈Zc+t_1z＝0，该平面记为π₁；

在图像平面上，通过直线提取处理算法，提取线激光在图像平面上的直线方程：A₁x+B₁y+C₁＝0，得到A₁、B₁、C₁；

则线激光在相机坐标系下的直线方程为(空间方程)：

求解直线与光心确定的平面，建立平面束方程，有A₁X+B₁Y+C₁+L(Z-f')＝0，代入光心坐标(0,0,0)，解得则该平面方程为：

该平面记为π₂；根据空间几何定理，两平面交线的方向矢量等于两平面法矢量的叉积，则线激光平面与该状态下靶标平面交线k₁的方向矢量为：

其中，r₁₆、r₁₇、r₁₈已通过P4P算法得到，f′为镜头焦距，则可计算得l₁₁、m₁₁、n₁₁数值；

下面求取线激光平面与该状态下靶标平面交线上的一点，联立π₁、π₂：

可取x₁＝0，可得到

其中，t_1z通过P4P算法解算得到；

将上述得到的直线方向矢量与直线上一点坐标(x₁,y₁,z₁)带入直线方程的标准形式，即可得到交线k₁的标准形式方程为

第二步：任意变换靶标的姿态，保证靶标四点与投射的线激光都在相机成像区域内，重复第一步的计算过程得到交线k₂的方程为：

同样，计算得l₁₂、m₁₂、n₁₂数值；

联合式(1)和式(2)，则可得线激光平面的法矢量：

将得到的l₁₁、m₁₁、n₁₁和l₁₂、m₁₂、n₁₂代入式(3)即可得到l,m,n的数值，通过以上计算过程，得到的平面上一点(x₁,y₁,z₁)与平面的法矢量则可以得到线激光在相机坐标系下的光平面方程：

lX+m(Y-y₁)+n(Z-z₁)＝0(4)

二、求解具有平行线特征的目标空间三维姿态；

将具有平行线特征的被测目标物体放置到单目相机成像区域内，架设位置保证相机能够对目标成一定大小的清晰图像，对相机的架设位置无特殊要求，线激光投射到被测目标物体上，且线激光不与平行线特征平行，下面对被测目标物体在相机坐标系下的三维姿态进行求解；

第一步：求解投射到被测目标平面上的线激光在相机坐标系下的方向矢量；

将前一步标定得到的线激光相机坐标系平面方程lX+m(Y-y₁)+n(Z-z₁)＝0整理为一般形式，即：A₀X+B₀Y+C₀Z+D₀＝0，其中，A₀＝l、B₀＝m、C₀＝n、D₀＝-m×y₁-n×z₁；通过图像处理算法，提取投射到被测目标平面上的线激光在相机图像平面上的方程为：A₀'x+B₀'y+D₀'＝0，重复求取平面方程π₂的解算过程，可以得到线激光照射到被测目标表面的直线，相机光心平面在相机坐标系下的平面方程为提取得到A₀'、B₀'、D₀'；

不难证明，两平面交线的方向矢量等于两平面法向矢量的叉积，则线激光照射到被测目标表面的直线的空间方向矢量为：

将A₀'、B₀'、D₀'和A₀、B₀、C₀代入式(5)，得l₁,m₁,n₁。

第二步求解被测目标的平行双直线在相机坐标系下的方向矢量；

在被测目标长度方向边缘轮廓分别设为L₁和L₂，L₁和L₂平行，相机所成图像经计算机处理后，即可得到平行线指向在相机坐标系中的表述；在被测目标物体表面上建立世界坐标系O_w-X_wY_wZ_w，Y_w轴与直线L₁和L₂平行，Z_w垂直于L₁和L₂所确定的平面，X_w轴方向符合右手定则，如图3所示。

对物空间的两条或多条平行线，此处以两条平行线进行说明，多条平行性原理类似，物空间各线所成图像为物空间直线，光心O所成平面与图像平面的交线如图2所示，物空间直线L₁和L₂所成图像为L₁′和L₂′，L₁、L₁′、O共面，L₂、L₂′、O共面；物像几何关系概括为：平面L₁O和平面L₂O相交，O点在交线上，L₁′在平面L₁O上，L₂′在平面L₂O上。

在图4中，描述了物空间平行线L₁与光心所成平面和L₂与光心所成平面的交线、物直线、像直线之间的几何关系；容易证明：在两个相交平面上，若存在两条直线平行，则这两条直线必平行于两平面的交线。由于L₁和L₂为物空间的平行线，其方向矢量相同，设为L₁O和L₂O平面的交线方向矢量也为平面L₁O和平面L₁′O为同一平面，平面L₂O和平面L₂′O为同一平面，故通过图像中像直线L₁′和L₂′以及光心O，可求得交线，即可求得平行线L₁和L₂的方向矢量具体推导如下：

经过图像处理和目标边缘轮廓提取，在图像坐标系oxy中，L₁′和L₂′的平面方程表达式分别为：

A₃x+B₃y+D₃＝0(6)

A₂x+B₂y+D₂＝0(7)

在图像坐标系oxy中，在图像平面上，通过直线提取处理算法，得A₃、B₃、D₃、A₂、B₂、D₂；

则其在相机坐标系下的空间直线方程分别为：

求解直线L₁与光心确定的平面，建立平面束方程，有A₃X+B₃Y+D₃+L(Z-f')＝0，代入光心坐标(0,0,0)，解得则该平面方程为：

同理，直线L₂与光心确定的平面方程为：

则平面交线方程为：

根据空间几何定理，两平面交线的方向矢量等于两平面法矢量的叉积，根据式(12)，求得交线方向矢量为：

这也是L₁和L₂直线方向矢量的表达式，将求得的A₃、B₃、D₃、A₂、B₂、D₂代入式(13)，得到平行双直线在相机坐标系下的方向矢量；

第三步通过坐标系的旋转变换求解姿态角与方位角；

平行线L₁、L₂与结构光均在世界坐标系Z_W＝0平面内，故Z_W轴在相机坐标系中的方向矢量为：

相机坐标系与世界坐标系间的转换关系为：

其中，θ为俯仰角，γ为侧倾角；

代入：(X_c,Y_c,Z_c)^T＝(l,m,n)^T，(X_W,Y_W,Z_W)^T＝(0,0,1)^T，则可求解出θ和γ分别为：

考虑再绕世界坐标系Z_w的方位角，则有：

由于双平行线与Y_W轴方向平行，故在世界坐标系下，双平行线的方向矢量可表示为(0，1，0)^T，代入(X_c,Y_c,Z_c)^T＝(l₂,m₂,n₂)^T，(X_W,Y_W,Z_W)^T＝(0,1,0)^T，记：

则：

可解得方位角

通过以上求解，就可以得到具有平行线特征或者表面可刻画平行线标识的物体，在相机坐标下的三维姿态信息。

为验证本发明的可行性，搭建原理验证平台，设计和加工了被测目标平台，目标平台固装在三维姿态旋转台上，以调整变换目标台的三维姿态，并将三维姿态旋转台的姿态值作为真值，检测测量系统的三维姿态测量精度，被测目标物体上表面如图6所示，以误差来验证本发明测量方法的正确性，说明本发明测量方法有效；在本实施例下，本发明的俯仰角θ和横滚角γ测量误差在0.03°以内，方位角测量误差在0.02以内，无粗大误差。