空间二连杆机器人的动力学建模及其动态过程仿真

空间二连杆机器人的动力学建
模
及其动态过程仿真
作者：td
一引言
1.机器人机械臂的运动学与动力学分析方法
目录
空间二连杆机器人的动力学建模 (1)
及其动态过程仿真 (1)
作者：td (1)
一引言 (1)
1.1用户界面模块（ADAMS/View） (4)
1.2求解器模块（ADAMS/Solver） (5)
1.3后处理模块（ADAMS/PostProcessor） (6)
二．空间二连杆机器人adams建模仿真 (6)
2.1空间二连杆串联机器人 (6)
在ADAMS中用长方形连杆模拟机械臂，对两自由度的机械臂分别进行运动学分析动力学分析。

(6)
2.1.1运动学分析 (6)
2.1.2运动学分析 (9)
机器人的运动学和动力学既包含有一般机械的运动学、动力学内容，又反映了机器人的
独特内容。

工业机器人的运动学主要讨论了运动学的正问题和逆问题。

假设一个构型已知的机器人，即它的所有连杆长度和关节角度()1q t ,()2q t ,()3q t …()n q t ，…都是已知的，其中n 为自由度数，那么计算机器人末端执行器相对于参考坐标系的位姿就称为运动学的正问题分析。

换言之，如果已知机器人所有的关节变量，用正运动学方程就能计算任一瞬间机器人的位姿。

然而，如果希望机器人的末端执行器到达一个期望的位姿，就必须要知道机器人操作臂每一个连杆的几何参数和所有关节的角矢量()12,,T n q q q q =⋅⋅⋅利用操作臂连杆几何参数和末端执行器期望的位姿来求解关节角矢量是运动学逆问题。

运动学正问题可以利用齐次变换法来求解。

设i 杆坐标系相对于基座坐标系的位姿齐次变换矩阵是b i T ，则
1231b i n n T A A A A A -=⋅⋅⋅⋅⋅⋅ ()11-
式中i A 为i 杆坐标系相对于1i -杆坐标系的坐标变换矩阵。

相对于正运动学方程，机器人逆运动学方程显得更为重要。

由于按给定末端执行器的位姿求解关节变量是在关节空间中进行非线性方程的求解，其中涉及多值性和奇异现象，因此，这一逆问题成为机器人运动学中的一个重要内容。

机器人的控制器将用这些方程来计算关节值，并以此来运行机器人到达期望的位姿。

机器人逆问题可有多种解法，如逆变换法、旋量代数法、数值迭代法、几何法等，其中Jaeobian 矩阵的速算法占有重要的地位。

机器人作为多自由度可编程的工作系统，在运动学中研究的内容还有末端操作器运动规划、工作空间确定、位姿精度分析与补偿等。

目前，对于一般机器人运动学的逆问题大部分都得到了解决，但是，对于有任意结构和有冗余自由度机器人的运动学逆问题，研究得还不够充分。

机器人操作臂的动力学建模主要是研究各主动关节的驱动力与操作臂运动的关系。

机器
人操作臂是一个十分复杂的动力学系统。

机器人动力学方程的非线性特点和强耦合性使得对它的研究十分困难和复杂。

目前人们已经提出了许多种动力学建模方法，分别基于不同的力学方程和原理。

C ．T ．Lin ，Calafiore 等对Newton —Euler 动力学建模方法和Lagrange 方法在简化递推过程及减少运算次数上做了不少有益的工作；有些学者从计算机符号代数方向推导并行算法来进行研究；T ．R ．Kane 等发展了利用偏速度和广义力建模的Kane 方程法；有些学者利用广义d ’Alembert 原理来进行建模；还有人研究用图论进行机器人动力学分析的方法。

其中以Newton —Euler 动力学建模方法及d ’Alembert 建模方法(或以这两种方法为基础)应用最为普遍。

Newton —Euler 方法具有递推的形式，非常适合于数值计算，与
Lagrange方法相比效率要高很多，它是至今最有效、也是使用最广泛的一种方法。

但实现经典控制的动力学实时算法还有待继续研究和改进。

考虑到构件的弹性变形，近年来，有限元法、结构动力学与多体系统动力学理论相继被引入到机器人动力学的研究中，并已取得一些研究成果。

