CN104793497A - 基于多体系统离散时间传递矩阵法的机器人动力学建模方法 - Google Patents

Abstract

本发明公开了一种履带式移动机器人的多体系统离散时间传递矩阵法统一动力学建模方法。其核心包括：建立多体系统动力学模型化整为零，确定状态矢量；确定各元件传递矩阵U和传递方程z_O＝Uz_I；由元件的传递矩阵拼装系统的总传递矩阵U_all和总传递矩阵方程U_allz_all＝0；根据边界条件，求解系统传递方程，得到t_i时刻边界状态矢量；应用元件的传递方程，求得t_i时刻系统各联接点状态矢量。本发明避免了以往研究移动式机器人时对移动平台和机械臂分别建模，而忽略其耦合和相互影响所带来的误差，同时理论性强，思路清晰，编程容易实现，可达到对这类移动机器人精确控制的目的。

Description

基于多体系统离散时间传递矩阵法的机器人动力学建模方法

技术领域

本发明涉及移动机器人的动力学建模技术领域。

本方法主要涉及一种履带式移动机器人的统一动力学建模方法，特别是用多体系统理论对一类移动机器人进行动力学建模，可用于移动机器人的控制器设计。

背景技术

近些年，由于地震、海啸等自然灾害频发及恐怖组织等人为因素对全球各国造成了巨大的生命和财产损失。随着机器人技术的迅速发展，救援机器人在地震灾害、核污染等复杂、危险环境代替人类完成检测、搜寻和营救等任务。履带式移动机器人系统是由一个机械手固定在一个履带式移动平台上构成。移动机器人同时具有移动和操作两大功能。平台的移动扩大了机械手的工作空间，使机械手具有几乎无限大的操作空间且能以更加适合的姿态来执行任务。救援机器人普遍存在体积大、能耗高、作业能力差和动态特性差等方面的问题，因此，设计轻质、低耗、适应性强、动态特性好的多功能救援机器人，成为这一研究领域的关键问题。

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

移动机器人的数学模型包括运动学模型和动力学模型两部分。且运动学及动力学建模与分析是其它各方面研究的基础，也是救援机器人开展救援工作的前提。但从现有的研究来看，基于救援机器人运动学的研究远远多于基于动力学的研究。为了实现机器人移动机构的快速运动，通常比较简单的方法是基于运动学提出轨迹规划算法，控制其速度。但是，当救援任务对机器人的动力特性要求较高的情况下，仅考虑运动学因素是不够的，建立动力学模型是实时控制的需要，利用它可以实现最优控制，达到良好的动态性能和最优指标。目前，有关救援机器人动力学的研究还不充分，其原因是救援机器人动力学模型较运动学复杂得多，而基于动力学的控制结构设计难度大、计算效率低。由于动态的、非结构化的环境因素使移动机械手受到的随机干扰，如地面的不规则，轮子的滑动等。系统的实际控制输入是由驱动器产生的，从动力层面考虑系统的控制问题与实际相符，设计动力学控制器将会使系统性能和可实现性极大提高。

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

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

目前，对履带式移动机器人采用统一的运动学和动力学建模加以控制的研究很少，已有方法对移动平台的非完整约束也缺乏有效的措施。

发明内容

本发明目的是克服现有技术存在的上述不足，提供一种履带式移动机器人的基于多体系统离散时间传递矩阵法的统一动力学建模方法。利用刚性多体系统离散时间传递矩阵法，建立具有单臂操作的救援机器人整体较完备的动力学模型，研究系统的动力学特性，对于丰富和深化救援机器人的研究内容，实现救援机器人高性能的运动和动力特性具有重要的理论意义和应用价值。

本发明所采用的技术方案是：

基于多体系统离散时间传递矩阵法的移动机器人动力学建模方法，建立多体系统动力学模型化整为零，确定状态矢量；确定各元件传递矩阵U和传递方程z_O＝Uz_I；由元件的传递矩阵拼装系统的总传递矩阵U_all和总传递矩阵方程U_allz_all＝0；根据边界条件，求解系统传递方程，得到t_i时刻边界状态矢量；应用元件的传递方程，求得t_i时刻系统各联接点状态矢量。

