CN102788572A - 一种工程机械吊钩姿态的测量方法、装置及系统 - Google Patents

Abstract

本发明公开了一种工程机械吊钩姿态的测量方法、设备及系统，该方法包括：不断获取至少两个相机对吊钩上标记点同时拍摄的各图像，分别确定各图像中像素点的三维坐标；根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含标记点的图像；对仅包含标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。本发明不需要在吊钩上安装任何测量仪器和设备，就能够实现对吊钩姿态的测量，测量方式简单、方便、实时性较好且测量精度较高。

Description

一种工程机械吊钩姿态的测量方法、装置及系统

技术领域

本发明涉及工程测量技术领域，尤指一种工程机械吊钩姿态的测量方法、装置及系统。

背景技术

在工程机械的起吊作业过程中，吊钩不仅要发生垂直方向的移动，还有横向的移动，由于惯性或者外力作用，通过钢丝绳悬挂在起重机臂上部的吊钩及货物会形成相应的摆动，因此吊钩的运动是三维运动轨迹。

现有对吊钩姿态的测量方法有多种，例如对待测量的吊钩分别建立第一坐标系和第二坐标系，其中第二坐标系与吊钩的运动相关，第一坐标系与吊钩的运动无关，从而使将吊钩的姿态参数的改变反映在两个坐标系之间位置关系的改变上，然后再用角度测量仪测得两个坐标系坐标轴之间的角度关系，用加速度测量计获得吊钩在第二坐标系预定方向的加速度，根据角度关系与加速度获得吊钩在第一坐标系相应坐标轴上的加速度；最后根据吊钩在第一坐标系坐标轴上的加速度获得吊钩在第一坐标系中的姿态参数。

这种方式由于针对每个待测量的吊钩，都需要分别布置一套角速度测量仪和加速度测量仪，布置传感器的工作量较大，测量成本较高的问题。

例如还有一种方式是在工程机械的底部安装一个摄像机和标志杠，在吊钩上设置一标识物，摄像机连接有图像处理器，摄像机采集标识物信息，图像处理器对摄像机采集的标识物的信息进行处理并计算出标识物的具体位置，进而得到吊钩的位置。这种方式还是一种二维测量方式，在吊钩同时发生横向、垂直方向的运动时，这种测量方式准确度较低。

发明内容

本发明提供一种工程机械吊钩姿态的测量方法、装置及系统，用以实现对工程机械的吊钩进行非接触式测量，从而避免现有测量方式中布置传感器的工作量较大，测量成本较高，以及测量方式准确度较低的问题。

本发明实施例提供的工程机械吊钩姿态的测量方法，包括：

不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的各图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；

根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；

对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；

根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

本发明实施例提供的工程机械吊钩姿态的测量设备，包括：

获取模块，用于不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的各图像；

三维坐标确定模块，用于分别确定各图像中像素点在世界坐标系中的三维坐标；

拼接模块，用于根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

提取模块，用于针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；

测量模块，用于对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

本发明实施例提供一种工程机械吊钩姿态的测量系统，包括：

至少两个相机，用于对工程机械吊钩上标记点同时进行拍摄；

工程机械吊钩姿态的测量设备，用于不断获取所述至少两个相机对工程机械吊钩上标记点同时拍摄的各图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

本发明实施例的有益效果包括：

本发明实施例提供的工程机械吊钩姿态的测量方法、装置及系统，使用至少两个相机对工程机械吊钩上的标记点进行拍摄，并获取至少两个相机对吊钩上的标记点拍摄的各图像，分别确定各图像中像素点在世界坐标系中的三维坐标；根据各图像中各像素点的三维坐标，将至少两个相机同时拍摄的图像拼接成一帧图像，这样，在拼接后的各图像中提取出标记点的图像，根据标记点的运动状态参数，实现对工程机械吊钩姿态的测量。本发明实施例使用非接触测量的方式，不需要在吊钩上安装任何测量仪器和设备，就能够实现对吊钩姿态的测量，并且，利用吊钩上标记点图像分析的方法获取吊钩的运动参数，测量方式简单、方便，且测量精度较高，另外，由于可以实时获取相机拍摄的图像，并对图像进行处理获取吊钩上标记点的运动参数，这种测量方式，实时性很好，可以及时快速地获知吊钩的工作姿态变化情况。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例提供的工程机械吊钩姿态的测量方法的流程图；