对于关节柔性机器人以及考虑到运动副间隙影响的机器人动力学问题，目前国内外研究得还比较少。

2.Simulink介绍
简介
Simulink是MATLAB最重要的组件之一，它提供一个动态系统建模、仿真和综合分析的集成环境。

在该环境中，无需大量书写程序，而只需要通过简单直观的鼠标操作，就可构造出复杂的系统。

Simulink具有适应面广、结构和流程清晰及仿真精细、贴近实际、效率高、灵活等优点，并基于以上优点Simulink已被广泛应用于控制理论和数字信号处理的复杂仿真和设计。

同时有大量的第三方软件和硬件可应用于或被要求应用于Simulink。

功能
Simulink是MATLAB中的一种可视化仿真工具，是一种基于MATLAB的框图设计环境，是实现动态系统建模、仿真和分析的一个软件包，被广泛应用于线性系统、非线性系统、数字控制及数字信号处理的建模和仿真中。

Simulink可以用连续采样时间、离散采样时间或两种混合的采样时间进行建模，它也支持多速率系统，也就是系统中的不同部分具有不同的采样速率。

为了创建动态系统模型，Simulink提供了一个建立模型方块图的图形用户接口(GUI) ，这个创建过程只需单击和拖动鼠标操作就能完成，它提供了一种更快捷、直接明了的方式，而且用户可以立即看到系统的仿真结果。

Simulink是用于动态系统和嵌入式系统的多领域仿真和基于模型的设计工具。

对各种时变系统，包括通讯、控制、信号处理、视频处理和图像处理系统，Simulink提供了交互式图形化环境和可定制模块库来对其进行设计、仿真、执行和测试。

.
构架在Simulink基础之上的其他产品扩展了Simulink多领域建模功能，也提供了用于设计、执行、验证和确认任务的相应工具。

Simulink与MATLAB紧密集成，可以直接访问MATLAB 大量的工具来进行算法研发、仿真的分析和可视化、批处理脚本的创建、建模环境的定制以及信号参数和测试数据的定义。

特点
具有丰富的可扩充的预定义模块库和交互式的图形编辑器来组合和管理直观的模块图；
它以设计功能的层次性来分割模型，实现了对复杂设计的管理；通过Model Explorer 导航、创建、配置、搜索模型中的任意信号、参数、属性来生成模型代码。

它提供API用于与其他仿真程序的连接或与手写代码集成并使用Embedded MATLAB™模块在Simulink和嵌入式系统执行中调用MATLAB算法。

在Simulink中可使用定步长或变步长运行仿真，根据仿真模式(Normal,Accelerator,Rapid Accelerator)来决定以解释性的方式运行或以编译C代码的形式来运行模型。

它具有图形化的调试器和剖析器来检查仿真结果，诊断设计的性能和异常行为。

并可访问MATLAB从而对结果进行分析与可视化，定制建模环境，定义信号参数和测试数据。

3.ADAMS软件简介
ADAMS是以计算多体系统动力学（Computational Dynamics of Multibody Systems）为基础，包含多个专业模块和专业领域的虚拟样机开发系统软件，利用它可以建立复杂机械系统的运动学和动力学模型，其模型可以是刚体的，也可以是柔性体，以及刚柔混合体模型。

如果在产品的概念设计阶段就采取ADAMS进行辅助分析，就可以在建造真实的物理样机之前，对产品进行各种性能测试，达到缩短开发周期、降低开发成本的目的。

ADAMS软件使用交互式图形环境和零件库、约束库、力库，创建完全参数化的机械系统几何模型，其求解器采用多刚体系统动力学理论中的拉格朗日方程方法，建立系统动力学方程，对虚拟机械系统进行静力学、运动学和动力学分析，输出位移、速度、加速度和反作用力曲线。

ADAMS软件的仿真可用于预测机械系统的性能、运动范围、碰撞检测、峰值载荷以及计算有限元的输入载荷等。

1.1用户界面模块（ADAMS/View）
ADAMS/View是ADAMS系列产品的核心模块之一，采用以用户为中心的交互式图形环境，将图标操作，菜单操作，鼠标点击操作与交互式图形建模，仿真计算，动画显示，优化设计，X-Y曲线图处理，结果分析和数据打印等功能集成在一起。

