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

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

背景技术：

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

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

技术实现要素：

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

本发明步骤是：

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

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

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

本发明的具体步骤是：

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

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

⑴连杆1的动能和势能：

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

连杆2的动能和势能：

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

连杆3的动能和势能

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

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

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

⑸连杆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+2d3；

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

⑸连杆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类似人的腕关节，主要是考虑到炮弹质量 过重，为减小末端重物产生的附加力矩，从而保证负载的方向始终垂直向下。