CN113608440B - 一种考虑绳长变化的船用吊艇系统减摆控制方法 - Google Patents

Abstract

本发明公开一种考虑绳长变化的船用吊艇系统减摆控制方法。解决大型舰船的吊艇系统减摆控制存在响应速度慢和位移饱和而导致传统位置控制效果不佳的问题。包括：首先构建考虑绳长变化和海浪横摇运动的动力学模型，对其部分反馈线性化，设计位置控制模式，减小强干扰带来的误差，但其使系统频繁减摆导致柔顺性下降，其次构建含有期望刚度、期望阻尼和期望质量的二阶环境耦合动力学模型，引入隐形弹力函数，补偿接触力信息；但因环境刚度受到强干扰时变，需要在线调整参数，为了避免参数动态变化引起系统震荡，最后设计环境补偿函数与自适应律，保证系统柔顺性与稳定性。该方法可有效减小摆角，保证工作艇位姿稳定，提高收放效率。

Description

一种考虑绳长变化的船用吊艇系统减摆控制方法

技术领域

本发明属于船舶特辅装置运动控制领域，特别涉及一种考虑绳长变化的船用吊艇系统减摆控制方法。

背景技术

工作艇是大型舰船联络通讯、运送人员、维护保养、航道测量和供应补给的重要设备。船用吊艇系统是舰船上存放、降落和回收工作艇的装置。如何在海洋强干扰环境下减小摆角，解决响应速度慢和位移饱和导致传统位置控制效果不佳的问题，成为亟待解决的难点。

论文《一类欠驱动机械系统的非线性控制研究》虽然也提出了绳长变化的起重机模型，但是没有考虑海浪横摇运动对吊艇系统与工作艇的位姿影响，只是将外界干扰处理成白噪声；而论文《非结构环境下的机器人自适应变阻抗力跟踪控制方法》模拟实际接触力时，没有引用隐性弹力函数，只用环境刚度代替，且只使用单一阻抗闭环控制，没有设计位置控制，没有考虑力/位混合控制，也没有提高系统柔顺性。

而专利《一种船用减摇起重机》、《一种单参数调整的欠驱动吊车作业全过程自抗扰控制方法》和《多段匀变速起重机智能防摇摆控制方法》等均采用起重机或小车作为控制对象，相比于本发明所控制的机械臂，没有形成多自由度闭环反馈控制系统，不能通过运动学逆解解算出需要补偿的角度，也不能利用舵机控制关节，形成多自由度闭环反馈控制。控制系统的状态量与减摆工作机理不同，因而本发明并不是上述文献所提出的方法的简单结合。

发明内容

发明目的在于提供一种可考虑绳长变化、响应度速快、无位移饱和、避免频繁减摆和在线调参的船用吊艇系统减摆控制方法，其具体步骤如下：

S1：根据动力学方程与运动学方程，对船用吊艇系统进行建模，并考虑绳长变化和海浪横摇对船用吊艇系统的运动影响规律；

S2：根据SAO公约制定的摆角安全范围，对船用吊艇系统的动力学方程进行线性化；

S3：针对船用吊艇系统的欠驱动系统特性，利用部分线性反馈化原理，进一步对线性化的动力学方程处理，为控制律的设计提供理论依据；

S4：设计PD控制律，实现位置控制模式，根据运动学逆解，解算出各个关节需要补偿的角度，并通过运动学正解，得到实际位置，减小摆角；

S5：设计柔顺阻抗控制律，实现力/位混合控制模式，模拟实际接触力，引入环境刚度和参考力，补偿力信息，提高系统柔顺性；

S6：设计自适应律，针对海浪横摇运动，进行在线阻抗参数调整，预估外界环境变化，并减小因修改惯性系数引起的系统震荡，进行动态补偿。

实际船用吊艇系统受到海浪强干扰而产生摆角，为了便于分析，对实际船用吊艇系统的运动进行抽象，并建立其三维拉格朗日方程：