图2为本发明实施例提供的相机标定过程中图像坐标和世界坐标系的示意图；

图3为本发明实施例提供的工程机械吊钩姿态的测量设备的结构示意图；

图4为本发明实施例提供的工程机械吊钩姿态的测量系统的结构示意图之一；

图5为本发明实施例提供的工程机械吊钩姿态的测量系统的结构示意图之二。

具体实施方式

下面结合说明书附图，对本发明实施例提供的一种工程机械吊钩姿态的测量方法、装置及系统的具体实施方式进行说明。

本发明实施例提供的工程机械吊钩姿态的测量方法，如图1所示，包括如下步骤：

S101、不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；

S102、根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

S103、针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含标记点的图像；

S104、对仅包含标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；

S105、根据测算出的标记点运动状态参数的变化，对吊钩姿态进行测量。

下面分别对上述各步骤分别进行详细说明。

上述步骤S101中，由于吊钩上的各标记点的运动情况能够反映整个吊钩的姿态变化情况，为了方便对吊钩的姿态进行测量，可以通过对吊钩的各个标记点的运动参数进行测量的方式来实现对吊钩姿态的测量，又由于吊钩的体积较大，在具体实施时，需要使用至少两个相机分别对吊钩上的各标记点进行拍摄，然后将拍摄的图像拼接起来，实现对整个吊钩姿态的非接触性测量。

上述步骤S101中，由于至少两个相机分别对吊钩的标记点进行同时拍摄，因此需要不断获取至少两个相机在相同时刻拍摄的图像，并且在步骤S102中，需要将至少两个相机拍摄的图像进行拼接，从而得到整个吊钩的图像。

本发明实施例提供的工程机械吊钩姿态的测量方法中，可根据实际测量的需要，在吊钩上的相应位置设置对应的标记点，标记点的数量可以是1个或多个，吊钩上的各标记点可采用十字丝、圆形、对角圆等各种形状的标记点。

上述步骤S101中，可以通过下述方式确定各像素点的三维坐标：

根据各图像中的像素点在图像坐标系的二维坐标，以及预先确定的图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，分别将各图像中像素点的二维坐标转换为对应的三维坐标。

其中，图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，可以预先通过相机标定过程来确定。

相机标定过程可采用非接触式测量中的相机标定方法，非接触式测量是基于双目立体视觉测量原理，该原理是基于视差，根据三角法原理进行三维信息的获取，由两个相机的图像平面和标定物之间构成一个三角形。首先以标定物为参照物，标定出一个相机的内外参数，然后以该相机和标定物为参照物，标定出另一个相机的内外参数。

以两个相机为例，具体标定过程如下：

(1)设置标定物，具体包括：打印一张模板，模板以固定间距组成的若干个十字丝组成，并将该模板并贴在一个平板上，将贴有模板的平板放置到两个相机的共同视场中，其位置信息已知。

(2)两个相机从不同角度拍摄若干张(大于或等于3张)模板图像；

(3)检测每幅图像中的目标点(例如十字丝中心)；

(4)根据标定公式，分别获得两个相机的内部参数以及外部参数；

一般来说，相机的内外参数包括：比例系数、有效焦距、主点、平移矩阵和旋转矩阵。在相机标定方法中，可先根据标定物，利用标定公式，标定一个相机，然后根据标定物，以及该相机来标定另一个相机。其中，标定公式即计算上述内外参数的过程，具体来说包括下述四个方面：(1)旋转矩阵 $R=\left[\begin{array}{c}{r}_1{r}_2{r}_3\\ r_4{r}_5{r}_6\\ r_7{r}_8{r}_9\end{array}\right],$ 平移矩阵 $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$ 的计算，具体计算过程属于现有技术，在此不再赘述；(2)确定图像采集设备主点，主点是图像的中心，可以近似为图像的中心坐标值；(3)图像采集设备比例系数的标定：以十字形状标记为例，事先用尺量好十字标记的长和宽，然后在计算机上计算十字图像长和宽所占的像素点，最后换算成一个像素点代表多大的实际物理尺寸；(4)有效焦距的计算：根据成像原理，当物距远大于相距的时候，焦距F＝物距V(可以近似为镜头到物体的距离)，在相机放置时候，该数值用卷尺可以量出。

