CN108132598B - 移动装弹机械臂系统动力学模型与轨迹控制 - Google Patents

Abstract

一种移动装弹机械臂系统动力学模型与轨迹控制，属于控制技术领域。本发明步骤是：首先利用拉格朗日方程建立了移动装弹机械臂系统的动力学模型，并通过仿真检验模型的准确性；然后设计了滑模变结构轨迹跟踪控制器；考虑系统存在参数不确定性和外部干扰影响时，该不确定状态难以测量获得，在滑模变结构轨迹跟踪控制器的基础上，引入自适应模糊控制思想逼近不确定部分的上界，重新推导出自适应模糊滑模控制器。本发明所设计的控制器可以很好地跟踪上系统的期望轨迹，达到了预期控制目标。

Description

移动装弹机械臂系统动力学模型与轨迹控制

技术领域

本发明属于控制技术领域。

背景技术

导弹发射时的速度一直受到导弹转载速度的制约，如果在导弹发射过程中采用人工指挥吊车完成导弹转载工作，必定耗费人力物力，同时准确性及实时性也难以保证。移动装弹机械臂是一种装置在汽车类移动平台上，将吊装炮弹与运输转载两种任务集于一身的机械臂系统，通常情况下是由支腿、转台、机架和起重吊臂等四大部分组成。与传统工业机械臂系统相较，移动装弹机械臂系统体现出功能多、效率高、平稳性强、快速性好及作用范围大等特点，因此近年来此类大型移动机械臂受到越来越多学者的密切关注。

有些文献基于微分同胚和非线性输入变换，将移动机械臂的动力学模型分解成四个低维的子系统，采用滑模控制和二阶滑模控制设计了鲁棒输出跟踪控制器，解决了非完整约束情况下惯性参数的不确定性问题。有些文献以四轮移动平台和三自由度吊臂组成的系统为对象，分别设计了滑模控制器和非奇异终端滑模控制器，并比较了两种控制器的性能。有些文献首先建立了移动吊臂的动力学模型并做了简化处理，然后在参考坐标系下，研究了包含电机驱动动力学的滑模变结构控制问题。有些文献在系统存在不确定性和外扰作用下，研究了非完整约束轮式移动吊臂滑模轨迹跟踪控制问题，并利用backstepping法保证了速度跟踪性能。有些文献基于串级控制思想，设计了两个控制器：一个是考虑控制饱和的最小范数运动学控制器，另一个是动力学补偿器。有些文献在不加装关节力/力矩传感器的情况下，针对多移动吊臂搬运负载的物理系统，设计了滑模控制方法，该方法可以确保跟踪误差趋近于0。有些文献针对滑模控制的抖振问题，研究了移动机械臂的模糊滑模控制方法，利用模糊控制改善了滑模趋近律，有效抑制了控制力矩的抖振。

发明内容

本发明的目的是为了解决上述存在的问题，根据装弹机械臂的各个关节的特点，针对每个关节机理设计一个移动装弹机械臂系统动力学模型。

本发明步骤是：

①首先利用拉格朗日方程建立了移动装弹机械臂系统的动力学模型，并通过仿真检验模型的准确性；

②然后设计了滑模变结构轨迹跟踪控制器；

③考虑系统存在参数不确定性和外部干扰影响时，该不确定状态难以测量获得，在滑模变结构轨迹跟踪控制器的基础上，引入自适应模糊控制思想逼近不确定部分的上界，重新推导出自适应模糊滑模控制器。

本发明的具体步骤是：

①装弹机械臂动力学建模：

首先求得系统的总动能T和总势能V：

⑴连杆1的动能和势能：

⑵连杆2上任一点在惯性坐标系的表示

连杆2的动能和势能：

⑶连杆3上任一点在惯性坐标系的表示

连杆3的动能和势能

⑷连杆4上面任一点在惯性坐标系的表示，连杆4即伸缩杆，

设伸缩杆中每段的最大伸缩长度为ΔD，当伸缩杆全部缩回时的长度为固定在连杆3中的长度D，当一个伸缩杆伸出Δx，则3段伸缩杆总伸缩长度d＝3Δx；根据多级伸缩结构可知，伸缩部分的整体伸出长度为D+2d/3；

伸缩杆子系统的动能和势能

⑸连杆5上任一点在惯性坐标系的表示

连杆5的动能和势能

⑹矩形刚体的夹具视为连杆6，连杆6上任一点在惯性坐标系的表示