ADAMS/View采用简单的分层方式完成建模工作。

采用Parasolid内核进行实体建模，并提供了丰富的零件几何图形库，约束库和力/力矩库，并且支持布尔运算，支持FORTRAN/77和FORTRAN/90中的函数。

除此之外，还提供了丰富的位移函数，速度函数，加速度函数，接触函数，样条函数，力/力矩函数，合力/力矩函数，数据元函数，若干用户子程序函数以及常量和变量等[3]。

ADAMS/View模块界面如图1.1所示。

图1.1 ADAMS/View界面
1.2求解器模块（ADAMS/Solver）
ADAMS/Solver是ADAMS系列产品的核心模块之一，是ADAMS产品系列中处于心脏地位的仿真器。

该软件自动形成机械系统模型的动力学方程，提供静力学，运动学和动力学的解算结果。

ADAMS/Solver有各种建模和求解选项，以便精确有效地解决各种工程应用问题。

ADAMS/Solver可以对刚体和弹性体进行仿真研究。

为了进行有限元分析和控制系统研究，用户除要求软件输出位移，速度，加速度和力外，还可要求模块输出用户自己定义的数据。

用户可以通过运动副，运动激励，高副接触，用户定义的子程序等添加不同的约束。

用户同时可求解运动副之间的作用力和反作用力，或施加单点外力。

1.3后处理模块（ADAMS/PostProcessor）
MDI公司开发的后处理模块ADAMS/Postprocessor，用来处理仿真结果数据，显示仿真动画等。

既可以在ADAMS/View环境中运行，也可脱离该环境独立运行。

ADAMS/PostProcessor的主要功能是为观察模型的运动提供了所需的环境，用户可以向前，向后播放动画，随时中断播放动画，而且可以选择最佳观察视角，从而使用户更容易地完成模型排错任务；为了验证ADAMS仿真分析结果数据的有效性，可以输入测试数据，并测试数据与仿真结果数据进行绘图比较，还可对数据结果进行数学运算，对输出进行统计分析；用户可以对多个模拟结果进行图解比较，选择合理的设计方案；可以帮助用户再现ADAMS中的仿真分析结果数据，以提高设计报告的质量；可以改变图表的形式，也可以添加标题和注释；可以载入实体动画，从而加强仿真分析结果数据的表达效果；还可以实现在播放三维动画的同时，显示曲线的数据位置，从而可以观察运动与参数变化的对应关系。

二．空间二连杆机器人adams建模仿真
2.1空间二连杆串联机器人
在ADAMS中用长方形连杆模拟机械臂，对两自由度的机械臂分别进行运动学分析动力学分析。

2.1.1运动学分析
下面是建立模型并对模型进行
设置分析的详细过程。

（1）启动ADAMS/View，在欢迎对
话框中选择新建模型，模型取名为
Robot_arm，并将单位设置为MMKS，
然后单击OK。

（2）打开坐标系窗口。

按下
2.1创建连杆设置 2.2 孔设置
F4键，或者单击菜单【View】→【Coordinate Window】后，打开坐标系窗口。

当鼠标在图形区移动时，在坐标窗口中显示了当前鼠标所在位置的坐
（3）创建机械臂关节1（连杆）。

单击方形杆按钮，勾选连杆的长、宽、深
选项，分别将其设置为50.0cm、4.0cm、4.0cm，如图2.1所示。

在图形区单击鼠标左键，然后将连杆拖至水平位置时，在单击鼠标左键。

（4）在方形杆的右端面打孔。

在几何建模工具栏单击打孔按钮，将
径Radius设置为1.0cm，深度设置为1.0cm，如图2.2所示。

然后通过菜单栏，选择修改工作平面为YOZ平面。

在图形区模型附近单击鼠标左键，
在与方形杆的YOZ平面上单击鼠标左键。

然后可以通过【Edit】→【Move】修改孔的位置到（500，0.0，0.0），如图2.3
所示。

（5）用（3）的方法在关节1右端孔中
心处创建关节2，如图2.4所示。

然后通
过【Edit】→【Move】修改关节2与方
形杆右端面重合。

