CN114634150B - 一种基于激光位置传感器的鹤管自动对位系统 - Google Patents

Abstract

本发明公开了一种基于激光位置传感器的鹤管自动对位系统，该系统可分为检测系统和控制系统两个分系统，其中检测系统是为了得到罐口坐标。在检测系统中，首先建立了基于三台主激光发射器、一台副激光发射器和一台激光接收器的罐口检测系统。然后将主激光发射器到接收器的距离当作半径，在空间中建立三个球面，并利用三球定位原理求出三个球面的两交点。最后利用副激光发射器，验证得出真实的罐口坐标。在控制系统中，首先建立了鹤管的运动学方程，并在已知罐口坐标的情况下，计算逆运动学方程得到各关节的期望角度。进一步，建立鹤管的动力学模型，并设计基于非奇异快速终端滑模的控制器。最后利用李雅普诺夫第二法验证系统的稳定性并利用Matlab进行仿真验证。

Description

一种基于激光位置传感器的鹤管自动对位系统

一、技术领域

本发明涉及石油机械的自动化生产领域，具体地说是一种应用于汽车大鹤管自动装卸过程中的测控系统，其中包括利用位置传感器对汽车罐口进行识别的检测系统和保证鹤管能够准确对接罐口的控制系统。

二、背景技术

汽车大鹤管是一种石化行业流体装卸过程中的专用装备，多用于油库、码头储存的液体介质向运输车辆的转运过程。它采用旋转接头将相邻的刚性管道连接，通过关节的旋转改变鹤管的位姿，保证鹤管的末端连杆能顺利伸入运输车辆的油罐罐口。目前市面上的鹤管主要为人力驱动鹤管，其采用最为传统的装卸车方式，即人工牵引大鹤管进行油罐车罐口的对位，这种方式下操作工人劳动强度大，易疲劳，且罐口定位不准确，很容易引发安全事故。因此，快速准确地进行大鹤管与罐口的自动定位对于提高工业生产效率、避免安全事故具有重大的意义。目前，大鹤管自动对位系统只能实现半自动化，即现场工人利用遥控设备控制鹤管转动，利用人眼确定罐口位置，待鹤管转动到罐口正上方之后，控制鹤管伸入罐口内部。这种定位的缺点是定位主要靠人工，且工人只能利用人眼得到期望末端执行器的位置，很难快速计算出各关节的期望角度，需要不断调整关节角度，定位的效率低。因此实现鹤管的自动对位是目前该领域的主要研究方向。

三、发明内容

有鉴于此，本发明提出了一种基于激光位置传感器的鹤管自动对位系统，该系统是一套同时包含罐口检测模块和鹤管控制模块的一键式操作系统，其先进程度是衡量油库现代化水平的重要标志之一。在系统的检测模块中，本专利采用了基于激光发射器和接收器的定位系统，利用三球交会原理加辅助检测装置确定罐口坐标。在系统的控制模块中，采用了逆运动学求解期望轨迹的算法，并根据非奇异快速终端滑模算法设计了鹤管的非线性控制器，保证鹤管能够准确伸入罐口，完成鹤管的自动对位。

为实现上述目的，本发明提供了一种基于激光位置传感器的鹤管自动对位系统，包括：

S1：设计一套基于激光位置传感器的定位系统；

对于鹤管的自动对位系统研究，罐口检测系统是对位成功的基础，本发明设计的激光位置传感器定位系统由三台主激光发射器、一台副激光发射器和一台激光接收器组成。所有设备通过无线蓝牙进行通讯，设置发射器和接收器的时钟同步。如图1所示为现场实际生产过程中的模型示意图，其中油罐车停在现场的预设鹤位中，当油罐车在鹤位上停稳之后，将蓝牙接收器放置在油罐车的罐口中心，即待求解的罐口位置。激光发射器分别放置在栈桥栅栏上的不同位置，如图所示，将主激光发射器2定义为坐标系原点，进一步通过位置测量将主激光发射器1、主激光发射器3和副激光发射器的位置坐标表示出来。最后分别运行四个激光发射器，并得到四个激光发射器到接收器的距离。

