运动控制系统（第2版新形态版）课件 第三章 运动学和动力学建模

1运动建模与控制系统设计

第三章运动学和动力学建模Ch.3ModelingofKinematicsanddynamicsMotionModelingandControlSystemDesign6学时2

目录基本术语

运动学建模动力学建模12自由度步行机器人案例倒立摆案例小结33.1基本术语

机构学把机构的运动看作只与其几何约束方式有关，与受力、质量和时间等无关的学科，而现实的运动是与时间和空间密切相关的。

运动学就是用来描述机构运动中坐标变化的一组方程。

动力学研究机构动态方程的建立，是一组描述机构运动和驱动力之间动态特性的数学方程。4完整和非完整系统非完整约束是指含有系统广义坐标导数且不可积的约束，与之相反的就是完整系统。保守力和非保守力一个粒子，从起点移动到终点，由于受到作用力，所做的功，不因为路径的不同而改变，则称此力为保守力。假若一个物理系统里，所有的作用力都是保守力，则称此系统为保守系统。反之，则成为非保守力和非保守系统。3.1基本术语5广义坐标广义坐标是不特定的坐标。描述完整系统位形的独立变量。广义坐标对时间的导数称广义速度。刚体与柔性体刚体是物理学上的一种假设存在的理想物体，不会发生变形的称为刚体，与之对应的，会发生变形的，称为柔性体。3.1基本术语6直角坐标系

笛卡尔坐标系，满足右手法则，适合于描述平动。圆柱坐标系

坐标（ρ,θ,μ）来表示。适合于描述平动和转动的混合运动。

3.1基本术语7球坐标系

平面极坐标系的扩展，适合于描述平动和转动的混合运动。固定与移动坐标系

固定坐标系即坐标系的原点和各轴的方向不随时间发生变化的坐标系。移动坐标系即坐标系的原点或各轴的方向随时间发生变化的坐标系。3.1基本术语83.2运动学建模

运动学处理运动的几何学以及与时间有关的量，而不考虑引起运动的力。

位置运动学则只处理运动的几何学，而不考虑运动的时间。机器人的位置运动学存在有两类问题：1.根据关节变量求手部位姿的位置运动学正问题；2.根据手部位姿求关节变量的位置运动学逆问题，又称为手臂解。9

如果点p在H系中的位置为，则相对于B系的位置矢量可由矢量相加得出，即

称为坐标平移变换方程。(1)平动的坐标变换3.2运动学建模10(2)绕坐标轴转动某个角度的变换H系相对B系绕z轴转动角

,若将H系的3个单位矢量表示在B系中，则有：实现两个坐标系之间转动变换的矩阵R3.2运动学建模11同理可得，绕X轴与Y轴旋转的变换矩阵分别为：3.2运动学建模12(3)

复合运动的坐标表示对于任意一点P在B和H系中的描述有以下的关系3.2运动学建模13A矩阵称为齐次矩阵(Homogeneousmatrix)，将转动和移动组合在一个4×4矩阵中。齐次矩阵用途很广，一般形式为：旋转矩阵３×３平移矢量３×１透视变量１×３比例因子１×１齐次矩阵＝3.2运动学建模14(4)变换的顺序

(A)如果用一个描述平移和（或）旋转的变换C，左乘一个坐标系的变换T，那么产生的平移和（或）旋转就是相对于静止坐标系进行的。(B)如果用一个描述平移和（或）旋转的变换C，右乘一个坐标系的变换T，那么产生的平移和（或）旋转就是相对于运动坐标系进行的。3.2运动学建模15(5)变换的封闭性坐标变换具有封闭性，如图下Z-A-E和P-Q从某一变换弧开始，顺箭头方向为正方向，逆箭头方向为逆变换，一直连续列写到相邻于该变换弧为止（但不再包括该起点变换），如果包括该起点变换，则会得到一个单位变换。3.2运动学建模16将齐次变换分解为和臂杆相关的变换，以及和关节相关的变换，方便编程。如具有n个关节自由度的机器人系统，其机构内部的运动可用D-H参数来建立齐次变换矩阵描述：3.2运动学建模D-H参数17Denavit-Hartenberg坐标系3.2运动学建模2026/10/1718

变换矩阵的确立