2.3孔的位置
（6）如步骤（5），在连杆二的左端面
创建1kg的球体，用于模拟机械手抓取
物体的状况。

（
（
2.4 二连杆模型
（7）添加约束。

在关节1的左端与大地之
间添加转动副，在关节1与关节2结合处添加转动副。

单击工具栏中的旋转副按钮，并将创建旋转副的选项设置为2Bod-1Loc 和Normal Grid，然后在图形区单击关节1和大地，之后需要选择一个作用点，将鼠标移动到关节1的Marker1处出现center 信息时，按下鼠标左键后就可以创建旋转副，旋转副的轴垂直于工作栅格。

然后用同样的方法创建关节1与关节2之间的旋转副，如图 2.5所示。

2.5创建旋转副
2.6 旋转驱动设置
（8）按同样方法在连杆2与小球之间添加固
定副。

（9）添加驱动。

在运动副1（Joint1）和运
动副2（Joint2）上分别添加旋转驱动。

单击
主工具栏的旋转驱动按钮，然后在选择上
面创建的旋转副1，然后在图形区单击鼠标
右键，在快捷菜单中中选择Modify，在编辑
对话框中将驱动函数设置为10d*sin(time)，
如图2.6所示。

用同样的方法在旋转副2上创建
旋转驱动，并将驱动函数设置为10d*time*(-1)。

（10）运行仿真计算。

单击主工具栏的仿真计算
按钮，将仿真类型设置为Default，仿真时间End
Time设置为25，仿真步数Steps设置为500，然 2.7 末端运动轨迹
后单击运行按钮进行仿真计算。

（11）绘制运动轨迹。

单击菜单【Review】→【Create Trace Spline】，然后选择小球中心的Marker点，再选择关节1与大地的铰接点，鼠标移动到Joint1处，单击鼠标右键，在弹出对话框中选择ground，单击OK创建运动轨迹，如图2.7所示。

2.8 后处理模块界面
（12）结果后处理。

按下键盘上的F8键，界面将从View模块直接进入到PostProcess模块，后处理模块界面如图2.8所示。

在后处理模块，通过菜单【View】→【Load Animation】可以载入动画。

在仿真动画中可以播放两种动画，一种是在时间域内进行的运动学和动力学仿真计算动画；另一种是在频率域内的，播放通过现行化或者在震动模块中的计算模型
的振型动画。

单击播放按钮后开始播放动画，如果在播放同时按下记录按钮，在播放动画的同时也将动画保存到动画文件中，动画文件位于ADAMS的工作目录下。

2.9 机械臂末端速度曲线
在后处理模块中，通过菜单【View】→【Load Plot】，通过选择相应的选项，绘制出相应的结果曲线。

如果2.9、2.10所示，分别绘制出机械臂末端点的速度曲线和加速度曲线。

2.10 机械臂末端加速度曲线
2.1.2运动学分析
（1）创建机械臂模型。

按照2.1.1节的（1）~（6）步创建同样的机械臂，
并添加运动副约束。

（2）添加驱动。

与运动分析不同，动力学分析添加的驱动为单分量力矩。

单击工具栏上的单分量力矩选项，将选项设置为Space Fixed、Normal to Grid
和Constant，然后勾选Torque项并输入1，然后在图形区单击关节1，再在其上单击任何一点。

用同样的方法添加关节2的驱动，并将其值设置为-2，如图2.11所示。

（3）运动学计算仿真。

单击
菜单【Simulate】→【Iteractive
Controls】，打开交互式仿真控制
对话框，在对话框中将仿真时间
End Time设置为10，仿真步数
Steps设置为5000，仿真类型Type
设置为Dynamic，单击仿真计算
按钮，观看仿真动画，模型将在
重力和驱动力矩作用下运动。

2.11 添加单分量力矩
（4）绘制运动轨迹。

单击菜单【Review】→【Create Trace Spline】，然后选择
关节2右端面的Marker点，再选择关
节1与大地的铰接点，鼠标移动到Joint1
处，单击鼠标右键，在弹出对话框中选
择ground，单击OK创建运动轨迹，如
图2.12所示。

（5）结果后处理。

在后处理模块，
通过菜单【View】→【Load Animation】
可以载入动画。

