CN103663149A - 一种起重机吊臂末端位置检测设备、方法、系统及起重机 - Google Patents

Abstract

本发明公开了一种起重机吊臂末端位置检测设备、方法、系统及起重机。该设备包括接收装置，用于接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；计算装置，用于根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面与水平面的夹角、以及所述单目成像装置成像面的中心在以所述起重机吊臂根部为原点的第一坐标系中的坐标计算所述靶标中心在所述第一坐标系中的位置。本发明相对于现有技术中通过全站仪、经纬仪以及GPS进行位置测量的方案相比成本更低，并且可以实现实时地动态位置测量而无需人工干预，提高了位置测量的自动化水平。

Description

一种起重机吊臂末端位置检测设备、方法、系统及起重机

技术领域

本发明涉及起重机，具体地，涉及一种起重机吊臂末端位置检测设备、方法、系统及起重机。

背景技术

在起重机结构中，起重机臂架是影响整机起吊性能、工作稳定性和安全性的决定性因素，对于履带吊臂架而言更是如此。在履带吊吊装重物的作业过程中，履带吊臂架会发生一定的挠曲变形，履带吊吊臂末端会发生一定的位移，如果位移量过大，可能引发事故，甚至会造成灾难性的后果。为了避免履带吊作业时产生过大的挠曲变形而妨碍正常作业，需要对该位移进行实时测量和显示，将其控制在一定范围内。

为了测量起重机臂架的挠度，现有技术中存在针对门式起重机的多种技术方案，例如通过GPS、经纬仪、全站仪和激光测距仪、摄影测量等方式来进行测量，但现有技术还不存在型履带吊臂架末端位移进行实时测量技术方案，无法满足作业安全的需要。

发明内容

本发明的目的是提供一种起重机吊臂末端位置检测设备、方法、系统及起重机，用于实时检测起重机吊臂末端的位置，以保证起重机作业的安全性。

为了实现上述目的，本发明提供一种起重机吊臂末端位置检测设备，该设备包括：接收装置，用于接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；计算装置，用于根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面与水平面的夹角、以及所述单目成像装置成像面的中心在以所述起重机吊臂根部为原点的第一坐标系中的坐标计算所述靶标中心在所述第一坐标系中的位置。

相应地，本发明提供了一种起重机吊臂末端位置检测方法，该方法包括：接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面与水平面的夹角、以及所述单目成像装置成像面的中心在以所述起重机吊臂根部为原点的第一坐标系中的坐标计算所述靶标中心在所述第一坐标系中的位置。

相应地，本发明提供了一种起重机吊臂末端位置检测系统，该系统包括所述的设备；该系统还包括：靶标，安装在所述起重机吊臂末端，用于提供标记点；单目成像装置，用于对所述标记点成像。

相应地，本发明提供了一种起重机，该起重机包括所述的系统。

本发明相对于现有技术中通过全站仪、经纬仪以及GPS进行位置测量的方案相比成本更低，并且可以实现实时地动态位置测量而无需人工干预，提高了位置测量的自动化水平。

本发明的其他特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

附图是用来提供对本发明的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本发明，但并不构成对本发明的限制。在附图中：

图1是本发明提供的起重机吊臂末端位置检测设备示意图；

图2是本发明提供的第二坐标系和第三坐标系映射关系示意图；

图3是本发明提供的标记点提取结果示意图；

图4是本发明提供的第一坐标系、第二坐标系和第三坐标系位置关系示意图；

图5是本发明提供的起重机吊臂末端位置检测方法示意图；

图6是本发明提供的起重机吊臂末端位置检测系统示意图；

附图标记说明

1    第一坐标系                    2    第二坐标系

3    第三坐标系                    4    成像面

100  接收装置                      200  计算装置

300  靶标                          400  单目成像装置

具体实施方式

以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。

