机器人学第七章(机器人动力学的凯恩方法)

第七章 机器人动力学的凯恩方法7.1 引言机器人动力学凯恩方程方法是建立在凯恩动力学方程基础上的，因而本章首先介绍凯恩动力学方程。

7.1.1 质点系的凯恩动力学方程设一质点系具有n 个质点，该质点系的动力学普遍方程为()[]01=⋅-∑=ni i i i ir a m fδ （7-1）式中 i f ——作用于第i 质点主动力矢量；i m ——质点i 的质量；i a ——质点i 的加速度矢量；i r ——质点i 在参考坐标系中的位置矢量；i r δ——质点i 的微分位移；“·”——数量积符号。

设质点系为完全系，即它具有l 个自由度和l 个广义坐标，则()t q q q r r li i (21)= （7-2）式中 i q ――广义坐标；t ——时间变量； 质点i 的线速度为j lj q i j l j j i i i q v q q r dt r v j ∑∑===⎪⎪⎭⎫ ⎝⎛∂∂=∂=1.1 式中j i j i q i qvq r v j ∂∂=∂∂=. （7-3）凯恩（kane ）定义,j i q i j v v q =∂∂为质点I 相对于广义速度的偏速度。

微分i r δ可表示为j lj q i j lj j ii q v q q r r j δδδ∑∑===∂∂=1.1 （7-4）将（7-4）代入（7-1）式，得(), 110j ll i i i i q j i j f m a v q δ==⎡⎤-⋅=⎢⎥⎣⎦∑∑ 交换求和符号，得(), 110j ln i i i i q j i j f m a v q δ==⎡⎤-⋅=⎢⎥⎣⎦∑∑因为j q 是独立变量，故(), 10j nii i i q j fm a v =-⋅=∑ j=1,2,．．．，l （7-5） 或, , 110j j nnii q i i i q j i fv m a v ==⋅-⋅=∑∑这就是质点系的凯恩动力学方程(Kane Dynamics Equation ),可以改写为', 1', 101,2,,_______j j j j nj i i q i n j i i i q i F j l F f v F m a v F ==⎫⎪+==⋅⋅⋅⎪⎪=⎬⎪⎪=⎪⎭⋅⋅∑∑广义主动力广义惯性力 （7-6）7.1.2 刚体的凯恩动力学方程如图7-1所示将刚体看成是由n 个质点组成的。

设刚体的质心为C ，以C 为力的简化中心并设作用于刚体的主动力的合力为C Q ，合力矩为C N ：∑==ni i c f Q 1（7-7）()∑=⨯=ni i i c R f N 1（7-8）当刚体以角速度ω旋转时，其中点i 的速度为c i i v v R ω=+⨯其中 i R ——点到质心C 的位置矢量；i v ——质心C 的线速度。

Z 点对广义速度的偏速度为(), ωj i i c i q j j jR v v v q q q ∂⨯∂∂==+∂∂∂ 或, , j j j q i q C q i v v R ω=+⨯ （7-9）式中, j C q v ——质心C 相对于j q的偏速度： , j cC q jv v q ∂=∂ （7-10） jqω——刚体相对于j q的偏角速度： ωωj q jq ∂=∂ （7-11） 于是作用在刚体上相对于j q的广义力为 ()(),,111,11ωωj j jj jn n nj i i q i C q i i q i i i nni C q i i q i i F f v f v f R f v f R ======⋅=⋅+⋅⨯=⋅+⨯⋅∑∑∑∑∑或(),11 ωj j j C C q C q nc i i n c i i i F Q v N Q f N f R ==⎫⎪=⋅+⋅⎪⎪=⎬⎪⎪=⨯⎪⎭∑∑ （7-12）相对于j q的广义惯性力为 ()111',,j j j n n nji i i q i i C q i i i q i i i F m a v m a v m a R ω====-⋅=-⋅-⨯∑∑∑而 ()()dt dH R v m dt d R a m cn i i i i ni i i i =⎪⎭⎫ ⎝⎛⨯=⨯∑∑==11式中动量矩c H 用刚体的惯性张量表示为⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡----=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡z y x zz zyzxyz yy yx xz xyxxz y x I I I I I I I I I H H H ωωω （7-13）因此()1nCi i i i dH m a R I I dt ωωω==⨯=+⨯∑ （7-14） 得广义惯性力表示为()112',,,,j jnji i C q q i F m a v I I j l ωωωω==-⋅-+⨯⋅=⋅⋅⋅∑ （7-15）将（7-12）和（7-15）式合并，从而得到刚体的凯恩动力学方程为112,, ,,, j j j j nC C q C q i i C q C q i C Q v N m a v N N I I j l ωωωωω=⎫⋅+⋅=⋅+⋅⎪⎬⎪=+⨯=⋅⋅⋅⎭∑ （7-16） 式中 I ——刚体相对于质心C 的惯性张量。

7.2 机器人杆件速度、加速度及偏速度的递推计算公式如图9-2所示，杆件坐标系均设置在各杆件上编号关节处,n 个自由度的机器人有n 个关节。

图中i e ~——指定杆件坐标系各iz 轴方向的单位矢量，共有i =0，1，2，．．．，n 个，它们均是以杆件坐标系描述的常矢量，[]T ie e100~0==； i p ~——以杆件坐标系｛i -1｝的原点为始点到以｛i ｝系原点为终点的矢量，但它是以｛i ｝系描述的矢量；i R ~——以杆件坐标系｛i ｝描述的第i 号杆件质心i C 的位置矢量；两相邻坐标系{i -1}及{i }中速度、加速度等的关系可用变换矩阵i A 中的旋转子矩阵C i i1-及R i i 1-相联系。