S2：利用三球交会定位原理确定接收器位置即罐口坐标：其中三台主激光发射器可通过三球交会确定两个交点，再利用副激光发射器进行比较选择，得出最终坐标；

如图2所示，已知三个主激光发射器的位置坐标为(x_i,y_i,z_i),i＝1,2,3，三个主激光发射器到接收器的距离为r₁,r₂,r₃，假设罐口坐标为(x₀,y₀,z₀)，通过已知和假设条件可得到如下方程：

求解上式中的非线性方程组，得到两组实数解，进一步判断两组解中的真实解。已知副激光发射器的位置坐标为(x₄,y₄,z₄)，副激光发射器到接收器的距离为r₄，计算坐标(x₄,y₄,z₄)到两组实数解坐标的距离，并与r₄进行差值运算，其中更接近0的为待求解的罐口坐标(x₀,y₀,z₀)。

S3：设计特殊的三自由度鹤管的机械结构，并规定鹤管与液相接口的连接轴中心为坐标原点，根据右手定则建立笛卡尔坐标系，在该坐标系下建立三自由度鹤管的运动学方程，并将步骤2中得到的罐口坐标进行坐标变换，转换成该笛卡尔坐标空间的坐标；

所设计的特殊三自由度鹤管机械结构如图3所示，其中关节1控制连杆1在水平方向进行转动；关节2控制连杆2在水平方向进行转动；关节3控制连杆2进行上下摆动，并为关节4和连杆3提供驱动力；关节4为受约束关节，起到平衡作用，在连杆2和辅助连杆的共同作用下，与关节3和关节4共同组成平行四边形结构，其作用是保证连杆3能够完全垂直于地面；如图3，以关节1的中心为坐标原点，依据右手定则建立笛卡尔坐标系，并利用连杆3末端位置与各关节角度的关系，建立系统的运动学方程。

通过测量得出，连杆1长度为l₁，连杆2长度为l₂，连杆3长度为h₃，关节1到关节3的垂直距离为h₁。定义鹤管的空间坐标系，原点在关节1的中心，鹤管的末端坐标为(x_h,y_h,z_h)，则其运动学方程的具体表达式如下：

x_h＝l₁cosθ₁+l₂cosθ₃cos(θ₁+θ₂)

y_h＝l₁sinθ₁+l₂cosθ₃sin(θ₁+θ₂)。

z_h＝-h₁-h₃+l₂sinθ₃

进一步，进行坐标变换，已知在激光定位器坐标系下罐口的位置为(x₀,y₀,z₀)，激光定位器坐标的原点位置为主激光发射器2的位置，经测量，其在鹤管空间坐标系下的坐标为(x_j,y_j,z_j)，则罐口在鹤管空间坐标系下的坐标为(x₀+x_j,y₀+y_j,z₀+z_j)，该坐标即鹤管末端的期望坐标。

S4：在罐口位置坐标已知的情况下，求解逆运动学方程，得出三自由度鹤管的各关节期望角度；

已知鹤管的运动学方程和罐口坐标(x₀+x_j,y₀+y_j,z₀+z_j)，进一步将罐口坐标带入系统的运动学方程，可直接解出关节3的旋转角度θ₃，其表达式如下：

/>

令

则运动学方程可改写成如下形式：

利用三角函数万能公式，令t₁＝tan(θ₁/2)，t₂＝tan(θ₂/2)，则：

进一步，将运动学方程展开并带入三角函数万能公式，

将已知参数带入方程，化简可得到关于t₁和t₂的四次方程组：

利用python中solve函数可对该方程进行求解，进一步，利用反三角函数，得到各关节的期望转角(θ₁,θ₂,θ₃)，该期望转角即为鹤管跟踪控制的期望轨迹。

S5：利用欧拉—拉格朗日建模法建立鹤管的动力学模型；

假设鹤管的各部分密度均匀，质心均位于连杆中间，定义r₁＝1/2l₁，r₂＝1/2l₂，则具体的动力学建模过程如下：

连杆1的质心坐标为：

连杆1质心速度的平方：