(5)根据旋转矩阵和平移转换矢量，获得两个相机共同视场中某个点的三维坐标的表达式，该表达式表征了目标点在图像中的二维坐标与该目标点的三维坐标之间的转换关系。

如图2所示，假设o-XYZ是左相机的世界坐标系，O1-X₁Y₁为左相机的图像坐标系，f₁为左相机的有效焦距；Or-x_ry_rz_r为右相机的世界坐标系，f_r为右相机的有效焦距。

由现有的相机透视变换模型可以得到：

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\mathrm{MRT}\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{r}_1{r}_2{r}_3{t}_x\\ r_4{r}_5{r}_6{t}_y\\ r_7{r}_8{r}_9{t}_z\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$ (公式1)

上式中，MRT＝[R T]；且 $R=\left[\begin{array}{c}{r}_1{r}_2{r}_3\\ r_4{r}_5{r}_6\\ r_7{r}_8{r}_9\end{array}\right],$ $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$ 分别为O-x y z坐标系与Or-x_r y_rz_r坐标系之间的旋转矩阵和平移矩阵。其中，

r₁＝cosA_YcosA_Z-sinA_YsinA_XsinA_Z

r₂＝cosA_XsinA_Z

r₃＝sinA_YcosA_Z+sinA_YsinA_XsinA_Z

r₄＝-cosA_YsinA_Z-sinA_YsinA_XcosA_Z

r₅＝cosA_XcosA_Z

r₆＝-sinA_YsinA_Z+cosA_Ysin A_XcosA_Z

r₇＝-sinA_YcosA_X

r₈＝-sinA_X

r₉＝cosA_YcosA_X

上述公式中的A_X，A_Y，A_Z分别为在世界三维坐标系下，A_X为标定过程中拍摄的目标点相对于X轴的倾斜角，A_Y为该目标点相对于Y轴的倾斜角；A_Z为该目标点相对于Z轴的倾斜角。这三个角度可以通过安装在固定相机的平台上的角度传感器测得。

较佳地，在本发明实施例中，由于所需求得的吊钩的运动变化值(也就是吊钩姿态)是相对量而不是绝对量(吊钩姿态通过同一个标记点在吊钩工作过程中三维坐标的相对变化量确定)，而平移矩阵的意义是计算世界坐标系原点移动至图像坐标系原点的平移量，相当于是做坐标平移，因此，在具体标定过程中，可以不需要按照现有平移矩阵的计算方式来计算出平移矩阵的具体数值，而将其整体直接作为零值考虑，即： $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]=0.$ 将 $T=\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]=0$ 代入上述公式1，

变为：

$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=\mathrm{MRT}\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{r}_1{r}_2{r}_3\\ r_4{r}_5{r}_6\\ r_7{r}_8{r}_9\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

在本发明实施例中，由于相机成像时不一定是水平正对着工程机械吊钩的，所以会有相对于工程机械吊钩的姿态角A_X，A_Y，A_Z，因此较重要是需要在标定过程中把目标点的姿态角反算成水平状态下的坐标。而平移矩阵只是坐标系的平移，坐标原点不同而已，在本发明实施例计算工程机械吊钩的运动变化量这种相对量时可以不需完成平移矩阵具体数值的计算，直接将其设置为0即可。

由此可以计算出像素点的三维坐标(x，y，x)表达式，即图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系为：

x＝zX₁/f₁

y＝zY₁/f₁

$z=\frac{f_1(f_r*t_x-X_r*t_z)}{X_r(r_7{X}_1+r_8{Y}_1+f_1*r_9)-f_r(r_1*X_1+r_2*Y_1+f_1*r_3)}$

其中，该像素点可为标记点上的一个像素点，以左相机采集到的一幅图片为例，在图像中有一个标记点，即可为该标记点上的一个像素点，在左相机中的二维坐标为(X₁，Y₁)。如果像素点在右相机中的二维坐标为(Xr，Yr)，那么上式中x＝zxr/fr；y＝zYr/fr，z的计算公式与上式相同。