式中：T为拉格朗日算子，x₁、y₁为工作艇的坐标，m为工作艇质量，l为吊索长度，f_x、f_y和f_l为广义力，θ₁、θ₂为工作艇受海浪运动影响产生的摆角，考虑到安全性和高效性相关的减摆技术要求，保证工作艇在空间各个方向的摆角均处于SAO公约规定范围内(-7°＜θ₁，θ₂＜7°)，故有|θ₁|＜＜1°，|θ₂|＜＜1°，则

与

必然成立，同时，为了研究摆动规律及摆角的影响因素，对拉格朗日方程组进行如下线性化近似为：

式中：M为母船质量。

设计位置控制的PD控制律：

式中：x_d，y_d和l_d是期望输入，k₁～k₈是PD控制系统的增益系数。

本发明适用于机械臂的减摆控制，不同于其他类型的船用起重机。机械臂的主要特征是具有多自由度闭环反馈控制系统。该系统可以通过运动学逆解，解算出需要补偿的角度，利用舵机控制关节，形成多自由度闭环反馈控制。PD控制器收到给定的期望位置控制伺服电机，伺服电机带动减速机产生扭矩，控制减摆臂产生角度，减小摆角，最后通过运动学正解得到实际位置。

位置控制虽然具有响应速度快特点，由于海浪横摇运动过于强烈，需要频繁减小摆角，导致系统柔顺性下降，减摆臂刚性弯折，引发安全事故。因而需要引入力/位混合控制模式，改善系统柔顺性。由于海浪产生的干扰力较为复杂，造成减摆臂与环境间的接触力跟踪效果较差，因此为更好地描述减摆臂与环境间接触力关系，实现与环境间的接触力跟踪，引入隐性弹力函数F_y：

式中：K_y为环境刚度，X_m为位置控制模式的控制输出，X_e为外界干扰产生位置偏差；由于x与y相互解耦，且不考虑与l耦合关系，x的控制律求解与y相同；则x的控制律求解如下：

构建二阶质量-阻尼-刚度与环境耦合的动力学模型：

式中：M(x)为减摆臂关节的惯性系数矩阵；M_d为期望惯性矩阵；

为减摆臂关节的向心力与哥式力系数矩阵；g(x)为减摆臂关节的重力系数矩阵；f_a，f_e为分别为减摆臂的关节驱动力和外界作用力；

力/位混合控制模式先得到船用吊艇系统与环境间接触力信息，由隐形弹力函数得到实际的接触力F_y，计算出F_y与期望力F_r偏差，输入到阻抗控制模型，修正或补偿船用吊艇系统末端参考位置X_d，将补偿后参考位置X_z输入船用吊艇系统控制器，X_z经过逆运动学求解得各个关节角θ_z，然后通过位置控制模式控制减摆臂的关节运动，在横纵摇干扰下经船用吊艇系统位置控制系统计算得到的实际关节角度θ_m，依据运动学正解解算出船用吊艇系统末端实际控制运动的位置X，减摆臂运动到该位置时会产生新的实际接触力信息F_y，进入下一个力控制计算周期。通过力/位混合控制模式，实现接触力信息的补偿，最终改善系统柔顺性；

但是由于K_y处于动态变化，因此需要对外界环境进行预估，构建预估环境位移函数X_Estiamte和预估环境位置误差函数E_Estiamte为：

式中：λ为环境补偿系数，E_x为位置误差，E＝X_z-X_d。

为了使稳态误差为零，引入自适应阻抗参数来补偿时变的误差，考虑系统的稳定性与柔顺性，对阻抗参数中的期望惯性矩阵M_d、期望阻尼矩阵B_d进行自适应调节，由于M_d的变化容易引起系统的震荡，破坏系统稳定性，因而需要对M_d进行动态补偿，动态补偿b_compensate(t)的时域表达式为：

