【技术领域】：

本发明涉及移动机械手的控制技术领域。

本方法主要涉及一种移动机械手的统一动力学建模方法，特别是用广义系统理论对一类非完 整移动机器人进行动力学建模和控制，可用于实体移动机械手的控制器设计。

【背景技术】：

履带式移动机械手系统是由一个机械手固定在一个履带式移动平台上构成。目 前，机械手已被广泛应用于流水生产线产品的抓取操作上。移动机械手同时具有移动和操作两大 功能。平台的移动扩大了机械手的工作空间，使机械手具有几乎无限大的操作空间且能以更加适 合的姿态来执行任务。

一.建立动力学模型是实时控制的需要

移动机器人的数学模型包括运动学模型和动力学模型两部分。建立动力学模型是实时控制的需 要，利用它可以实现最优控制，达到良好的动态性能和最优指标。目前对于移动机械手的控制大多 集中在运动学方面，控制输入为系统的速度，此类控制方法存在如下问题：(1)完全速度跟踪假设 在实际中并不成立；(2)忽略了移动机械手所受干扰对控制系统的影响。这种运动学控制器在系统 运动速度不大，无干扰或干扰很小的情况下还基本能够保证系统的性能，但当系统的运动速度增大， 不确定性较强的情况下，很难保证控制性能。移动机械手的不确定性主要来自两个方面：(1)移动 平台与操作手的相互耦合作用而引起的模型的不精确；(2)由于动态的、非结构化的环境因素使移 动机械手受到的随机干扰，如地面的不规则，轮子的滑动等。

系统的实际控制输入是由驱动器产生的，从动力层面考虑系统的控制问题与实际相符，设计动 力学控制器将会使系统性能和可实现性极大提高。

二.机械手和移动平台的强耦合作用要求设计移动机械手的统一控制器

目前，国内外对移动机械手的控制主要采取分别控制的思想。其代表性研究有：(1)Yamamoto Y用“首选操作区”的概念实现了机械手末端跟踪一个运动物体的表面，其实质是仅考虑机械手一 个子系统的控制；(2)GoldenbergAA为移动机械手的两个子系统分别设计了基于神经网络的控制 器，提出了需要较少控制参数的鲁棒阻尼控制；(3)Chung J H利用非线性交互控制进行运动学冗 余度求解，分别设计了用于机械手控制的鲁棒自适应控制器和用于移动载体的线性化控制器。上 述分散控制的方法可以降低控制器设计的难度，但由于工业机械手质量大，运动速度快，运动过 程中会产生很大的耦合力作用在移动平台上；同时移动平台的运动也会干扰机械手的控制，影响 控制精度。为此有学者尝试将移动机械手看成一个整体进行控制。其代表性的研究有：(1) Jagannathan S通过建立离散的模糊逻辑系统来逼近整个系统的动力学模型，实现移动载体和机械 手的位置和速度跟踪。其优点是可以部分克服动力学耦合和外部扰动的影响，但其不足之处是必 须满足持续的激励条件；(2)Yamamoto Y利用非线性反馈对耦合进行集中补偿，但实验证明这种 补偿对于由平台移动引起的跟踪误差效果较好，但很难补偿由于机械手的运动引起的跟踪误差。

目前，对履带式移动机械手采用统一的运动学和动力学建模加以控制的研究很少，已有方法 对移动平台的非完整约束也缺乏有效的措施，为了解决上述问题，本发明提供了一种履带式移动 机械手的广义系统统一动力学建模方法。

【发明内容】：

本发明目的是克服现有技术存在的上述不足，提供一种履带式移动机械手的广义系 统统一动力学建模方法。

本发明所采用的技术方案是：首先利用绝对坐标计算履带式移动机械手的动能和势能；接着 运用机器人的拉格朗日动力学方法对履带式移动机械手进行统一的动力学建模；最后基于移动机 械手的复杂性及移动平台具有非完整约束，将上述模型改写为广义系统下的统一动力学模型，应 用广义系统的控制理论对其进行建模和研究。

具体建模方法如下：