连杆6的动能和势能

其中，J₁和J₂分别为连杆1和关节2的转动惯量；ρ₁为连杆1的面密度；A₁为微元面积；ρ₂，ρ₃，ρ₅和ρ₆分别为连杆2、连杆3、连杆5和吊具部分矩形刚体的线密度；m₂，m₃和m₆分别为关节2、关节3和末端回转关节的质量；M₄为伸缩杆的总质量；l₁，l₂，l₃，l₅和l₆分别为连杆1、连杆2、连杆3、连杆5和矩形刚体的长度；

⑺将所求动能与势能带入拉格朗日方程

其中，L＝T-V，q是广义坐标，Q是广义力，n为连杆个数；

整理得系统动力学方程为

其中，M(q)为惯性矩阵；

为离心力与哥氏力项；G(q)为重力项；Q＝τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T为广义力；q＝[θ₁,θ₂,θ₃,d₄,θ₇,θ₈,]^T为广义坐标；

②滑模轨迹控制器：

⑴将系统动力学方程整理成

其中ψ表示系统总体不确定性；ΔM(q)，

ΔG(q)为系统参数中的不确定性；d为外部干扰项；

为系统模型的标称参数；以下将

简写成

其它类似；

⑵不确定性ψ具有已知上界ψ^*，即存在：||ψ||≤ψ^*

定义位置误差、速度误差以及滑模面

e＝q_d-q (24)

设计滑模控制器为

其中，k和λ均为正定对角矩阵；

③自适应模糊滑模轨迹控制器

⑴将含有不确定性的移动装弹机械臂系统动力学方程式(22)改写为

其中

为系统的总体不确定项的变形形式；

⑵利用模糊系统逼近机械臂系统动力学中的总体不确定性F，从而使得控制器中不再出现难以测量的ψ^*

其中

⑶定义θ_Fi的最优参数估计为

⑷定义系统的最小逼近误差项

⑸不确定性ψ具有上界，但是该上界未知，则F亦有未知上界，即||F||≤F^*；

⑹系统最小逼近误差ω有界，即||ω||≤p^*，其中p^*＝||ω||_max；

⑺考虑移动装弹机械臂系统动力学模型式(28)，以及第⑸、第⑹步定义式(27)中的位置误差和滑模面形式，设计自适应模糊滑模控制器为

选取自适应更新律为

其中，K和λ为正定对角矩阵，Γ_F为对称正定矩阵。

本发明应用拉格朗日方程对移动装弹机械臂系统进行了数学建模，得到了系统的动力学模型。当系统存在外界干扰和参数不确定性时，通过定义总体不确定项，并从不确定性项上界已知和未知两种情况，分别设计了滑模控制器和自适应模糊滑模控制器。仿真结果表明，本申请所设计的控制器可以很好地跟踪上系统的期望轨迹，达到了预期控制目标。

附图说明

图1是移动装弹机械臂模型示意图；

图2是伸缩连杆(连杆4)示意图；

图3是关节角的跟踪曲线图，其中从(a)至(f)分别是从关节1至关节6的位置图；

图4是两种控制下轨迹跟踪误差曲线比较，其中从(a)至(f)分别是从关节1至关节6的误差曲线比较图。

具体实施方式

由于装弹机类的大型移动机械臂是一个高度非线性、强耦合的非线性系统。装载机械臂的移动平台一般质量较重、动力学响应慢，与此同时，整个系统还存在模型参数的不确定性、未建模动态及外界干扰等多种不确定性因素的影响，因此动力学建模和控制问题变得十分复杂。尤其是当系统不确定性难以测量，且上界未知的情况下，传统的滑模变结构控制便难以应用。

本发明的步骤是：

①首先利用拉格朗日方程建立了移动装弹机械臂系统的动力学模型，并通过仿真检验模型的准确性；

②然后设计了滑模变结构轨迹跟踪控制器；

③考虑系统存在参数不确定性和外部干扰影响时，该不确定状态难以测量获得，在滑模变结构轨迹跟踪控制器的基础上，引入自适应模糊控制思想逼近不确定部分的上界，重新推导出自适应模糊滑模控制器。