式中：b_d与e_Estiamte(t)为B_d和E_Estiamte的时域表达式，ξ(t)为补偿函数，f_y(t)与f_r(t)为F_y和F_r的时域表达式，σ取10^-5，防止分母为了零，ε为采样率，α为更新率；

则参数更新自适应律为：

式中：m_d为M_d的时域表达式。

本发明具有如下有益效果：

(1)采用伺服电机作为减摆臂的驱动单元，具有响应速度快，无液压缸位移饱和的缺点；

(2)采用位置控制与力/位混合控制的双重减摆控制模式，避免频繁减摆造成的减摆臂弯折或断裂的情况；

(3)针对海浪的强干扰，区别于传统阻抗控制方法，设计环境预估函数与自适应律，对系统进行在线参数整定的同时避免惯性系数改变引起的系统震荡，确保吊艇减摆系统的稳定性与柔顺性，提高减摆效率。

附图说明

为了更清楚地说明本发明实施例或现有技术方案，下面将对实施例或现有技术描述中所需使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1一种考虑绳长变化的船用吊艇系统减摆控制方法流程图；

图2船用吊艇系统的三维物理模型；

图3基于PD控制的位置控制模式；

图4船用吊艇系统的减摆臂工作机理；

图5基于柔顺阻抗控制的力/位混合控制模式；

图6一种考虑绳长变化的船用吊艇系统减摆控制方法仿真结果图。

具体实施方式

下面结合附图对本发明进行详细描述：

为了使本发明的目的、技术方案以及优点更加清楚明白，以下结合附图及实施案例，对本发明进行进一步的详细说明，应当理解，此处所述的具体实施案例仅用于解释本发明，并不用于限定本发明。

S1：根据动力学方程与运动学方程，对船用吊艇系统进行建模，并考虑绳长变化和海浪横摇对船用吊艇系统的运动影响规律；

S2：根据SAO公约制定的摆角安全范围，对船用吊艇系统的动力学方程进行线性化；

S3：针对船用吊艇系统的欠驱动特性，利用部分线性反馈化原理，进一步对线性化的动力学方程处理，为控制律的设计提供理论依据；

S4：设计PD控制律，实现位置控制模式，根据运动学逆解，解算出各个关节需要补偿的角度，并通过运动学正解，得到实际位置，减小摆角；

S5：设计柔顺阻抗控制律，实现力/位混合控制模式，模拟实际接触力，引入环境刚度和参考力，补偿力信息，提高系统柔顺性；

S6：设计自适应律，针对海浪横摇运动，进行在线阻抗参数调整，预估外界环境变化，并减小因修改惯性系数引起的系统震荡，进行动态补偿。

所述步骤S1具体包含以下步骤：

实际船用吊艇系统受到海浪强干扰而产生摆角，为了便于分析，对实际船用吊艇系统的运动进行抽象，可以得到图2的船用吊艇系统的三维物理模型。工作艇通过刚性吊索与船用吊艇系统相连。其中母船质量为M，工作艇质量为m，吊索长度为l。如图2所示，F₀为世界坐标系，F₁为非惯性坐标系。而母船会受到海浪强干扰发生横摇运动，假设此时母船横摇角为

工作艇因船体摇晃，产生摆角，摆角可以分解为X方向的θ₁和Y方向的θ₂；工作艇在世界坐标系F₀下的坐标为(x₁,y₁,z₁)，则B点相对于非惯性坐标系F₁的坐标如下：

则工作艇在各个方向上的速度为：

船用吊艇系统的三维拉格朗日方程为：

式中：T为拉格朗日算子，f_x、f_y和f_l为广义力。为方便设计控制律，将船用吊艇系统的动力学模型改写成矩阵形式为：