具体建模方法如下：

第一、归纳履带式移动机器人的结构模型，即按照传统的方法提取待研究的履带式移动机器人的主要特征，包括车体、机械臂杆件、转动关节亦即中心刚体和机械臂末端，其结构示意模型如附图1所示；

第二、化整为零，将该机器人整体机械结构机械进行编号：0—地面，1—车体，2,4,6—中心刚体，3,5,7—机械臂，8—机械臂末端；

第三、确定该机器人各个元件的质心坐标、外形尺寸、质量等基本参数信息，如表1所示：

表1

给出车体运动规律为：

$x_1=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

中心刚体运动规律：

$\mathrm{\θ}=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{\mathrm{\π}}{6}\·[\frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}]& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

式中，t—历时时间；

T_a—运动起始的延时时间；

T_b—终止时间；

第四、由各个刚体连体坐标系的坐标经转换得到惯性系下的坐标，

r_O＝r_I+r_IO，

r_C＝r_I+r_IC，

即由原点固定在连体坐标系输入端I的位置坐标、质心C的位置坐标(c₁,c₂,c₃)、输出端O的位置坐标(b₁,b₂,b₃)和转角等计算出刚体输出端相对于惯性系原点的矢径r_O，刚体质心相对于惯性系原点的矢径r_C，r_IC＝Al_IC，r_IO＝Al_IO及其一、二阶导数。

式中，r_O输出点O在惯性系中的位置坐标；

r_I输入点I在惯性系中的位置坐标；

r_C质心C在惯性系中的位置坐标；

r_IO输出点O相对于输入点I在惯性系中的位置坐标；

r_IC质心C相对于输入点I在惯性系中的位置坐标；

A—连体坐标系向惯性系的坐标转换矩阵；

$A=\left[\begin{array}{ccc}{c}_y{c}_z& s_x{c}_y{c}_z-c_x{s}_z& c_x{s}_y{c}_z+s_x{c}_z\\ c_y{s}_z& s_x{s}_y{s}_z+c_x{c}_z& c_x{s}_y{s}_z-s_x{c}_z\\ -s_y& s_x{c}_y& c_x{c}_y\end{array}\right]$

$A\left(\mathrm{ti}\right)=A\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_i\right)+\mathrm{\Φ}\left(t_{i-1}\right)$

其中：

$\begin{array}{c}\mathrm{\Φ}\left(t_{i-1}\right)=A\left(t_{i-1}\right)\{I-{\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_{i-1}\right)-{\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_{i-1}\right)-{\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_{i-1}\right)+[\frac{1}{2}{{\tilde{T}}_1}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_x}^2\left(t_{i-1}\right)\\ +\frac{1}{2}{{\tilde{T}}_2}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_y}^2\left(t_{i-1}\right)+\frac{1}{2}{{\tilde{T}}_3}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_z}^2\left(t_{i-1}\right)+{\tilde{T}}_2\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_y\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\\ +{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_y\left(t_{i-1}\right)+{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\left]\mathrm{\Δ}{T}^2\right\}\end{array}$

l_IC质心C相对于输入点I在连体坐标系中的位置坐标；

l_IO输出点O相对于输入点I在连体坐标系中的位置坐标；

$\begin{array}{ccc}{T}_1=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],& T_2=\left[\begin{array}{c}0\\ c_x\\ -s_x\end{array}\right],& T_3=\left[\begin{array}{c}-s_y\\ s_x{c}_y\\ c_x{c}_y\end{array}\right]\end{array},$

是叉乘矩阵；

s_x、s_y、s_z分别为不共面三轴的sin角位移，c_x、c_y、c_z分别为不共面三轴的cos角位移；

第五、采用速度(角速度)和加速度(角加速度)的线性化、坐标转换矩阵的线性化以及Newmark-β逐步积分法等对状态矢量进行线性化计算，推导空间运动刚体的传递矩阵U_i，