则连杆1的动能为：

同理，连杆2的动能为：

连杆3为受约束连杆，动能只计算平移的部分即可：

由上式可知，三自由度鹤管的总动能为E_k＝E_k1+E_k2+E_k3。

进一步，计算鹤管的重力势能，由于连杆1只有水平旋转，不考虑重力势能，机械臂的总势能和连杆2，3的重力势能可分别表示为：

E_p＝E_p2+E_p3

E_p2＝m₂gr₂(1+sinθ₃)。

E_p3＝m₃g[(l₂+r₃)+l₂sinθ₃]

由拉格朗日函数，可知L＝E_k-E_p，利用拉格朗日方程计算各关节的力矩，得到系统的动力学方程，力矩的具体计算公式如下：

将四个关节电机输入转矩作为输入，建立动力学方程如下：

其中θ＝[θ₁ θ₂ θ₃]^T表示三自由度鹤管的三个转动角度，τ＝[τ₁ τ₂ τ₃]^T，f∈R^3×3代表摩擦力和空气阻力系数，M(θ)∈R^3×3为三自由度鹤管的惯性矩阵；

为系统的哥氏力和向心力矩阵；G(θ)∈R³表示系统的重力项；ω∈R³表示系统所受到的外部扰动。

S6：根据动力学模型设计非奇异快速终端滑模控制器，利用李雅普诺夫第二法进行系统稳定性证明，并利用Matlab进行仿真分析。

滑模变结构控制算法、对外界干扰和模型误差具有良好的鲁棒性，在非线性控制中得到了广泛的应用。本发明中采用的终端滑模算法是滑模变结构控制算法的一种。由于该算法设计的控制器可保证系统在有限时间内收敛，因此，相对于一般的滑模变结构算法，该方法更利于实际的工程机械控制。以下是终端滑模面的表达式：

其中α＞0，p和q均为正奇数，且满足0＜p/q＜1，e＝θ-θ_d，θ_d表示各关节的期望轨迹，该滑模形式可以保证系统状态有限时间收敛至平衡点，但采用终端滑模算法设计控制器时存在一个控制奇异性问题，即当系统处于状态空间的一个特殊子空间时，终端滑模控制器的控制输出可能达到无穷大。

因此，为了避免上述的控制奇异性问题，有学者提出了非奇异终端滑模控制理论，其滑模面表达式如下：

其中α＞0，p和q均为正奇数，且满足1＜p/q＜2，采用该滑模面设计控制器时，可以保证控制器中所有项的指数均为正，从而避免控制奇异性的问题。

本发明中采用的控制算法为非奇异快速终端滑模，与终端滑模和非奇异终端滑模相比，该算法优势明显，既避免了设计控制器时的奇异问题，又提高了系统状态的收敛速度。以下是非奇异快速终端滑模面的表达式：

其中α＞0，β＞0，p,q,g,h均为正奇数，且满足1＜p/q＜2，p/q＜g/h。根据非奇异终端滑模面的表达式，很明显，当系统状态接近滑动面时，位置误差e的高阶项e^g/h被忽略。因此，滑模面的形式类似于非奇异终端滑模。而当系统状态远离滑模面时，位置误差e的高阶项e^g/h起主导作用，因此非奇异快速终端滑模的收敛速度大于非奇异终端滑模。进一步，设计鹤管跟踪控制器并验证系统稳定性。

滑模控制算法可分为滑动阶段和到达阶段，其中滑动阶段即是系统状态沿着滑模面向平衡点收敛的过程，而到达阶段是指系统状态从状态空间中任意位置，到达滑模面的过程。为了设计合适的滑模控制器，首先，需要保证系统在滑动阶段的稳定性，即系统状态到达滑模面后，能够保持在滑模面上移动，而不会偏离滑模面，即当s＝0时，保证

因此，对非奇异快速终端滑模面进行求导，可得：

其中，

diag表示对角矩阵，同理，

根据鹤管系统的动力学方程可知：

其中，

表示系统的已知非线性项。

根据前两个为了保证

设计系统的等效控制器，形式如下：

