第3章〓机器人动力学
对于动力学，有两个相反的问题： 其一是已知机械手各关节的作用力或力矩，求各关节的位移、速度和加速度，求得运动轨迹； 其二是已知机械手的运动轨迹，即各关节的位移、速度和加速度，求各关节所需要的驱动力或力矩。

工业机器人动力学分析的两类问题是： 

(1) 给出已知的轨迹点的关节位置、速度和加速度，求相应的关节力矩向量τ，用以实现对机器人的动态控制。

(2) 已知关节驱动力矩，求机器人系统的相应各瞬时的运动，用于模拟机器人运动。

分析机器人动力学的方法很多，如拉格朗日方法、牛顿欧拉方法、高斯方法、凯恩方法等。其中，拉格朗日方法不仅能使求解复杂的系统动力学方程简单，而且容易理解。

3.1刚体的转动惯量
3.1.1刚体定轴转动与惯性矩 


在物理学中，刚体定轴转动微分方程： 


Iω·=τ(3.1)


式中，I为绕固定轴的惯性矩(也称为转动惯量)； τ是作用在固定轴上的合外力矩。对于一个质量为m的质点，其在直线上运动的动力学问题可以用牛顿第二定律描述： 


mv·=f或mx¨=f(3.2)




图31圆盘绕过圆心轴惯性矩


比较式(3.1)和式(3.2)可以发现，刚体定轴转动和质点直线运动的动力学方程的形式是完全相同的。因此，I可以看作刚体定轴转动的惯性质量。

下面以图31所示质量为M，半径为r的均匀圆盘绕过圆心的Z轴的惯性矩计算问题，给出惯性矩的定义： 


I=∫Vr2dm(3.3)


式(3.3)给出了任意刚体绕固定轴惯性矩的定义，其中dm是微元体质量，r是微元体到转轴的距离，V是刚体的体积，因此式(3.3)表示在整个体积上积分。



对于图31所示均匀圆盘，面密度ρ=M/(πR2)，取极坐标微元体，则 


I=∫Vr2dm=∫R0∫2π0r2ρrdrdθ=2πρR44=2πMπR2R44=12MR2(3.4)


例31如图32所示匀质杆，质量为M，杆长为L，计算绕质心的惯性矩。




图32匀质杆绕质心惯性矩




解： 匀质杆的线密度ρ=M/L，取微元体dx，则 


I=∫L/2-L/2x2dm=2∫L/20x2ρdx=2ρ(L/2)33
=2MLL33·8=112ML2



平行移轴定理： 刚体绕任意平行于质心轴的惯性矩为



I=CI+Md2(3.5)


式中，CI表示刚体绕质心轴的惯性矩； M为刚体质量； d为两轴之间的距离。若已知刚体绕质心轴的惯性矩，则刚体绕任意平行轴的惯性矩可以非常方便地利用平行移轴定理［式(3.5)］进行计算。

例如，计算图32所示匀质杆绕杆端点的惯性矩，根据平行移轴定理 


I=CI+Md2=112ML2+ML22=13ML2(3.6)


可以验证，式(3.6)计算结果与采用积分方法相同。

3.1.2刚体的惯性张量

对于在三维空间自由运动的刚体，存在无穷多个可能转轴，计算绕所有转轴的惯性矩显然是不现实的。因此需要考虑这样的问题： 是否存在一个量，它能够表示刚体绕任意转轴的惯性矩？答案是肯定的，该量就是刚体的惯性张量。惯性张量描述了刚体的三维质量分布，若在某坐标系下表示出来，它就是一个3阶对称矩阵。图33所示的一个刚体，其上定义了固连的坐标系{A}。



图33空间刚体的惯性张量



在坐标系{A}中惯性张量为 


AI=Ixx-Ixy-Ixz
-IxyIyy-Iyz
-Ixz-IyzIzz(3.7)


惯性张量是一个对称矩阵，各元素的值为