第一、归纳移动机械手的结构模型，即按照传统的方法提取待研究的移动机械手的主要特征，

得出其结构图，并将其放入绝对坐标系中；

第二、确定各个关节的质心绝对坐标(xci，yci，zci)；

第三、计算整个系统的动能 $K=\underset{i=0}{\overset{n}{\Sigma }}(\frac{1}{2}{m}_i{v}_i^2+\frac{1}{2}{I_i\omega }_i^2)$

其中，用0表示履带式移动平台的参数；i＝1-n分别表示机械手的n个关节，

I为第i关节的转动惯量；Ωi为第i关节的角速度，

vi为第i关节的质心绝对线速度，即 $v_i^2={\stackrel{·}{x}}_{\mathrm{ci}}^2+{\stackrel{·}{y}}_{\mathrm{ci}}^2+{\stackrel{·}{z}}_{\mathrm{ci}}^2$

对履带式移动平台，设其质心坐标为xF，yF，则：

$\left\{\begin{array}{c}\stackrel{·}{x_F}=(\frac{r{K}_r}{2}\cos {\theta }_P-\frac{r{K}_r{l}_F}{2b}\sin {\theta }_P)\stackrel{·}{{\theta }_L}+(\frac{r{K}_l}{2}\cos {\theta }_P+\frac{r{K}_l{l}_F}{2b}\sin {\theta }_P)\stackrel{·}{{\theta }_R}\\ \stackrel{·}{y_F}=(\frac{r{K}_r}{2}\sin {\theta }_P+\frac{r{K}_r{l}_F}{2b}\cos {\theta }_P)\stackrel{·}{{\theta }_L}+(\frac{r{K}_l}{2}\sin {\theta }_P-\frac{r{K}_l{l}_F}{2b}\cos {\theta }_P)\stackrel{·}{{\theta }_R}\end{array}\right.$

kl，kr分别表示左右履带相对于地面实际位移滑动参数，这两个参数的目的是为了引入对履 带-地面之间相互作用的一个估计；

第四、计算势能 $P=\underset{i=1}{\overset{n}{\Sigma }}{m}_{i}g{z}_{\mathrm{ci}};$

第五、计算拉格朗日函数L＝K-P；

第六、编程实现 $T_i=\frac{\partial }{\partial t}\left(\frac{\partial L}{\partial {\stackrel{·}{\theta }}_i}\right)-\frac{\partial L}{\partial {\theta }_i};$

第七、建立广义系统模型

(1)、step6等式右边整理可得： $M\stackrel{··}{q}+V\stackrel{·}{q}+G\left(q\right)=T$

其中q＝(xF，yF，θP，θ1，θ2，θ3，θ4).， $\stackrel{·}{q}=(\stackrel{·}{x_F},\stackrel{·}{y_F},\stackrel{·}{{\theta }_P},\stackrel{·}{{\theta }_1},\stackrel{·}{{\theta }_2},\stackrel{·}{{\theta }_3},\stackrel{·}{{\theta }_4})$

$K=\frac{1}{2}{\stackrel{·}{q}}^{T}M\stackrel{·}{q},V(q,\stackrel{·}{q})=(\stackrel{·}{M}+\frac{1}{2}\stackrel{·}{q}\frac{\partial M}{\partial q}),G\left(q\right)=\frac{\partial P}{\partial p}$

(2)、将2整理为 $\stackrel{··}{q}=M^{-1}T-M^{-1}V\stackrel{·}{q}-M^{-1}G$

(3)、考虑非完整约束 $\stackrel{·}{x_F}\sin {\theta }_P-\stackrel{·}{y_F}\cos {\theta }_P-l_F\stackrel{·}{{\theta }_P}=0$

建立广义系统模型 $E\stackrel{·}{X}=f\left(x\right)+g\left(x\right)T$

其中： $\left\{\begin{array}{c}{x}_1=q\\ x_2=\stackrel{·}{q}\end{array}\right.,$ $E={\left[\begin{array}{ccc}{I}_7& & \\ & I_7& \\ & & 0\end{array}\right]}_{15\times 15}$

$f\left(x\right)={\left[\begin{array}{c}{x}_2\\ M^{-1}[V{x}_2-G\left(x_1\right)]\\ h(x_1,x_2)\end{array}\right]}_{15\times 1},$ $g\left(z\right){\left[\begin{array}{c}0\\ M^{-1}B\\ 0\end{array}\right]}_{15\times 1}.$

本发明的优点和积极效果：

本发明方法主要涉及一种移动机械手的统一动力学建模方法，特别是用广义系统理论对一类 非完整移动机器人进行动力学建模和控制，其优势在于：(1)可以有效克服原来方法中分别对移 动平台和机械手进行控制时，由于忽略移动平台和机械手之间的耦合力而造成的控制误差；(2) 说明书中给出的程序框图可以应用于任何类型的移动机械手动力学建模，使建模方法模式化，方 便实施；(3)将非完整约束用广义系统的代数方程来表示，可以直接用广义系统已有的控制方法 直接对这类移动机械手设计控制器，有效的克服了非完整约束所带来的控制难度。将此方法应用 于天津理工大学的一个履带式移动机械手上，控制效果比较理想(见实施例1)。

【附图说明】：

图1是本发明所用机器人的结构示意图；

图2是本发明的程序框图；

图3是采用本发明的方法建立模型后，用反馈线性化控制时得到的控制力矩，其中，

图3-A，是移动平台上机械手的关节1的输入力矩图；

图3-B，是移动平台上机械手的关节2的输入力矩图；

图3-C，是移动平台上机械手的关节3的输入力矩图；

图3-D，是移动平台上机械手的关节4的输入力矩图；

图3-E，是移动平台右轮的输入力矩图；

图3-F，是移动平台左轮的输入力矩图；

图4是采用本发明的方祛建立模型后，用反馈线性化控制时得到的关节跟踪曲线，其中，

图4-A，是移动平台上机械手的关节1跟踪阶跃响应的曲线；

图4-B，是移动平台上机械手的关节2跟踪阶跃响应的曲线；

图4-C，是移动平台上机械手的关节3跟踪阶跃响应的曲线；

图4-D，是移动平台上机械手的关节4跟踪阶跃响应的曲线；

图4-E，是移动平台从起始点(-1，0)开始跟踪(0，0)为圆心，半径为2的圆的曲线。

【具体实施方式】：

本发明提供的履带式移动机械手的广义系统统一动力学建模方法的具体步骤如下：

第一、归纳移动机械手的结构模型，即按照传统的方法提取待研究的移动机械手的主要特征， 得出其结构图，并将其放入绝对坐标系中；

第二、确定各个关节的质心绝对坐标(xci，yci，zci)；

第三、计算整个系统的动能 $K=\underset{i=0}{\overset{n}{\Sigma }}(\frac{1}{2}{m}_i{v}_i^2+\frac{1}{2}{I_i\omega }_i^2)$

其中，用0表示履带式移动平台的参数；i＝1-n分别表示机械手的n个关节，

I为第i关节的转动惯量；Ωi为第i关节的角速度，

vi为第i关节的质心绝对线速度，即 $v_i^2={\stackrel{·}{x}}_{\mathrm{ci}}^2+{\stackrel{·}{y}}_{\mathrm{ci}}^2+{\stackrel{·}{z}}_{\mathrm{ci}}^2$

对履带式移动平台，设其质心坐标为xF，yF，则：

$\left\{\begin{array}{c}\stackrel{·}{x_F}=(\frac{r{K}_r}{2}\cos {\theta }_P-\frac{r{K}_r{l}_F}{2b}\sin {\theta }_P)\stackrel{·}{{\theta }_L}+(\frac{r{K}_l}{2}\cos {\theta }_P+\frac{r{K}_l{l}_F}{2b}\sin {\theta }_P)\stackrel{·}{{\theta }_R}\\ \stackrel{·}{y_F}=(\frac{r{K}_r}{2}\sin {\theta }_P+\frac{r{K}_r{l}_F}{2b}\cos {\theta }_P)\stackrel{·}{{\theta }_L}+(\frac{r{K}_l}{2}\sin {\theta }_P-\frac{r{K}_l{l}_F}{2b}\cos {\theta }_P)\stackrel{·}{{\theta }_R}\end{array}\right.$

kl，kr分别表示左右履带相对于地面实际位移滑动参数，这两个参数的目的是为了引入对履 带一地面之间相互作用的一个估计；

第四、计算势能 $P=\underset{i=1}{\overset{n}{\Sigma }}{m}_{i}g{z}_{\mathrm{ci}};$

第五、计算拉格朗日函数L＝K-P；

第六、编程实现 $T_i=\frac{\partial }{\partial t}\left(\frac{\partial L}{\partial {\stackrel{·}{\theta }}_i}\right)-\frac{\partial L}{\partial {\theta }_i};$

第七、建立广义系统模型

(1)、step6等式右边整理可得： $M\stackrel{··}{q}+V\stackrel{·}{q}+G\left(q\right)=T$

其中q＝(xF，yF，θP，θ1，θ2，θ3，θ4).， $\stackrel{·}{q}=(\stackrel{·}{x_F},\stackrel{·}{y_F},\stackrel{·}{{\theta }_P},\stackrel{·}{{\theta }_1},\stackrel{·}{{\theta }_2},\stackrel{·}{{\theta }_3},\stackrel{·}{{\theta }_4})$

$K=\frac{1}{2}{\stackrel{·}{q}}^{T}M\stackrel{·}{q},V(q,\stackrel{·}{q})=(\stackrel{·}{M}+\frac{1}{2}\stackrel{·}{q}\frac{\partial M}{\partial q}),$ $G\left(q\right)=\frac{\partial P}{\partial p}$

(2)、将2整理为 $\stackrel{··}{q}=M^{-1}T-M^{-1}V\stackrel{·}{q}-M^{-1}G$

(3)、考虑非完整约束 $\stackrel{·}{x_F}\sin {\theta }_P-\stackrel{·}{y_F}\cos {\theta }_P-l_F\stackrel{·}{{\theta }_P}=0$

建立广义系统模型 $E\stackrel{·}{X}=f\left(x\right)+g\left(x\right)T$

其中： $\left\{\begin{array}{c}{x}_1=q\\ x_2=\stackrel{·}{q}\end{array}\right.,$ $E={\left[\begin{array}{ccc}{I}_7& & \\ & I_7& \\ & & 0\end{array}\right]}_{15\times 15}$

$f\left(x\right)={\left[\begin{array}{c}{x}_2\\ M^{-1}[V{x}_2-G\left(x_1\right)]\\ h(x_1,x_2)\end{array}\right]}_{15\times 1},$ $g\left(z\right){\left[\begin{array}{c}0\\ M^{-1}B\\ 0\end{array}\right]}_{15\times 1}.$

实施例1：

具体参数如下：

车体几何参数：r＝0.105；kl＝1；kr＝1；lf＝0.09；b＝0.205；l0＝0.6；

机械手几何参数：l1＝0.1；l2＝0.6；l3＝0.35；l4＝0.1；

机械手质心位置：pc1＝0.07/0.1；pc2＝0.27/0.6；pc3＝0.112/0.35；pc4＝0.031/0.1；

移动机械手各关节质心高度：zc0＝0.25；ll0＝0.25；ll1＝0.6；zc1＝ll1+pc1*l1；

移动机械手各关节质量：m0＝25；m1＝6.84；m2＝3.24；m3＝1.54；m4＝1.74；

重力加速度：g＝9.8

移动机械手各关节转动惯量：j0＝0.5512；j1＝0.077；j2＝0.18；j3＝0.035；j4＝0.005

用说明书所述的方法建立广义系统统一动力学模型后，直接加入统一的PID协调控制器， $\tau =k_d\stackrel{·}{e}+k_{p}e,$ 取kp＝diag(50 50 50 30 30 30 30)，kd＝diag(100 100 100 100 100 100 100)， 即可得到图3，图4的控制效果。克服了以往控制中分别对移动平台和机械手建模，分别设计控制 器时，由于忽略移动平台和机械手之间的耦合力而造成的控制误差。