单击播放按钮后开始播
放动画，在播放同时按下记录按钮，将动画保存到动画文件中。

2.11末端运动轨迹
2.13 机械臂x方向末端速度曲线
在后处理模块中，通过菜单【View】→【Load Plot】，通过选择相应的选项，绘制出相应的结果曲线。

如果2.13、2.14所示，分别绘制出小球的速度曲线和加速度曲线。

2.14 机械臂末端x方向加速度曲线
三．空间二连杆机器人Simulink 建模仿真
1.模型参数的确
定
由于Newton —Euler 方法适合于数值计算，且具有概念
清晰，推导形式化的优点。

而空间二连杆连杆数目较少，在此可以选用
Newton —Euler 方法建立运动学和动力学模型。

为了便于和adams 模型计算结果相比较，
以验证matlab 建模的正确性，matlab 模型的参数均采用adams 中设置的模型参数。

右图是模型连杆构件和小球细节图。

三．连杆及小球参数：
根据adams 建立的模型，测量连杆及小球的质量，长度，半径等参数如下。

2.运动学分析
2.1杆端加速度关系推导
如右图所示，杆一和杆二构成开环矢量， 312R R R =+;
令连杆1长1L ，连杆2长2L ;由图（2）可见只要设1R 方向和X 轴正向成1θ（theta1，逆时针），2R 方向和Z 轴正向成2θ（theta2,逆时针），即可表示出杆端P 点的运动学方程。

图（1）
因此得坐标关系式：
()()()()()()()112211122122cos sin sin sin sin cos cos P P P x L L y L L z L θθθθθθθ=⨯+⨯⨯⎡⎤⎢⎥=⨯-⨯⨯⎢⎥⎢⎥
=⨯⎣⎦
对其求一次导得速度方程：
312R R R =+
()()()()()()()()()()()()1112221122111122212121
222sin cos sin sin cos cos cos cos sin sin sin P P P dx L L L dt dy L L L dt dz L dt ωθωθθωθθωθωθθωθθωθ⎡⎤
=⨯⨯-+⨯⨯⨯+⨯⨯⨯⎢⎥⎢⎥⎢⎥=⨯⨯-⨯⨯⨯+⨯⨯⨯⎢⎥⎢⎥⎢⎥=-⨯⨯⎢⎥⎣⎦
求二次导得加速度方程：
312V V V =+ 方程（1）：
()()()()()()()()()22
111122122212
2212
sin sin cos cos sin sin sin P d x L L L L dt αθαθθαθθωθθ=-+++- ()()()()()221222112211112cos cos sin sin cos L L L ωωθθωθθωθ+--
方程（2）：
()()()()()()222
111111222122212
cos sin cos cos sin cos P d y L L L L dt
αθωθαθθωθθ=--+ ()()()()()()222112212122112sin cos sin cos sin sin L L L ωωθθωθθθθα+⨯⨯++
方程（3）：
()()22222222
cos sin P
L L dt
ωθαθ=-- 2.2连杆1质心加速度关系推导
设连杆1质心距端点距离为c r ，xyz 各方向的加速度分别为cx a ，cy a ，cz a 。

连杆1在平面 Oxy 内作平面运动，质心C 点的加速度即为向心加速度与切向加
速度的合成。

但该方法相对较繁琐，同2.1，可以用矢量方程求导得到。

如图，有矢量方程：
112
c R R =
将其展开为坐标分量式，然后求一次，二次导得到其
加速度关系，如2.1所示过程，最后得到连杆1质心
C 的加速度方程如下式：
连杆1质心的加速度方程 方程（4）：
()()21111cos sin cx c c a r r θωθα=--
方程（5）
()()21111sin cos cy c c a r r θωθα=--
以及方程
0cz a =
（注：其中0cz
a =，很明显可以得到，
在程序中没有把cz a 作为新的变量，即没有第三个方程）
2.3连杆2质心加速度关系推导 如右图，有1OD D R R R =+
同2.1,2.2所示过程，用矢量求导的方法得到连杆2质心D 点的加速度方程如下： 由此得到 方程（6）
()()()()()()1111212212
sin sin cos cos sin D
Dx D D a L r r dt
αθαθθαθθ==-++ ()()()()()()()()
2222211221121111sin sin 2cos cos sin sin cos D D D r r r L ωθθωωθθωθθωθ+-+--
方程（7）
()()()()()()2221111112212212
cos sin cos cos sin cos D
Dy D D d y a L L r r dt
αθωθαθθωθθ==--+()()()()()()221121212112sin cos sin cos sin sin D D D r r r ωωθθωθθθθα+⨯⨯++
方程（8）
()()22
22222
cos sin D Dz D D d z a r r dt
ωθαθ==--
3.动力学分析
采用Newton —Euler 方法建立动力学模型。

