CN102829769A - 基于结构光视觉传感器的集装箱位置姿态测量方法 - Google Patents

Abstract

本发明涉及一种利用结构光视觉传感器实现集装箱吊装过程中吊具与集装箱相对位置姿态的测量方法。本发明采用的技术方案是：在吊具上安装能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，吊装过程中向集装箱表面投射结构光并拍摄图像，通过处理图像获取集装箱边缘、角孔和/或顶点等结构特征，利用特征坐标解算集装箱相对吊具的位置、姿态关系，用于集装箱吊装的自动控制。本方法可实现集装箱自动吊装，也可用于其他物体的相对位置姿态关系的高精度测量。

Description

基于结构光视觉传感器的集装箱位置姿态测量方法

技术领域

本发明涉及摄像测量、机械自动化领域，特别是一种利用结构光视觉传感器实现集装箱吊装过程中吊具与集装箱相对位置姿态的测量方法。

技术背景

在集装箱的移动运输过程中，需要利用吊具进行集装箱的吊装作业。目前吊装作业均由人工操作，对操作人员技术及经验要求较高，特别是吊具的对销过程需将转锁连接集装箱的角件，作业难度较高，作业效率较低。为了提高集装箱移动、运输的作业效率，降低工作人员的劳动强度，促进吊装作业的自动化水平，需要自动测量集装箱相对吊具的位置姿态关系，进而实现吊具的自动控制。

本发明提出了一种基于结构光视觉传感器的集装箱位置姿态测量方法，用于吊装作业中集装箱相对吊具的位置、姿态关系的测量，为吊装自动控制提供测量信息。本发明的原理和方法也可应用于其他物体的位置姿态测量场合。

发明内容

本发明是：在集装箱吊装设备(正面吊、堆高机或门式起重机等)的吊具上安装能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，该传感器包含一个或多个结构光投射器（点阵、线阵、网格结构光或其组合等）、一个或多个摄像机和数据处理器，吊装过程中，结构光视觉传感器向集装箱表面投射结构光，同时拍摄图像，通过处理图像获取集装箱边缘、角孔和/或顶点结构特征，测量结构特征的坐标，进而计算集装箱相对吊具的位置、姿态关系。

1 本发明的基于结构光视觉传感器的集装箱位置姿态测量方法的实施过程为：

（1）构建能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，包含一个或多个结构光投射器（点阵、线阵、网格结构光或其组合等）、一个或多个摄像机、数据处理器等。将结构光视觉传感器安装在吊具上，使吊装过程中可对集装箱表面投射结构光并成像；

（2）标定结构光视觉传感器与吊具的相对位置姿态关系，如附图2所示；

（3）吊装过程中结构光视觉传感器向集装箱表面投射结构光并拍摄图像；

（4）处理图像，利用结构光的点、线、面等信息获取集装箱几何结构特征（如集装箱的边缘、顶点、角孔等）；

（5）根据集装箱结构特征的图像坐标，计算其在结构光视觉传感器坐标系的坐标方程表示，并转换到吊具坐标系；

（6）根据集装箱结构特征的测量结果计算集装箱相对吊具的位置、姿态关系，用于吊装控制。

2 本发明的基于结构光视觉传感器的集装箱位置姿态测量方法

（1）集装箱结构特征坐标方程计算方法

处理图像，提取集装箱的结构特征的图像坐标方程，根据摄像测量原理建立结构特征在结构光视觉传感器坐标系内的表示。设去除像差后集装箱某特征点图像坐标为（x_i,y_i），则其坐标(X_i,Y_i,Z_i)满足共线方程