为了检测起重机吊臂末端位置，本发明在所述起重机吊臂末端安装了靶标。靶标一般为圆形，上面可以沿圆周均匀分布地设置LED灯作为标记点，或者其他的标记点，例如荧光点标记点。本发明提供的起重机吊臂末端位置检测设备如图1所示，该设备包括：接收装置100，用于接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；计算装置200，用于根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面4与水平面的夹角、以及所述单目成像装置成像面4的中心在以所述起重机吊臂根部为原点的第一坐标系1中的坐标计算所述靶标中心在所述第一坐标系1中的位置。本发明中，为了计算臂架起重机吊臂末端的位置，将其转化为计算靶标中心的位置。为了表征靶标中心的位置，需要以所述起重机吊臂根部为原点建立第一坐标系1。因此，本发明在计算起重机吊臂末端位置的过程中，最终转化为靶标中心在第一坐标系1中的坐标。

为了求得靶标中心在第一坐标系1中的坐标，可以假设靶标为圆形靶标。同时建立以单目成像装置，例如单目相机或单目摄像机的成像面4中心为原点的第二坐标系2以及以靶标中心为原点的第三坐标系3。本发明可以根据靶标的单目成像装置成像数据来计算靶标中心在第二坐标系2中的坐标，具体过程说明如下。

可以先进行第二坐标系2和第三坐标系3之间的映射，建立第三坐标系3与第二坐标系2之间的映射关系。第二坐标系2与第三坐标系3之间的映射关系如图2所示。

第三坐标系3与第二坐标系2之间的映射关系可以用下式表示：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_o\\ y_o\\ z_o\\ 1\end{array}\right]---\left(1\right);$

其中(x₀,y₀,z₀,1)是任意一点在第三坐标系3中的齐次坐标，(x_c,y_c,z_c,1)是任意一点在第二坐标系2中的齐次坐标，R是旋转矩阵，t是平移向量。

根据上述的映射关系可以得到在第三坐标系3中任意的标记点

的坐标

在第二坐标系2中的坐标

即：

$\begin{array}{c}{P}_j^c=d_D+R_d\left({\mathrm{\ϕ}}_d\right){\left(P_j^d\right)}^T\\ d_D={(x_d,y_d,z_d)}^T\\ {\mathrm{\ϕ}}_d={({\mathrm{\ψ}}_d,{\mathrm{\θ}}_d,{\mathrm{\φ}}_d)}^T\end{array}---\left(2\right);$

其中，d_D和

是待求的未知量。

为了求得上述的d_D和

可以利用小孔成像的原理，通过比较标记点

在单目成像装置成像面4上的图像坐标以及通过预设的d_D和计算得到的投影坐标之间的差异，利用迭代的方法得到精确的d_D和

需要说明的是上述的d_D可以表征靶标中心在第二坐标系2中的坐标。

上述的d_D和

中共有6个变量，至少需要四个标记点的成像数据来计算出上述的d_D和

根据小孔成像原理，可以得到标记点在第二坐标系2中的图像坐标与投影到单目成像装置成像面4上的投影坐标之间的关系：

$\left[\begin{array}{c}{\hat{u}}_j\\ {\hat{v}}_j\end{array}\right]=\frac{f}{z_{c_j}}\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\end{array}\right]---\left(3\right);$

其中f为焦距，

是标记点在第二坐标系2中的坐标，可以在迭代过程中得到，而

是根据小孔成像原理得到的标记点在成像面4上的投影坐标，其可以用来与标记点在成像面4上的实际投影坐标比较，二者之间的距离可以作为迭代收敛的条件。

在计算d_D和

的过程中，首先需要将标记点在成像面4上提取出来。例如可以利用哈里斯角点检测、SUSAN检测以及尺度不变特征转换检测等方法来提取诸如标记点的特征点。提取出来的标记点如图3所示。

通过对连续多帧的特征点提取以后可以得到标记点在成像面4上的图像坐标点集{p₁,p₂,...p_n}，其中p_j＝(u_j,v_j),j＝1,2,......,n；同样地，根据小孔成像原理也可以计算出标记点的投影坐标点集

其中

${\hat{p}}_j=({\hat{u}}_j,{\hat{v}}_j),j=\mathrm{1,2},......,m.$

为了通过迭代的方式求出d_D和

需要将图像坐标点集和投影坐标点集一一对应起来，可以通过经典的数学指派问题来进行描述。

对于p_j,可以计算它们之间的欧式距离矩阵Err，矩阵大小为m×n：