本发明实施例并不限于使用上述标定方法，还可以使用现有技术中其他类似的基于双目立体视觉测量原理的相机标定方法来确定图像中的二维坐标与该目标点三维坐标之间的转换关系。

在上述步骤S102中，根据各图像中各像素点的三维坐标，将至少两个相机同时拍摄的图像拼接为一帧图像，拼接过程中，需要将两幅图像中具有相同三维坐标的像素点作为两幅图像重合部分，将两幅图像拼接到一起，依次类推，依次将多幅图像拼接到一起。

具体的拼接过程包括下述步骤：

对至少两个相机同时拍摄的图像分别进行几何校正；

对几何校正后的各图像进行图像配准；

将图像配准后的各图像进行融合，消除拼接痕迹；

输出拼接完成的图像。

对图像进行几何校正和图像配准以及图像融合的方法属于现有技术的范畴，在此不再详述。

在上述步骤S103中，识别并提取出吊钩上的标记点，生成仅包含标记点的图像，具体可以通过下述方式实现：

使用灰度聚类方法，根据拼接后的图像中吊钩标记点与图像背景之间的灰度差异，将拼接后的图像中的吊钩标记点的图像数据对象进行聚类；

根据将聚类后的吊钩标记点的图像数据对象，生成仅包含各吊钩标记点的图像。

聚类过程实质上就是指将图像对象的集合分组成为由类似的图像对象组成的多个类的分析过程，也就是在图像中，不断将具有相同特性(例如本发明实施例中使用的灰度)的图像归为一类，这样就可以实现图像中所有标记点的提取。

相机采集到吊钩和背景的图片后，由于打在吊钩上的标记点可以使用对光敏感的颜色例如采用红外反射标志等，所以在采集的图像中，标记点和背景会有比较大的灰度反差。在本发明实施例中，可采用以下两个判据对目标进行聚类：一是考察聚类中心邻域内各点灰度值与整个图像的灰度平均值(mean)的差距是否大于测度阈值D1，二是相邻点之间的灰度差是否大于测度阈值D2。设定两个测度阈值D1和D2，即可完成对标记点的识别，符合条件者为标记点。

本发明实施例可以采用现有图像处理技术中的灰度聚类方法实现，在此不再赘述。

上述S103中，对拼接后的图像提取出标记点之后，生成仅包含标记点的图像，也即去除了原有拼接后的图像中除了标记点之外的那部分之后的图像。

上述S104中，对仅包含标记点的各帧图像中标记点的三维坐标，使用光流跟踪算法，测算出标记点在的运动状态参数，具体通过下述方式实现：

对仅包含标记点的图像，计算光流场，得到标记点在振动过程中的速度和角度；

在本发明实施例中，可以用光强度差的平方和(SSD)作为距离度量(或使用互相关系数作为度量)，求出不同时刻的图像区域最佳匹配。速度(u，v)定义为不同时刻的图像区域(region)的位移量d＝(dx，dy)。

$\mathrm{SSD}(i,j,d_x,d_y)=\underset{i=-n}{\overset{n}{\mathrm{\Σ}}}\underset{j=-n}{\overset{n}{\mathrm{\Σ}}}{[I_1(x+i,y+i)-I1(x+i+d_x,y+j+d_y)]}^2$

匹配块的大小为(2n+1)×(2n+1)，搜索区大小为(2N+1)×(2N+1)，-N≤dx，dy≤N。在搜索区内SSD最小(或相关系数最大)的位置就是最佳匹配，其偏离中心点(x，y)的位移量(dx，dy)即光流，在本发明中，利用加权最小二乘法可得到真实速度的一个估计：

当给定两帧顺序图像里面经过I1(x+I，y+i)和I1(x+i+dx，y+i+dy)。以图像I1中的每个像素点(x，y)为中心建立一个大小为(2n+1)×(2n+1)的相关块SSD。在图像I1(x+i+dx，y+i+dy)的对应像素点(x，y)为中心的(2N+1)×(2N+1)搜索区w s内寻找最佳匹配位置(N＞n)。其偏离中心点(x，y)的位移量(dx，dy)为了找到最佳匹配，可以对定义在d上的相似度量，如归一化互相关系数进行最大化(模板匹配法)；也可对某一距离度量，如光强度差的平方和(SSD)进行最小化。