其中，I表示一个三阶单位矩阵，I₁表示一个三维列向量。

当系统状态处于到达阶段时，系统状态的收敛速度取决于到达速度，因此，本发明设计了如下双幂次滑模趋近律：

其中σ₁和σ₂均为可调节的权重系数，m₁,m₂,n₁,n₂均为正奇数，且满足0＜m₁/n₁＜1，m₂/n₂＞1。根据滑模趋近律，设计系统的切换控制律如下：

进一步，根据系统的等效控制律和切换控制律，设计鹤管跟踪控制器形式如下：

为了证明本发明中所设计的非奇异快速终端滑模控制器在到达阶段的收敛性，选用李雅普诺夫第二法进行控制器稳定性验证，选择如下李雅普诺夫方程，具体表达式为：

对李亚普诺夫方程进行求导，并将系统的带入其中：

为了证明系统的稳定性，将系统状态分成两种情况进行讨论：

当系统状态处于Ω₁时，由于p,q,m₁,n₁,m₂,n₂均为正奇数，很明显可以得出

根据李雅普诺夫稳定性判据，系统在到达阶段是稳定的，系统状态渐近收敛到滑模面s＝0。/>

而当系统状态处于Ω₂时，将控制器带入方程

可以得到如下方程：

两式做除法，可得：

根据初始条件，由于系统状态处于Ω₂，当e＞0且

时，s＞0，因此很明显可以看出/>

同理可证当e＜0且/>

时，s＜0，/>

综上所述，在这种情况下，系统不可能保持在该状态稳定不动，而是将继续收敛到平衡点。

为了验证系统的有限时间收敛性，首先介绍有限时间收敛定理如下：

假设一个正定函数V(t)满足如下不等式：

其中λ＞0，0＜γ＜1，则存在时间常数t₁保证，当时间t＞t₁时，V(t)＝0，时间常数t₁的表达式如下：

验证本发明中系统状态有限时间收敛过程如下，已知李雅普诺夫方程的导数形式如下：

将

带入上式，

其中

根据有限时间收敛定理，可知，系统状态可在有限时间内收敛到滑模面，同理也可证明系统在到达滑模面后也会在有限时间内收敛到平衡点。

最后，利用Matlab对控制器进行仿真验证，定义三个关节的期望轨迹分别为：

θ_1d＝sin(pi*t)，θ_2d＝cos(pi*t)，θ_3d＝sin(pi*t)。

则，三个关节的期望角速度分别为：

θ_1d＝pi*cos(pi*t)，θ_2d＝-pi*sin(pi*t)，θ_3d＝pi*cos(pi*t)。

假设三个关节的初始角度和角速度分别为：

θ(0)＝(1 0 -1.5)^T,

仿真结果见附图中的图4，5，6。如图4所示，鹤管的三个关节实际轨迹可以在短时间内跟踪到参考轨迹(红线)。图5中的实际关节角速度(蓝线)也可以跟踪到鹤管的期望角速度(红线)，这表明当鹤管各关节的实际轨迹跟踪到期望轨迹之后，实际轨迹将始终与期望轨迹保持一致，完成既定的加油任务。通过对图4和图5的分析，我们认识到基于非奇异快速终端滑模算法的控制器能够保证自动鹤管系统的状态在有限的时间内收敛到期望轨迹，误差收敛到0，系统稳定。此外，我们还将该控制算法与非奇异终端滑模控制算法进行了比较，结果表明了本发明中算法的优越性。在图6中，我们可以清楚地看到，两种控制算法都可以保证自动鹤管系统的稳定性，跟踪误差收敛到0，但本发明中算法具有更好的跟踪效果，能够在更短的时间内跟踪到期望的轨迹。

四、附图说明

图1是鹤管自动对位系统结构示意图

图2是鹤位及激光定位装置示意图

图3是三球定位原理示意图

图4是三自由度鹤管结构示意图

图5是鹤管三关节的位置跟踪仿真图

图6是鹤管三关节的速度跟踪仿真图

图7是非奇异快速终端滑模控制算法与非奇异终端滑模的算法比较

图8是鹤管自动对位操作流程示意图