$\left[\begin{array}{cccc}d({\hat{p}}_1,p_1)& d({\hat{p}}_1,p_2)& ...& d({\hat{p}}_1,p_n)\\ d({\hat{p}}_2,p_1)& d({\hat{p}}_2,p_2)& ...& d({\hat{p}}_2,p_n)\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ \·& \·& \·& \·\\ d({\hat{p}}_m,p_1)& d({\hat{p}}_m,p_2)& ...& d({\hat{p}}_m,p_n)\end{array}\right]---\left(4\right);$

通过求解出最小的距离和就可以得出点集之间的一一对应关系。数学指派问题可以用如下的数学模型来描述。

假设两个点集A、B，s_ik表示集合A中的点i与集合B中的点k之间的欧式距离，那么要建立A、B两个点集之间的一一对应的关系，即转化为如下公式：

$\left\{\begin{array}{cc}\min {\mathrm{\Σ}}_{i=1}^N{\mathrm{\Σ}}_{k=1}^N{s}_{\mathrm{ik}}{x}_{\mathrm{ik}}& \\ {\mathrm{\Σ}}_{i=1}^N{x}_{\mathrm{ik}}=1& k=1...N\\ {\mathrm{\Σ}}_{i=1}^N{x}_{\mathrm{ik}}=1& i=1...N\\ s_{\mathrm{ik}}>0& \\ x_{\mathrm{ik}}\∈\left\{\mathrm{0,1}\right\}& \end{array}\right.---\left(5\right);$

S={s_ij|s_ij=dist(a_i,b_j),a_i∈A,b_j∈B,i=1...N,j=1...M}    (6)；

对于图像坐标和投影坐标之间的一一对应也可以相应地转化为数学指派问题，具体过程如下。

P表示标记点在第三坐标系3上的位置：

P={p1,._..,p_N}    (7)；

A表示点集P通过旋转矩阵R和转移向量t以及投影变换到第二坐标系2中成像面4的投影点集：

A={a₁,...,a_N}    (8)；

$p_i\stackrel{R,t}{\→}{a}_i---\left(9\right);$

B表示图像检测得到的图像点集：

B={b₁,...,b_M}    (10)；

C表示点集A与点集B对应点匹配，定义如下：

C={(a_i,b_j)|a_i∈A,b_j∈B}    (11)；

上述的指派问题的求解可以由匈牙利算法快速求解，从而能够将投影点集和图像点集一一对应起来。

将投影点集和图像点集对应起来之后，可以通过迭代算法求解出d_D和

上述的d_D和

可以通过LHM算法来计算。在第二坐标系2中，标记点的位置和欧拉角姿态记作：

$\left[\begin{array}{cc}T& R\end{array}\right]=\left[\begin{array}{cccccc}{x}_t^c& y_t^c& z_t^c& {\mathrm{\ψ}}_t^c& {\mathrm{\θ}}_t^c& {\mathrm{\φ}}_t^c\end{array}\right]---\left(12\right).$

LHM算法可以通过求解对象空间的线性误差的最小值来解决位置估计问题。

假设在k时刻检测的标记点j的坐标是

记为：

$h_j\left(k\right)={\left[\begin{array}{ccc}{u}_j& v_j& 1\end{array}\right]}^T---\left(13\right)$

在采样时刻k，矩阵^CT_T表示吊臂末端圆形靶上标记点（LED）在位姿估计向量

下，从第三坐标系3投影到第二坐标系2的变换矩阵，其中 $\stackrel{\‾}{X}\left(k\right)=[{\stackrel{C-}{x}}_T,{\stackrel{C-}{y}}_T,{\stackrel{C-}{z}}_T,{\stackrel{C-}{\mathrm{\ψ}}}_T,{\stackrel{C-}{\mathrm{\θ}}}_T,{\stackrel{C-}{\mathrm{\ϕ}}}_T].$

第k时刻的对象空间线性误差向量用e_j表示，定义如下：

$e_j\left(k\right)={(I-V_j\left(k\right))}^c{T}_T{\left(\stackrel{\‾}{X}\left(k\right)\right)}^T{P}_{\left(j\right)}---\left(14\right);$

$V_j\left(k\right)=\left[\begin{array}{cc}\frac{h_j\left(k\right)h_j^T\left(k\right)}{h_j^T\left(k\right)h_j\left(k\right)}& 0\\ 0& 1\end{array}\right]---\left(15\right);$

那么位姿估计问题等价于使得误差平方和最小化的参数求解问题。

$E\left(\stackrel{\‾}{X}\left(k\right)\right)=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\left|\right|e_j\left(k\right)\left|\right|}^2---\left(16\right);$