$U_i=\left[\begin{array}{ccccc}{I}_3& {\mathrm{\Ψ}}_{\mathrm{IO}}& O_{3\×3}& O_{3\×3}& \mathrm{\Φ}\left(t_{i-1}\right)l_{\mathrm{IO}}\\ O_{3\×3}& I_3& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ U_{31}& U_{32}& I_3& U_{34}& U_{35}\\ -{\mathrm{mAI}}_3& -\mathrm{nA}{\mathrm{\Ψ}}_{\mathrm{IC}}& O_{3\×3}& I_3& U_{45}\\ O_{1\×3}& O_{1\×3}& O_{1\×3}& O_{1\×3}& 1\end{array}\right]$

式中，

${\mathrm{\Ψ}}_{\mathrm{IO}}=A\left(t_{i-1}\right)\left[\begin{array}{ccc}{\tilde{T}}_1\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_2\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_3\left(t_{i-1}\right)l_{\mathrm{IO}}\end{array}\right];$

I₃＝[1,0,0；0,1,0；0,0,1]；

O_3X3＝[0,0,0；0,0,0；0,0,0]；

O_1X3＝[0,0,0]；

m为刚体质量；

U₃₁、U₃₂、U₃₄、U₃₅、U₄₅分别为对应关节的传递矩阵；

第六、根据第三步确定的已知参数信息，并应用MATLAB分别编写i＝1时的传递矩阵，即得到t_i时刻编号分别为0-7的各个元件的传递矩阵即U₁，U₂…U₇，由得到的传递矩阵再求得i>1时的传递矩阵；

第七、将各个关节相对运动坐标值变换到坐标系的绝对值，即将传递矩阵最后一列重新附值。如下所示，

UU1＝eye(13)；UU2＝eye(13)；UU3＝eye(13)；

UU1(5,13)＝UU1(5,13)+thy_10(i)；

UU2(5,13)＝UU2(5,13)+thy_20(i)；

UU2(6,13)＝UU2(6,13)+thz_20(i)；

UU3(5,13)＝UU3(5,13)+thy_30(i)；

UU3(6,13)＝UU3(6,13)+thz_30(i)；

第八、由各个元件的传递矩阵，拼装系统的总传递矩阵U_all；

U_all＝U₇U₆U₅U₄U₃U₂U₁；

第九、确定边界条件，

$\left\{\begin{array}{c}{z}_{\mathrm{1,0}}={[x_I,y_I,z_I,\mathrm{0,0,0,0,0,0},F,\mathrm{0,0,1}]}_{\mathrm{1,0}}^T\\ z_{\mathrm{8,7}}={[x_o,y_o,z_o,{\mathrm{\θ}}_{o1},{\mathrm{\θ}}_{o2},{\mathrm{\θ}}_{o3},\mathrm{0,0,0,0,0,0,1}]}_{\mathrm{8,7}}^T\end{array}\right.;$

第十、应用MATLAB编程实现求解，求解系统传递方程Z_8,7＝U_allZ_0,1；得到各个元件的位移，角位移，内力矩与内力曲线图，如附图3～图14所示。

本发明的优点和积极效果

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

附图说明

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

图2是本发明的流程图。

图3是机器人各部件在X方向的位移。

图4是机器人各部件在Y方向的位移。

图5是机器人各部件在Z方向的位移。

图6是机器人各部件在X方向旋转的角度。

图7是机器人各部件在Y方向旋转的角度。

图8是机器人各部件在Z方向旋转的角度。

图9是机器人各部件在X方向的内力矩。

图10是机器人各部件在Y方向的内力矩。

图11是机器人各部件在Z方向的内力矩。

图12是机器人各部件在X方向的内力。

图13是机器人各部件在Y方向的内力。

图14是机器人各部件在Z方向的内力。

具体实施方式

实施例1、基于多体系统离散时间传递矩阵法的移动机器人动力学建模方法

本发明方法包括：

第一、归纳履带式移动机器人的结构模型，即按照传统的方法提取待研究的履带式移动机器人的主要特征，其结构模型如附图1所示，包括车体、机械臂杆件、转动关节(中心刚体)和机械臂末端。

第二、化整为零，将该机器人整体机械结构进行编号：0—地面，1—车体，2,4,6—中心刚体，3,5,7—机械臂，8—机械臂末端。

第三、确定该机器人各个元件的质心坐标，外形尺寸，质量等基本参数信息，如表1所示：

表1

给出车体运动规律为：

$x_1=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

中心刚体运动规律：

$\mathrm{\θ}=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{\mathrm{\π}}{6}\·[\frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}]& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

