第3章 CHAPTER 3 运动学和动力学建模 视频讲解 3．1基本术语 机构学把机构的运动看作只与其几何约束方式有关，与受力、质量和时间等无关的学科，而现实的运动是与时间和空间密切相关的。运动学（kinematics）模型就是用来描述机构运动中坐标变化的一组方程，处理运动的几何学及与时间有关的量，而不考虑引起运动的力。动力学（dynamics）研究机构动态方程的建立，是一组描述机构运动和驱动力之间动态特性的数学方程。 3．1．1完整约束系统和非完整约束系统 非完整约束(nonholonomic constrain)系统是指含有系统广义坐标导数且不可积的约束，与之相反的就是完整约束(holonomic constrain)系统。典型的非完整约束系统(简称非完整系统)包括车辆、移动机器人、某些空间机器人、水下机器人、欠驱动机器人和运动受限机器人等。因此，非完整系统的控制研究具有广泛应用背景和重要应用价值。经典力学对非完整系统做了基础性研究。从19世纪80年代末起，由于机器人及车辆控制的需要，开始对非完整系统的控制问题进行深入研究。由于非完整约束是对系统广义坐标导数的约束，不减少系统的位形自由度，这使得系统的独立控制个数少于系统的位形自由度，给其控制设计带来很大困难。非完整系统不能用连续的状态反馈整定。 3．1．2保守力和非保守力 在一个物理系统里，假若一个粒子，从起始点移动到终结点，由于受到作用力所做的功不因为路径的不同而改变，则称此力为保守力(conservative force)。假若一个物理系统里，所有的作用力都是保守力，则称此系统为保守系统； 反之，则称为非保守力和非保守系统。 3．1．3广义坐标 广义坐标(generalized coordinates)是不特定的坐标,描述完整系统 位形的独立变量。对于含有n个质点的质点系，在空间有3n个坐标。若这些质点间存在k个有限约束，则约束方程可写为fs(x1，x2，…，x3n； t)＝0(s＝1，2，…，k)。利用约束方程消去3n个坐标中的k个变量，剩下N＝3n－k个变量是独立的。利用变量转换，可将这N个变量用其他任意N个独立变量q1，q2，…，qN来表示。因此，3n个x坐标可用N个q表示为xi＝xi(q1，q2，…，qN； t)(i＝1，2，…，3n)。这种相互独立的变量称为广义坐标，其数目N等于完整系统的自由度。常用的广义坐标有线量和角量两种。例如，对约束在空间固定曲线上运动的质点，可用自始点计量的路程s作为广义坐标； 用细杆约束在竖直平面内摆动的质点，可用杆与铅垂线的夹角θ作为广义坐标。广义坐标对时间的导数称为广义速度。 3．1．4刚体与柔性体 刚体是物理学上的一种假设存在的理想物体，不会发生变形的称为刚体，与之对应，会发生变形的称为柔性体。 柔性机械臂、涡轮机叶片、直升机旋翼以及带有柔性附件的人造卫星等都是刚柔耦合系统。建立这类结构的模型，需要考虑大范围刚体运动与弹性小变形运动的耦合问题。 3．1．5直角坐标系 运动在数学模型上表现为质点的空间坐标值随时间的变化。所以，对质点的空间坐标的描述是理论分析的基础。要全面地确定一个物体在三维空间中的状态需要有3个位置自由度和3个姿态自由度。前者用来确定物体在空间中的具体方位，后者则是确定物体的指向。物体的6个自由度的状态称为物体的位姿。直角坐标系是最常见的坐标系，又名笛卡儿坐标系，由Cartesian建立，满足右手法则，特点为3个坐标轴X、Y、Z相互垂直，如图3．1所示。为易于想象和理解，3个坐标轴的位置精度不变。空间点P的位置由一组坐标(x,y,z)来表示。空间位置可由沿3个坐标轴的平动获得。直角坐标适合描述平动，如常用于数控机床的三轴平动工件台的运动描述。 3．1．6圆柱坐标系 圆柱坐标系如图3．2所示，空间P点的位置由一组坐标(ρ，θ，μ)来表示。笛卡儿坐标与圆柱坐标的转换关系为 x=ρcosθ，ρ≥0 y=ρsinθ，0≤θ≤2π z=μ，－∞≤μ≤+∞(3.1) 图3．1直角坐标系示意图 图3．2圆柱坐标系示意图 圆柱坐标系适合描述平动和转动的混合运动(平动多，转动少的情况)，例如常用于描述摇臂钻床钻头的空间运动轨迹。 3．1．7球坐标系 图3．3球坐标系示意图 球坐标系是平面极坐标系的扩展。空间点P的位置由一组坐标(ρ，ω，)来表示，如图3．3所示。笛卡儿坐标与球坐标的转换关系为 x=ρcosωsin，ρ≥0 y=ρsinωsin，0≤ω≤π z=ρcos，0≤≤2π(3.2) 圆柱坐标系适合描述平动和转动的混合运动(平动少，转动多的情况)，例如常用于描述球铰链的空间运动轨迹。 3．1．8固定坐标系与移动坐标系 固定坐标系即坐标系的原点和各轴的方向不随时间发生变化的坐标系，例如世界坐标系、工业机器人的基座坐标系，用于描述空间的绝对初始位置(零位)。 移动坐标系即坐标系的原点或各轴的方向随时间发生变化的坐标系，例如工业机器人的连杆坐标系。移动坐标系往往是跟随运动刚体一起变化的。注意，运动是相对的，所以坐标系是固定或是移动的，也是相对的。工业机器人的连杆子坐标系相对于基座坐标系是移动的，相对于连杆本身又是静止的。 3．2运动学建模 3．2．1坐标变换 1. 平动的坐标变换 设手坐标系H与基坐标系B具有相同的姿态，但H系坐标原点与B系的原点不重合。用矢量r-0来描述H系相对于B系的位置， 图3．4平动坐标变换 称r-0为H系相对于B系的平移矢量，如图3．4所示。 如果点P在H系中的位置为r-，则r-相对于B系的位置矢量r-p可由矢量相加得出，即 r-p=r-0+r-(3.3) 称为坐标平移变换方程。 2. 转动的坐标变换 以绕z轴转动为例来研究绕坐标轴转动某个角度的表示法。设H系从与B系相重合的位置绕B系的z轴转动θz角，H系与B系的关系如图3．5所示。 若将H系的3个单位矢量表示在B系中，则有 n-=cosθz sinθz 0，o-=－sinθz cosθz 0，a-=0 0 1 实现两个坐标系之间的转动关系的矩阵，又叫转动矩阵R，可表示为 图3．5H系相对B系绕z轴转动 θz角的坐标关系 R=n-o-a-=cosθz－sinθz0 sinθzcosθz0 001(3.4) 同理可得，绕其他轴旋转的变换矩阵为 Rot(x,θ)=100 0cosθx－sinθx 0sinθxcosθx(3.5) Rot(y,θ)=cosθy0sinθy 010 －sinθy0cosθy(3.6) 3. 复合运动的坐标替换 基坐标系B和手坐标系H的原点不重合，而且两坐标系的姿态也不相同的情况，如图3．6所示。 图3．6复合运动坐标变换 对于任意一点P在B和H系中的描述有以下的关系 r-p=r-0+Rr-H(3.7) r-p 1=Rr-0 01r-H 1(3.8) x y z 1= a Rb c 01u v w 1=A·u v w 1(3.9) A=a R3×3b c 01×31(3.10) A矩阵称为齐次矩阵(homogeneous matrix)，将转动和移动组合在一个4×4矩阵中。 齐次矩阵用途很广，一般形式为 齐次矩阵= 旋转矩阵3×3-平移矢量3×1 透视变量1×3|比例因子1×1 例3.1坐标旋转变换举例。 已知一个矢量U绕z轴旋转90°变成V，则用旋转矩阵表示为 V=RotZ,90°U 一个矢量U先后绕x、y轴分别旋转90°、60°得到V，用旋转矩阵表示为 V=RotY,60°RotX,90°U 例3.2利用齐次矩阵表示手的转动。 手的转动可以表示为绕x轴的侧摆Rot(X,Φx)，绕y轴的俯仰Rot(Y,Φy)和绕z轴横滚Rot(Z,Φz)，依次构成的复合转动RPY(Φz,Φy,Φx)。 RPY(Φz,Φy,Φx)=Rot(Z,Φz)Rot(Y,Φy)Rot(X,Φx) =cΦz－sΦz00 sΦzcΦz00 0010 0001cΦy0sΦy0 0100 －sΦy0cΦy0 00011000 0cΦx－sΦx0 0sΦxcΦx0 0001 式中，s=sin，c=cos。 从上式可见，手腕的转动计算比较复杂。 4. 变换的顺序 一般的齐次坐标变换过程可以分为以下两种情况。 (1) 如果用一个描述平移和(或)旋转的变换C，左乘一个坐标系的变换T，那么产生的平移和(或)旋转就是相对于静止坐标系进行的。 (2) 如果用一个描述平移和(或)旋转的变换C，右乘一个坐标系的变换T，那么产生的平移和(或)旋转就是相对于运动坐标系进行的。 5. 变换的封闭性 坐标变换具有封闭性，如图3．7所示。 图3．7坐标变换的封闭特性 图中，Z—A—E和P—Q都表示坐标变换。实际上，可以从封闭的有向变换图的任一变换开始列变换方程。从某一变换弧开始，顺箭头方向为正方向，逆箭头方向为逆变换，一直连续列写到相邻于该变换弧为止(但不再包括该起点变换)。如果包括该起点变换，则会得到一个单位变换。 注意变换的顺序以及变换的参考基坐标系。 (1) 如果是相对于基坐标系B的运动，其相应的齐次变换矩阵左乘原齐次变换矩阵。 (2) 如果是相对于手坐标系H的运动，其相应的齐次变换矩阵右乘原齐次变换矩阵。 视频讲解 3．2．2DH参数 通常描述机构运动学的方法有DH(DenavitHartenberg)法、Duff法和牧野法等。DH坐标系法的优点是将齐次变换分解为和臂杆相关的变换，以及和关节相关的变换，为具体的编程和数值求解计算等带来方便。对于多个零部件组成、具有多个运动副关节的机构内部的运动，可用DH参数来建立齐次变换矩阵来描述。具有n个关节自由度的机器人系统，其齐次矩阵可表示为 A=A1A2…Ai…An 式中，每一个齐次变换矩阵有6个参数。 为建立运动学方程，需要讨论相邻连杆运动关系。以回转副连接的两杆件的DH参数的确定为例。对于两个相邻臂杆Ci和Ci－1，设关节轴线分别为zi－1、zi和zi+1。为描述相邻臂杆间平移和转动的关系，DH坐标系的参数有4个： 两个相邻关节轴线zi和zi+1的公共垂线间距离为ai(连杆长度)； 由zi和zi+1公垂线组成的平面Q，zi+1与平面Q的夹角为αi(扭转角)； 与关节i相邻的两个公垂线ai与ai-1之间的距离为di(连杆偏移量)； 与关节i相邻的两个公垂线ai与ai-1在以zi为法线的平面上的投影夹角为θi(关节角)。ai、αi、di和θi参数称为DH参数，DH坐标系如图3．8所示。 图3．8DH坐标系 DH法是为每个关节处的臂杆坐标系建立4×4齐次变换矩阵，描述与前一个臂杆坐标系的关系。 不失一般性，按照DH参数的定义，通用的关节坐标变换为 Ai=Rot(zi-1，θi)Trans(zi-1,di)Trans(xi，ai)Rot(xi,αi) =cosθi-sinθicosaisinθisinaiαicosθi sinθicosθicosai-cosθisinaiaisinθi 0sinaicosaidi 0001 图3．8中建立的DH坐标系，若是针对仿人机器人上肢体，臂杆Ci－1的坐标系经过两次旋转和一次平移可以变换到连杆Ci的坐标系。参照图3．8和图3．9，3次变换分别如下。 第一次： 臂杆沿xi－1轴平移ai－1，将Oi－1移动到O′i－1，记为Trans(ai－1,0,0)。变换后的臂杆Ci－1坐标系如图3．9(a)所示。 第二次： 以xi－1轴为转轴，旋转αi－1角度，使新的zi－1轴与zi轴同向，记为Rot(xi－1,αi－1)。变换后的臂杆Ci－1坐标系如图3．9(b)所示。 第三次： 以zi轴为转轴，旋转θi角度，使新的xi－1轴与xi轴同向，记为Rot(zi,θi)。变换后的臂杆Ci－1坐标系如图3．9(c)所示。 图3．9上肢体坐标系变换示意图 经过3次变换，坐标系Oi-1xi-1y i-1z i-1与坐标系Oix iy izi完全重合。通过上述3次变换，建立了两相邻臂杆Ci和Ci－1之间的相对关系，并记为矩阵Ai。3次变换构成的总齐次变换矩阵Ai(DH矩阵)为 Ai=Trans（ai-1,0,0）Rot（xi-1,αi-1）Rot（zi,θi） = 100ai-1 0100 0010 0001 1000 0cosαi-1-sinαi-10 0sinαi-1cosαi-10 0001 cosθi-sinθi00 sinθicosθi00 0010 0001 = cosθi-sinθi0ai-1 sinθicosαi-1cosθicosαi-1 -sinαi-1-sinαi-1 sinθisinαi-1cosθisinαi-1 cosαi-1cosαi-1 0001 例3.3列写图3.10中垂直3关节手臂运动学方程(坐标变换矩阵)，包括建立坐标系、各关节DH参数、各关节变换矩阵，综合得到手臂运动学方程。 首先建立基坐标系和各个关节坐标系，如图3.11所示。 而后分析各个关节的DH参数，可得表3.1所示结果。 表3.1例3.3的DH参数表 参数joint1joint2joint3 θiθ1θ2θ3 ai0a2a3 续表 参数joint1joint2joint3 did100 αi90°00 图3.10例3.3示意图（1） 图3.11例3.3示意图（2） 根据齐次变换矩阵建立方法，分别建立各个关节的变换矩阵为 A1=c10s10 s10－c10 010d1 0001，A2=c2－s20c2a2 s2c20s2a2 0010 0001，A3=c3－s30c3a3 s3c30s3a3 0010 0001 进而计算 A=A1A2A3 A的各项元素为 A11=c1c2c3－c1s2s3，A12=－c1c2s3－c1s2c3，A13=s1 A14=c1c2c3a3－c1s2s3a3+c1c2a2 A21=s1c2c3－s1s2s3，A22=－s1c2s3－s1s2c3，A23=－c1 A24=s1c2c3a3－s1s2s3a3+s1c2a2，A31=s2c3+c2s3，A32=－s2s3+c2c3 A33=0，A34=s2c3a3+c2s3a3+s2a2+d1，A41=A42=A43=0，A44=1 代入原始位置时的参数： θ1=θ2=θ3=0，可得原始位置时的变换矩阵为 A=100a2+a3 00－10 010d1 0001 代入转动后的参数： θ1=θ2=0，θ3=90°，如图3.12所示，可得此时的变换矩阵为 A=0－10a2 00－10 100d1+a3 0001 代入转动后的参数： θ1=θ2=θ3=90°，如图3.13所示，可得此时的变换矩阵为 A=0010 －100－a3 0－10d1+a2 0001 图3.12例3.3示意图（3） 图3.13例3.3示意图（4） 3．2．3速度运动学分析 本节将进一步讨论运动的几何学及与时间有关的量，即讨论机器人的速度运动学问题。速度运动学问题重要是因为操作机不仅需要达到某个(或一系列的)位置，而且常需要按给定的速度达到这些位置。 所谓微分运动指的是无限小的运动，即无限小的移动和无限小的转动。微分运动既可以用指定的当前坐标系来描述，也可以用基础坐标系来描述。 对于微分移动(平动)的齐次变换矩阵T可表示为 Trans(dx,dy,dz)=100dx 010dy 001dz 0001(3.11) 式中，dx、dy、dz是微分位移矢量在基础坐标系或当前坐标系的分量。 微分转动中δx很小，所以sinδx=·δx，cosδx=·1，可得 Rot(x,δx)=1000 01－δx0 0δx10 0001，Rot(y,δy)=10δy0 0100 －δy010 0001， Rot(z,δz)=1－δz00 δz100 0010 0001 Rot(x,δx)Rot(y,δy)Rot(z,δz)= 1－δzδy0 δxδy+δz1－δxδyδz－δx0 －δy+δxδzδyδz+δx10 0001 =· 1－δzδy0 δz1－δx0 －δyδx10 0001(3.12) 上面的近似等式是在略去二阶与三阶无穷小量的条件下获得的。 定理3.1绕任意单位矢量=［Kx,Ky,Kz］T转动δθ的微分转动等效于绕轴x、y、z的3个微分转动δx、δy、δz，并有 δx=Kxδθ，δy=Kyδθ，δz=Kzδθ 于是总的转动微分Rot(,δθ)可由如下的齐次矩阵描述 Rot(,δθ)=Rot(x,δx)Rot(y,δy)Rot(z,δz)=1 Kzδθ －Kyδθ 0－Kzδθ 1 Kxδθ 0Kyδθ －Kxδθ 1 00 0 1 0 (3.13) 定理3.2微分转动与其转动次序无关。 Rot(x,δx)Rot(y,δy)=1000 01－δx0 0δx10 000110δy0 0100 －δy010 0001 Rot(y,δy)Rot(x,δx)=1 0 －δy 0δxδy 1 δx 0δy －δx 1 00 0 0 1 略去二阶无穷小量后得 Rot(x,δx)Rot(y,δy)=Rot(y,δy)Rot(x,δx)(3.14) 考虑操作机的手爪位姿r和关节变量θ的关系用正运动学方程r=f(θ)表示的情况。以6连杆运动机构为例 r1=f1(θ1,θ2，…，θ6)…r6=f6(θ1,θ2，…，θ6) drdt=Jdθdt，J=f(θ1,θ2,…，θ6)θT J即为雅可比矩阵。通过J可以实现从关节速度到基坐标速度的变换。 J= f1θ1f1θ2…f1θ6 ︙︙︙ f6θ1f6θ2…f6θ6=［Jij］6×6，Jij=fiθj(3.15) 同样对于m×n维空间的机器人，其雅可比矩阵为 J=f1θ1f1θ2…f1θn ︙︙…︙ fmθ1fmθ2…fmθn=Jijm×n(3.16) 对于同时具有平动和转动的运动机构，末端的角速度和线速度，在基坐标系中的描述记为 x·=νn ωn(3.17) 结果可表示为一个雅可比矩阵形式 x·=J(Θ)Θ·(3.18) 式中，Θ为n×1的机械手关节(旋转或平移关节)的位移矢量。 雅可比矩阵J(Θ)表明了机械手关节速度与末端(手爪)直角坐标速度之间的线性变换关系。 例3.4已知操作机的位姿为 T=0015 1002 0100 0001 求先实施转动Rot(x,0．1)，再实施移动Trans(1,0,0．5)的微分运动后操作机的新位姿。 定义微分算子 Δ=Trans(dx,dy,dz)Rot(,dθ)－I 由于δx=0．1，dx=1； δy=0，dy=0； δz=0，dz=0．5，计算可得 Δ=0－δzδydx δz0－δxdy －δy－δx0dz 0000=0001 00－0．10 00．100．5 0000 操作机的新位姿为 T+ΔT=0016 1－0．102 0．1100．7 0001 视频讲解 3．3动力学建模 1. 牛顿欧拉方程 面向平动 f=ma（3.19） 式中，m为物体质量(kg)； a为物体线加速度(m/s2)； f为力(N)。 面向转动 Jcω·+ω×(Jcω)=τ（3.20） 式中，Jc为物体转动惯量(kg·m2)； ω为物体角速度(rad/s)； τ为力矩(N·m)。 2. 拉格朗日(Lagrange)力学方程 对于保守系统，Lagrange方程是用s个独立变量来描述力学体系的运动，是一组二阶微分方程。其基本形式为 ddtTq·i－Tqi=Qi，i=1,2,…，s（3.21） 式中，q1,q2,…,qs为所研究力学体系的广义坐标； Q1,Q2,…,Qs为作用在此力学体系上的广义力； T为系统总动能。 对于同时受到保守力和耗散力作用的、由n个关节部件组成的机械系统(非保守系统)，其Lagrange方程应为 ddtTq·i－Tqi+Vqi+Dq·i=Fqi（3.22） 式中，qi为广义坐标，表示系统中的线位移或角位移的变量； Fqi为作用在系统上的广义力； T、V和D分别为系统总的动能、势能和耗散能，分别为 T=∑ni=1Ti，V=∑ni=1Vi，D=∑ni=1Di（3.23） 3. 拉格朗日函数方法 对于具有外力作用的非保守机械系统，其拉格朗日函数L可定义为 L=T－V（3.24） 若操作机的执行元件控制某个转动变量θ时，则执行元件的总力矩τθ应为 τθ=ddtLθ·－Lθ（3.25） 若操作机的执行元件控制某个移动变量r时，则施加在运动方向r上的力Fr应为 Fr=ddtLr·－Lr（3.26） Lagrange方程是以广义坐标表达的任意完整系统的运动方程式，方程式的数目和系统的自由度数是一致的。理想约束反力不出现在方程组中，因此建立运动方程式时只需分析已知的主动力，而不必分析未知的约束反力。 Lagrange方程是以能量观点建立起来的运动方程式。为了列出系统的运动方程式，只需要从两个方面去分析： 一个是表征系统运动的动力学量——系统的动能和势能； 另一个是表征主动力作用的动力学量——广义力。因此，用Lagrange方程来求解系统的动力学方程可以大大简化建模过程。两种方法各有优劣，比较情况如表3．2所示。 表3．2拉格朗日法与牛顿欧拉法的比较 比较项拉格朗日法牛顿欧拉法 方程较简单、紧凑较复杂、不紧凑 加速度不需要求解加速度，只需速度需要求解加速度 内作用力不需要求解内作用力需要求解内作用力 摩擦损耗因不考虑内作用力，所以不考虑摩擦损耗可考虑摩擦损耗 自由度随着自由度的增加，虽然方程本身不会变得复杂，但由于关节之间的耦合作用，会使求解任一关节驱动力(力矩)变得复杂。而且，自由度越大，计算的复杂度也越大，计算量会急剧增加随着自由度增加，方程本身也不会变得很复杂，只是方程中矩阵的行数会增加。因此方法着眼于每一个连杠的运动，所以每一个关节驱动力(力矩)的计算量并不会随着自由度的增加而增加 适用性适用于自由度较少的情况适用于自由度较多的情况 所得结果仅能求得关节的驱动力(力矩)既可以求得关节的驱动力(力矩)，也可以求得相互作用的内作用力(力矩) 编程不易于编程易于编程 因为两种方法并不都考虑摩擦损耗，所以两种方法求得的结果不完全相同。 例3.5图3.14所示的θr操作机，m1=10kg，m2=1~5kg，r1=1m，r=1~2m，θ·max=1s-1，θ¨max=1s-2，r·max=1m/s，r¨max=1m/s2。 图3.14例3.5示意图 对于下面的三种工作情况，计算力矩Tθ。 (1) 手臂水平，并伸至全长，静止，m2=5kg。 (2) 手臂水平，并伸至全长，转动和伸缩都以以最大速率运动，m2=5kg。 (3) 手臂水平，并伸至全长，承受最大转动加速度，m2=5kg。 求解情况(1) Tθ=m1r1+m2rgcosθ=D1=196kg·m2/s2 求解情况(2) Tθ=D1+2m2rr·θ·=216kg·m2/s2 求解情况(3) Tθ=D1+m1r21+m2r2θ¨=226kg·m2/s2 经分析可知，力矩中重力项通常远大于其他项，且重力项随角度变化很大； 惯性力项通常大于科氏力项和向心力项。 视频讲解 视频讲解 3．4倒立摆案例 一阶小车直线倒立摆的结构如图3．15所示。 图3．15倒立摆系统结构图 图中，θ为倒立摆摆体摆动角度，r为皮带轮半径。 为便于建模，假设： (1) 除皮带外，全部对象(摆体、小车、导轨等)均视为刚体； (2) 各部分的摩擦力(力矩)与相对速度(角速度)成正比； (3) 施加在小车上的驱动力与加在功率放大器上的输入电压u成正比，比例系数设为G0； (4) 皮带轮与传送带之间无滑动，传送带无伸长现象； (5) 信号与力的传递无延时。 首先需计算均匀杆(长度为2L，质量为m)的转动惯量。当均匀杆绕一端转动时，其转动惯量为 J=∫2L0l2ρdl=83ρL3，ρ=m2L，J=43mL2 杆相对质心的转动惯量为 Jc=∫L－Ll2ρdl=13mL2 所以 J=Jc+mL2 3．4．1牛顿欧拉法倒立摆动力学建模 1. 小车部分 小车部分的受力分析如图3．16所示。 图中，m0表示小车质量； N表示轨道反力； F0表示小车滑动摩擦系数； fx表示摆体对小车作用力的水平分量； fy表示摆体对小车作用力的垂直分量。 考虑到小车只有水平方向(X)的运动，故可列写小车运动方程 m0r¨=G0u－fx－F0·r·(3.27) 2. 摆体部分 摆体部分的受力分析如图3．17所示。 图中，m1表示摆体质量； L表示摆体质心c到支点距离； F1表示摆体转动摩擦系数； J1c表示摆体绕质心转动惯量； J1表示摆体绕支点的转动惯量； f′x小车对摆体作用力的水平分量； f′y小车对摆体作用力的垂直分量。 图3．16小车受力分析图 图3．17摆杆受力分析图 考虑到摆体为一平面运动体，则其运动可以分解为平动和绕质心转动两部分，质心加速度等于质心平动加速度和绕质心转动加速度之和。 计算质心加速度。水平分量为 acx=r¨+θ¨Lcosθ－θ·2Lsinθ 垂直分量为 acy=－(θ¨Lsinθ+θ·2Lcosθ) 列写摆体动力学方程式(平动部分) f′x=m1acx=m1(r¨+θ¨Lcosθ－θ·2Lsinθ) f′y－m1g=m1acyf′y=m1acy+m1g f′y=－m1(θ¨Lsinθ+θ·2Lcosθ)+m1g 列写摆体动力学方程式(转动部分) J1cθ¨=－f′xLcosθ+f′yLsinθ－F1θ· 代入f′x、f′y，整理可得 J1cθ¨=－m1(r¨+θ¨Lcosθ－θ·2Lsinθ)·Lcosθ －［m1(θ¨Lsinθ+θ·2Lcosθ)－m1g］Lsinθ－F1θ· J1cθ¨=－m1Lcosθ·r¨－m1L2cos2θ·θ¨+m1L2sinθ·cosθ·θ·2－m1L2sin2θ·θ¨ －m1L2sinθ·cosθ·θ·2+m1gLsinθ－F1θ· J1cθ¨=－m1L2θ¨－m1Lcosθ·r¨+m1gLsinθ－F1θ· (J1c+m1L2)θ¨+m1Lcosθ·r¨=m1gLsinθ－F1θ· 令 J1=J1c+m1L2 m1Lcosθ·r¨+J1θ¨=m1gLsinθ－F1θ· 将fx=f′x代入小车式(3.27)，可得 m0r¨=G0u－fx－F0·r·=G0u－m1(r¨+θ¨Lcosθ－θ·2Lsinθ)－F0·r· (m0+m1)r¨+m1Lcosθ·θ¨=G0u+m1θ·2Lsinθ－F0·r· 二方程联立，可得矩阵形式 m0+m1m1Lcosθ m1LcosθJ1r¨ θ¨+F0－m1Lsinθ·θ· 0F1r· θ·+0 －m1gLsinθ=G0u 0 (3.28) 对应于动力学一般形式 M(q)q¨+C(q,q·)q·+G(q)=τ(3.29) 从左向右，依次称为惯性项、向心项哥氏项、重力项和广义力项。 3．4．2拉格朗日方程法倒立摆动力学建模 应用非保守系统的拉格朗日方程，则小车和摆体的动能、势能和耗散能分别为 T0=12m0r·2，V0=0，D0=12F0r·2 T1=12J1cθ·2+12m1ddtr+Lsinθ2+ddtLcosθ2 V1=m1gLcosθ，D1=12F1θ·2 当qi=r时，Fqi=G0u。 当qi=θ时，Fqi=0，于是有 T=T0+T1=12m0r·2+12J1cθ·2+12m1［(r·+Lcosθ·θ·)2+(－Lsinθ·θ·)2］ V=V0+V1=m1gLcosθ，D=D0+D1=12F0r·2+12F1θ·2 当qi=r时，即对小车而言 Tr·=m0r·+m1(r·+Lcosθ·θ·)=(m0+m1)r·+m1Lcosθ·θ· ddtTr·=(m0+m1)r¨+m1L－sinθ·θ·2+cosθ·θ¨ =m0+m1r¨+m1Lcosθ1·θ¨－m1Lsinθ1·θ·2 Tr=0，Vr=0，Dr·=F0r· 当qi=θ时，即对摆体而言 Tθ·1=J1cθ·+m1r·Lcosθ+L2cos2θ·θ·+L2sin2θ·θ·=J1cθ·+m1r·Lcosθ+L2θ· ddtTθ·=J1cθ¨+m1Lr¨cosθ－r·sinθ·θ·+Lθ¨ Tθ=－m1Lr·sinθ·θ·，Vθ=－m1gLsinθ，Dθ·=F1θ· 得到单级倒立摆的动力学方程为 m0+m1m1Lcosθ m1LcosθJ1c+m1L2r¨ θ¨+F0－m1Lsinθ·θ· 0F1r· θ·=G0u m1gLsinθ(3.30) 于是可以得到与式(3.28)相同的结果。 当考虑在不稳定平衡点附近的线性化时，可令 cosθ≈1,sinθ≈θ,sinθ·θ·≈0,θ≈0 于是可得简化动力学方程 m0+m1m1L m1LJ1c+m1L2r¨ θ¨+F00 0F1r· θ·=G0u m1gL1θ(3.31) 3．4．3拉格朗日函数法倒立摆动力学建模 本节应用拉格朗日函数，也可以求解倒立摆动力学建模问题。 建立Lagrange函数 L=T-V 小车部分 T0=12m0r·2，V0=0 摆体部分 T1=12J1cθ·2+12m1ddtr+l1sinθ2+ddtl1cosθ2 =12J1cθ·2+12m1r·2+m1l1r·θ·+12m1l21θ·2 V1=m1gl1cosθ 则 L=12(m0+m1)r·2+12(J1c+m1l21)θ·2+m1l1r·θ·－m1gl1cosθ 对于小车(r)而言，L求偏导和导数，可得 Lr·=(m0+m1)r·+m1l1θ·，ddtLr·=(m0+m1)r¨+m1l1θ¨，Lr=0 对应广义坐标r的广义力为 Fr=G0u－F0r· 可得第一个方程 G0u－F0r·=(m0+m1)r¨+m1l1θ¨ 对于摆杆θ而言，L求偏导和导数，可得 Lθ·=(J1c+m1l21)θ·+m1l1r·=J1θ·+m1l1r·， ddtLθ·=J1θ¨+m1l1r¨，Lθ=m1gl1sinθ=·m1gl1θ 对应广义坐标θ的广义力为－F1θ·2。可得第二个方程 －F1θ·=J1θ¨+m1l1r¨－m1gl1θ 写成矩阵形式，便得到与前两种方法在平衡点附近线性化后相同的结果。 m0+m1m1l1 m1l1J1r¨ θ¨+F00 0F1r· θ·=G0u m1gl1θ(3.32) 3．5小结 本章阐述了典型的坐标系，以及动静坐标系之间的齐次变换矩阵； 利用DH参数建立起相互由运动副链接的运动体内部各连杆之间的坐标变换关系； 以6自由度双腿机器人为例，论述了多连杆复杂系统的位置运动学的建模方法和逆问题求解方法，关键是理清运动之间的传递关系及动静坐标转换； 给出了速度运动学建模方法； 以6自由度双腿机器人为例，论述了单腿支撑和双腿支撑状态下，系统的动力学建模方法和逆问题求解方法； 而后推广应用到12自由度步行机器人的运动学和动力学建模中； 结合实际案例分析了拉格朗日和牛顿欧拉建模方法的优劣； 建立了小车倒立摆案例系统的完整动力学模型。 习题 3.1如图3.18所示，手坐标上长度为1的矢量a，先沿z轴移动10为b，再分别绕x轴和xH轴转90°为c和d，通过齐次变换矩阵运算，求解b、c、d矢量值，写出求解过程，并注意运算中的左、右乘关系。 3.2列写图3.19所示转动平动三关节手臂运动学方程(坐标变换矩阵)，包括建立坐标系、各关节DH参数、各关节变换矩阵，综合得到手臂运动学方程。 图3.18习题3.1示意图 图3.19习题3.2示意图 3.3设手坐标上向量a=u v w 1，先沿z轴移动10为b，再绕x轴转90°为c，最后绕xH轴转90°为d。分别求解b、c、d向量值，写出计算过程。 3.4以人体手臂为对象（肩关节有3个转动自由度，肘关节有1个转动自由度，腕关节有2个转动自由度），建立机构简图，并建立运动学模型。 3.5以人体手臂为对象（肩关节有2个转动自由度，肘关节有1个转动自由度，不考虑腕关节和手掌），建立机构简图，并建立动力学模型。 3.6测量个人手臂的机构参数和关节转动角度范围，并代入习题3.5所得的运动学模型，利用MATLAB求解人体躯干保持静止时，拳头可达工作空间的具体数值。 3.7简述运动学正问题和运动学逆问题。 3.8编程题： 求解图3．20所示运动机构在摆动过程膝关节中心点、踝关节中心点和脚跟、脚尖点的运动轨迹， 图3.20习题3.8示意图 画图表示运动轨迹。运动中，髋关节中心点(R(0),X(0))固定，摆角运动给定为（仅供参考，参数可自己修改） θ(1)1=－θ(2)1=sin(3t)，θ(1)2=－θ(2)2=sin(5t)，θ(1)3=－θ(2)3=sin(4t) 摆动时长t为0．8s，机构参数通过测量个人的双腿得到。 以直立为初始姿态，此时各个角度的初值为0。 3.9求图3.21所示的2自由度机械手的雅可比矩阵。 3.10求解θr操作机的雅可比矩阵及其逆矩阵。 3.11利用牛顿欧拉方法建立图3.22所示圆轨一级倒立摆的动力学模型。 图3.21习题3.9示意图 图3.22习题3.11示意图