移动装弹机械臂由连杆、套筒、伸缩关节、旋转关节和回转关节组成，其简化结构如图1所示。吊臂的基座定义为自由度1，调整末端姿态关节定义为自由度8，二者皆为回转关节，可以通过控制回转的角度，实现调节机械臂抓取炮弹的空间位置指向；吊臂的两个旋转关节分别定义为自由度2和3；吊臂还有一部分具有三段式同步伸缩功能，定义为自由度4，5和6，该结构可以有效让吊臂末端达到更远的位置；此外，吊臂还有一个腕关节，可定义为自由度7，该关节主要是考虑到炮弹质量过重，为减小末端重物产生的附加力矩，从而保证负载的方向始终垂直向下。

本发明的具体步骤是：

①装弹机械臂动力学建模：

首先求得系统的总动能T和总势能V：

⑴连杆1的动能和势能：

⑵连杆2上任一点在惯性坐标系的表示

⑵连杆2的动能和势能：

⑶连杆3上任一点在惯性坐标系的表示

连杆3的动能和势能

⑷连杆4上面任一点在惯性坐标系的表示，连杆4即伸缩杆，

由于伸缩关节坐标建立在伸缩杆的末端，因此在计算伸缩部分的动能和势能时需要考虑伸缩关节及下一关节的动能和势能。设三个伸缩杆中每段的最大伸缩长度为ΔD，当伸缩杆全部缩回时的长度为固定在连杆3中的长度D，由于伸缩部分采用套筒式同步结构，且为等速伸缩，即当一个伸缩杆伸出Δx，则3段伸缩杆总伸缩长度d＝3Δx；根据多级伸缩结构可知，伸缩部分的整体伸出长度为D+2d/3；

伸缩杆子系统的动能和势能

⑸连杆5上任一点在惯性坐标系的表示

连杆5的动能和势能

⑹矩形刚体的夹具视为连杆6，连杆6上任一点在惯性坐标系的表示

连杆6的动能和势能

其中，J₁和J₂分别为连杆1和关节2的转动惯量；ρ₁为连杆1的面密度；A₁为微元面积；ρ₂，ρ₃，ρ₅和ρ₆分别为连杆2、连杆3、连杆5和吊具部分矩形刚体的线密度；m₂，m₃和m₆分别为关节2、关节3和末端回转关节的质量；M₄为伸缩杆的总质量；l₁，l₂，l₃，l₅和l₆分别为连杆1、连杆2、连杆3(伸缩杆的基臂)、连杆5和矩形刚体的长度。

⑺将所求动能与势能带入拉格朗日方程

其中，L＝T-V，q是广义坐标，Q是广义力，n为连杆个数；

整理得系统动力学方程为

其中，M(q)为惯性矩阵；

为离心力与哥氏力项；G(q)为重力项；Q＝τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T为广义力；q＝[θ₁,θ₂,θ₃,d₄,θ₇,θ₈,]^T为广义坐标。

②滑模轨迹控制器也叫滑模变结构轨迹跟踪控制器：

⑴移动装弹机械臂的动力学方程是一个非线性、强耦合、时变的复杂系统，考虑到实际系统中会存在参数不确定性和外部干扰等影响，将系统动力学方程整理成

其中ψ表示系统总体不确定性；ΔM(q)，

ΔG(q)为系统参数中的不确定性；d为外部干扰项；

为系统模型的标称参数；为推导控制器书写方便，以下将

简写成

其它类似。

⑵不确定性ψ具有已知上界ψ^*，即存在：||ψ||≤ψ^*

定义位置误差、速度误差以及滑模面

e＝q_d-q (24)

考虑道移动装弹机械臂系统动力学模型如式(22)，以及不确定性ψ具有的已知上界ψ*，若设计滑模控制器为

则可使得该机械臂系统具有良好的轨迹跟踪性能，并可以保证系统闭环渐近稳定。其中，k和λ均为正定对角矩阵。

证明1：选取Lyapunov函数如下

对式(26)求导，并把式(24)(25)(27)带入得

对V₁求导，把式(b)带入

中，可得

由于有式(c)的存在，因此

V₁(s(t))≤V₁(s(0))(d)

即函数V₁为非增且有界的函数，V₁(s(∞))存在。因此可知，s(t)是有界的。此时定义V₂＝-s^Tks，则有：

对函数V₂进行积分有：

由于

有界，根据Barbalat引理可知，当t→∞时，V₂(t)→0，也就是s(t)→0。此外，若s＝0，轨迹跟踪误差也将渐近趋近于零，证毕。

③自适应模糊滑模轨迹控制器

