第 4 章机器人动力学

4.1 引言 第4章机器人动力学机器人动力学研究内容正问题：已知作用在机器人机构上的力和力矩，求机器人机构各关节的位移、速度、加速度反问题：已知机`器人机构各关节的位移、速度和加速度，求作用在各关节上的驱动力或驱动力矩 • 机器人动力学研究方法 • 动力学模型：根据机构的结构和运动学、动力学关系建立起来的一组动力学方程 • 数学工具：矢量方法、张量方法、旋量方法及矩阵方法等 • 力学原理：能量守恒定理、达朗贝尔原理、虚功原理、拉格朗日方程、动量矩定理、哈密尔顿原理、牛顿－欧拉方程、凯恩方程等

4.2 牛顿—欧拉方程法 第4章机器人动力学 • 牛顿方程和欧拉方程是建立在牛顿第二定律基础之上的，即通过力、力矩，动量和动量矩等物理量来描述刚体的动力学性能。牛顿—欧拉方程刚体随质心的平动可以用牛顿方程，绕质心的转动可以用欧拉方程分别建立其力学模型。

4.2.1 惯量张量 第4章机器人动力学如图4-1所示，设刚体的质量为，刚体坐标系下的惯量张量由六个量组成，可用3×3对称矩阵表示为式中图4.1

4.2.1 惯量张量 第4章机器人动力学已知相对于某一坐标系的惯量张量，可根据平行轴定理及相似变换求得相对于另一坐标系的惯量张量。若两坐标系之间有平行移动关系若两坐标系之间有旋转变换关系主惯量

4.2.2 牛顿—欧拉方程 第4章机器人动力学如图4-1所示，假设刚体的质量为，质心在C点，质心处的位置矢量用表示，质心处的加速度用表示，绕质心的角速度用表示，则绕质心的角加速度为，根据牛顿方程可得作用在刚体质心C处的力为根据欧拉方程作用在刚体上的力矩为图4.1 以上两式合称为牛顿—欧拉方程。

4.2.2 牛顿—欧拉方程 第4章机器人动力学即写在矩阵形式为欧拉方程式可由动量矩定理推得，设P点为刚体上的任意一点，其质量为，相对于质心C的矢径为，速度为，刚体相对于质心C的动量矩为由于根据动量矩定理所以

——构件作用在构件上的力 ——构件作用在构件上的力矩 ——构件作用在构件上的力 ——构件作用在构件上的力矩 ——作用在第i个构件上的外力化简到质心处的合力，即外力的主矢 ——作用在第i个构件上的外力矩化简到质心处的合力矩，即外力的主矩 4.2.3 作用力和力矩第4章机器人动力学如图4.2所示，将第i个构件作为隔离体进行分析，作用在其上的外力有：图 4.2 构件受力图

4.2.3 作用力和力矩 第4章机器人动力学上述力和力矩包括了运动副中的约束反力、驱动力、摩擦力等引起的作用力和作用力矩。作用在第 i个构件上的所有力（包括约束反力、驱动力、摩擦力、重力、外力等）化简到质心的总的合力为相对于质心的总的合力矩为按欧拉方程有式中按牛顿方程有所以为构件相对于其质心的惯性张量

例4.1 如图4.3所示的平面两自由度机器人机构，连杆质心为，质量为，驱动力矩为，角速度为，角加速度为。连杆质心为，质量为，驱动力矩为角速度为，角加速度为。 4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程第4章机器人动力学图 4.3 平面两自由度机器人机构

连杆的牛顿—欧拉方程为 4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程第4章机器人动力学选取关节和关节处的转角，为系统的广义坐标，可以写出连杆的牛顿—欧拉方程为由以上几式可解得考虑到式中

4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程第4章机器人动力学所以

4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程第4章机器人动力学综合以上几式，并考虑到可解得

4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程4.2.4应用牛顿—欧拉方程法建立机器人机构动力学方程第4章机器人动力学以上两式进一步写成式中

——广义坐标 ——广义速度 4.3 拉格朗日方程法第4章机器人动力学拉格朗日方程是基于能量项对系统变量及时间的微分而建立的。对于简单系统拉格朗日方程法相较于牛顿—欧拉方程法更显复杂，然而随着系统复杂程度的增加，拉格朗日方程法建立系统运动微分方程变得相对简单。系统拉格朗日方程为式中 ——系统的广义坐标数 ——作用在第i个广义坐标上的广义力或广义力矩 ——拉格朗日函数为系统的动能和位能之差

引入用4×4变换矩阵表示第个构件 的刚体坐标系相对于基坐标系的齐次线性变换矩阵。如图4.4所示，若坐标系中任意一点P的位置矢量为，在基坐标系中，同一点P的位置矢量为，则有 4.3.2 速度分析第4章机器人动力学机器人系统一般具有结构复杂的连杆机构，在利用拉格朗日方程法建立其动力学方程之前，需要求得机器人机构各连杆上某一点的位移、速度、加速度等物理量，进而求得机构的动能和位能，从而导出机器人机构的速度、动能、位能和动力学方程的一般表达式。图 4.4机器人机构

齐次线性变换矩阵的元素是可导的，用 表示的元素对时间一次求导建立起来的速度矩阵 4.3.2 速度分析第4章机器人动力学速度的平方点P在坐标系下的速度为速度式中：Trace为矩阵的迹的运算符号，对于阶方阵来说，矩阵的迹就是其主对角线上的各元素之和。加速度

式中的积分称为连杆的伪惯量 矩阵，记为设构件上任意一点P的质量为，则该点的动能为构件的动能等于构件上的所有点的动能积分，即 4.3.3 动能第4章机器人动力学其中

相对于平面的转动惯量、、 与相对于轴的转动惯量、、之间存在一定的变换关系 4.3.3 动能第4章机器人动力学根据以上三式可以由相对于轴的转动惯量求得相对于平面的转动惯量

4.3.3 动能 第4章机器人动力学整个机构所有构件的动能为式中：是第个驱动电机转子或传动装置在广义坐标上的等效转动惯量，若是移动运动副，则为等效质量。为广义坐标。机构所有驱动电机及传动装置的总动能为此外，在机器人机构系统中还存在驱动各构件运动的驱动和传动元件，如驱动电机或液压马达的转子，减速器的齿轮等，驱动电机和传动装置的的动能可以根据其广义速度写出机构总的动能为

若构件上任意一点P的质量为 ，则该点的位能为构件位能为式中，为构件的质量，为构件质心在其刚体坐标系中的矢径，系统总的位能为 4.3.4 位能第4章机器人动力学式中g为重力加速度矢量

坐标转换矩阵仅与前 个广义坐标有关，所以当时，所以有式中：，考虑到为对称矩阵，即，所以有式中，n为机构的构件数。求拉格朗日函数L对的偏导数得 4.3.5 动力学方程第4章机器人动力学拉格朗日函数所以

再求拉格朗日函数对的偏导数得 4.3.5 动力学方程第4章机器人动力学于是上式进一步写成

交换式中的下标，将换成，换成， 换成，设作用在各广义坐标上的广义力为，则得 4.3.5 动力学方程第4章机器人动力学所以上式进一步写成式中

例4.2 如图4.5所示的平面两自由度机器人机构系统，集中质量、分别位于连杆1和连杆2的末端A、B处，连杆长度分别为、，推导该机构动力学方程。 4.3.6应用拉格朗日方程法建立机器人机构动力学方程第4章机器人动力学该机构由两个连杆机构组成，系统的动能为两连杆动能之和，即连杆1的动能为图 4.5 平面两自由度机器人机构

4.3.6应用拉格朗日方程法建立机器人机构动力学方程4.3.6应用拉格朗日方程法建立机器人机构动力学方程第4章机器人动力学为了计算连杆2的动能，需要首先计算B点的运动速度，为此列出B点的位置方程为连杆2的动能为上式求导得B点速度系统的总动能为 B点速度的平方为

4.3.6应用拉格朗日方程法建立机器人机构动力学方程4.3.6应用拉格朗日方程法建立机器人机构动力学方程第4章机器人动力学连杆1所位能为对于第一个广义坐标连杆2所位能为系统的总位能为拉格朗日函数

4.3.6应用拉格朗日方程法建立机器人机构动力学方程4.3.6应用拉格朗日方程法建立机器人机构动力学方程第4章机器人动力学对于第二个广义坐标相对于第一个广义坐标的动力学方程为相对于第二个广义坐标的动力学方程为

4.3.6应用拉格朗日方程法建立机器人机构动力学方程4.3.6应用拉格朗日方程法建立机器人机构动力学方程第4章机器人动力学以上两式写成矩阵形式为式中

4.4 凯恩方程法 第4章机器人动力学凯恩（Kane）方程是构建机器人机构动力学模型的另一种常用方法，既适合于完整系统，也适合于非完整系统。凯恩（Kane）方程以广义速率代替广义坐标作为系统的独立变量，应用达朗贝尔原理及虚位移原理建立系统的动力学方程。牛顿—欧拉方程、拉格朗日方程和凯恩方程在本质上是等效的，但对于机器人机构这样多自由度的复杂系统，凯恩方程可以有效减少计算步骤，提高计算效率。

其中、为t 的函数 4.4.1 广义速率、偏速度和偏角速度第4章机器人动力学对于具有n个自由度的质点系，可以通过n个广义坐标（i=1，2，…，n）来描述其相对于基础坐标系oxyz的运动， n个广义坐标对应的广义速度（i=1, 2, …, n）。引入n个变量，使得定义为系统相对于基础坐标系的广义速率。由于（i=1，2，…，n）是独立的，所以广义速率（i=1，2，…，n）也是独立的，构成了质点系的n个独立完整变量，质点系中任一点P相对于基础坐标系的速度都可以用这n个广义速率唯一地表示。设系统中任一质点的径矢为 r， r为广义坐标及t 的函数（r=1，2，…，n）于是该点的速度

定义为基础坐标系下相对于广义速率的第 r 偏速度。它是广义坐标和时间t 的矢量函数。对于定常系统，，偏速度只是广义坐标的矢量函数。式中：、、刚体坐标系下各坐标轴上的单位矢量；、、各坐标轴上的分量，有 4.4.1 广义速率、偏速度和偏角速度第4章机器人动力学将代入上式有若系统中任一刚体的角速度为，在刚体坐标系中可以表示为即式中

定义为基础坐标系下相对于广义速率的第 r偏角速度。一般来说，偏角速度是广义坐标和时间 t的矢量函数。对于定常系统，，偏角速度只是广义坐标的矢量函数。式中：为以及 t的函数，有 4.4.1 广义速率、偏速度和偏角速度第4章机器人动力学式中 j=1, 2, 3 所以上式可以写成

如图4.6所示，设质点系S有n个自由度， 为上的任意一质点，其质量为，相对于基础坐标系的矢径为，作用力为。根据达朗倍尔原理和虚位移原理可以得到质点系S的动力学普遍方程为若系统相对于基础坐标系的广义速率为（r=1, 2,…, n），系统中点的速度，可写成广义速率的线性组合的形式，即 4.4.2 凯恩方程第4章机器人动力学于是图 4.6 质点系S

系统对应于的广义主动力定义为 系统对应于的广义惯性力定义为式中为质点的惯性力，有 4.4.2 凯恩方程第4章机器人动力学上式中，得的变分由此可得即由于变分为相互独立的，所以 r =1，2，…，n 上式即为质点系的凯恩动力学方程。凯恩方程表明系统的广义主动力与广义惯性力之和应等于零。

在刚体上任取一点，不失一般性，取刚体的质心到各质点C的径矢为 ，力系向点 C简化，得主矢R和主矩L 式中为刚体质心C点处的第r个偏速度，为刚体的第r个偏角速度。于是刚体上的所有作用力所对应的广义主动力为 4.4.3 广义主动力第4章机器人动力学上式简化得由于引入广义主动力可以消除理想约束的约束反力。运动副中的约束反力在不计及接触处的摩擦力的情况下，即是理想约束。这种约束的约束反力，总是成对出现的，约束反力的合效应为零，所以理想约束力所对应的广义主动力为零。在计算广义主动力时，可将理想约束力排除，这也是引进广义主动力的概念的主要优点。 r =1，2，…，n

考虑到、，且 设刚体的质心为，绕质心转动的角速度矢量为，角加速度矢量为，刚体上任意质点质量为（i=1，2，…，m），各质点的径矢为（i=1，2，…，m），质心的加速度为，为刚体质心C点处的第r个偏速度，为刚体的第r个偏角速度。刚体上任意质点的偏速度为 4.4.4 广义惯性力第4章机器人动力学式中、为各质点的惯性力向C点简化，得惯性力系的主矢和主矩 i=1，2，…，m 所以有于是广义惯性力为式中为刚体坐标系下的惯量张量

例4.4 如图4.7所示，该机构由两个构件 和组成，其质心分别为和，，集中质量为和，、为两个旋转关节，、为广义坐标，固定在构件上的刚体坐标系用单位矢量、、表示，固定在构件上的刚体坐标系用单位矢量、、表示，则两个坐标系之间的变换关系为 4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学应用凯恩方法建立动力学方程的过程可分为运动分析、求广义惯性力、求广义主动力及建立动力学方程的四大步骤。下面以图4.7所示的2R两自由度机械手为例说明具体的分析过程。图 4.7 2R两自由度机械手

构件的角速度为 (1) 选择广义速率，广义速率的选取原则是使方程能够得到简化，一般在机器人动力学中往往把构件间的相对速度作为广义速率，即，这样得到的动力学方程的形式较简单，所以本例中取 (2) 运动分析，构件的角速度为 4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学角加速度为构件对第一个广义速率的偏角速度为、对第二个广义速率的偏角速度为，有角加速度为构件对第一个广义速率的偏角速度为、对第二个广义速率的偏角速度为，有构件质心速度为所以偏速度为

构件的惯性力主矢为 对第一个广义速率的广义惯性力为 4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学构件质心速度为惯性力主矩为所以偏速度为 (3) 求广义惯性力对第一个广义速率的广义惯性力为惯性张量为构件的惯性力主矢为

对第一个广义速率的广义惯性力为 4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学惯性张量为惯性力主矩为对第一个广义速率的广义惯性力为所以广义惯性力为

构件主矢和主矩分别为 构件对第一个广义速率的广义主动力为构件对第二个广义速率的广义主动力为 (4) 求广义主动力构件和构件的重力分别为若作用在关节和关节上的驱动力矩分别为和，则得构件主矢和主矩分别为构件对第一个广义速率的广义主动力为构件对第二个广义速率的广义主动力为 4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学所以广义主动力为

4.4.5 应用凯恩方法建立机器人机构的动力学方程第4章机器人动力学 (5) 凯恩方程对第一个广义速率有，所以对第二个广义速率有，所以最后得到的动力学方程为

第 4 章 机器人动力学