Ixx=∫V(y2+z2)ρdvIxy=∫Vxyρdv
Iyy=∫V(x2+z2)ρdvIxz=∫Vxzρdv
Izz=∫V(x2+y2)ρdvIyz=∫Vyzρdv(3.8)


惯性张量中Ixx、Iyy和Izz称为惯性矩，交叉项Ixy、Ixz和Iyz称为惯性积。显然，惯性张量中元素的数值与坐标系的选择有关。一般存在某个坐标系，使得交叉项全为0，该坐标系称为惯性主轴坐标系，坐标轴称为惯性主轴。对于质量均匀分布的规则物体，惯性主轴就是物体的对称轴。

例32如图34所示质量均匀分布的长方形刚体，密度为ρ，质量为M，计算其惯性张量。




图34质量均匀分布的长方形刚体



解： 单元体dv=dxdydz，根据式(3.8)得


Ixx=∫V(y2+z2)ρdv=ρ∫H0∫L0∫W0(y2+z2)dxdydz
=ρW∫H0∫L0(y2+z2)dydz=ρW∫H0(L3/3+Lz2)dz
=ρWHL33+LH33=M3(L2+H2)


同理,可以得到另外两个惯性矩：


Iyy=M3(W2+H2),Izz=M3(W2+L2)


下面计算惯性积： 


Ixy=∫Vxyρdv=ρ∫H0∫L0∫W0xydxdydz=ρ∫H0∫L0yW2/2dydz
=ρW2L24∫H0dz=ρW2L2H4=M4WL


同理,可以得到另外两个惯性积：


Ixz=M4WH,Iyz=M4HL


至此，我们已经计算出了式(3.7)中所有6个分量的值。对于惯性张量的计算问题，平行移轴定理也是成立的，下面给出其中两个表达式，其余的3个表达式与此类似： 



AIzz=CIzz+M(x2c+y2c)
AIxy=CIxy+M(xcyc)


3.2刚体动力学
3.2.1刚体的动能与位能
1. 刚体的动力学方程



拉格朗日函数L定义为系统的动能K和位能P之差，即


L=K-P(3.9)


其中，K和P可以用任何方便的坐标系来表示。

系统动力学方程式，即拉格朗日方程如下： 


Fi=ddtLq·i-Lqi,i=1,2,…,n(3.10)


式中，qi为表示动能和位能的坐标； q·i为相应的速度； Fi为作用在第 i 个坐标上的力或是力矩，是由qi为直线坐标或角坐标决定的。这些力、力矩和坐标称为广义力、广义力矩和广义坐标，n为连杆数目。

2. 刚体的动能与位能


根据力学原理，对图35所示的一般物体平动所具有的动能和位能进行计算如下： 


图35一般物体的动能与位能







K=12M1x21+12M0x20
P=12k(x1-x0)2-M1gx1-M0gx0
D=12(x·1-x·0)2
W=Fx1-Fx0




式中，K、P、D和W分别表示物体所具有的动能、位能、所消耗的能量和外力所做的功； M0和M1为支架和运动物体的质量； 
x0和x1为运动坐标； g为重力加速度； k为弹簧胡克系数； c为摩擦系数； F为外施作用力。对于这一问题，存在两种情况： 

(1) x=0,x1 为广义坐标： 


ddtKx·1-Kx1+Dx·1+Px1=Wx1


其中，左式第一项为动能随速度(或角速度)和时间的变化； 第二项为动能随位置(或角度)的变化； 第三项为能耗随速度变化； 第四项为位能随位置的变化。右式为实际外加力或力矩。表示为一般形式： 


M1x¨1+c1x·1+dx1=F+M1g


(2) x0=0,x0和x1 均为广义坐标，有下式： 


M1x¨1+c(x·1-x·0)+k(x1-x0)-M1g=F
M0x¨0+c(x·1-x·0)-k(x1-x0)-M0g=-F


或用矩阵形式表示为


M100M0x¨1x¨0+c-c-ccx·1x·0+k-k-kkx1x0=F-F