若已知四个参数、、及就完全确定了连杆和连杆之间的相对关系。对此，我们建立和坐标系之间的变换关系。对于旋转关节可以确定以下的齐次矩阵2026/10/1719

以移动副连接的两杆件的D-H参数的确定

若杆件以移动副相连接时，则连杆的坐标系的建立与参数的规定同回转副连接的杆件的规定相类似，但是连杆的长度已经没有意义，故可以令其为零。可得齐次矩阵为

2026/10/1720

从关节变量到手部位姿

——运动学正问题

三种简化情况的齐次变换矩阵

只具有伸缩臂的操作机

关节变量是伸缩臂的长度d

2026/10/1721

操作机（平行轴情况）

2026/10/1722简化转动关节

条件：杆件均为直杆，即为公共法线，与x轴重合。故d=023上肢体坐标系变换示意图3.2运动学建模24三次变换构成的总齐次变换矩阵(D-H矩阵)为3.2运动学建模25

位置运动学分析腿1腿2(R(0),X(0))ω0d1d2h0(R(2),X(2))(R(1),X(1))d3踝髋ObfXYZ右图为具有6个自由度步行机器人运动学模型。3.2运动学建模26

即已知和，求解，和

。腿i的正运动学方程为:

(3-11)

(3-12)(1)运动学正问题3.2运动学建模27公式(3-11)中的ω(1)=ω0，ω(2)=－ω0。进一步简化（3-11）和（3-12），令3.2运动学建模28可得(3-13)，其中(3-14)3.2运动学建模29其中根据方程左右侧对应元素相等，就可以求得运动学的正问题。3.2运动学建模30(2)运动学逆问题

根据运动学模型，在已知，和的情况下，可以得到髋和踝的位置为3.2运动学建模31从踝到髋的位置矢量为将公式(3-11)和(3-12)中的和代入上式，并化简可得(3-15)3.2运动学建模32将公式(3-15)左右乘，得

(3-16)将公式(3-16)展开，可得

3.2运动学建模33将公式(3-17)的第1行的平方和第3行的平方相加，可得(3-17)由此可得3.2运动学建模34将公式(3-16)左右乘，可得

(3-18)将公式(3-18)展开，可得(3-19)3.2运动学建模35由公式(3-19)的第1行可得到此，求得了和将公式(3-11)变形，可得(3-20)3.2运动学建模36由公式(3-20)展开，可得

(3-21)

由公式(3-21)的第1行，1列元素和第3行，1列元素可得

到此，就求得了运动学的逆问题。3.2运动学建模37速度运动学分析(3-22)(3-23)3.2运动学建模38

定理一：绕任意单位向量转动的微分转动等效于绕轴x，y，z的3个微分转动，，，并有

总的转动微分可由如下的齐次矩阵描述

(3-24)

3.2运动学建模39

定理二：微分转动与其转动次序无关。略去二阶无穷小量后得：(3-25)3.2运动学建模

考虑操作机的手爪位姿r和关节变量的关系用正运动学方程表示的情况。以6连杆运动机构为例……，即为雅可比矩阵。通过可以实现从关节速度到基坐标速度的变换。，(3-26)3.2运动学建模4041同样对于m×n维的空间的机器人，其雅可比矩阵

(3-27)对于同时具有平动和转动的运动机构，末端的角速度和线速度，在基坐标系中的描述记为

(3-28)3.2运动学建模42结果可表示为一个雅可比矩阵形式

(3-29)

式中，Θ为n×1的机械手关节(旋转或平移关节)的位移向量。雅可比矩阵J(Θ)表明了机械手关节速度与末端(手爪)直角坐标速度之间的线性变换关系。3.2运动学建模43动力学建模方法运动系统的动态特性可用一组数学方程来描述，称为系统的动态运动方程。机器人动力学研究的是机器人运动与关节力之间关系的方法。

动力学正问题是根据各关节的驱动力/力矩，求解系统的运动（关节位移、速度和加速度）；

动力学逆问题是已知系统运动的位移、速度和加速度，求解所需要的关节力/力矩。3.3动力学建模44动力学建模方法运动系统的动力学模型可据已知的物理定律求得，包括牛顿-欧拉法、拉格朗日法、高斯法、凯恩法、旋量法等。

使用最为广泛的是牛顿－欧拉法和拉格朗日方法。