$\left\{\begin{array}{c}\frac{x_i-C_x}{F_x}=-\frac{a_1(X_i-X_c)+b_1(Y_i-Y_c)+c_1(Z_1-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\\ \frac{y_i-C_y}{F_y}=-\frac{a_2(X_i-X_c)+b_2(Y_i-Y_c)+c_2(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\end{array}\right.---\left(1\right)$

其中F_x、F_y为像机等效焦距，C_x、C_y为像机像主点坐标，(X_c,Y_c,Z_c)为像机光心在结构光视觉传感器坐标系W-XYZ下的坐标。a_i、b_i、c_i为摄像机在结构光视觉传感器坐标系的旋转矩阵R₁的各元素：

$R_1=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ a_2& b_2& c_2\\ a_3& b_3& c_3\end{array}\right]---\left(2\right)$

上述特征点同时在结构光投影面或线上，满足结构光投影方程：

$f(X_i,Y_i,Z_i)=0---\left(3\right)$

联立方程(1)和(3)，可求解得到特征点坐标(X_i,Y_i,Z_i)，该坐标为特征点在结构光视觉传感器坐标系的表示。

假设结构光视觉传感器坐标系W-XYZ与吊具坐标系W＇-X＇Y＇Z＇的相对位移、姿态关系分别为T₂,R₂，则特征点坐标在吊具坐标系的表示为

$\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]=R_2(\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right]-T_2)\text{---}\left(4\right)$

类似地，还可以求解特征线、特征面等集装箱结构特征在吊具坐标系W＇-X＇Y＇Z＇内的坐标方程表示（如附图2所示）。

（2）集装箱位置姿态计算方法

设集装箱上某特征点在吊具坐标系的坐标为(X＇_i,Y＇_i,Z＇_i)，且其在集装箱坐标系W＂-X＂Y＂Z＂的坐标为(X＂_i,Y＂_i,Z＂_i)。假设吊具坐标系W＇-X＇Y＇Z＇与集装箱坐标系W＂-X＂Y＂Z＂的相对位移、姿态关系分别为T,R，则有

$\left[\begin{array}{c}{X}_i^{\′\′}\\ Y_i^{\′\′}\\ Z_i^{\′\′}\end{array}\right]=R(\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]-T)\text{---}\left(5\right)$

根据多个特征点、线或面的不同表示，可联立求解得到T,R(如附图3所示)。

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

本发明利用结构光视觉传感器，通过图像处理提取集装箱边缘、角孔和/或顶点等几何结构特征，根据结构光视觉传感器测量模型和结构光视觉传感器与吊具的相对位置姿态关系，求解特征在吊具坐标系的坐标方程表示，进而计算得到集装箱相对吊具的位置姿态关系，可实现集装箱自动吊装，也可用于其他物体的相对位置姿态关系的高精度测量，具有广泛、重要的应用前景。

附图说明

图1基于结构光视觉传感器实现集装箱位置姿态测量示意图，

图2结构光视觉传感器坐标系与吊具坐标系的关系，

图3吊具坐标系与集装箱坐标系的关系，

图4结构光视觉传感器的数学模型。

具体实施方式

如图1，基于结构光视觉传感器实现集装箱位置姿态测量系统由安装在集装箱搬运设备上的能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器向集装箱表面投射结构光并拍摄图像，通过处理图像获取集装箱边缘、角孔和/或顶点等结构特征，利用特征解算集装箱相对吊具的位置、姿态关系，结构光视觉传感器由摄像机、结构光投射器、数据处理器等构成，其中摄像机用于对投射有结构光的集装箱成像，数据处理器用于图像分析和坐标解算。

如图2，结构光视觉传感器和吊具固定后，结构光视觉传感器坐标系与吊具坐标系的相对位置姿态关系T₂,R₂的标定，有多种公知的方法，具体可参照已有技术的摄像测量、摄影测量、计算机视觉的相关内容。

如图3，通过集装箱同一结构特征在吊具坐标系和集装箱坐标系中的坐标表示，可得到吊具坐标系和集装箱坐标系的相对位置姿态关系T,R，从而得到集装箱相对吊具的位置姿态关系。

如图4，在实时图上选取结构光视觉传感器投射出的结构光与集装箱表面相交形成变形特征线上的拐点或断点作为特征点，利用已知的摄像机参数和经过标定的结构光视觉传感器与吊具的相对位置姿态关系以及光平面的方程，求解成像模型，即可得到特征点在结构光视觉传感器坐标系中的坐标。并通过坐标变换得到特征点在吊具坐标系中的坐标。

本发明的基于结构光视觉传感器的集装箱位置姿态测量方法的实施过程如下：

（1）构建能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，包含一个线阵结构光投射器、一个摄像机和数据处理器等。将结构光视觉传感器安装在吊具上，使吊装过程中可对集装箱表面投射结构光并成像；

（2）利用摄像测量、计算机视觉等领域的已有标定方法标定结构光视觉传感器与吊具的相对位置姿态关系。

（3）吊装过程中结构光视觉传感器向集装箱表面投射线阵结构光并拍摄图像，在图像上识别线阵结构光与集装箱表面相交形成变形特征线上的拐点或断点作为特征点，如图4所示，结构光视觉传感器发射出的光平面π_i与集装箱表面相交，产生一个反映集装箱表面轮廓的特征线，棱边和角件上孔的边缘与光平面的交点形成特征线上的拐点或断点。通过图像处理可将这些特征点在像平面的坐标提取出来。

（4）利用摄像机参数和结构光投射器投射出每束线的光平面的方程，求解特征点在结构光视觉传感器坐标系内的坐标，根据结构光视觉传感器与吊具的相对位置姿态关系将特征点坐标转换到吊具坐标系内表示。

如附图4所示，π_i为结构光投射器投射的线束光平面，π_c为图像平面。结构光视觉传感器坐标系为W-XYZ，O为图像主点(光轴与像平面的交点)，图像坐标系为I-xy，吊具坐标系为W＇-X＇Y＇Z＇。

特征点即在结构光束平面上，又在连接光心和像点坐标的射线上，两者相交即可确定特征点坐标。根据结构光视觉传感器坐标系W-XYZ到吊具坐标系W＇-X＇Y＇Z＇中的3×3的旋转矩阵R₂和3×1的平移向量T₂，可将特征点坐标转换到吊具坐标系表示。

设摄像机对集装箱连续成像，采集到的实时图像为I。在实时图I上选取结构光与集装箱表面棱边的交点或与角孔的交点作为特征点，设第i个特征点的图像坐标为(x_i,y_i)，其在结构光视觉传感器坐标系的坐标为(X_i,Y_i,Z_i)，由中心投影成像模型，建立以下方程：

$\left\{\begin{array}{c}\frac{{\tilde{x}}_i-C_x}{F_x}=-\frac{a_1(X_i-X_c)+b_1(Y_i-Y_c)+c_1(Z_1-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\\ \frac{{\tilde{y}}_i-C_y}{F_y}=-\frac{a_2(X_i-X_c)+b_2(Y_i-Y_c)+c_2(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\end{array}\right.---\left(6\right)$

其中F_x、F_y为像机等效焦距，C_x、C_y为像机像主点坐标，(X_c,Y_c,Z_c)为摄像机光心在结构光视觉传感器坐标系W-XYZ下的坐标。a_i、b_i、c_i为摄像机在结构光视觉传感器坐标系的旋转矩阵R₁的各元素：

$R_1=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ a_2& b_2& c_2\\ a_3& b_3& c_3\end{array}\right]---\left(7\right)$

上述特征点同时在结构光投影面或线上，同时满足结构光光束平面方程：

$A_{i}x+B_{i}y+C_{i}z+D_i=0---\left(8\right)$

其对应的法向矢量为：n_i=[A_i,B_i,C_i]^T。上式中A_i、B_i、C_i、D_i均可通过对光平面的标定得到。

联立方程式(6)、(8)，得：

$\left\{\begin{array}{c}\frac{{\tilde{x}}_i-C_x}{F_x}=-\frac{a_1(X_i-X_c)+b_1(Y_i-Y_c)+c_1(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\\ \frac{{\tilde{y}}_i-C_y}{F_y}=-\frac{a_2(X_i-X_c)+b_2(Y_i-Y_c)+c_2(Z_i-Z_c)}{a_3(X_i-X_c)+b_3(Y_i-Y_c)+c_3(Z_i-Z_c)}\\ A_i{X}_i+B_i{Y}_i+C_i{Z}_i+D_i=0\end{array}\right.---\left(9\right)$

可求得第i个特征点在结构光视觉传感器坐标系下的坐标(X_i,Y_i,Z_i)。

利用摄像测量、计算机视觉等领域的已有标定方法标定结构光视觉传感器坐标系W-XYZ与吊具坐标系W＇-X＇Y＇Z＇的相对位移、姿态关系分别为T₂,R₂，则特征点坐标在吊具坐标系的表示为

$\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]=R_2(\left[\begin{array}{c}{X}_i^{}\\ Y_i^{}\\ Z_i^{}\end{array}\right]-T_2)\text{---}\left(10\right)$

（5）根据集装箱结构特征的测量结果计算集装箱相对吊具的位置、姿态关系。

设第i和第j个特征点都在集装箱棱边上，因此可确定集装箱棱边所在的直线方程在吊具坐标系的表示为：

$\frac{x-X_i^{\′}}{X_i^{\′}-X_j^{\′}}=\frac{y-Y_i^{\′}}{Y_i^{\′}-Y_j^{\′}}=\frac{z-Z_i^{\′}}{Z_i^{\′}-Z_j^{\′}}---\left(11\right)$

通过集装箱两条棱边所在的直线方程还可求解集装箱的顶点坐标，通过角件上孔的边缘与结构光的交点及孔的几何形状确定孔中心的坐标。

由于集装箱顶点、角孔等在集装箱坐标系的位置坐标已知，根据其在吊具坐标系内的表示可确定集装箱相对吊具的位置姿态关系，即由

$\left[\begin{array}{c}{X}_i^{\′\′}\\ Y_i^{\′\′}\\ Z_i^{\′\′}\end{array}\right]=R(\left[\begin{array}{c}{X}_i^{\′}\\ Y_i^{\′}\\ Z_i^{\′}\end{array}\right]-T)\text{,i=1,2,...---}\left(12\right)$

可确定T,R。

Claims (3)

1.基于结构光视觉传感器的集装箱位置姿态测量方法，其特征是：

在集装箱吊装设备的吊具上安装能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，该传感器包含一个或多个结构光投射器、一个或多个摄像机和数据处理器，吊装过程中，结构光视觉传感器向集装箱表面投射结构光，同时拍摄图像，通过处理图像获取集装箱边缘、角孔和/或顶点结构特征，测量结构特征的坐标，进而计算集装箱相对吊具的位置、姿态关系；

包括如下过程：

步骤1、构建能够敏感集装箱边缘和/或角孔信息的结构光视觉传感器，包含一个或多个结构光投射器、一个或多个摄像机和数据处理器，将结构光视觉传感器安装在吊具上，使吊装过程中可对集装箱表面投射结构光并成像； 

步骤2、标定结构光视觉传感器与吊具的相对位置姿态关系；

步骤3、吊装过程中结构光视觉传感器向集装箱表面投射结构光并拍摄图像； 

步骤4、处理图像，利用结构光的点、线、面等信息获取集装箱几何结构特征；

步骤5、根据集装箱结构特征的图像坐标，计算其在结构光视觉传感器坐标系的坐标方程表示，并转换到吊具坐标系；

步骤6、根据集装箱结构特征的测量结果计算集装箱相对吊具的位置、姿态关系，用于吊装控制；

该基于结构光视觉传感器的集装箱位置姿态测量方法包括集装箱结构特征坐标方程计算和集装箱位置姿态计算。

2.根据权利要求1所述的基于结构光视觉传感器的集装箱位置姿态测量方法，其特征在于，集装箱结构特征坐标方程计算，具体过程为：

提取集装箱的结构特征的图像坐标方程，根据摄像测量原理建立结构特征在结构光视觉传感器坐标系内的表示，设去除像差后集装箱某特征点图像坐标为                                                

，则其坐标

满足共线方程

              (1)

其中、

为像机等效焦距，

、

为像机像主点坐标，为像机光心在结构光视觉传感器坐标系W-XYZ下的坐标，

为摄像机在结构光视觉传感器坐标系的旋转矩阵

的各元素：

                         (2)

上述特征点同时在结构光投影面或线上，满足结构光投影方程：

                          (3)

联立方程(1)和(3)，可求解得到特征点坐标

，该坐标为特征点在结构光视觉传感器坐标系的表示；

假设结构光视觉传感器坐标系W-XYZ与吊具坐标系

的相对位移、姿态关系分别为

，则特征点坐标在吊具坐标系的表示为

                      (4)。

3.根据权利要求1所述的基于结构光视觉传感器的集装箱位置姿态测量方法，其特征在于，集装箱位置姿态计算具体为：

设集装箱上某特征点在吊具坐标系的坐标为

，且其在集装箱坐标系

的坐标为

，假设吊具坐标系

与集装箱坐标系

的相对位移、姿态关系分别为，则有

                   (5)

根据多个特征点、线或面的不同表示，可联立求解得到

。

Images