式中：M(q)为惯性矩阵，C(q)为科氏力和离心力矩阵，g(q)为重力矩阵，q＝(q₁,q₂)^T，q₁＝(x,y,l)^T为吊艇减摆控制系统的输入，q₂＝(θ₁,θ₂)^T为系统的状态量，τ为广义力矩阵，τ＝(u,0)^T，u＝(f_x,f_y,f_l)^T。

所述步骤S2具体包含以下步骤：

考虑到安全性和高效性相关的减摆技术要求，保证工作艇在空间各个方向的摆角均处于SAO公约规定范围内(-7°＜θ₁，θ₂＜7°)，故有|θ₁|＜＜1°，|θ₂|＜＜1°，则

与

必然成立。同时，为了研究摆动规律及摆角的影响因素，对拉格朗日方程组进行如下线性化近似为：

线性化的船用吊艇系统的三维拉格朗日方程为：

所述步骤S3具体包含以下步骤：

将式(4)改写成以下形式：

式中：M₁₁、M₁₂、M₂₁和M₂₂是惯性矩阵M(q)中的元素，

和

是广义力，均由科氏力和离心力矩阵与重力矩阵项生成。由于式(7)中含有零项，直接设计控制律较为困难，因此根据马克思庞的部分反馈线性化理论，将欠驱动系统转化为全驱动系统。根据上述公式可知：

将式(8)代入式(7)，构造新的全驱动系统为：

令

作为新构造的全驱动系统的系统输入，则

与

的表达式为：

所述步骤S4具体包含以下步骤：

设计位置控制的PD控制律：

式中：x_d，y_d和l_d是期望输入，k₁～k₈是PD控制系统的增益系数。

本发明适用于机械臂的减摆控制，不同于其他类型的船用起重机。机械臂的主要特征是具有多自由度闭环反馈控制系统。该系统可以通过运动学逆解，解算出需要补偿的角度，利用舵机控制关节，形成如图3所示的多自由度闭环反馈控制。减摆工作机理如4所示，PD控制器收到给定的期望位置控制伺服电机，伺服电机带动减速机产生扭矩，控制减摆臂产生角度，减小摆角，最后通过运动学正解得到实际位置，作为S5的控制输入。

所述步骤S5具体包含以下步骤：

位置控制模式虽然具有响应速度快特点，由于海浪横摇运动过于强烈，需要频繁减小摆角，导致系统柔顺性下降，减摆臂刚性弯折，引发安全事故。因而需要引入力/位混合控制模式，改善系统柔顺性。如图5所示，由于海浪产生的干扰力较为复杂，造成减摆臂与环境间的接触力跟踪效果较差，因此为更好地描述减摆臂与环境间接触力关系，实现与环境间的接触力跟踪，引入隐性弹力函数F_y：

式中：K_y为环境刚度，X_m为位置控制模式的控制输出，X_e为外界干扰产生位置偏差。由于x与y相互解耦，且不考虑与l耦合关系，因而以x为例，进行详细说明。通过模拟产生弹性力的期望刚度矩阵K_d、产生黏性力的期望阻尼矩阵B_d和产生惯性力的期望质量矩阵M_d，构建二阶质量-阻尼-刚度与环境耦合的动力学模型：

式中：M(x)为减摆臂关节的惯性系数矩阵；

为减摆臂关节的向心力与哥式力系数矩阵；g(x)为减摆臂关节的重力系数矩阵；f_a，f_e为分别为减摆臂的关节驱动力和外界作用力。

力/位混合控制模式先得到船用吊艇系统与环境间接触力信息，由隐形弹力函数得到实际的接触力F_y，计算出F_y与期望力F_r(F_r＝30000N)偏差，输入到阻抗控制模型，修正或补偿船用吊艇系统末端考位置X_d，将补偿后参考位置X_z输入船用吊艇系统控制器，X_z经过逆运动学求解得各个关节角θ_z大小，然后通过位置控制模式控制减摆臂的关节运动，在横纵摇干扰下经船用吊艇系统位置控制系统计算得到的实际关节角度θ_m，依据运动学正解解算出船用吊艇系统末端实际控制运动的位置X，减摆臂运动到该位置时会产生新的实际接触力信息F_y，进入下一个力控制计算周期。通过力/位混合控制模式，实现接触力信息的补偿，最终改善系统柔顺性。