$u_c=\frac{\mathrm{\ΣR}(d_x,d_y)d_x}{\mathrm{\ΣR}(d_x,d_y)},$ $v_c=\frac{\mathrm{\ΣR}(d_x,d_y)d_y}{\mathrm{\ΣR}(d_x,d_y)};$

u_c是C点X方向速度，v_c是C点Y方向速度。

根据计算出的速度值和角度值，得出吊钩上标记点的平移运动速度矢量和绕轴旋转速度矢量，根据吊钩上标记点的三维坐标、平移运动速度矢量和绕轴旋转速度矢量，计算标记点在吊钩工作过程中的运动矢量。

吊钩上标记点运动可以分解为三维平移和三维旋转，设三维平移运动的速度矢量为：T，绕轴旋转速度矢量为：w；

根据现有技术中的小孔成像模型，某个着色节点的三维坐标为P(X，Y，Z)的运动矢量P’(U_CX，U_CY，U_CZ)，其中三维坐标根据以下公式求得：

x＝zX1/f1

y＝zY1/f1

$z=\frac{f1(\mathrm{fr}*t_x-\mathrm{Xr}*t_z)}{\mathrm{Xr}(r_{7}X1+r_8{Y1+f1*r}_9)-\mathrm{fr}(r_1*X1+r_2*Y1+f1*r_3)}$

$=\frac{f1(\mathrm{fr}*t_y-\mathrm{Yr}*t_z)}{\mathrm{Yr}(r_{7}X1+r_{8}Y1+f1*r_9)-\mathrm{fr}(r_1*X1+r_5*Y1+f1*r_6)}$

(U_CX和U_CY)计算出来的是在二维上水平和垂直方向的速度，通过转换后的三维坐标算出目标点在Z方向的，为：

$\left[\begin{array}{c}{u}_{\mathrm{cx}}\\ u_{\mathrm{cy}}\\ u_{\mathrm{cz}}\end{array}\right]=\mathrm{\ω}\×\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]+T$

其中，u_cx，u_cy，X，Y，Z已知。因此，计算出w＝(u_cx-u_cy)/x-y；

T＝u_xc-[(u_cx-u_cy)/x-y]*x；因此很容易计算出u_cz的值。

因此，上述步骤S105中，根据不同时刻吊钩上每个标记点的运动矢量的变化，就可以实现对整个吊钩不同时刻姿态参数(例如瞬时速度、加速度等参数)的测量。

进一步地，在本发明实施例提供的工程机械吊钩姿态的测量方法在具体实施过程中，由于工程机械的作业环境复杂，吊钩难免会有被其他物体遮挡的情况发生。例如若出现连续多个拼接后的图像，提取出吊钩上的标记点失败的情况，则可确定吊钩的标记点发生遮挡；

此时，可以使用卡尔曼滤波算法，根据最近的提取成功的拼接后的图像中标记点所在的位置，依次预测出发生遮挡的多个拼接后的图像中标记点的三维坐标，

根据预测出的三维坐标，继续对吊钩姿态进行测量。

卡尔曼滤波(Kalman Filter)算法的大体过程简介：

起重机吊钩上目标点成像时，当吊钩距离相机较远时，目标所成的像较小或者发生转动时，标记点的图像会发生大小变化，并且由于臂架转动导致的成像角度发生变化，导致标记的目标形状发生变化；噪声主要分两部分，主要是相机的内部噪声，主要特征是室外成像环境下，阳光亮度高，灰尘大等因素，这些噪声可以近似认为服从高斯分布或白噪声。

通过上面的拼接，将各个相机采集的图片拼接成一幅图片，各个标记点也在图片中，并且具备一个图像坐标系。由于目标运动的连续性，目标第k+1帧的位置必定在第k帧位置的某个小领域内，以连续的三帧图像邻域轨迹预测算法为例，设目标i在第k帧的位置分别为

第k，k+1，k+2帧的位置分别为

则应满足：

$x_i^{k+1}-\mathrm{\α}\≤x_i^k\≤x_i^{k+1}+\mathrm{\α}$

$y_i^{k+1}-\mathrm{\α}\≤y_i^k\≤y_i^{k+1}+\mathrm{\α}$