以这两种方法为基础，描述运动系统动力学特性的运动方程式有多种形式，如拉格朗日-欧拉方程，递推拉格朗日方程，牛顿-欧拉方程，广义达朗贝尔方程等。3.3动力学建模45动力学建模方法3.3.1牛顿－欧拉方程面向平动面向转动3.3动力学建模例题-操作机对于下面的三种工作情况，计算力矩。(1)手臂水平，并伸至全长，静止(2)手臂水平，并伸至全长，转动和伸缩都以以最大速率运动(3)手臂水平，并伸至全长，承受最大转动加速度463.3动力学建模对于下面的三种工作情况，计算力矩。(1)手臂水平，并伸至全长，静止(2)手臂水平，并伸至全长，转动和伸缩都以以最大速率运动(3)手臂水平，并伸至全长，承受最大转动加速度47动力学建模方法3.2.2拉格朗日(Lagrange)力学方程对于保守系统，其基本形式为，对于非保守系统，其Lagrange方程应为3.3动力学建模48

(3)拉格朗日函数方法对于具有外力作用的非保守机械系统，其拉格朗日动力函数L可定义为

若操作机的执行元件控制某个转动变量θ时，则执行元件的总力矩应为

若操作机的执行元件控制某个移动变量r时，则施加在运动方向r上的力应为3.3动力学建模49假设以下2个前提：(A)脚与地面之间平面接触；(B)脚与地面之间的摩擦力足够大，以致不会发生脚与地面之间的滑动。6自由度步行机器人的动力学建模3.3动力学建模50动力学也有两个相反问题：正问题和逆问题。动力学正问题使用较少,在此不作描述。求解动力学逆问题运用的是牛顿－欧拉法。

1)单支撑状态此时，步行机器人系统是一个开放链，具有一个固定的基本(支撑)部件B1。假设腿1与地面接触，腿2与地面分离。对部件Bi，i=1,2,…7，运用牛顿－欧拉法，建立方程：3.3动力学建模51对于公式(3-30)，(3-31)，定义(3-30)(3-31)利用上述定义，将公式(3-30)，(3-31)写成矩阵形式，可得3.3动力学建模52每个关节的关节转矩可以通过把作用在关节的内部相互作用转矩投影到相应的转轴而求得。因此，可得(3-32)(3-33)3.3动力学建模53式中，为3×1的矩阵；m为18×1的矩阵；H为6×18的矩阵。代表了各个关节的坐标变换情况。3.3动力学建模54由公式(3-33)，可得2)双支撑状态此时，步行机器人的两条腿都是支撑腿，都与地面接触，因此，此状态的步行机器人是一个封闭链系统。3.3动力学建模55运用牛顿-欧拉法，建立方程

(3-34)定义用矩阵形式表示牛顿－欧拉方程，可得

(3-35)3.3动力学建模56利用伪逆矩阵的方法和的矩阵结构，公式(3-35)的解如下

其中

3.3动力学建模57矢量和可以进一步简化为：3.3动力学建模58动力学建模方法3.3动力学建模比较项拉格朗日法牛顿－欧拉法方程较简单、紧凑较复杂、不紧凑加速度不需要求解加速度，只需速度需要求解加速度内作用力不需要求解内作用力需要求解内作用力摩擦损耗因不考虑内作用力，所以不考虑摩擦损耗考虑摩擦损耗自由度随着自由度的增加，虽然方程本身不会变得复杂，但由于关节之间的耦合作用，会使求解任一关节驱动力（力矩）变得复杂。而且，自由度越大，计算的复杂度也越大，计算量会急剧增加。随着自由度增加，方程本身也不会变得很复杂，只是方程中矩阵的行数会增加。因此方法着眼于每一个连杠的运动，所以每一个关节驱动力（力矩）的计算量并不会随着自由度的增加而增加。适用性适用于自由度较少的情况适用于自由度较多的情况所得结果仅能求得关节的驱动力（力矩）既可以求得关节的驱动力（力矩），也可以求得相互作用的内作用力（力矩）编程不易于编程易于编程596061动力学建模方法3.3.3其他建模方法-Udwadia-Kalaba方程

3.3动力学建模62动力学建模方法3.3.3其他建模方法-Udwadia-Kalaba方程

