一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法

1.本发明属于欠驱动系统领域，涉及欠驱动起重机系统运动控制技术，具体涉及一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法。


背景技术：

2.欠驱动系统是一类输入小于输出的系统。欠驱动系统的控制问题一直是一个具有挑战性的问题，欠驱动系统控制问题的研究有助于不完全约束系统控制理论的发展。许多学者致力于欠驱动系统的研究。船用起重机作为一种必不可少的机电系统，在货物的生产、转移和运输中发挥着越来越重要的作用。作为一种欠驱动系统，船用起重机在执行载荷运输任务时往往表现出复杂的动力学特性。同时，由于工作环境的特殊性，与陆用固定起重机相比，船用起重机会受到更复杂的外部干扰。当船舶受到风、浪等海洋环境的影响时，会产生高达6个自由度方向(偏航、摇摆、浪涌、俯仰、横摇和升沉)的扰动，影响起重机运输载荷的定位效果，使载荷摆角发生晃动，并可能进一步导致安全事故。
3.在过去的几十年里，陆用起重机的控制问题得到了广泛的研究，研究者提出了许多有意义的控制方法，包括轨迹规划、滑模控制、输入整形控制、自适应控制、最优控制等。然而，由于工作环境的复杂性，现有的陆用起重机控制策略不能直接适用于船载起重机的控制问题。因此，有必要对船用起重机在海面扰动作用下的控制方法进行研究。
4.通过对以往针对船用旋转起重机控制算法的分析，我们发现了以下几个需要解决的难点：
5.1)与实际海况对船载起重机平台产生的复杂扰动相比，现有文献简化了扰动的影响，只研究了实际扰动在一个或两个方向上的扰动分量。在恶劣的海况下，所提出的控制算法可能无法进行有效的定位。
6.2)实际的起重机绳长通常是时变的，这是许多控制器设计过程中没有考虑到的。随着钢丝绳长度的变化，起重机系统的固有频率也会发生变化，这大大增加了系统的复杂性。因此，对绳长时变问题进行研究是十分必要的。但是船用旋转起重机是一种常见的欠驱动系统，控制变量多，干扰复杂。如果再考虑绳长的变化，将进一步加强系统的耦合性，使控制问题的研究成为一项非常困难的任务。
7.3)在实验验证中，通常通过在匹配通道中输入干扰信号来模拟实际干扰。实际的扰动往往是复杂的，这种方法通常不能完全验证所提出方法的实用性。


技术实现要素：