3.2.2拉格朗日方程
1. 单摆拉格朗日方程




图36单摆


单摆由一根无质量杆末端连接一集中质量m，杆长为l，其上作用力矩τ，试建立系统的动力学方程，如图36所示。



选择θ为描述单摆位置的广义坐标，先用广义坐标表示集中质量的位置，然后再对时间求导得到速度：


x=lsinθ,y=-lcosθ
x·=lθ·cosθ,y·=lθ·sinθ


系统的动能为


K=12mv2=m2(x·2+y·2)=m2(l2θ·2cos2θ+l2θ·2sin2θ)=12ml2θ·2


取坐标原点为势能零点，则系统的势能为


P=mgy=-mglcosθ


系统的拉格朗日函数为


l=K-P=12ml2θ·2+mglcosθ


根据上式计算相应的导数：


ddtlθ·=ddt(ml2θ·)=ml2θ¨
lθ=-mglsinθ


代入到拉格朗日方程得系统的动力学方程：


τ=ml2θ¨+mglsinθ


2. 二连杆机械手的拉格朗日方程




下面先来考虑二连杆机械手(图37)的动能和位能。这种运动机构具有开式运动链，与复摆运动有许多相似之处。图中，m1和m2为连杆1和连杆2的质量，且以连杆末端的点质量表示； d1和d2分别为两连杆的长度； θ1和θ2为 广义坐标； g 为重力加速度。先计算连杆1的动能和位能，再计算连杆2的动能和位能。


图37二连杆机械手



K1=12m1v21，ν1=d1θ·1
P1=m1gh1，h1=-d1cosθ1



先计算连杆1的动能K1和位能P1：


K1=12m1d21θ·21，P1=-m1gd1cosθ1


再计算连杆2的动能K2和位能P2：


K2=12m2v22，P2=mgy2


式中，


v22=x·22+y·22
x2=d1sinθ1+d2sin(θ1+θ2)
y2=-d1cosθ1-d2cos(θ1+θ2)


则得


K2=12m2d21θ·21+12m2d22(θ·1+θ·2)+m2d1d2cosθ2(θ·21+θ·1θ·2)
P2=-m2gd1cosθ1-m2gd2cos(θ1+θ2)


二连杆机械手系统的总动能和总位能分别为


K=K1+K2
=12(m1+m2)d21θ·21+12m2d22(θ·1+θ·2)2+m2d1d2cosθ2(θ·21+θ·1θ·2)
P=P1+P2
=-(m1+m2)gd1cosθ1-m2gd2cos(θ1+θ2)


二连杆机械手系统的拉格朗日函数L为


L=K-P
=12(m1+m2)d21θ·21+12m2d22(θ·21+2θ·1θ·2+θ·22)+
m2d1d2cosθ2(θ·21+θ·1θ·2)+(m1+m2)gd1cosθ1+m2gd2cos(θ1+θ2)


对L求导数，得