五、具体实施方式

下面结合附图对本发明作进一步描述：

参照图7鹤管自动对位操作流程示意图，并结合图1，2，3，本发明一种基于激光位置传感器的鹤管自动对位系统具体实施过程如下：

(1)图1中可以清晰的看到，油罐车司机根据现场标识的鹤位，将罐车停在鹤位中任意位置，且保证罐车各部分完全在鹤位之内；

(2)现场操作人员将无线激光接收器放置在罐口中心位置，并等待识别结束之后打开罐口；

(3)三个主激光发射器按照序号分别进行工作，并在接收器上读取激光传输时间，并换算成各主激光器到接收器的距离，分别记为d₁，d₂和d₃；

(4)分别将d₁，d₂和d₃作为球体半径，以三个主激光发射器的位置作为球心，在空间中建立三个球面方程，并将三个方程联立求解出三个球面相交的两个交点坐标；

(5)启动副激光发射器，并在接收器上读取激光传输时间，并换算成副激光发射器到接收器的距离，记为d₄；

(6)计算副激光发射器到两个交点坐标的距离，并与d₄进行减法计算，其结果绝对值小的则为实际的罐口坐标；

(7)工作人员打开罐口，等待计算完成；

(8)已知鹤管的运动学方程，并将罐口的坐标当作鹤管的期望位置，进行逆运动学方程求解，解得的结果即为鹤管各关节的期望转角；

(9)将鹤管的期望转角带入系统的动力学方程，经过计算，得到系统完成该工作所需的各关节力矩；

(10)将各关节所需的力矩传输给可编程控制器，并转换成脉冲传递给系统的执行器伺服电机；

(11)伺服电机控制鹤管，伸入罐口，完成鹤管的自动对位程序。

Claims (5)

1.针对一种基于激光位置传感器的鹤管自动对位系统，其特征在于，包括：

S1：设计一套基于激光位置传感器的定位系统；

S2：利用三球交会定位原理确定接收器位置即罐口坐标：其中三台主激光发射器可通过三球交会确定两个交点，再利用副激光发射器进行比较选择，得出最终坐标；

S3：设计带有四个旋转关节的三自由度鹤管的机械结构，其中关节3、关节4与连杆2和辅助连杆共同构成平行四边形结构，保证鹤管的连杆3始终垂直于地面，并规定鹤管与液相接口的连接轴中心为坐标原点，根据右手定则建立笛卡尔坐标系，在该坐标系下建立三自由度鹤管的运动学方程，并将步骤2中得到的罐口坐标进行坐标变换，转换成该笛卡尔坐标空间的坐标；

S4：在罐口位置坐标已知的情况下，求解逆运动学方程，得出三自由度鹤管的各关节期望角度；

S5：利用欧拉—拉格朗日建模法建立鹤管的动力学模型，有以下几个步骤：

S51：分别建立连杆1、关节2、关节3、连杆2、关节4和连杆3的动能与各关节旋转角度和角速度之间关系的表达式，其中辅助连杆属于连杆2的附属结构，因此其动能包含建立在连杆2的动能表达式里；

S52：由于鹤管系统为三自由度动力系统，其三个自由度分别为关节1、2、3，因此将受约束的关节4与关节3整合为一个驱动力矩，计算求得三自由度鹤管系统的总动能和总势能的差值，并对关节1、2、3的角度和角速度分别求偏导，即可得到各关节的力矩；

S53：利用欧拉—拉格朗日建模法建立的系统动力学模型：

其中，θ＝[θ₁ θ₂ θ₃]^T为三自由度鹤管的各关节转角，第四关节为受约束关节，其驱动力矩为关节3提供，因此不再将关节4的角度体现在动力学模型中，

和/>

分别代表各关节的转动角速度和加速度，M(θ)∈R^3×3为三自由度鹤管的惯性矩阵，/>

为系统的哥氏力和向心力矩阵，G(θ)∈R³表示系统的重力项，f∈R^3×3为系统的摩擦力矩阵，ω∈R³表示系统所受到的外部扰动，τ＝[τ₁ τ₂ τ₃]^T为系统的力矩输入；