8.发明目的：为了克服现有技术中存在的不足，提供一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法。
9.技术方案：为实现上述目的，本发明提供一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，包括如下步骤：
10.s1：基于拉格朗日动力学方程建立考虑连续海浪扰动的五自由度船用旋转起重机
数学模型并分析其动力学特性；
11.s2：根据船用旋转起重机数学模型的动力学特性设计扰动观测器；
12.s3：选取参考轨迹，并且通过扰动观测器设计摆角抑制轨迹，将参考轨迹和摆角抑制轨迹进行结合形成复合轨迹；
13.s4：基于船用旋转起重机数学模型、扰动观测器设计pd控制器；
14.s5：根据复合轨迹，利用pd控制器输出船用旋转起重机实时轨迹。
15.进一步地，所述步骤s1中考虑连续海浪扰动的五自由度船用旋转起重机数学模型的表示如下：
[0016][0017][0018]
q1＝θ
1-α,q2＝θ
2-β,q3＝l(t),q4＝θ
3-α,q5＝θ
4-β,
[0019]
其中，ms(q)为船用旋转起重机系统的惯性矩阵，为向心-科里奥利矩阵，g(q)为重力矢量，τ为控制输入矢量，τf为船用旋转起重机系统的机械摩擦力，悬臂的变幅角和旋转角分别为θ1和θ2，船舶的横摇角，偏航角以及升沉扰动分别为α，β和h，负载的径向摆角和切向摆角分别为θ3和θ4，q为耦合之后系统新的状态变量，q1和q2为耦合后的悬臂变幅角和旋转角，q4和q5为耦合后的负载的径向摆角和切向摆角，为一阶导数，为其二阶导数；m与m分别为悬臂和负载的质量，l与l分别为悬臂的长度与吊绳的长度，g为重力加速度，对于驱动力/转矩，τ1为悬臂起幅方向驱动扭矩，τ2为悬臂水平旋转方向驱动扭矩，τ3为悬绳垂直方向驱动扭矩，τ
1f
，τ
2f
和τ
3f
分别为悬臂起幅方向和水平旋转方向的机械摩擦力和悬绳垂直方向的机械摩擦力，d
*
(1，...，5)为船舶升沉引起的扰动。
[0020]
进一步地，所述步骤s1中动力学特性包括悬臂定位特性以及摆角摆动特性；在船用旋转起重机运动过程中，存在两个方向的扭矩，其一个方向是悬臂起幅方向的扭矩，另一个方向是悬臂水平旋转方向的转动扭矩；这时，由于离心运动的参与，使系统动力学特性变得十分复杂，同时，由于海浪运动的干扰，起重机系统的负载摆动特性更加复杂，通过分析海浪扰动对船用旋转起重机各个驱动力方向上的影响，将海浪运动产生的持续偏航角度和悬臂水平旋转方向的角度进行耦合，将海浪运动产生的横摇扰动角度和悬臂起幅方向的角度进行耦合，海浪运动产生的升沉方向的扰动则通过扰动观测器进行跟踪，实现了在海浪影响下的负载精确定位和摆角的抑制目标。
[0021]
进一步地，所述步骤s1中船用旋转起重机数学模型包括摩擦力前馈补偿模型，摩擦力前馈补偿模型的表达如下：
[0022][0023][0024][0025]
其中，f
11
、f
12
、f
21
、f
22
、f
31
、f
32
为摩擦力前馈补偿模型的参数，f
11
、f
12
和f
13
的值与最大静摩擦力对应，f
21f22
和f
23
是粘性摩擦系数，ε是静态摩擦系数。
[0026]
进一步地，所述步骤s2中设计的扰动观测器为非线性扰动观测器，具体为：
[0027][0028][0029]
d＝ms(q)-1d*
[0030]
其中：p＝[p
1 p
2 p
3 p
4 p5]
t
，是对与升沉扰动有关的量d的估计；p是辅助向量，l1是r5×5的常数矩阵，其中p和l1里的都是可选取的常数。
[0031]
进一步地，所述步骤s3中参考轨迹为s型轨迹，表示为：
[0032][0033][0034]
其中，x表示耦合后的悬臂起幅角度q1或者耦合后的水平旋转角度q2；q(x)d，q(x)0和t
q(χ)d
分别为悬臂起幅角度和旋转目标角度/位置、初始角度/位置和到达时间。
[0035]
进一步地，所述步骤s3中摆角抑制轨迹的设计包括如下：
[0036]
对动力学方程进行线性化处理：
[0037][0038][0039][0040][0041][0042]
设计如下李雅普诺夫函数：
[0043][0044]
对李雅普诺夫方程v
t
进行求导，并且进行数学转换：
[0045][0046]
为了满足设计了如下的摆角抑制轨迹：
[0047][0048][0049]
其中，q
1r
(t)、q
2r
(t)以及q
3r
(t)分别是起伏角、旋转角以及绳长的参考轨迹，其数学公式是s形轨迹；
[0050]
对上式进行积分，得到：
[0051][0052][0053]
其中，
[0054]
进一步地，所述步骤s3中复合轨迹的表达如下：
[0055][0056][0057]q3c
(t)＝q
3r
(t)。
[0058]
进一步地，扰动观测器还包括李雅普诺夫方程：
[0059][0060]
其中：为扰动估计误差值；
[0061]
对李雅普诺夫方程v1进行求导：
[0062][0063]
得出
[0064][0065]
其中，p1是一个正定对称矩阵，q1是一个正定矩阵，λm是一个可调矩阵。
[0066]
进一步地，所述步骤s4中pd控制器的表达如下：
[0067][0068][0069][0070]
其中，e1＝q
1-q
1c
，e2＝q
2-q
2c
，e3＝q
3-q
3c
为误差信号，是双通道扰动观测器所得的匹配扰动估计值，通过使用上述的pd控制器去跟踪复合轨迹，通过pd控制器中的实现了对匹配扰动的抑制，通过复合轨迹中与有关的量，实现了对升沉扰动引起的摆角的抑制。
[0071]
为了克服背景技术中提出的问题，本发明提供的一种考虑连续海浪扰动的船用旋
转起重机实时轨迹规划方法主要包括如下两个方面：
[0072]
1、研究了五自由度船用旋转起重机在连续偏航、横摇和升沉作用下的动力学特性，与以往的模型将波动扰动视为集中扰动不同，该模型在建模中考虑了来自三个不同方向的扰动，使得不同方向的扰动对整个系统的影响非常清晰，这对控制器分析非常有利。
[0073]
2、将扰动观测器与实时轨迹规划相结合，将参考轨迹与摆角抑制轨迹相结合，在精确定位的同时实现了摆角抑制。
[0074]
基于上述内容，本发明主要解决的技术问题是：在受到船舶持续偏航，横摇以及升沉扰动时，船用旋转起重机的负载摆动特性会变得十分复杂，这使得系统动力学模型会变得更加复杂，在实现悬臂与负载的准确定位同时，不能快速抑制负载的摆动。
[0075]
有益效果：本发明与现有技术相比，通过耦合可测量的船舶偏航和横摇扰动，采用的观测器通过对状态变量、辅助变量和控制输入进行积分，克服了标准级联动力学模型无法描述欠驱动系统的困难，实现了对升沉扰动的估计并保证误差值在很小的区间内。在此基础上结合动力学方程，得到实际的升沉扰动加速度信号。同时将参考轨迹与摆角抑制轨迹相结合，最终能够实现高效的轨迹跟踪与摆动抑制，从而达到控制效果。
附图说明
[0076]
图1为本发明第一个实施例所述的一种船用旋转起重机摆动抑制控制方法的控制框图；
[0077]
图2为本发明第一个实施例所述的一种船用旋转起重机摆动抑制控制方法的旋转起重机数学模型结构原理示意图；
[0078]
图3为本发明第二个实施例所述的一种船用旋转起重机摆动抑制控制方法的扰动观测器估计效果图；
[0079]
图4为本发明第二个实施例所述的一种船用旋转起重机摆动抑制控制方法的实验结果示意图；
[0080]
图5为本发明第二个实施例所述的一种船用旋转起重机摆动抑制控制方法的对比控制器lqr(linear-quadratic-regulator)的实验结果示意图；
[0081]
图6为本发明第二个实施例所述的一种船用旋转起重机摆动抑制控制方法的实验平台示意图。
具体实施方式
[0082]
下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本技术所附权利要求所限定的范围。
[0083]
实施例1：
[0084]
如图1所示，本发明提供一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，包括如下步骤：
[0085]
s1：基于拉格朗日动力学方程建立考虑连续海浪扰动的五自由度船用旋转起重机数学模型并分析其动力学特性：
[0086]
参照图2，令建立船用旋转起重机数学模
型：
[0087][0088][0089]
τ＝[τ
1 τ
2 τ
3 0 0]
t
[0090]
τf＝[τ
1f τ
2f τ
3f 0 0]
t
[0091]
q＝[q
1 q
2 q
3 q
4 q5]
t
[0092][0093]
q1＝θ
1-α，q2＝θ
2-β，q3＝l(t)，q4＝θ
3-α，q5＝θ
4-β，
[0094]
其中，ms(q)为船用旋转起重机系统的惯性矩阵，为向心-科里奥利矩阵，g(q)为重力矢量，τ为控制输入矢量，τf为船用旋转起重机系统的机械摩擦力，悬臂的变幅角和旋转角分别为θ1和θ2，船舶的横摇角，偏航角以及升沉扰动分别为α，β和h，负载的径向摆角和切向摆角分别为θ3和θ4，q为耦合之后系统新的状态变量，q1和q2为耦合后的悬臂变幅角和旋转角，q4和q5为耦合后的负载的径向摆角和切向摆角，为一阶导数，为其二阶导数；m与m分别为悬臂和负载的质量，l与l分别为悬臂的长度与吊绳的长度，g为重力加速度，对于驱动力/转矩，τ1为悬臂起幅方向驱动扭矩，τ2为悬臂水平旋转方向驱动扭矩，τ3为悬绳垂直方向驱动扭矩，τ
1f
，τ
2f
和τ
3f
分别为悬臂起幅方向和水平旋转方向的机械摩擦力和悬绳垂直方向的机械摩擦力，d
*
(1，...，5)为船舶升沉引起的扰动。
[0095]
本实施例中船用旋转起重机数学模型还包括摩擦力前馈补偿模型，通过摩擦力前馈补偿模型消除船用旋转起重机驱动机构产生的摩擦力，通过在系统的前向通道中补偿摩擦力对系统产生的扰动减少由于摩擦力导致的定位误差，摩擦力前馈补偿模型表示为如下：
[0096][0097][0098][0099]
其中，f
11
、f
12
、f
21
、f
22
、f
31
、f
32
为摩擦力前馈补偿模型的参数，f
11
、f
12
和f
13
的值与最大静摩擦力对应，f
21f22
和f
23
是粘性摩擦系数，ε是静态摩擦系数。
[0100]
本实施例中船用旋转起重机系统的惯性矩阵如下：
[0101]
[0102][0103]m12
＝mlq3s1s5,
[0104]m13
＝-mlc
1-4
c5,
[0105]m14
＝mlq3s
1-4
c5,
[0106]m15
＝mlq3c
1-4
s5,
[0107]m21
＝m
12
,
[0108][0109]m23
＝mlc1s5,
[0110]m24
＝-mq
32
c4s5c5,
[0111]m25
＝mq
32
s4+mlq3c1c5,
[0112]m31
＝m
13
,
[0113]m32
＝m
23
,
[0114]m33
＝m,
[0115]m34
＝0,
[0116]m35
＝0,
[0117]m41
＝m
14
,
[0118]m42
＝m
24
,
[0119]m43
＝m
34
,
[0120]m44
＝mq
32c52
,
[0121]m45
＝0,
[0122]m51
＝m
15
,
[0123]m52
＝m
25
,
[0124]m53
＝m
35
,
[0125]m54
＝m
45
,
[0126]m55
＝mq
32
,
[0127]
其中，m
ij
表示矩阵坐标，i＝1，2
…
5，j＝1，2
…
5。向心-科里奥利矩阵如下：
[0128][0129]c11
＝0,
[0130][0131][0132]
[0133][0134][0135][0136][0137][0138][0139][0140][0141]c33
＝0,
[0142][0143][0144][0145][0146][0147][0148][0149][0150][0151][0152][0153]
其中c
ij
表示矩阵坐标，i＝1，2
…
5，j＝1，2
…
5。重力向量如下：
[0154][0155]
g2＝0,
[0156]
g3＝-mgc4c5,
[0157]
g4＝mgq3s4c5,
[0158]
g5＝mgq3c4s5,
[0159]
扰动向量d
*
如下：
[0160][0161]
[0162][0163][0164][0165]
本实施例中根据船用旋转起重机数学模型分析特性，特性具体包括其摆动特性(单摆系统)，负载(杆平动特性)等；在船用旋转起重机运动过程中，存在两个方向的扭矩，其一个方向是悬臂起幅方向的扭矩，另一个方向是悬臂水平旋转方向的转动扭矩；这时，由于离心运动的参与，使系统动力学特性变得十分复杂，同时，由于海浪运动的干扰，起重机系统的负载摆动特性更加复杂，通过分析海浪扰动对船用旋转起重机各个驱动力方向上的影响，将海浪运动产生的持续偏航角度和悬臂水平旋转方向的角度进行耦合，将海浪运动产生的横摇扰动角度和悬臂起幅方向的角度进行耦合，海浪运动产生的升沉方向的扰动则通过扰动观测器进行跟踪，实现了在海浪影响下的负载精确定位和摆角的抑制目标。
[0166]
s2：根据船用旋转起重机数学模型的动力学特性设计非线性扰动观测器，具体为：
[0167]
首先对动力学方程进行数学转换：
[0168][0169]
d＝ms(q)-1d*
[0170]
非线性扰动观测器设计如下：
[0171][0172][0173]
d＝ms(q)-1d*
[0174]
其中：p＝[p1p
2 p
3 p
4 p5]
t
，是对与升沉扰动有关的量d的估计。p是辅助向量，l1是r5×5的常数矩阵，其中p和l1里的都是可选取的常数。
[0175]
遵循旋转起重机模型的动力学规则，并基于船用旋转起重机数学模型的能量函数设计如下李雅普诺夫方程，通过设计如下的李雅普诺夫方程，证明了所提扰动观测器(注：李雅普诺方程只是一个理论证明方法所用到的方程。此李雅普诺夫方程只是用来证明所提扰动观测器的有效性，其中会用到船用旋转起重机数学模型的一些参数)：
[0176][0177]
其中：为扰动估计误差值。
[0178]
对李雅普诺夫方程v1进行求导：
[0179][0180]
可以得出：
[0181][0182]
其中p1是一个正定对称矩阵，q1是一个正定矩阵，λm是一个可调矩阵。利用所提出的扰动观测器，通过改变参数值，扰动估计误差可以收敛到一个可接受的范围内。该李雅普诺夫方程是一个基于误差变量设计的类能量方程，为了保证系统的稳定，该方程被设计成一个正定方程，结合控制系统的定位要求以及消摆的控制目标，该方程的一阶导数经过推导被证明半负定。
[0183]
非线性观测器通过对状态变量、辅助变量和控制输入进行积分，克服了标准级联动力学模型无法描述欠驱动系统的困难。实现了升沉扰动的估计。基于所提出的扰动观测器，可以通过估计扰动来实现对扰动d的跟踪，并保证误差值在很小的区间内。然后可以得到动力学方程中的扰动估计值结合建模中的动力学方程，可以得到实际的升沉扰动加速度信号的观测值：
[0184][0185]
其中，是扰动估计值向量的第一个值。
[0186]
s3：选取参考轨迹，并且通过扰动观测器设计摆角抑制轨迹，将参考轨迹和摆角抑制轨迹进行结合形成复合轨迹：
[0187]
利用如下参考轨迹进行船用旋转起重机系统的跟踪控制，以验证定位和消摆功能，其中，参考轨迹为s型轨迹：
[0188][0189]
q(χd).t∈[t
q(χ)d
,+∞)
[0190]
其中，χ表示耦合后的悬臂起幅角度q1或者耦合后的水平旋转角度q2以及绳长q3；q(χ)d，q(χ)0和t
q(χ)d
分别为悬臂起幅角、旋转角以及绳长目标角度/位置、初始角度/位置和到达时间。
[0191]
实际上，只要轨迹满足定位起点与终点约束要求，可行参考轨迹的选择是任意的。以下轨迹通常可用，例如阶梯型轨迹、s形轨迹和输入成形轨迹。然而，阶梯型轨迹是一种非连续轨迹，会导致系统执行器的初始输出过大，影响控制效果，不利于执行器的长期运行。对于输入成形轨迹，系统绳索长度的变化将导致振动系统的固有频率相应变化。然后，在使用前必须随时重新设计输入成形轨迹，这非常不方便。在本实施例中，选择上述所示轨迹，它具有以下优点：1)曲线相对于时间是连续的，因此所需的系统状态可以是不间断的；2)轨迹包含具有平滑效果的正弦函数部分；3)到达时间可根据实际需求人工调整。
[0192]
设计摆角抑制轨迹：
[0193]
首先对动力学方程进行线性化处理：
[0194][0195][0196][0197]
设计如下李雅普诺夫函数：
[0198][0199]
对李雅普诺夫方程v
t
进行求导，并且进行数学转换(注此李雅普诺夫方程和扰动观测器中所用到的不是一个，此李雅普诺夫方程是用来证明在所提方法下，可以实现负载的精确定位和摆角的抑制)：
[0200][0201]
为了满足设计了如下的摆角抑制轨迹：
[0202][0203][0204]
其中，q
1r
(t)、q
2r
(t)以及q
3r
(t)分别是起伏角、旋转角以及绳长的参考轨迹；
[0205]
对上式进行积分，得到：
[0206][0207][0208]
将设计的参考轨迹和摆角抑制轨迹结合起来，可以得到最终的复合轨迹：
[0209][0210][0211]q3c
(t)＝q
3r
(t)
[0212]
在摆角抑制轨迹中，h可以看作一个可变参数，而不是一个可积项。通过分析欧拉拉格朗日动力学方程可以看出，升沉扰动h对起重机系统的影响实际上是对重力加速度g的影响。当船舶受到升沉扰动时，h可以看作是对重力加速度的补偿。在控制过程中，该补偿参数无需人为调整，而是由观测器估计并实时更新，使得摆角抑制轨迹不仅随摆角摆动而变化，而且随升沉扰动的变化而变化。
[0213]
s4：基于船用旋转起重机数学模型、扰动观测器设计pd控制器，pd控制器具体为：
[0214][0215][0216]
[0217]
其中，e1＝q
1-q
1c
，e2＝q
2-q
2c
，e3＝q
3-q
3c
为误差信号，是双通道扰动观测器所得的匹配扰动估计值，通过使用上述的pd控制器去跟踪所提的复合轨迹，通过pd控制器中的实现了对匹配扰动的抑制，通过复合轨迹中与有关的量，实现了对升沉扰动引起的摆角的抑制；
[0218]
本实施例中，类似pd部分的增益(k
p1
，k
d1
，k
p2
，k
d2
，k
p3
和k
d3
)都是正的增益，k
p1
，k
d1
，k
p2
，k
d2
，k
p3
和k
d3
的初值分别设为130、60、110、70、80和65，其值可根据实际情况调节；需要注意的是，调节k
p1
，k
p2
和k
p3
可提高定位速度，但调节过大通常会产生超调和振荡现象；k
d1
，k
d2
和k
d3
将会对的因过大的k
p1
，k
p2
和k
p3
所产生的不良输出响应起到一定的阻尼效应；最后，前馈摩擦模型的相关参数f
11
、f
12
、f
21
、f
22
在离线辨识后，不需要改变其值的选取，而ε静态摩擦系数，其值选为0.01。
[0219]
s5：根据复合轨迹，利用pd控制器输出船用旋转起重机实时轨迹。
[0220]
在实际应用场合，船舶受到持续偏航、横摇以及升沉扰动的情况下，起重机的摆动特性更加明显，因此抑制摆角的同时实现安全的定位任务更加具有挑战性和实际工程意义；因此本实施例主要针对船用旋转起重机轨迹跟踪和摆动抑制问题，为此首先建立基于拉格朗日动力学方程建立具有质点负载的五自由度船用旋转起重机数学模型并分析特性。以便于对整个系统进行动力学分析设计非线性扰动观测器，该观测器可有效的估计升沉扰动，在此基础上设计了参考轨迹以及摆角抑制轨迹一定程度上实现起重机运行的要求(快速定位、有效消摆)。最后通过pd控制器来验证所提轨迹规划方法的优势，主要表现在能够快速有效的实现船用旋转起重机的轨迹跟踪和负载摆角抑制。其中以摩擦力前馈补偿模型作为前馈补偿的方式。
[0221]
本实施例的另一方面，在参数选择方面，由于平台采用pd控制器需要调节基础参数给定值以及参数的比例增益值；本方法过程简单，增益和参数的数目不多，不会过于受模型和物理条件限制，并且，各项增益对应的响应效果明确，因此实际应用中参数调节过程不会复杂，较好响应的增益容易确定；通过摩擦力前馈补偿模型简单实现消除摩擦力影响，从而有效避免了因可驱动机构运动所产生的摩擦对控制效果的不利影响，例如产生定位滞后/超前导致的摆动剧烈增加消摆控制难度。
[0222]
实施例2：
[0223]
为了验证实施例1中所提供方法的实际效果，本实施例中选择传统控制器lqr进行测试，以科学论证的手段对比试验结果，以验证本方法所具有的真实效果。
[0224]
参照图6，为了进行实验，本实施例搭建了一个实验平台。具体来说，硬件部分包括角度编码器、电子调速器、电机、pc、6自由度运动平台和电机电源。pc包含一个运动控制板，用于传输角度编码器采集的角度信号。在matlab\simulink中进行计算后，编译生成代码后通过运动控制板将控制信号传输到电子调速器进而控制电机，然后通过串口通讯对运动控制板上的实验数据进行实时的监控和记录，可驱动部分的位置信号来自编码器的计数；负载/吊钩摆动角度信息来自角度编码器，其电压信号通过a/d转换器传输到控制板。为了准确模拟实际情况，本实施例通过钢索实现了臂架起幅角的旋转，并且通过6自由度运动平台模拟海浪产生的持续偏航横摇以及升沉扰动。
[0225]
利用控制器lqr与使用本发明控制方法的控制器进行实验，控制器lqr的控制公式
为：
[0226][0227][0228][0229]
对于lqr控制器，该控制器增益为k
11
＝45，k
12
＝24，k
13
＝4.5，k
14
＝-0.1，k
21
＝45，k
22
＝31，k
23
＝3.9，k
24
＝5.5，k
31
＝18，k
32
＝12，利用上述构建的实验平台计算使用本发明方法与lqr控制器所用方法的振幅，其结果如下：
[0230]
参照图3可以看出，非线性扰动观测器观测到的扰动值在一段时间后可以相对平滑地跟踪波浪引起的起伏扰动。观测值与实际扰动之间的误差信号随时间减小，最终稳定在可接受的范围内。因此，所提出的观测器能较好地观测到五自由度船载旋转起重机的升沉扰动。
[0231]
参照图4和图5可以看出，通过对扰动产生的不确定性力进行前馈补偿，在所提控制方法下，系统的定位性能都得到了改善。在摆角抑制方面，所提控制方法明显优于对比lqr控制器。该方法通过在轨迹中加入摆角信息，使得定位过程中的最大摆角值明显小于对比lqr控制方法，且摆角在定位过程后能够更快稳定。