首先分析连杆1的受力，建立其动力
学方程；其次分析连杆2受力，建立其动力学方程；最后分析小球受力，建立其动力学方程。

3.1连杆1的动力学模型
如右图所示是连杆1的受力分析图：（在本例中关节1处01Z M 和关节2处21R M 各是电机输入力
矩）
由于连杆1做定轴转动，故可用定轴转动刚体动力学分析方法建立其动力学模型。

即：
__ ; out total mass out total J M m a F α==
代入本例中各力，且结合21212121,;,V R V R F F M M 和21212121,;,X Y X Y F F M M 的
关系：()()()()()()()()21211211212112112121121121211211sin cos cos sin M sin cos M cos sin X V R Y V
R X V R Y V R F F F F F F M M M M θθθθθθθθ=⨯+⨯⎡⎤⎢⎥=-⨯+⨯⎢⎥⎢⎥
=⨯+⨯⎢⎥⎢⎥
=-⨯+⨯⎣⎦ 故可得连杆1的动力学方程：
方程（9）：
()()101211211sin cos cx x V R m a F F F θθ=+⨯+⨯
方程（10）：
()()101211211cos sin cy y V R m a F F F θθ=-⨯+⨯
方程（11）：
10121z z m g F F =+
方程（12）：
()()()010112111sin sin X z c z c M F r F L r θθ-⨯⨯+⨯-⨯
()()211211sin cos 0V R M M θθ+⨯+⨯=
方程（13）：
()()()01211211011cos sin cos Y V R Z c M M M F r θθθ-++
()()2111cos 0Z c F L r θ--=
方程（14）：
()()()110121*********sin sin Z z z x c V c y c J M M F r F L r F r αθθ=++-⨯--
即：
()()()()()()()()()()()()()()10121121110121121110121010112111211211012112110112111110sin cos cos sin sin sin sin cos 0cos sin cos cos 0cx x V R cy y V R z z X z c z c V R Y V R Z c Z c Z m a F F F m a F F F m g F F M F r F L r M M M M M F r F L r J M θθθθθθθθθθθθα=+⨯+⨯=-⨯+⨯=+-⨯⨯+⨯-⨯+⨯+⨯=-⨯+⨯+--==()()()121011211011 sin sin z z x c V c y c M F r F L r F r θθ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥++⨯⨯-⨯--⎣⎦
3.2连杆2的动力学模型
如右图，连杆2的底端受到连杆1的反作用力，顶端受到小球（机械手所夹物体）的作用力。

3.2.1先分析连杆2的加速度运动方程。

将12123232,;,V R V R F F F F 分别分解到X ，Y 轴上。

得到
()()()()()()()()12121121121211213232132132321321sin cos cos sin sin cos cos sin X V R Y V R X
V R Y
V R F F F F F F F F F F F F θθθθθθθθ=⨯+⨯⎡⎤⎢⎥=-⨯+⨯⎢⎥⎢⎥=⨯+⨯⎢⎥⎢⎥
=-⨯+⨯⎣⎦
且注意到12123232,;,V R V R F F F F 和21212323,;,V R V R F F F F 是作用力和反作用力的关系。

因此
1221122132233223 V V R R V V R
R F F F F F F F F =-⎡⎤⎢⎥=-⎢
⎥⎢⎥=-⎢⎥=-⎣⎦，方程中用21212323,;,V R V R F F F F 分别代替12123232,;,V R V R F F F F 。