S6：根据动力学模型设计非奇异快速终端滑模控制器，利用李雅普诺夫第二法进行系统稳定性证明，并利用Matlab进行仿真分析。

2.根据权利要求1所述的一种基于激光位置传感器的鹤管自动对位系统，步骤(1)中，所述设计一套基于激光位置传感器的定位系统，其特征在于：

S11：该定位系统由三台主激光发射器、一台副激光发射器和一台激光接收器组成，其中激光发射器安装于鹤管栈桥上不同位置，接收器放置于油罐车罐口；

S12：利用无线蓝牙通讯，保证发射器和接收器的时钟一致，当任意发射器与接收器进行通讯连接后，发射器发出的激光被接收器接收，得出发射器与接收器的距离；

S13：规定空间中某一点为笛卡尔坐标系原点，并测量各激光发射器坐标。

3.根据权利要求1所述的一种基于激光位置传感器的鹤管自动对位系统，步骤(2)中，所述利用三球交会定位原理确定接收器位置即罐口坐标：其中三台主激光发射器可通过三球交会确定两个交点，再利用副激光发射器进行比较选择，得出最终坐标，其特征在于：

S21：已知三个主激光发射器与激光接收器之间距离，分别以三个主激光发射器位置为球心坐标，以三个距离为半径，在空间中建立三个球体，并分别写出三个球面的方程表达式；

S22：根据三球交会定理可知，三个球面相交两个交点，进一步利用前面得到得三个球面方程联立求解出两个交点坐标；

S23：以副激光发射器为球心，以副激光发射器到接收器的距离为半径，分别求解两个交点到球心的距离并与半径相减，其中误差较小的一个为代求的位置坐标，即罐口坐标。

4.根据权利要求1所述的一种基于激光位置传感器的鹤管自动对位系统，步骤(3)中，所述设计带有四个旋转关节的三自由度鹤管的机械结构，其中关节3、关节4与连杆2和辅助连杆共同构成平行四边形结构，保证鹤管的连杆3始终垂直于地面，并规定鹤管与液相接口的连接轴中心为坐标原点，根据右手定则建立笛卡尔坐标系，在该坐标系下建立三自由度鹤管的运动学方程，并将步骤2中得到的罐口坐标进行坐标变换，转换成该笛卡尔坐标空间的坐标，其特征在于：

S31：设计带有四个旋转关节的三自由度鹤管的机械结构，连杆1在关节1的驱动下水平方向转动，连杆2分别在关节2和关节3的驱动下在水平和竖直方向转动，保证鹤管能够顺利到达装车位置并深入油罐，关节4为关节3的随动关节，连杆2和辅助连杆及关节3和关节4共同组成平行四边形结构，保证连杆3能够始终垂直于地面；

S32：规定以液相接口的中心为坐标原点，依据右手定则建立笛卡尔坐标系，并利用鹤管末端位置与各关节角度的关系，建立系统的运动学方程；

S33：将激光定位系统的坐标系和鹤管的坐标系进行坐标变换，将罐口坐标转换成鹤管坐标系下的坐标。

5.根据权利要求1所述的一种基于激光位置传感器的鹤管自动对位系统，步骤(6)中，所述根据动力学模型设计非奇异快速终端滑模控制器，利用李雅普诺夫第二法进行系统稳定性证明，并利用Matlab进行仿真分析：

S61：利用非奇异快速终端滑模算法设计系统的滑模面：

其中α和β为正实数，p、q、g、h均为正奇数，且满足1＜p/q＜2，p/q＜g/h:

S62：根据非奇异快速终端滑模算法设计带有权重的双幂次滑模趋近律：

其中m₁、n₁、m₂、n₂均为正奇数，且满足0＜m₁/n₁＜1，m₂/n₂＞1，σ₁和σ₂为可调节的控制权重系数；

S63：根据非奇异快速终端滑模面和双幂次滑模趋近律设计等效控制律和切换控制律，并整合设计系统的控制器，进一步利用李雅普诺夫方程进行系统的稳定性证明。

Images