仿照第六章处理杆件坐标系及杆件质心的速度及加速度的方法，并考虑到坐标系设置方法上的区别，不难得到如下所述的速度及加速度递推计算公式：2qx 1n qa 22 2R()()()110111100111011 ii i i i i i i i i i i i i i i i i i i i i i i i i i c i i i i ii i R s q e R s R q e q e v Rv p s Rq e v v s R v R ωωωωωωω----------=+=+⨯+=+⨯+-=+⨯=()()()()110012 i i i i i i i ii i i i c i i i i i i v p p s R qe q e v v R R ωωωωωωω--⎫⎪⎪⎪⎪⎪⎬⎪+⨯+⨯⨯⎪⎪+-⨯+⎪=+⨯+⨯⨯⎪⎭（7-17）式中 i q——广义坐标对时间的1阶导数，即关节轴的数量速度； i q——广义坐标对时间的2阶导数，即关节轴的数量加速度； i s ——关节类型识别符号；⎩⎨⎧为移动关节为转动关节i i s i 01 （7-18） 与第六章相同，令000Tv g ⎡⎤⎣⎦= （7-19）式中 g ——重力加速度。

上式是假定绝对参考系的0Z 轴垂直于地面且指向向上的。

若0Y 轴垂直于地面，则[]Tg v000-= 其中负号表示0Y 轴指向地心（0Y 于重力场同方向）。

偏速度的递推公式为：()11101110010, , , , , .Re Re j j j j j j i i i q i i q i i i i q i i i q ii q i i q i i i R j i s j i j i p Rv v s p s ωωωω------⎧<⎪⎪==⎨⎪>⎪⎩⨯+=⨯+-()()101010, , , ,Re Re j j i j j i q i q i i ic q i i q i i i i i j i j i j i v R v s p R s ωω--⎧<⎪⎪=⎨⎪>⎪⎩+⨯=⨯+⨯+- j i j i j i ⎫⎪⎪⎪⎪⎪⎪⎪⎪⎬⎪⎪⎪⎧⎪<⎪⎪⎪=⎪⎨⎪⎪>⎪⎪⎩⎭（7-20）例7-1 如图7-3所示的平面包2自由度机器人，1θ 、2θ 为已知，试用（7-17）及（7-20）式计算各杆的速度、加速度及偏速度。

杆件的质心均在杆件的末端。

解：11101100001C S R S C ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦=-， 22212200001C S R S C ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦=-， 120R R ==[]1100TP l =，[]2200TP l =，00ω=，00ω=，00v =，[]000Tv g =。

式中 g ——重力加速度。

0Y 轴与重力场反向，故g 取正。

i =1时：()110010100TR e ωωθθ⎡⎤=+=⎣⎦11001, Tθω⎡⎤⎣⎦=1111100111000000l v p Rv l ωθθ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦=⨯+=⨯=1111100, C v v l θθ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦∂==∂ ()111100010101111000000001C S R e e S C ωωωθθθθ⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥=+⨯+=-⨯=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦()111100111111211111111111000001000000000000 C S v Rv p p S C g l l l S g l C g ωωωθθθθθ⎡⎤⎡⎤⎢⎥⎢⎥=+⨯+⨯⨯=-⨯⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎡⎤⎛⎫-+⎡⎤⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥+⨯+⨯⨯=+⎢⎥⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦⎝⎭⎣⎦()1211111*********C l S g v v R R v l C g θωωωθ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦-+=+⨯+⨯⨯==+i =2时：()2222112022211200000000011C S R e S C ωωθθθθθ⎛⎫⎡⎤⎡⎤⎡⎤⎡⎤ ⎪⎢⎥⎢⎥⎢⎥⎢⎥=+=-+= ⎪⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥+⎣⎦⎣⎦⎣⎦⎣⎦⎝⎭ 12001, θω⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦=， 22001, θω⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦= 1x2θ1θ1lX 平面2自由度机器人()22222112222111212121212100000000100 0C S l v Rv p S C l l S l l C ωθθθθθθθ⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦=+⨯=-+⨯+=++()212122222121210C l S v v R v l l C θωθθθ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦=+⨯==++2112212, 0Cl S v l l C θ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦=+， 222, 00C v l θ⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦= ()2211120202222112212000000 000000001R e e C S S C ωωωθθθθθθθθ⎛⎫⎡⎤⎡⎤⎡⎤⎡⎤⎡⎤⎡⎤ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦⎝⎭=+⨯+=-+⨯+=+()2222222112212121221112222111000 00000000 00010v p p Rv l l l S g C S S C l C g ωωωθθθθθθθθ⎛⎫⎡⎤⎡⎤⎡⎤⎡⎤⎡⎤ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥ ⎪⎣⎦⎣⎦⎣⎦⎣⎦⎣⎦⎝⎭⎡⎡⎤⎢⎢⎥⎢⎢⎥⎢⎥⎣⎦⎣=⨯+⨯+=⨯+⨯⨯+++-++-+⎤⎥⎥⎢⎥⎢⎥⎦()22222222C v p p v v ωωω=⨯+⨯+= （同上式）此例所得各质心的速度及加速度的计算结果与上一章的计算方法得到的结果是完全相同（例7-2将用此例的偏速度）。