⑴在实际工程现场，系统不确定性的上界是很难测量得到的，因此含有ψ^*的控制器基本不可实现，这就要求寻找一种不含ψ^*的机械臂轨迹跟踪控制器。由于模糊系统可以任意精度逼近非线性函数，因此将含有不确定性的移动装弹机械臂系统动力学方程式(22)改写为

其中

为系统的总体不确定项的变形形式。

⑵利用模糊系统逼近机械臂系统动力学中的总体不确定性F，从而使得控制器中不再出现难以测量的ψ^*

其中

⑶定义θ_Fi的最优参数估计为

⑷定义系统的最小逼近误差项

⑸不确定性ψ具有上界，但是该上界未知，则F亦有未知上界，即||F||≤F^*。

⑹系统最小逼近误差ω有界，即||ω||≤p^*，其中p^*＝||ω||_max。

⑺考虑移动装弹机械臂系统动力学模型式(28)，以及第⑸、第⑹步定义式(27)中的位置误差和滑模面形式，设计自适应模糊滑模控制器为

选取自适应更新律为

则可使得该机械臂系统具有良好的轨迹跟踪性能，并可以保证系统闭环渐近稳定。其中，K和λ为正定对角矩阵，Γ_F为对称正定矩阵。

证明2：选取Lyapunov函数如下

对式(A)关于时间求导数得

考虑到

因此对滑模面式(26)求一阶导数，并把(32)代入化简得

考虑到

把式(33)(34)(D)(E)和(F)一起代入式(B)中化简整理有

由于有式(G)的存在，因此

与证明1的证明类似，可根据Barbalat引理证得系统跟踪误差将渐近趋近于零，证毕。

仿真研究

为了检验系统建模的准确性以及所设计控制器的有效性，对军事工程中某型号移动装弹机械臂进行了仿真研究。系统参数选取如表1所示：

表1移动装弹机械臂系统参数

为验证建模的准确性，首先保证移动装弹机械臂的动力学模型参数满足如下两条性质：

性质1：惯性矩阵M为对称正定矩阵；

性质2：

为反对称矩阵。

然后，通过给定期望的关节角度表达式计算期望力矩，再把期望力矩代入到模型的实际力矩中，得到实际的关节角度曲线，比较两曲线的结果。

期望关节角度轨迹选取如下：

关节1：θ₁(t)＝π/12+0.1t+0.1t²+0.1t³；

关节2：θ₂(t)＝π/6+0.2t+0.1t²+0.01t³；

关节3：θ₃(t)＝π/3+0.1t+0.1t²+0.01t³；

伸缩部分：d(t)＝0.3t+0.1t²+0.1t³；

关节5：θ₅(t)＝-0.1t-0.2t²-0.01t³；

吊具：θ₆(t)＝-π/8+0.1t+0.1t²+0.2t³。

仿真时间设定为3s，仿真结果如图3所示。

从图3的仿真结果可以看出，装弹移动机械臂的实际关节角度与设定的期望关节角度基本匹配，从而验证了该机械臂动力学建模的准确性。这为研究装弹移动机械臂的轨迹跟踪控制问题奠定了理论基础。

自适应模糊系统采用单点模糊化、乘积推理和中心平均加权解模糊所构成的模糊逻辑系统对系统总体不确定性进行逼近。自适应参数

和

的初值分别选择为：

模糊系统其他参数为：r_f＝50,r_p＝0.1,n＝5。

控制器中的其它控制参数选取

λ＝diag(15,10,10,100,5,20),

Γ_F＝diag(10,10,10,10,10,10).

期望关节角度的轨迹选取与模型验证时的选取相同。

仿真时间设定为8s，结果如图4所示

图4是机械臂在两种控制器作用下关节轨迹跟踪误差曲线。在滑模控制器的作用下，移动装弹机械臂的关节至少3秒后便可跟踪上期望轨迹，机械臂在自适应模糊滑模控制器作用下，关节2和关节3跟踪上期望轨迹最多只需2秒即可，且关节1和关节8均为回转关节，关节4实为套筒伸缩部分，它们的跟踪时间与旋转关节相比更短，均在0.5秒左右。比较图4中的两组误差曲线可知，当系统存在未知上界的不确定性和外部干扰时，自适应模糊滑模控制器的控制性能较常规滑模控制器更好。