式中，t—历时时间；本例中T_a＝0.02ms；T_b＝5ms；且第一连杆绕y轴转角初始角度θ_1y＝π/6，第二连杆绕z轴转角初始角度θ_2z＝π/6，第三连杆绕z轴转角初始角度θ_3z＝π/6。

第四、由各个刚体连体坐标系的坐标经转换得到惯性系下的坐标，

r_O＝r_I+r_IO

r_C＝r_I+r_IC

即由原点固定在连体坐标系输入端I的位置坐标、质心C的位置坐标(c₁,c₂,c₃)、输出端O的位置坐标(b₁,b₂,b₃)，转角等计算出刚体输出端相对于惯性系原点的矢径r_O，刚体质心相对于惯性系原点的矢径r_C，r_IC＝Al_IC，r_IO＝Al_IO及其一、二阶导数。

式中，r_O输出点O在惯性系中的位置坐标(取值详见表1)；

r_I输入点I在惯性系中的位置坐标(取值详见表1)；

r_C质心C在惯性系中的位置坐标(取值详见表1)；

r_IO输出点O相对于输入点I在惯性系中的位置坐标(取值详见表1)；

r_IC质心C相对于输入点I在惯性系中的位置坐标(取值详见表1)；

$A=\left[\begin{array}{ccc}{c}_y{c}_z& s_x{c}_y{c}_z-c_x{s}_z& c_x{s}_y{c}_z+s_x{c}_z\\ c_y{s}_z& s_x{s}_y{s}_z+c_x{c}_z& c_x{s}_y{s}_z-s_x{c}_z\\ -s_y& s_x{c}_y& c_x{c}_y\end{array}\right]$

$A\left(\mathrm{ti}\right)=A\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_i\right)+\mathrm{\Φ}\left(t_{i-1}\right)$

其中：

$\begin{array}{c}\mathrm{\Φ}\left(t_{i-1}\right)=A\left(t_{i-1}\right)\{I-{\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_{i-1}\right)-{\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_{i-1}\right)-{\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_{i-1}\right)+[\frac{1}{2}{{\tilde{T}}_1}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_x}^2\left(t_{i-1}\right)\\ +\frac{1}{2}{{\tilde{T}}_2}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_y}^2\left(t_{i-1}\right)+\frac{1}{2}{{\tilde{T}}_3}^2\left(t_{i-1}\right){{\stackrel{\·}{\mathrm{\θ}}}_z}^2\left(t_{i-1}\right)+{\tilde{T}}_2\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_y\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\\ +{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_y\left(t_{i-1}\right)+{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{\·}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\left]\mathrm{\Δ}{T}^2\right\}\end{array}$

l_IC质心C相对于输入点I在连体坐标系中的位置坐标(取值详见表1)；

l_IO输出点O相对于输入点I在连体坐标系中的位置坐标(取值详见表1)；