$x_i^{k+2}-\mathrm{\α}\≤x_i^{k+1}\≤x_i^{k+2}+\mathrm{\α}$

$y_i^{k+2}-\mathrm{\α}\≤y_i^k\≤y_i^{k+1}+\mathrm{\α}$

其中α为领域大小，该大小根据根据精度确定，如果现场干扰物多，则该值适当取大，增加鲁棒性。如果满足上式则判断目标i为真实目标点，从此时开始记录下目标位置否则认为是虚假目标点予以删除，以此类推，继续找第k+3帧及以后帧的目标。

如果此时连续几帧图片都没有跟踪上目标点，这是利用卡尔曼滤波算法进行跟踪估计。在本方案中利用如下：

1)首先对第k帧图片里面的所有目标点区域利用领域轨迹判断在第k+1帧中是否存在相应的目标点位置，如果只存在一个目标位置则更新目标位置若不存在目标位置判定，则目标丢失；

然后对第k+1帧中存在的目标点在第k帧中寻找匹配点，如果有目标点不存在相应的匹配点，则认为该目标是新增目标并记录其位置信息。

2)如果只存在一个新目标位置信息(当前目标点在图像坐标系下的x，y坐标)，则利用此刻新的位置进行该目标运动轨迹的卡尔曼(kalman)估计，进一步修正滤波中的状态估计值。

3)如果丢失目标，则用kalman滤波预测出目标在第k+1帧中的位置，同时更新目标位置，并标记为预测值。连续预测次数不能大于设定值T(该值＜1/相机帧率)，否则由于长时间没有目标观测值的修正，如果工程机械臂架出现加速或转弯会使预测值于真实值直接的误差越来越大，最终完全偏离真实轨迹。所欲当连续预测次数等于T时，认为目标已经完全丢失，抛弃目标，停止预测。

4)综合上述两种情况，得到第k+1帧的实际目标数量和位置。

最后，上面计算出的标记点二维坐标全部都代入到以下公式中：

x＝zX1/f1

y＝zY1/f1

$z=\frac{f1(\mathrm{fr}*t_x-\mathrm{Xr}*t_z)}{\mathrm{Xr}(r_{7}X1+r_{8}Y1+f1*r_9)-\mathrm{fr}(r_1*X1+r_2*Y1+f1*r_3)}$

$=\frac{f1(\mathrm{fr}*t_y-\mathrm{Yr}*t_z)}{\mathrm{Yr}(r_{7}X1+r_8{Y1+f1*r}_9)-\mathrm{fr}(r_1*X1+r_5*Y1+f1*r_6)}$

求出吊钩的各个目标点的三维坐标。

卡尔曼滤波器是一种效果较优的信息处理器。首先要利用系统的过程模型来预测下一状态的系统。假设现在的系统状态是K，根据系统的模型，可以基于系统的上一状态而预测出现在的状态。

这样，本发明实施例可以利用现有的卡尔曼滤波器，依次确定标记点被遮挡的当前帧图像的前1帧、前2帧......是否发生遮挡，直到找到最近的未发生遮挡的那一帧，然后根据找到的最近的未发生遮挡的那一帧图像中标记点的位置，预测出发生遮挡的下一帧中标记点的位置，然后根据预测出的下一帧中标记点的位置，继续预测该下一帧图像的下一帧图像中标记点的位置，依次类推，直至将发生遮挡的各帧图像中标记点的位置都预测完成为止。

基于同一发明构思，本发明实施例还提供了一种工程机械吊钩姿态的测量设备和系统，由于这些设备和系统所解决问题的原理与前述工程机械吊钩姿态的测量方法相似，因此该设备和系统的实施可以参见前述方法的实施，重复之处不再赘述。

具体地，本发明实施例提供的一种工程机械吊钩姿态的测量设备，如图3所示，包括：

获取模块301，用于不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的各图像；

三维坐标确定模块302，用于分别确定各图像中像素点在世界坐标系中的三维坐标；

拼接模块303，用于根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

提取模块304，用于针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含标记点的图像；

测量模块305，用于对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

进一步地，上述三维坐标确定模块302，具体用于根据各图像中的像素点在图像坐标系的二维坐标，以及预先确定的图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，分别将各图像中像素点的二维坐标转换为对应的三维坐标。