所述步骤S6具体包含以下步骤：

由于海浪干扰过于频繁且强烈，造成K_y未知或者动态变化，因而需要设计自适应律，对阻抗参数M_d、B_d和K_d进行在线调参，更好地补偿接触力信息，改善系统柔顺性。首先构建位置误差E_x与力误差ΔF的表达式为：

则二阶阻抗方程为：

由于系统参考力F_r不为零，则X_z≠X_d，为了使吊艇减摆系统稳定，保证系统稳态误差为零，取K_d等于零，则有：

将式(13)代入式(16)，则有：

选择合适的M_d和B_d可以使式(17)成立，但是由于K_y处于动态变化，因此需要对外界环境进行预估，构建预估环境位置函数X_Estiamte和预估环境位置误差函数E_Estiamte为：

式中：λ为环境补偿系数。

为了使稳态误差为零，引入自适应阻抗参数来补偿时变的误差，考虑系统的稳定性与柔顺性，对阻抗参数中的M_d、B_d进行自适应调节。由于M_d的变化容易引起系统的震荡，破坏系统稳定性，因而需要对M_d进行动态补偿，动态补偿b_compensate(t)的时域表达式为：

式中：b_d与e_Estiamte(t)为B_d和E_Estiamte的时域表达式，ξ(t)为补偿函数，f_y(t)与f_r(t)为F_y和F_r的时域表达式，σ取10^-5，防止分母为了零，ε为采样率，α为更新率。

则参数更新自适应律为：

式中：m_d为M_d的时域表达式。

通过Matlab/Simulink软件进行仿真验证，(x_d,y_d,l_d)＝(4,3,2)，M＝3200kg，m＝1500kg，仿真时间为100秒，仿真结果如图6所示，在考虑绳长变化与海浪横摇运动的情况下，θ₁最大摆动角度为18.80°，最终维持在-2.53～2.49°之间，其减摆效率为86.76％；θ₂最大摆动角度为12.92°，最终在-1.90～1.90°之间浮动，其减摆效率为85.33％。θ₁与θ₂均符合SAO公约给出的安全摆角范围，达到减摆效果。

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更改或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化与修饰，均属于本发明技术方案的范围内。

Claims (1)

1.一种考虑绳长变化的船用吊艇系统减摆控制方法，其特征在于，包括以下步骤：

S1：根据动力学方程与运动学方程，对船用吊艇系统进行建模，并考虑绳长变化和海浪横摇对船用吊艇系统的运动影响规律；

S2：根据SAO公约制定的摆角安全范围，对船用吊艇系统的动力学方程进行线性化；

S3：针对船用吊艇系统的欠驱动系统特性，利用部分反馈线性化原理，进一步对线性化的动力学方程处理，为控制律的设计提供理论依据；

S4：设计PD控制律，实现位置控制模式，根据运动学逆解，解算出各个关节需要补偿的角度，并通过运动学正解，得到实际位置，减小摆角；

S5：设计柔顺阻抗控制律，实现力/位混合控制模式，模拟实际接触力，引入环境刚度和参考力，补偿力信息，提高系统柔顺性；

S6：设计自适应律，针对海浪横摇运动，进行在线阻抗参数调整，预估外界环境变化，并减小因修改惯性系数引起的系统震荡，进行动态补偿；

实际船用吊艇系统受到海浪强干扰而产生摆角，为了便于分析，对实际船用吊艇系统的运动进行抽象，并建立其三维拉格朗日方程：