$T_1=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],T_2=\left[\begin{array}{c}0\\ c_x\\ -s_x\end{array}\right]=\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right],T_3=\left[\begin{array}{c}-s_y\\ s_x{c}_y\\ c_x{c}_y\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$

是叉乘矩阵；

s_x、s_y、s_z分别为不共面三轴的sin角位移，c_x、c_y、c_z分别为不共面三轴的cos角位移；

在本例中A—连体坐标系向惯性系的坐标转换矩阵值为：

$A=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

第五、采用速度(角速度)和加速度(角加速度)的线性化，坐标转换矩阵的线性化以及Newmark-β逐步积分法等对状态矢量进行线性化计算，推导空间运动刚体的传递矩阵U_i，

$U_i=\left[\begin{array}{ccccc}{I}_3& {\mathrm{\Ψ}}_{\mathrm{IO}}& O_{3\×3}& O_{3\×3}& \mathrm{\Φ}\left(t_{i-1}\right)l_{\mathrm{IO}}\\ O_{3\×3}& I_3& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ U_{31}& U_{32}& I_3& U_{34}& U_{35}\\ -{\mathrm{mAI}}_3& -\mathrm{nA}{\mathrm{\Ψ}}_{\mathrm{IC}}& O_{3\×3}& I_3& U_{45}\\ O_{1\×3}& O_{1\×3}& O_{1\×3}& O_{1\×3}& 1\end{array}\right]$

式中，

${\mathrm{\Ψ}}_{\mathrm{IO}}=A\left(t_{i-1}\right)\left[\begin{array}{ccc}{\tilde{T}}_1\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_2\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_3\left(t_{i-1}\right)l_{\mathrm{IO}}\end{array}\right];$

I₃＝[1,0,0；0,1,0；0,0,1]；

O_3X3＝[0,0,0；0,0,0；0,0,0]；

O_1X3＝[0,0,0]；

m为刚体质量(取值详见表1)；

U₃₁、U₃₂、U₃₄、U₃₅、U₄₅分别为对应关节的传递矩阵；

第六、根据已知参数信息，并应用MATLAB分别编写i＝1时的传递矩阵，即得到t_i时刻0-7的各个元件的传递矩阵即U₁，U₂…U₇，由得到的传递矩阵再求得i>1时的传递矩阵；

第七、将各个关节相对运动坐标值变换到坐标系的绝对值，即将传递矩阵最后一列重新附值。如下所示，

UU1＝eye(13)；UU2＝eye(13)；UU3＝eye(13)；

UU1(5,13)＝UU1(5,13)+thy_10(i)；

UU2(5,13)＝UU2(5,13)+thy_20(i)；

UU2(6,13)＝UU2(6,13)+thz_20(i)；

UU3(5,13)＝UU3(5,13)+thy_30(i)；

UU3(6,13)＝UU3(6,13)+thz_30(i)；

第八、由各个元件的传递矩阵，拼装系统的总传递矩阵U_all；

U_all＝U₇U₆U₅U₄U₃U₂U_1°

第九、确定边界条件，

$\left\{\begin{array}{c}{z}_{\mathrm{1,0}}={[x_I,y_I,z_I,\mathrm{0,0,0,0,0,0},F,\mathrm{0,0,1}]}_{\mathrm{1,0}}^T\\ z_{\mathrm{8,7}}={[x_o,y_o,z_o,{\mathrm{\θ}}_{o1},{\mathrm{\θ}}_{o2},{\mathrm{\θ}}_{o3},\mathrm{0,0,0,0,0,0,1}]}_{\mathrm{8,7}}^T\end{array}\right..$

第十、应用MATLAB编程实现求解，求解系统传递方程Z_8,7＝U_allZ_0,1；得到各个元件的位移，角位移，内力矩与内力曲线图。

图3～图5为救援机器人各个元件分别在x、y、z三个方向的位移，从图中可以看出，在x方向即机器人整体的前进方向位移逐步增加；y方向的5、6、7三个元件的位移增加，而其余保持基本不变；z方向的5、6、7三个元件的位移逐步减小，而其余保持基本不变；符合运动规律。图6～图8为救援机器人各个元件在x、y、z三个方向的转角位移，各个元件在x方向均无转角；元件2-7在y方向均有转角，且转动规律相同，元件1无变化；元件4-7在z方向均有转角，而1-3元件没有转角变化，元件6、7和元件4、5的转角位移分别相同，符合运动规律。图9～图14为传递矩阵法得到的各方向内力矩与内力曲线图。图中所示内力矩曲线即是机械臂实现运动关节所提供的驱动力矩。可以看出相比其他部件，机器人末端关节中心刚体6和杆件7受到的内力矩相同且最小，杆件7所受内力最小。在运动过程中，内力与内力矩曲线较为平稳无突变，以此来选择电动机可以满足运动要求。如附图3～14所示。

Claims (1)

1.基于多体系统离散时间传递矩阵法的机器人动力学建模方法，其特征在于该方法包括：

第一、归纳履带式移动机器人的结构模型，即按照传统的方法提取待研究的履带式移动机器人的主要特征，包括车体、机械臂杆件、转动关节亦即中心刚体和机械臂末端；

第二、化整为零，将该机器人整体机械结构进行编号：0—地面，1—车体，2,4,6—中心刚体，3,5,7—机械臂杆件，8—机械臂末端；

第三、确定该机器人各个元件的质心坐标，外形尺寸，质量的基本参数信息：

给出车体运动规律为：

$x_I=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

各中心刚体运动规律：

$\mathrm{\θ}=\left\{\begin{array}{cc}0& x\≤T_a\\ \frac{\mathrm{\π}}{6}\·[\frac{t-T_a}{T_b-T_a}-\frac{1}{2\mathrm{\π}}\sin \frac{2\mathrm{\πt}}{T_b-T_a}]& T_a\≤x\≤T_b\\ 0& x\≥T_b\end{array}\right.$

式中，t—历时时间；

T_a—运动起始的延时时间；

T_b—终止时间；

第四、由各个刚体连体坐标系的坐标经转换得到惯性系下的坐标，

r_O＝r_I+r_IO，

r_C＝r_I+r_IC，

即由原点固定在连体坐标系输入端I的位置坐标、质心C的位置坐标(c₁，c₂，c₃)、输出端O的位置坐标(b₁，b₂，b₃)和转角计算出刚体输出端相对于惯性系原点的矢径r_O，刚体质心相对于惯性系原点的矢径r_C，r_IC＝Al_IC，r_IO＝Al_IO及其一、二阶导数，

式中，r_O输出点O在惯性系中的位置坐标；

r_I输入点I在惯性系中的位置坐标；

r_C质心C在惯性系中的位置坐标；

r_IO输出点O相对于输入点I在惯性系中的位置坐标；

r_IC质心C相对于输入点I在惯性系中的位置坐标；

A—连体坐标系向惯性系的坐标转换矩阵；

$A=\left[\begin{array}{ccc}{c}_y{c}_z& s_x{c}_y{c}_z-c_x{s}_z& c_x{s}_y{c}_z+s_x{c}_z\\ c_y{s}_z& s_x{s}_y{s}_z+c_x{c}_z& c_x{s}_y{s}_z-s_x{c}_z\\ -s_y& s_x{c}_y& c_x{c}_y\end{array}\right]$

$A\left(\mathrm{ti}\right)=A\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_i\right)+A\left(t_{i-1}\right){\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_i\right)+\mathrm{\Φ}\left(t_{i-1}\right)$

其中：

$\begin{array}{c}\mathrm{\Φ}\left(t_{i-1}\right)=A\left(t_{i-1}\right)\{I-{\tilde{T}}_1\left(t_{i-1}\right){\mathrm{\θ}}_x\left(t_{i-1}\right)-{\tilde{T}}_2\left(t_{i-1}\right){\mathrm{\θ}}_y\left(t_{i-1}\right)-{\tilde{T}}_3\left(t_{i-1}\right){\mathrm{\θ}}_z\left(t_{i-1}\right)+[\frac{1}{2}{{\tilde{T}}_x}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_x}^2\left(t_{i-1}\right)\\ +\frac{1}{2}{{\tilde{T}}_2}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_y}^2\left(t_{i-1}\right)+\frac{1}{2}{{\tilde{T}}_3}^2\left(t_{i-1}\right){{\stackrel{.}{\mathrm{\θ}}}_z}^2\left(t_{i-1}\right)+{\tilde{T}}_2\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_y\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\\ +{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_2\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_y\left(t_{i-1}\right)+{\tilde{T}}_3\left(t_{i-1}\right){\tilde{T}}_1\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_z\left(t_{i-1}\right){\stackrel{.}{\mathrm{\θ}}}_x\left(t_{i-1}\right)\left]{\mathrm{\ΔT}}^2\right\}\end{array}$

l_IC质心C相对于输入点I在连体坐标系中的位置坐标；

l_IO输出点O相对于输入点I在连体坐标系中的位置坐标；

$T_1=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],T_2=\left[\begin{array}{c}0\\ c_x\\ -s_x\end{array}\right],T_3=\left[\begin{array}{c}-s_y\\ s_x{c}_y\\ c_x{c}_y\end{array}\right],$