该算法通过迭代，改善位姿向量旋转部分的估算值，然后估计相关的平移分量，找到满足要求的估计值

即[T R]。实际上，这是通过利用线性方程组来实现的：

${\hat{h}}_j={\left[\begin{array}{ccc}{\hat{u}}_j& {\hat{v}}_j& 1\end{array}\right]}^T---\left(18\right);$

其中， $\left[\begin{array}{cc}{\hat{u}}_j& {\hat{v}}_j\end{array}\right]$ 表示第三坐标系3下点^TP_(j)投射到第二坐标系2中成像面4的坐标。

通过上述算法得到圆形靶标中心到摄像机坐标系的相对距离信息d_D＝(x_d,y_d,z_d)^T（即通过LHM算法得到的T），已知成像面4的中心在第一坐标系1中的坐标为[x_ccd y_ccd z_ccd]，以及第二坐标系2中的成像面4与水平面的夹角α，那么可以得到靶标中心相对于第一坐标系1下的位置信息[x_ed y_ed z_ed]：

$\left[\begin{array}{c}{x}_{\mathrm{ed}}\\ y_{\mathrm{ed}}\\ z_{\mathrm{ed}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\\ 0& -\cos \mathrm{\α}& \sin \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{ccd}}\\ y_{\mathrm{ccd}}\\ z_{\mathrm{ccd}}\end{array}\right]---\left(19\right)$

在得到靶标中心在第一坐标系1中的位置之后，可以计算出靶标中心与第一坐标系1原点之间的距离，通过比较在不同时刻的该距离，可以反映出靶标中心的位移，也同时反映除了起重机吊臂末端的位移。上述第一坐标系1、第二坐标系2和第三坐标系3之间的位置关系如图4所示，单目成像装置一般安装在云台上，并且尽可能保证α与吊臂的仰角相同。

相应地，本发明提供了一种起重机吊臂末端位置检测方法，该方法包括：接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面4与水平面的夹角、以及所述单目成像装置成像面4的中心在以所述起重机吊臂根部为原点的第一坐标系1中的坐标计算所述靶标中心在所述第一坐标系1中的位置。图5示出了其具体的流程，该方法包括：接收靶标的单目成像装置成像数据（步骤501）；根据单目成像装置成像数据计算第二坐标系2和第三坐标系3之间的映射参数（步骤503），映射参数包括旋转矩阵和平移向量；根据映射参数中平移向量以及成像面4与水平面的夹角计算所述平移向量在第一坐标系1中的分量（步骤505）；将第二坐标系2的原点在第一坐标系1中的坐标与上述分量分别相加得到靶标中心在第一坐标系1中的坐标（步骤507）。

相应地，本发明提供了一种起重机吊臂末端位置检测系统，如图6所示，该系统接收装置100、计算装置200、靶标300、以及单目成像装置400；靶标300安装在所述起重机吊臂末端，用于提供标记点；单目成像装置400，用于对所述标记点成像。

相应地，本发明提供了一种起重机，该起重机包括所述的系统。

以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型，这些简单变型均属于本发明的保护范围。

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。

此外，本发明的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明的思想，其同样应当视为本发明所公开的内容。

Claims (12)

1.一种起重机吊臂末端位置检测设备，其特征在于，该设备包括：

接收装置，用于接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；

计算装置，用于根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面与水平面的夹角、以及所述单目成像装置成像面的中心在以所述起重机吊臂根部为原点的第一坐标系中的坐标计算所述靶标中心在所述第一坐标系中的位置。

2.根据权利要求1所述的设备，其特征在于，所述计算装置根据所述单目成像装置成像数据得到所述靶标中心在以单目成像装置成像面中心为原点的第二坐标系中的坐标。

3.根据权利要求2所述的设备，其特征在于，所述计算装置根据下式计算所述靶标中心在所述第一坐标系中的位置：