得到 方程（15）
()()()()22123121231sin cos DX V V R R m a F F F F θθ=-+⨯-+⨯
方程（16）
()()()()22123121231cos sin DY V V R R m a F F F F θθ=+⨯-+⨯
方程（17）
223212DZ Z Z m a F F m g =---
3.2.2
然后分析连杆2的刚体转动方程。

注意到连杆1在XOY 平面转动的同时，带动连杆2的关节点一起运动，设角速度为1ω；而连杆2相对连杆1在竖直平面内又做转动运动，设角速度为2ω。

因此连杆2的绝对角速度是角速度矢量1ω和角速度矢量
2ω的叠加。

而描述刚体的转动需要的是本体坐标系下的角速度矢量
body ω，故在连杆2上建立本体坐标系，各个坐标系的关系如上图所示。

从图中可以看出本体坐标系可用zyx 顺序旋转的欧拉角来描述本体坐标系相对惯性系的姿态。

即本体坐标系可由惯性系先绕z 轴顺时针旋转
12
πθ-，再绕坐标
系2的Y 轴逆时针旋转2θ得到。

故可用姿态旋转矩阵把1ω分解到本体坐标系中。

即有如下函数关系：
()()()()1221221cos 0 -sin 0 1 0sin 0 cos bodyx bodyy bodyz ωθθωθθω---⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥
⎣
⎦⎣⎦ 1111cos sin 022-sin cos 0220 0 1 ππθθππθθ⎡⎤⎛⎫⎛⎫
⎛⎫⎛⎫----⎢⎥ ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭⎝⎭⎝⎭⎢⎥⎢⎥⎛⎫⎛⎫
⎛⎫⎛⎫----⎢⎥
⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭⎝⎭⎝⎭⎢⎥
⎢⎥⎢⎥⎢⎥⎣⎦()()21121-sin 000
cos θωωθω⎡⎤
⎡⎤⎢⎥⎢⎥=⎢
⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦
；
而2ω就在本体坐标系上，故不必转换。

故绝对角速度矢量在本体坐标系上的分量式为：
()()()()2121222121-sin -sin 000cos cos abos bodyx abos bodyy abos bodyz ωθωθωωωωθωθωω---⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=+=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥
⎢⎥⎣⎦⎣
⎦⎣⎦⎣⎦ 已知欧拉方程在刚体固连系下的分量式为
J J T ωωω⨯+=
展开式为
()()()x x y z y z x y y z x x z y z z x y x y z
J J J T J J J T J J J T ωωωωωωωωω--=--=--= 其中,,x y z J J J 已经在adams 中测出。

由力的受力分析图可以得出在本体坐标系下
, , x y z T T T 的计算式。

将各分力对连杆2的质心取矩，并将各力矩投影到体坐标系的坐标轴上，得到
()()()()2321223212cos sin x V V Z Z T M M M M θθ=-+⨯++⨯
()23221-r - r R D R D F L F +⨯⨯
()()()21232122322r cos -r cos y R R V D V D T M M F F L θθ=--+⨯⨯-⨯⨯()()()2122322r sin -r sin Z D Z D F F L θθ-⨯⨯+⨯⨯
()()()()2321223212 sin cos z V V Z Z T M M M M θθ=-+⨯-+⨯
最后，把()()21221-sin ; = ; =cos x y z ωθωωωωθω=以及, , x y z T T T 的表达式代入上式得到连杆2的转动方程。

方程（18）
()()()()()12221212sin cos -cos x y z J J J αθθωωωωθ---
()()()()()232122*********cos sin -r - r V V Z Z R D R D M M M M F L F θθ=-+⨯++⨯+⨯⨯
方程（19）
()()()()221222123212sin cos r cos y z x R R V D J J J M M F αωθθθ+-=--+⨯⨯ ()()()()()23222122322-r cos r sin -r sin V D Z D Z D F L F F L θθθ-⨯⨯-⨯⨯+⨯⨯
方程（20）
()()()()()()12221212cos sin sin z x y J J J αθθωωωωθ----
()()()()2321223212sin cos V V Z Z M M M M θθ=-+⨯-+⨯
3.3小球的运动学和动力学方程 3.3.1小球的运动学方程
小球的运动学参量在小球半径R3不是很大时，其实和杆端的运动学参量差不多，如果为了精确计算，那么需要写出小球的运动学方程。