拉格朗日建立分析力学以来，对于相互约束的复杂机械系统进行动力学建模就成为了分析动力学领域的核心研究内容。许多数学家和物理学家在此问题上开展了大量的研究。起始，拉格朗日提出了处理约束运动Lagrange乘子法，但是,此方法需要针对具体问题去确定Lagrange乘子,而且对于拥有大自由度和不可积分约束的系统,其Lagrange乘子更不易获得。Gauss提出了新的理论用以处理约束运动问题。他通过最小化系统质点加速度函数清晰地描述了约束运动的本质。在此之后，Gibbs和Appell提出Gibbs-Appell方程。但是该方程需要明确具体的准坐标且同样具有对大自由度和非完整约束系统难以处理的问题。1993年，南加州大学的Udwadia等人提出了Udwadia-Kalaba方程（UK方程），该方程可被应用于可积分和不可积分约束系统。此动力学方法适用于完整/非完整、定常/非定常、理想/非理想各类约束，这是相比拉格朗日动力学的优势之一。

3.3动力学建模63动力学建模方法3.3.3其他建模方法-Udwadia-Kalaba方程

UK方程的使用过程概括为三步：第一步，将研究对象视为“无约束“系统。在无约束状态下，选取个广义坐标，，则该系统运动方程可以表示为

第二步，考虑系统中原有的约束，假设系统中存在k个完整约束，l个非完整约束，即根据U-K方程对上述约束求导，得到其二阶表达式，并整理为矩阵形式。

3.3动力学建模64动力学建模方法3.3.3其他建模方法-Udwadia-Kalaba方程第三步，将约束力施加于该系统，则系统真实运动方程为

其中，即为存在上述约束时，系统为满足约束所需的外加约束力。U-K方程则给出了约束力的精确表达式，该方程在物理学角度阐述了运动对象在需要完成特定运动轨迹时，所受到的外力。受约束系统的精确运动方程可以写为U-K方程提供了完全解析形式的约束力表达式，这对接下来的控制器设计有直接帮助。3.3动力学建模65

3.3动力学建模66

3.3动力学建模67

3.3动力学建模68腿1腿2(R(0),X(0))ω0d1d2h0(R(2),X(2))(R(1),X(1))d3踝髋ObfXYZ左图为具有12个自由度步行机器人运动学模型运动学正问题即已知和，求解和。腿i的正运动学方程为3.412自由度步行机器人案例12自由度步行机器人的运动学69(3-36)(3-37)3.412自由度步行机器人案例70步行机器人行走的每一个周期的开始，必然有一个脚的坐标系与基坐标系重合，所以公式(3-36)，(3-37)可以求解。对公式(3-36)和公式(3-37)进行详细计算，并进一步简化符号可得(3-38)(3-39)根据公式(3-38)和(3-39)，就可以求得运动学的正问题。3.412自由度步行机器人案例71(2)运动学的逆问题即已知和，求解和。根据运动学模型，在已知和的情况下，可以得到髋和踝的位置为，

3.412自由度步行机器人案例72将公式(3-36)和(3-37)中的和代入上式，并化简，可得将公式(3-40)左右乘，可得(3-40)(3-41)3.412自由度步行机器人案例73将公式(3-41)展开，可得(3-42)由公式(3-42)的第2行，可得由此可得将公式(3-42)左右乘，可得(3-43)3.412自由度步行机器人案例74将公式(3-43)展开，可得(3-44)将公式(3-44)的第1行的平方和第3行的平方相加，并且注意到，可得根据公式(3-44)的第1行，可得3.412自由度步行机器人案例75到此，求得了，和三者之间的几何关系，如下图所示。WVU(u,v,w)vwud1d23.412自由度步行机器人案例76将公式（3-36）变形，可得(3-45)由公式(3-45)计算，可得(3-46)由公式(3-46)的第1行，1列元素和第2行，1列元素可得3.412自由度步行机器人案例77由公式(3-46)的第1行，1列元素的平方和第3行，1列元素的平方之和以及第3行，1列元素可得由公式(3-47)的第3行，2列元素和第3行，3列元素可得3.412自由度步行机器人案例78假设以下2个前提：(A)脚与地面之间平面接触；(B)脚与地面之间的摩擦力足够大，以致不会发生脚与地面之间的滑动。12自由度步行机器人的动力学3.412自由度步行机器人案例79(1)动力学正问题步行机器人的动力学正问题就是在已知各个关节的关节力或力矩的情况下，求解各个关节的位移、速度和加速度，求得运动轨迹。动力学正问题使用较少，在此不作描述。(2)动力学逆问题步行机器人的动力学逆问题就是在已知各个关节位移、速度和加速度，求解各个部件Bi的惯性力、重力以及惯性力矩，从而求解各个关节的关节，i=1,2。3.412自由度步行机器人案例80在此状态，步行机器人系统是一个开放链，具有一个固定的基本(支撑)部件B1。假设腿1与地面接触，腿2与地面分离。对部件Bi，i=1,2,…7，运用牛顿－欧拉法，建立方程：(3-47)(3-48)3.412自由度步行机器人案例