$\left[\begin{array}{c}{x}_{\mathrm{ed}}\\ y_{\mathrm{ed}}\\ z_{\mathrm{ed}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\\ 0& -\cos \mathrm{\α}& \sin \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{ccd}}\\ y_{\mathrm{ccd}}\\ z_{\mathrm{ccd}}\end{array}\right];$

其中(x_ed,y_ed,z_ed)为所述靶标中心在所述第一坐标系中的位置，α为所述单目成像装置成像面与水平面的夹角，(x_d,y_d,z_d)为所述靶标中心在所述第二坐标系中的坐标，(x_ccd,y_ccd,z_ccd)为所述单目成像装置成像面的中心在所述第一坐标系中的坐标。

4.根据权利要求2或3所述的设备，其特征在于，所述单目成像装置成像数据包括所述靶标上至少四个标记点的成像数据；所述计算装置根据所述至少四个标记点在以所述靶标中心为原点的第三坐标系中的坐标以及所述至少四个标记点的成像数据计算所述第三坐标系映射到所述第二坐标系的平移向量，所述平移向量即为所述靶标中心在所述第二坐标系中的坐标。

5.根据权利要求1-3任意一项所述的设备，其特征在于，所述计算装置还用于在第一时刻计算所述靶标中心与所述第一坐标系原点之间的第一距离，在第二时刻计算所述靶标中心与所述第一坐标系原点之间的第二距离，并计算所述第一距离与第二距离的差值。

6.一种起重机吊臂末端位置检测方法，其特征在于，该方法包括：

接收安装于所述起重机吊臂末端的靶标的单目成像装置成像数据；

根据所述靶标的单目成像装置成像数据、所述单目成像装置成像面与水平面的夹角、以及所述单目成像装置成像面的中心在以所述起重机吊臂根部为原点的第一坐标系中的坐标计算所述靶标中心在所述第一坐标系中的位置。

7.根据权利要求6所述的方法，其特征在于，该方法还包括：

根据所述单目成像装置成像数据得到所述靶标中心在以单目成像装置成像面中心为原点的第二坐标系中的坐标。

8.根据权利要求7所述的方法，其特征在于，根据下式得到所述靶标中心在所述第一坐标系中的位置：

$\left[\begin{array}{c}{x}_{\mathrm{ed}}\\ y_{\mathrm{ed}}\\ z_{\mathrm{ed}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\\ 0& -\cos \mathrm{\α}& \sin \mathrm{\α}\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]+\left[\begin{array}{c}{x}_{\mathrm{ccd}}\\ y_{\mathrm{ccd}}\\ z_{\mathrm{ccd}}\end{array}\right];$

其中(x_ed,y_ed,z_ed)为所述靶标中心在所述第一坐标系中的位置，α为所述单目成像装置成像面与水平面的夹角，(x_d,y_d,z_d)为所述靶标中心在所述第二坐标系中的坐标，(x_ccd,y_ccd,z_ccd)为所述单目成像装置成像面的中心在所述第一坐标系中的坐标。

9.根据权利要求7或8所述的设备，其特征在于，所述单目成像装置成像数据包括所述靶标上至少四个标记点的成像数据；该方法还包括：

根据所述至少四个标记点在以所述靶标中心为原点的第三坐标系中的坐标以及所述至少四个标记点的成像数据计算所述第三坐标系映射到所述第二坐标系的平移向量，所述平移向量即为所述靶标中心在所述第二坐标系中的坐标。

10.根据权利要求6-8任意一项所述的方法，其特征在于，该方法还包括：

用于在第一时刻计算所述靶标中心与所述第一坐标系原点之间的第一距离，在第二时刻计算所述靶标中心与所述第一坐标系原点之间的第二距离，并计算所述第一距离与第二距离的差值。

11.一种起重机吊臂末端位置检测系统，其特征在于，该系统包括根据权利要求1-5任意一项所述的设备；该系统还包括：

靶标，安装在所述起重机吊臂末端，用于提供标记点；

单目成像装置，用于对所述标记点成像。

12.一种起重机，其特征在于，该起重机包括根据权利要求11所述的系统。

Images