进一步地，本发明实施例提供的工程机械吊钩姿态的测量设备，如图3所示，还可以包括：标定模块306，用于对布置在该吊钩上标记点前方的至少两个相机分别进行标定，获取各相机的内外参数；根据各相机的内外参数中的旋转矩阵和平移转换矢量，确定图像坐标系下二维坐标转换成世界坐标系下三维坐标的表达式。

进一步地，上述拼接模块303，具体用于对至少两个相机同时拍摄的图像分别进行几何校正；对几何校正后的各图像进行图像配准；将图像配准后的各图像进行融合，消除拼接痕迹；输出拼接完成的图像。

进一步地，上述提取模块304，具体用于使用灰度聚类方法，根据拼接后的图像中吊钩标记点与图像背景之间的灰度差异，将拼接后的图像中的吊钩标记点的图像数据对象进行聚类；根据将聚类后的吊钩标记点的图像数据对象，生成仅包含各吊钩标记点的图像。

进一步地，上述测量模块305，具体用于对该仅包含标记点的图像，计算光流场，得到标记点在吊钩运动过程中的速度和角度；根据计算出的速度和角度，得出标记点的平移运动速度矢量和绕轴旋转速度矢量，根据标记点的三维坐标、所述平移运动速度矢量和绕轴旋转速度矢量，计算标记点在振动过程中的运动矢量。

本发明实施例提供的一种工程机械吊钩姿态的测量系统，如图4所示，包括：

至少两个相机401，用于对工程机械吊钩上标记点同时进行拍摄；

工程机械吊钩姿态的测量设备402，用于不断获取至少两个相机401对工程机械吊钩上标记点同时拍摄的各图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

图5所示的是本发明实施例提供的测量系统架构的一个实例，图5中，吊钩上设置有标记点，至少两个相机对吊钩上的标记点进行拍摄，至少两个相机连接有工控机(或嵌入式系统)，该工控机(或嵌入式系统)就是用于实现非接触式测量的核心测量设备，负责对至少两个相机采集的图像进行拼接、聚类、光流场跟踪处理得到吊钩上标记点的运动状态参数，进而可以得到吊钩姿态的各种参数。

本发明实施例提供的工程机械吊钩姿态的测量方法、装置及系统，使用至少两个相机对工程机械吊钩上的标记点进行拍摄，并获取至少两个相机对吊钩上的标记点拍摄的图像，并将至少两个相机同时拍摄的图像拼接成一帧图像，对拼接后图像中像素点的二维坐标，转换成对应的三维坐标，这样，在拼接后的各图像中提取出标记点的图像，根据标记点的运动状态参数，实现对工程机械吊钩姿态的测量。本发明实施例使用非接触测量的方式，不需要在吊钩上安装任何测量仪器和设备，就能够实现对吊钩姿态的测量，并且，利用吊钩上标记点图像分析的方法获取吊钩的运动参数，测量方式简单、方便，且测量精度较高，另外，由于可以实时获取相机拍摄的图像，并对图像进行处理获取吊钩上标记点的运动参数，这种测量方式，实时性很好，可以及时快速地获知吊钩的工作姿态变化情况。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (14)

1.一种工程机械吊钩姿态的测量方法，其特征在于，包括以下步骤：

不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的各图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；

根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；

对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；

根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

2.如权利要求1所述的方法，其特征在于，分别确定各图像中像素点的三维坐标，包括：

根据各图像中的像素点在图像坐标系的二维坐标，以及预先确定的图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，分别将各图像中像素点的二维坐标转换为对应的三维坐标。

3.如权利要求2所述的方法，其特征在于，所述图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，通过下述方式确定：

对布置在所述吊钩上标记点前方的至少两个相机分别进行标定，获取各相机的内外参数；

根据各相机的内外参数中的旋转矩阵和平移转换矢量，确定图像坐标系下二维坐标转换成世界坐标系下三维坐标的表达式。

4.如权利要求1所述的方法，其特征在于，将至少两个相机同时拍摄的图像拼接为同一帧图像，包括：

对至少两个相机同时拍摄的图像分别进行几何校正；

对几何校正后的各图像进行图像配准；

将图像配准后的各图像进行融合，消除拼接痕迹；

输出拼接完成的图像。