1)单支撑状态81定义利用上述定义，将公式(3-47)，(3-48)，写成矩阵形式，可得(3-49)3.412自由度步行机器人案例82公式(3-49)可以在已知的条件下方便的求出m。每个关节的关节转矩可以通过把作用在关节的内部相互作用转矩投影到相应的转轴而求得。因此，可得(3-50)其中是6×1的矩阵，m是18×1的矩阵，H是18×12的矩阵。3.412自由度步行机器人案例833.412自由度步行机器人案例由公式(3-50)，可得842)双支撑状态此状态的步行机器人是一个封闭链系统。这个封闭链系统包含5个可运动刚体部件(Bi，i=2,3,…6。因B1，B7与地面接触，不可运动)，6个关节(，i=1,2,…6)和12个驱动器(τ(i)∈R(6)，i=1,2)。这个系统是一个冗余系统，系统的冗余自由度为3.412自由度步行机器人案例85用矩阵形式表示牛顿－欧拉方程，可得(3-51)利用伪逆矩阵的方法和的矩阵结构，公式(3-51)的解如下：(3-52)3.412自由度步行机器人案例86左图为步行机器人的动力学模型，运用拉格朗日法求解动力学逆问题。(1)详细求解过程定义部件上任意一点P的位置为部件上任一点的速度为步行机器人拉格朗日法动力学3.412自由度步行机器人案例87对于部件上任一点质点，其动能如下式所示对于部件积分，得部件的动能为式中，积分成为部件的伪惯量矩阵3.412自由度步行机器人案例88因此其中，伪惯量矩阵的一般表达式为3.412自由度步行机器人案例89设3.412自由度步行机器人案例根据理论力学或物理学可知，物体的转动惯量、矢量积为90于是可以把表示为(3-53)3.412自由度步行机器人案例91步行机器人模型的总动能为部件的传动装置动能为步行机器人模型的传动装置的总动能为3.412自由度步行机器人案例92模型的总动能为部件上任一点质点的质量dm，其位能为部件的位能为3.412自由度步行机器人案例93由于传动装置的位能一般较小，可以忽略不计。所以，步行机器人系统的总位能为系统的拉格朗日函数3.412自由度步行机器人案例94求导数可得由公式(3-53)可知，为对称矩阵，即,所以3.412自由度步行机器人案例95因此当p>i时，后面的关节变量对前面的各个部件不产生影响，即。可得因为3.412自由度步行机器人案例96因此再求项3.412自由度步行机器人案例97所以3.412自由度步行机器人案例98交换上式中的哑元，以i代替p，以j代替i，以m代替j，可得3.412自由度步行机器人案例99(2)简化求解过程假设此时，腿1是支撑腿，腿2向前迈进。此时系统的动能和位能如下3.412自由度步行机器人案例100式中，的坐标值可以通过运动学求得。具体情况如下3.412自由度步行机器人案例1013.412自由度步行机器人案例1023.412自由度步行机器人案例1033.412自由度步行机器人案例1043.412自由度步行机器人案例105系统的拉格朗日函数为根据拉格朗日方程就可以求解关节力矩。通过上述求解可知使用拉格朗日法求解多自由度步行机器人动力学逆问题非常复杂。3.412自由度步行机器人案例106

倒立摆系统结构图3.5倒立摆案例107为便于建模，假设：(A)除皮带外，全部对象(摆体、小车、导轨等)均视为刚体；(B)各部分的摩擦力(力矩)与相对速度(角速度)成正比；(C)施加在小车上的驱动力与加在功率放大器上的输入电压u成正比，比例系