是叉乘矩阵；

s_x、s_y、s_z分别为不共面三轴的sin角位移，c_x、c_y、c_z分别为不共面三轴的cos角位移；

第五、采用速度或角速度和加速度或角加速度的线性化、坐标转换矩阵的线性化以及Newmark-β逐步积分法对状态矢量进行线性化计算，推导空间运动刚体的传递矩阵U_i，

$U_i=\left[\begin{array}{ccccc}{I}_3& {\mathrm{\Ψ}}_{\mathrm{IO}}& O_{3\×3}& O_{3\×3}& \mathrm{\Φ}\left(t_{i-1}\right)l_{\mathrm{IO}}\\ O_{3\×3}& I_3& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ U_{31}& U_{32}& I_3& U_{34}& U_{35}\\ -m{\mathrm{AI}}_3& -m{\mathrm{A\Ψ}}_{\mathrm{IC}}& O_{3\×3}& I_3& U_{45}\\ O_{1\×3}& O_{1\×3}& O_{1\×3}& O_{1\×3}& 1\end{array}\right]$

式中，

${\mathrm{\Ψ}}_{\mathrm{IO}}=A\left(t_{i-1}\right)\left[\begin{array}{ccc}{\tilde{T}}_1\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_2\left(t_{i-1}\right)l_{\mathrm{IO}}& {\tilde{T}}_3\left(t_{i-1}\right)l_{\mathrm{IO}}\end{array}\right];$