式中：T为拉格朗日算子，x₁、y₁为工作艇的坐标，m为工作艇质量，l为吊索长度，f_x、f_y和f_l为广义力，θ₁、θ₂为工作艇受海浪运动影响产生的摆角，考虑到安全性和高效性相关的减摆技术要求，保证工作艇在空间各个方向的摆角均处于SAO公约规定范围，即为-7°＜θ₁，θ₂＜7°，故有|θ₁|＜＜1°，|θ₂|＜＜1°，则

与

必然成立，同时，为了研究摆动规律及摆角的影响因素，对拉格朗日方程组进行如下线性化近似为：

式中：M为母船质量；

设计位置控制的PD控制律：

式中：x_d，y_d和l_d是期望输入，k₁～k₈是PD控制系统的增益系数；

适用于机械臂的减摆控制，不同于其他类型的船用起重机，机械臂可以通过运动学逆解，解算出需要补偿的角度，利用舵机控制关节，形成多自由度闭环反馈控制，PD控制器收到给定的期望位置控制伺服电机，伺服电机带动减速机产生扭矩，控制减摆臂产生角度，减小摆角，最后通过运动学正解得到实际位置；

位置控制虽然具有响应速度快特点，由于海浪横摇运动过于强烈，需要频繁减小摆角，导致系统柔顺性下降，减摆臂刚性弯折，引发安全事故，因而需要引入力/位混合控制模式，改善系统柔顺性，由于海浪产生的干扰力较为复杂，造成减摆臂与环境间的接触力跟踪效果较差，因此为更好地描述减摆臂与环境间接触力关系，实现与环境间的接触力跟踪，引入隐性弹力函数F_y：

式中：K_y为环境刚度，X_m为位置控制模式的控制输出，X_e为外界干扰产生位置偏差；由于x与y相互解耦，且不考虑与l耦合关系，x的控制律求解与y相同；则x的控制律求解如下：

构建二阶质量-阻尼-刚度与环境耦合的动力学模型：

式中：M(x)为减摆臂关节的惯性系数矩阵；M_d为期望惯性矩阵；

为减摆臂关节的向心力与哥式力系数矩阵；g(x)为减摆臂关节的重力系数矩阵；f_a，f_e为分别为减摆臂的关节驱动力和外界作用力；

力/位混合控制模式先得到船用吊艇系统与环境间接触力信息，借由隐形弹力函数F_y，计算出F_y与期望力F_r偏差，输入到阻抗控制模型，修正或补偿船用吊艇系统末端参考位置X_d，将补偿后参考位置X_z输入船用吊艇系统控制器，X_z经过逆运动学求解得各个关节角θ_z，然后通过位置控制模式控制减摆臂的关节运动，在横纵摇干扰下经船用吊艇系统位置控制系统计算得到的实际关节角度θ_m，依据运动学正解解算出船用吊艇系统末端实际控制运动的位置X，减摆臂运动到该位置时会产生新的F_y，进入下一个力控制计算周期，通过力/位混合控制模式，改善系统柔顺性；

但是由于K_y处于动态变化，因此需要对外界环境进行预估，构建预估环境位移函数X_Estiamte和预估环境位置误差函数E_Estiamte为：

式中：λ为环境补偿系数，E_x为位置误差，E_x＝X_z-X_d；

为了使稳态误差为零，引入自适应阻抗参数来补偿时变的误差，考虑系统的稳定性与柔顺性，对阻抗参数中的期望惯性矩阵M_d、期望阻尼矩阵B_d进行自适应调节，由于M_d的变化容易引起系统的震荡，破坏系统稳定性，因而需要对M_d进行动态补偿，动态补偿b_compensate(t)的时域表达式为：

式中：b_d与e_Estiamte(t)为B_d和E_Estiamte的时域表达式，ξ(t)为补偿函数，f_y(t)与f_r(t)为F_y和F_r的时域表达式，σ取10^-5，防止分母为了零，ε为采样率，α为更新率；

则参数更新自适应律为：

式中：m_d为M_d的时域表达式。

Images