Lθ·1=(m1+m2)l21θ·1+m2l22(θ·1+θ·2)+m2l1l2(2θ·1+θ·2)cosθ2
ddtLθ·1=(m1+m2)l21θ¨1+m2l22(θ¨1+θ¨2）+m2l1l2(2θ¨1+θ¨2）cosθ2-m2l1l2(2θ·1θ·2+θ·22)sinθ2
=(m1+m2)l21+m2l22+2m2l1l2cosθ2θ¨1+m2l22+m2l1l2cosθ2θ¨2-
2m2l1l2sinθ2θ·1θ·2-m2l1l2sinθ2θ·22
Lθ1=-(m1+m2)gl1sinθ1-m2gl2sin(θ1+θ2)
Lθ·2=m2l22(θ·1+θ·2)+m2l1l2θ·1cosθ2
ddtLθ·2=m2l22(θ¨1+θ¨2）+m2l1l2cosθ2θ¨1-m2l1l2θ·1sinθ2
Lθ2=-m2l1l2(θ·21+θ·1θ·2)sinθ2-m2gl2sin(θ1+θ2)
T1=(m1+m2)l21+m2l22+2m2l1l2cosθ2θ¨1+m2l22+m2l1l2cosθ2θ¨2-
2m2l1l2sinθ2θ·1θ·2-m2l1l2sinθ2θ·22+(m1+m2)gl1sinθ1+m2gl2sin(θ1+θ2)
T2=m2l22(θ¨1+θ¨2）+m2l1l2cosθ2θ¨1-m2l1l2θ·1sinθ2+
m2l1l2(θ·21+θ·1θ·2)sinθ2+m2gl2sin(θ1+θ2)



求得力矩的动力学方程式


T1T2=D11D12D21D22θ¨1θ¨2+D111D122D211D222θ·21θ·22+D112D121D212D221θ·1θ·2θ·2θ·1+D1D2



比较可得本系统各系数如下： 

有效惯量


D11=(m1+m2)d21+m2d22+2m2d1d2cosθ2
D22=m2d22


耦合惯量


D12=m2d22+m2d1d2cosθ2=m2(d22+d1d2cosθ2)



向心加速度系数


D111=0
D122=-m2d1d2sinθ2
D211=m2d1d2sinθ2
D222=0


哥氏加速度系数


D112=D121=-m2d1d2sinθ2
D212=D221=0


重力项


D1=(m1+m2)gd1sinθ1+m2gd2sin(θ1+θ2)
D2=m2gd2sin(θ1+θ2)


其中，Di表示关节i处的重力； Dii为关节i的有效惯量； Dij为关节i和j间的耦合惯量； l1=d1,l2=d2。


3.3机械手动力学方程


分析由一组变换描述的任何机械手，求出其动力学方程(图38)。推导过程分5步进行： 

(1) 计算任一连杆上任一点的速度；

(2) 计算各连杆的动能和机械手的总动能；

(3) 计算各连杆的位能和机械手的总位能；

(4) 建立机械手系统的拉格朗日函数；

(5) 对拉格朗日函数求导，以得到动力学方程式。




图38四连杆机械手



1. 速度的计算

连杆3上点P的速度为


0vp=ddt(0rp)=ddt(T33rp)=T·33rp


对于连杆i上任一点的速度为


v=drdt=∑ij=1Tiqjq·jir


P点的加速度为


0ap=ddt(0vp)=ddt(T·33rp)=T·33rp=ddt∑3j=1T3qiq·i3rp
=∑3j=1T3qiddtq·i(3rp)+∑3k=1∑3j=12T3qjqkq·kq·j(3rp)
=∑3j=1T3qiq¨i(3rp)+∑3k=1∑3j=12T3qjqkq·kq·j(3rp)


速度的平方为


(0vp)2=(0vp)·(0vp)=Trace［(0vp)·(0vp)T］
=Trace∑3j=1T3qjq·j(3rp)·∑3k=1T3qkq·k(3rp)T
=Trace∑3j=1∑3k=1T3qj(3rp)(3rp)TT3Tqkq·jq·k


式中，Trace表示矩阵的迹。对于n阶方程来说，其迹即为它的主对角线上各元素之和。

任一机械手上一点的速度平方为


v2=drdt2=Trace∑ij=1Tiqjq·jir∑ik=1Tiqkq·kirT
=Trace∑ij=1∑ik=1TiqkirirTTiqkTq·kq·k


2. 动能的计算

令连杆3上任一质点P的质量为dm，则其动能为


dK3=12v2pdm
=12Trace∑3j=1∑3k=1T3qi3rp(3rp)TT3qkTq·iq·kdm
=12Trace∑3j=1∑3k=1T3qi(3rpdm3rpT)TT3qkTq·iq·k


任一机械手连杆i上位置向量ir的质点，其动能为


dKi=12Trace∑ij=1∑ik=1Tiqj
jrirTTTiqkq·jq·kdm
=12Trace∑ij=1∑ik=1Tiqj(irdmirT)TTTiqkq·jq·k


连杆3的动能为


K3=∫连杆3dK3=12Trace∑3j=1∑3k=1T3qj∫3连杆3r3prTpdmT3qkTq·jq·k


任何机械手上任一连杆i动能为


Ki=∫连杆idKi=12Trace∑ij=1∑ik=1TiqjIiTiqkq·jq·k


式中，Ii为伪惯量矩阵。

具有n个连杆的机械手总的功能为


K=∑ni=1Ki=12∑ni=1Trace∑nj=1∑ik=1TiqjIiTTiqkq·iq·k


连杆i的传动装置动能为


Kai=12Iaiq·2i


所有关节的传动装置总动能为


Ka=12∑ni=1Iaiq·2i


机械手系统(包括传动装置)的总动能为


Kt=K+Ka=12∑6i=1∑ij=1∑ik=1TraceTiqiIiTTiqkq·jq·k+12∑6i=1Iaiq·2i


3. 位能的计算

一个在高度h处质量为m的物体，其位能为


P=mgh


连杆i上位置ir处的质点dm，其位能为


dPi=-dmgT0r=-gTTiirdm


式中，gT=［gx,gy,gz,1］。



Pi=∫连杆idPi=-∫连杆igTTiirdm=-gTTi∫连杆iirdm
=-gTTimiiri=-migTTiiri


连杆上位置ir处的质点dm，其位能为


dPi=-dmgT0r=-gTTiirdm


机械手系统的总位能为


P=∑ni=1(Pi-Pai)≈∑ni=1Pi=-∑ni=1migTTiiri


4. 拉格朗日函数的建立


L=Kt-P=12∑ni=1∑ij=1∑ik=1TraceTiqiIiTTiqkq·jq·k+
12∑ni=1Iaiq·2i+∑ni=1migTTiiri,n=1,2，…，n


5. 动力学方程的推导

再据式(3.2)求动力学方程，先求导数


Lq·p=12∑ni=1∑ik=1TraceTiqpIiTTiqkq·k+
12∑ni=1∑ij=1TraceTiqiIiTTiqpq·j+Iapq·pp=1,2,…,n


据上式知，Ii为对称矩阵，即ITi=Ii，所以下式成立： 


TraceTiqjIiTTiqk=TraceTiqkITiWTiqj=TraceTiqkIiWTiqj
Lq·p=∑ni=1∑ik=1TraceTiqkIiTTiqpq·k+Iapq·p
ddtLq·p=∑ni=p∑ik=1TraceTiqkIiTTiqpq¨k+Iapq¨p+
∑ni=p∑ij=1∑ik=1Trace2TiqjqkIiTTiqiq·jq·k+
∑ni=p∑ij=1∑ik=1Trace2TiqpqkIiTTiqiq·jq·k
=∑ni=p∑ik=1TraceTiqkIiTTiqpq¨k+Iapq¨p+2∑ni=p∑ij=1∑ik=1Trace2TiqjqkIiTTiqkq·jq·k
Lqp=12∑ni=p∑ij=1∑ik=1Trace2TiqjqkIiTTiqkq·jq·k+
12∑ni=p∑ii=1∑ik=1Trace2TiqkqpIiTTiqjq·jq·k+∑ni=pmigTTiqpiri
=∑ni=p∑ij=1∑ik=1Trace2TiqpqjIiTTiqkq·jq·k+∑ni=pmigTTiqpiri


具有n个连杆的机械手系统动力学方程如下： 


Ti=∑nj=i∑jk=1TraceTjqkIjTTjqiq¨k+Iaiq¨i+
∑nj=1∑jk=1∑jm=1Trace2TiqkqmIjTTjqiq·kq·m-∑nj=1mjgTTiqiiri
Ti=∑nj=1Dijq¨j+Iaiq¨i+∑6j=1∑6k=1Dijkq·jq·k+Di