I₃＝[1,0,0；0,1,0；0,0,1]；

O_3X3＝[0,0,0；0,0,0；0,0,0]；

O_1X3＝[0,0,0]；

m为刚体质量；

U₃₁、U₃₂、U₃₄、U₃₅、U₄₅分别为对应关节的传递矩阵；

第六、根据第三步确定的已知参数信息，并应用MATLAB分别编写i＝1时的传递矩阵，即得到t_i时刻编号分别为0-7的各个元件的传递矩阵U₁，U₂…U₇，由得到的传递矩阵再求得i>1时的传递矩阵；

第七、将各个中心刚体相对运动坐标值变换到坐标系的绝对值，即将传递矩阵最后一列重新附值；如下所示：

UU₁＝eye(13)；UU₂＝eye(13)；UU₃＝eye(13)；

UU₁(5,13)＝UU₁(5,13)+thy_10(i)；

UU₂(5,13)＝UU₂(5,13)+thy_20(i)；

UU₂(6,13)＝UU₂(6,13)+thz_20(i)；

UU₃(5,13)＝UU₃(5,13)+thy_30(i)；

UU₃(6,13)＝UU₃(6,13)+thz_30(i)；

第八、由各个元件的传递矩阵拼装系统的总传递矩阵U_all；

U_all＝U₇U₆U₅U₄U₃U₂U₁；

第九、确定边界条件，

$\left\{\begin{array}{c}{z}_{\mathrm{1,0}}={{[x}_I,y_I,z_I,\mathrm{0,0,0,0,0,0},F,\mathrm{0,0,1}]}_{\mathrm{1,0}}^T\\ z_{\mathrm{8,7}}=[x_o,y_o,z_o,{\mathrm{\θ}}_{o1},{\mathrm{\θ}}_{o2},{\mathrm{\θ}}_{o3},\mathrm{0,0,0,0,0,0,1}{]}_{\mathrm{8,7}}^T\end{array}\right.$

第十、应用MATLAB编程实现求解，求解系统传递方程Z_8,7＝U_allZ_0,1；得到各个元件的位移、角位移、内力矩与内力曲线图。