5.如权利要求1所述的方法，其特征在于，从拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像，包括：

使用灰度聚类方法，根据拼接后的图像中吊钩标记点与图像背景之间的灰度差异，将拼接后的图像中的吊钩标记点的图像数据对象进行聚类；

根据将聚类后的吊钩标记点的图像数据对象，生成仅包含各吊钩标记点的图像。

6.如权利要求1-5任一项所述的方法，其特征在于，所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数，包括：

对所述仅包含所述标记点的图像，计算光流场，得到所述标记点在吊钩运动过程中的速度和角度；

根据计算出的速度和角度，得出所述标记点的平移运动速度矢量和绕轴旋转速度矢量，根据所述标记点的三维坐标、所述平移运动速度矢量和绕轴旋转速度矢量，计算所述标记点在振动过程中的运动矢量。

7.如权利要求1-5任一项所述的方法，其特征在于，还包括：

若连续多个拼接后的图像，提取出吊钩上的标记点失败，则确定所述标记点发生遮挡；

使用卡尔曼滤波算法，根据最近的提取成功的拼接后的图像中标记点所在的位置，依次预测出发生遮挡的多个拼接后的图像中标记点的三维坐标；

根据预测出的三维坐标，继续对吊钩姿态进行测量。

8.一种工程机械吊钩姿态的测量设备，其特征在于，包括：

获取模块，用于不断获取至少两个相机对工程机械吊钩上标记点同时拍摄的各图像；

三维坐标确定模块，用于分别确定各图像中像素点在世界坐标系中的三维坐标；

拼接模块，用于根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；

提取模块，用于针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；

测量模块，用于对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

9.如权利要求8所述的设备，其特征在于，所述三维坐标确定模块，具体用于根据各图像中的像素点在图像坐标系的二维坐标，以及预先确定的图像坐标系下二维坐标与世界坐标系下的三维坐标的转换关系，分别将各图像中像素点的二维坐标转换为对应的三维坐标。

10.如权利要求8所述的设备，其特征在于，还包括：

标定模块，用于对布置在所述吊钩上标记点前方的至少两个相机分别进行标定，获取各相机的内外参数；根据各相机的内外参数中的旋转矩阵和平移转换矢量，确定图像坐标系下二维坐标转换成世界坐标系下三维坐标的表达式。

11.如权利要求9所述的设备，其特征在于，所述拼接模块，具体用于对至少两个相机同时拍摄的图像分别进行几何校正；对几何校正后的各图像进行图像配准；将图像配准后的各图像进行融合，消除拼接痕迹；输出拼接完成的图像。

12.如权利要求9所述的设备，其特征在于，所述提取模块，具体用于使用灰度聚类方法，根据拼接后的图像中吊钩标记点与图像背景之间的灰度差异，将拼接后的图像中的吊钩标记点的图像数据对象进行聚类；根据将聚类后的吊钩标记点的图像数据对象，生成仅包含各吊钩标记点的图像。

13.如权利要求8-12任一项所述的设备，其特征在于，所述测量模块，具体用于对所述仅包含所述标记点的图像，计算光流场，得到所述标记点在吊钩运动过程中的速度和角度；根据计算出的速度和角度，得出所述标记点的平移运动速度矢量和绕轴旋转速度矢量，根据所述标记点的三维坐标、所述平移运动速度矢量和绕轴旋转速度矢量，计算所述标记点在振动过程中的运动矢量。

14.一种工程机械吊钩姿态的测量系统，其特征在于，包括：

至少两个相机，用于对工程机械吊钩上标记点同时进行拍摄；

工程机械吊钩姿态的测量设备，用于不断获取所述至少两个相机对工程机械吊钩上标记点同时拍摄的各图像，并分别确定各图像中像素点在世界坐标系中的三维坐标；根据各图像中各像素点的三维坐标，不断地将至少两个相机同时拍摄的图像拼接为一帧图像；针对拼接后的每帧图像，从该拼接后的图像中，识别并提取出吊钩上的标记点，生成仅包含所述标记点的图像；对仅包含所述标记点的各图像中标记点的三维坐标，使用光流跟踪算法，测算出所述标记点的运动状态参数；根据测算出的所述标记点运动状态参数的变化，对吊钩姿态进行测量。

Images