技术特征：
1.一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，包括如下步骤：s1：基于拉格朗日动力学方程建立考虑连续海浪扰动的船用旋转起重机数学模型并分析其动力学特性；s2：根据船用旋转起重机数学模型的动力学特性设计扰动观测器；s3：选取参考轨迹，并且通过扰动观测器设计摆角抑制轨迹，将参考轨迹和摆角抑制轨迹进行结合形成复合轨迹；s4：基于船用旋转起重机数学模型、扰动观测器设计pd控制器；s5：根据复合轨迹，利用pd控制器输出船用旋转起重机实时轨迹。2.根据权利要求1所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s1中考虑连续海浪扰动的船用旋转起重机数学模型的表示如下：法，其特征在于，所述步骤s1中考虑连续海浪扰动的船用旋转起重机数学模型的表示如下：法，其特征在于，所述步骤s1中考虑连续海浪扰动的船用旋转起重机数学模型的表示如下：τ＝[τ
1 τ
2 τ
3 0 0]
t
τ
f
＝[τ
1f τ
2f τ
3f 0 0]
t
q＝[q
1 q
2 q
3 q
4 q
5]t
q1＝θ
1-α,q2＝θ
2-β,q3＝l(t),q4＝θ
3-α,q5＝θ
4-β,其中，m
s
(q)为船用旋转起重机系统的惯性矩阵，为向心-科里奥利矩阵，g(q)为重力矢量，τ为控制输入矢量，τ
f
为船用旋转起重机系统的机械摩擦力，悬臂的变幅角和旋转角分别为θ1和θ2，船舶的横摇角，偏航角以及升沉扰动分别为α，β和h，负载的径向摆角和切向摆角分别为θ3和θ4，q为耦合之后系统新的状态变量，q1和q2为耦合后的悬臂变幅角和旋转角，q4和q5为耦合后的负载的径向摆角和切向摆角，为一阶导数，为其二阶导数；m与m分别为悬臂和负载的质量，l与l分别为悬臂的长度与吊绳的长度，g为重力加速度，对于驱动力/转矩，τ1为悬臂起幅方向驱动扭矩，τ2为悬臂水平旋转方向驱动扭矩，τ3为悬绳垂直方向驱动扭矩，τ
1f
，τ
2f
和τ
3f
分别为悬臂起幅方向和水平旋转方向的机械摩擦力和悬绳垂直方向的机械摩擦力，d
*
(1,...,5)为船舶升沉引起的扰动。3.根据权利要求1所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s1中动力学特性包括悬臂定位特性以及摆角摆动特性。4.根据权利要求2所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s1中船用旋转起重机数学模型包括摩擦力前馈补偿模型，摩擦力前馈补偿模型的表达如下：
其中，f
11
、f
12
、f
21
、f
22
、f
31
、f
32
为摩擦力前馈补偿模型的参数，f
11
、f
12
和f
13
的值与最大静摩擦力对应，f
21
f
22
和f
23
是粘性摩擦系数，ε是静态摩擦系数。5.根据权利要求2所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s2中设计的扰动观测器为非线性扰动观测器，具体为：法，其特征在于，所述步骤s2中设计的扰动观测器为非线性扰动观测器，具体为：d＝m
s
(q)-1
d
*
其中：p＝[p1p2p3p4p5]
t
，是对与升沉扰动有关的量d的估计；p是辅助向量，l1是r5×5的常数矩阵，其中p和l1里的都是可选取的常数。6.根据权利要求1所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s3中参考轨迹为s型轨迹，表示为：法，其特征在于，所述步骤s3中参考轨迹为s型轨迹，表示为：其中，χ表示耦合后的悬臂起幅角度q1或者耦合后的水平旋转角度q2；q(χ)
d
，q(χ)0和t
q(χ)d
分别为悬臂起幅角度和旋转目标角度/位置、初始角度/位置和到达时间。7.根据权利要求1所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s3中摆角抑制轨迹的设计包括如下：对动力学方程进行线性化处理：对动力学方程进行线性化处理：对动力学方程进行线性化处理：对动力学方程进行线性化处理：对动力学方程进行线性化处理：设计如下李雅普诺夫函数：
对李雅普诺夫方程v
t
进行求导，并且进行数学转换：为了满足设计了如下的摆角抑制轨迹：设计了如下的摆角抑制轨迹：其中，q
1r
(t)、q
2r
(t)以及q
3r
(t)分别是起伏角、旋转角以及绳长的参考轨迹；对上式进行积分，得到：对上式进行积分，得到：其中，8.根据权利要求7所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s3中复合轨迹的表达如下：
q
3c
(t)＝q
3r
(t)。9.根据权利要求5所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，扰动观测器还包括李雅普诺夫方程：其中：为扰动估计误差值；对李雅普诺夫方程v1进行求导：得出其中，p1是一个正定对称矩阵，q1是一个正定矩阵，λ
m
是一个可调矩阵。10.根据权利要求1所述的一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，其特征在于，所述步骤s4中pd控制器的表达如下：法，其特征在于，所述步骤s4中pd控制器的表达如下：法，其特征在于，所述步骤s4中pd控制器的表达如下：其中，e1＝q
1-q
1c
，e2＝q
2-q
2c
，e3＝q
3-q
3c
为误差信号，是双通道扰动观测器所得的匹配扰动估计值，通过使用上述的pd控制器去跟踪复合轨迹，通过pd控制器中的实现了对匹配扰动的抑制，通过复合轨迹中与有关的量，实现了对升沉扰动
引起的摆角的抑制。

技术总结
本发明公开了一种考虑连续海浪扰动的船用旋转起重机实时轨迹规划方法，包括：建立考虑连续海浪扰动的船用旋转起重机数学模型并分析其动力学特性；根据船用旋转起重机数学模型的动力学特性设计扰动观测器；选取参考轨迹，并且通过扰动观测器设计摆角抑制轨迹，将参考轨迹和摆角抑制轨迹进行结合形成复合轨迹；基于船用旋转起重机数学模型、扰动观测器设计PD控制器；利用PD控制器输出船用旋转起重机实时轨迹。本发明实现了对升沉扰动的估计并保证误差值在很小的区间内，在此基础上结合动力学方程，得到实际的升沉扰动加速度信号，同时将参考轨迹与摆角抑制轨迹相结合，最终能够实现高效的轨迹跟踪与摆动抑制，达到控制效果。果。果。


技术研发人员：郗焕 吴强颖 欧阳慧珉
受保护的技术使用者：南京工业大学
技术研发日：2023.07.05
技术公布日：2023/9/20