图1说明：移动装弹机械臂由连杆、套筒、伸缩关节、旋转关节和回转关节组成，其简化结构如图1所示。吊臂的基座定义为关节1，调整末端姿态安装夹持器的部分关节定义为关节6，二者皆为回转关节，可以通过控制回转的角度，实现调节机械臂抓取炮弹的空间位置指向；吊臂的关节2和关节3是两个旋转关节；吊臂的关节4是一个伸缩关节，它具有三段式同步伸缩功能，用d4，d5和d6便是伸缩关节的三个套筒结构(具体伸缩原理见图2)，该结构可以有效让吊臂末端达到更远的位置；吊臂的关节5类似人的腕关节，主要是考虑到炮弹质量过重，为减小末端重物产生的附加力矩，从而保证负载的方向始终垂直向下。

Claims (1)

1.一种移动装弹机械臂系统动力学模型与轨迹控制，其特征在于：

①首先利用拉格朗日方程建立了移动装弹机械臂系统的动力学模型，并通过仿真检验模型的准确性；

②然后设计了滑模变结构轨迹跟踪控制器；

③考虑系统存在参数不确定性和外部干扰影响时，该不确定状态难以测量获得，在滑模变结构轨迹跟踪控制器的基础上，引入自适应模糊控制思想逼近不确定部分的上界，重新推导出自适应模糊滑模控制器；

具体过程如下：

①装弹机械臂动力学建模：

首先求得系统的总动能T和总势能V：

⑴连杆1的动能和势能：

⑵连杆2上任一点在惯性坐标系的表示

连杆2的动能和势能：

⑶连杆3上任一点在惯性坐标系的表示

连杆3的动能和势能

⑷连杆4上面任一点在惯性坐标系的表示，连杆4即伸缩杆，

设伸缩杆中每段的最大伸缩长度为ΔD，当伸缩杆全部缩回时的长度为固定在连杆3中的长度D，当一个伸缩杆伸出Δx，则3段伸缩杆总伸缩长度d＝3Δx；根据多级伸缩结构可知，伸缩部分的整体伸出长度为D+2d/3；

伸缩杆子系统的动能和势能

⑸连杆5上任一点在惯性坐标系的表示

连杆5的动能和势能

⑹矩形刚体的夹具视为连杆6，连杆6上任一点在惯性坐标系的表示

连杆6的动能和势能

其中，J₁和J₂分别为连杆1和关节2的转动惯量；ρ₁为连杆1的面密度；A₁为微元面积；ρ₂，ρ₃，ρ₅和ρ₆分别为连杆2、连杆3、连杆5和吊具部分矩形刚体的线密度；m₂，m₃和m₆分别为关节2、关节3和末端回转关节的质量；M₄为伸缩杆的总质量；l₁，l₂，l₃，l₅和l₆分别为连杆1、连杆2、连杆3、连杆5和矩形刚体的长度；

⑺将所求动能与势能带入拉格朗日方程

其中，L＝T-V，q是广义坐标，Q是广义力，n为连杆个数；

整理得系统动力学方程为

其中，M(q)为惯性矩阵；

为离心力与哥氏力项；G(q)为重力项；Q＝τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T为广义力；q＝[θ₁,θ₂,θ₃,d₄,θ₇,θ₈,]^T为广义坐标；

②滑模轨迹控制器：

⑴将系统动力学方程整理成

其中ψ表示系统总体不确定性；ΔM(q)，

ΔG(q)为系统参数中的不确定性；d为外部干扰项；

为系统模型的标称参数；以下将

简写成

其它类似；

⑵不确定性ψ具有已知上界ψ^*，即存在：||ψ||≤ψ^*

定义位置误差、速度误差以及滑模面

e＝q_d-q (24)

设计滑模控制器为

其中，k和λ均为正定对角矩阵；

③自适应模糊滑模轨迹控制器

⑴将含有不确定性的移动装弹机械臂系统动力学方程式(22)改写为

其中

为系统的总体不确定项的变形形式；

⑵利用模糊系统逼近机械臂系统动力学中的总体不确定性F，从而使得控制器中不再出现难以测量的ψ^*

其中

⑶定义θ_Fi的最优参数估计为

⑷定义系统的最小逼近误差项

⑸不确定性ψ具有上界，但是该上界未知，则F亦有未知上界，即||F||≤F^*；

⑹系统最小逼近误差ω有界，即||ω||≤p^*，其中p^*＝||ω||_max；

⑺考虑移动装弹机械臂系统动力学模型式(28)，以及第⑸、第⑹步定义式(27)中的位置误差和滑模面形式，设计自适应模糊滑模控制器为

自适应更新律为

其中，K和λ为正定对角矩阵，Γ_F为对称正定矩阵。

Images