小球的运动学方程和杆端的运动学方程大同小异，只要把L2替换为（L2+R3）即可。

方程（21）
()()()()()()()()211112*********
sin sin cos cos sin ball
d x L L r L r dt αθαθθαθθ=-++++ ()()()()
222321sin sin L r ωθθ++-()()()()()()()22122321123211112cos cos sin sin cos L r L r L ωωθθωθθωθ++-+-
方程（22）
()()()()()22111111223212
cos sin cos cos ball
d y L L L r dt
αθωθαθθ=--+ ()()()223221sin cos L r ωθθ++
()()()()()()()()()22321122312123211
2sin cos sin cos sin sin L r L r L r ωωθθωθθθθα++⨯⨯++++方程（23）
()()()()22
232222322
cos sin P d z L r L r dt
ωθαθ=-+-+ 也可把方程（1），（2），（3）的杆端的加速度看成小球的加速度，相应只要把2L 的数值取成23L r +即可（即如果想要同时知道杆端和小球的加速度，那么可以把以上三个方程添加进入；如果只要知道其一，这三个方程可以去掉）。

3.3.2小球的动力学方程
3.3.2.1
首先分析小球的加速度方程
小球受力如图示，从图中可以很容易得到小球
的加速度方程。

如下：
方程（24）：
()()33231231sin cos X V R m a F F θθ=⨯+⨯
方程（25）：
()()33231231cos sin Y V R m a F F θθ=-⨯+⨯
方程（26）：
33233Z Z m a F m g =-
3.3.2.2
小球的刚体转动方程：
同连杆2的刚体转动方程建模过程一样，依据欧拉方程即可得出，且注意到对球体有Jx=Jy =Jz 。

在此只给出结果如下： 方程（27）：
()()()()()_12221232232233sin cos cos sin ball x V Z r J M M F r αθθωωθθ--=-+⨯
方程（28）：
()()_22323322332cos sin ball y R V Z J M F r F r αθθ=-⨯⨯+⨯⨯
方程（29）：
()()()()()_12221232232cos sin sin cos ball z V Z J M M αθθωωθθ-=⨯+⨯
由此，完成空间二连杆机械臂的运动学和动力学建模过程。

4.simulink 中模型的建立
4.1建模步骤： 4.1.1
新建名为robot 文件夹，在该文件夹下新建名为robot 的mdl 文件，设置当前工作目录为robot 文件夹。

首先从simulink 工具箱中添加必要的模块到simulink
工作窗中，包括Interpreted MATLAB Fcn 模块，Constant 模块，
Mux 模块，MA TLAB Function 模块，Scope and Floating Scope 模块
，To Workspace 模块，Integrator 模块
等。

4.1.2
将各模块按照合理顺序搭建成
simulink模型。

搭建成的模型如右图所示。

4.1.3
编写函数文件：
（1）
将上述29（26）个方程中的二次导
项作为未知变量，共有28个未知变
量。

由于机械臂有两个自由度，故只需知道两个变量即可求得其它变量，通常是已知运动求力和已知输入力矩求运动以及约束力等。

（2）
本仿真实验的函数文件包括两部分，第一部分是名为accrobot的函数用于组建包含所有未知变量的29（26）个方程，其中a矩阵是包含了所有未知变量的系数矩阵，b矩阵是包含已知量的列阵。

第二部分是用于根据输入的已知量求解其它未知量的函数，取名为solution函数。

两个函数放在一个函数文件中，robot 函数调用solution函数求解所有未知量。

注意，由于一些未知量是建立在参考系2上的，为了便于观察比对，还需要一个function函数模块将这些未知量转换为惯性系下的量。

此外，外了便于观察小球位置和速度，用运动学方程建立了第二个function模块求解小球的位置和速度。

需要注意的是，为了区别输入量中哪些量是已知量，哪些量是待求量，用Inf表示待求量。

所有变量通过Const 模块以列阵形式输入accrobot函数模块。

（3）
经过一番调试改错，simulink模型正常运行，
下面是用建立的simulink模型进行运动学计算
和动力学计算以及和adams中建立的模型运算
结果进行比对的过程。

小球的轨迹一
4.2用simulink模型进行运动学计算仿真：
4.2.1模型仿真过程。