动力学研究的是机械臂的运动与关节驱动力，力矩之间的联系。
主要是每个的位移，速度，加速度和关节力之间的的关系。
主要有
①拉格朗日法(第二类拉格朗日方程);
②牛顿-欧拉法；
③高斯法；
④凯恩法；
⑤旋转对偶法;
①拉格朗日法(第二类拉格朗日方程)以及运算
目的：计算机械臂某些特定位置时，各轴所需要最大力矩，这样就能为每个关节配置适合的电机(提供力)。
首先是计算公式，然后再根据实际情况，计算出特定位置时的关节力矩参数。
（1)计算流程(对于旋转关节来说);
(2)计算步骤；
假设在连杆i上有一点p,相对于坐标系i而言，p点的位置为
这个值是矢量值，是可以不用通过计算直接得到的。
相对于基坐标系而言，p的位置是未知的，是需要通过转换矩阵的得到的。设为
变换矩阵则为，
由于是需要算出相对于基坐标系而言的位置，而不是相对自身运动坐标系的位置，所以是符合左乘的规律。可以得到他们的关系是
此时得到的量相当于是位移量，我们需要使用的量是速度，加速度。所以需要对上面的等式进行求导。而且求导的变量是时间t,需要注意的是，变换矩阵的里面的变量是θ或者是q(一样)。所以就会得到以下的等式；
上图中的i表示的变换矩阵有几个小的变换矩阵组成，j则是表示关节符号，如j1则是指关节一。
求完速度后，我们就根据速度的等式去求加速度，对速度的等式求导，可以得到以下式子；
依旧是对求导；
需要求导的部分如下图所示。在对速度求导时，变换矩阵中有i个角，所以需要进行i次求偏导，求加速度时，则需要进行i的平方次的求导
接下来是求速度的平方，
总结：需要求三个值，速度，加速度，速度的平方；
动能的计算
先计算微量，在计算积分。积分的上下限是连杆i的长度。
这里都是根据公式列出，并无难点。
其中
这称为伪惯量矩阵，不是真的惯量矩阵。 同时这里也有一个变换；
如果令
那么
以上便是关于动能的推导。
位能的推导
假设一个在高度为h处质量为m的物体，其位能为p=mgh;
那么对于连杆上任意一个点的位能，可以这样表示：
动力学方程的推导
对于一个多自由度的机械臂来说，它的动力学方程是十分复杂的，需要多次的迭代运算，大家都是通过设计电脑程序完成计算，而且还需要对其简化，还要保证其准确性。
拉格朗日算子
假如机械臂为六自由度，那么就有六个连杆的动能相加，n就为6；
这里的p是一个虚标，可以进行替换
以上便是动力学方程的推导的全部过程，会发现十分复杂，若单纯使用原生动力学式子，可以使用Matlab机器人工具箱。若需要自己进行改进。。。。
学了一个下午，觉得十分抽象。
特别是向心力和哥氏力，关节越多，式子就越多。不过书上说只有机械手高速运动时，向心力和哥氏力才是重要的，我觉的这里是不是可以忽略掉，不过也是按照公式就可以写出来，简单。
对于二连杆的动力学方程，算是挺复杂的，我也发现大家写论文时，都是通过软件分析——matlab adams ansys
我根据上面的式子，写了一个Matlab的程序，仅仅是动力学过程，一些详细的数值并没有。
根据物体惯性计算公式i1xx=i1yy=(m1/2)(3R1^2 + L1^2)——物体绕其X轴和y轴转动惯量；
i1zz=(m1/2)R1^2; i1xz=i1xy=i1yz=0;六个轴的惯性参数都是这样计算的的。
syms theta1 theta2 theta3 theta4 theta5 theta6 d1 a2 a3 d4 d6
syms i1xx i1yy i1zz i1xy i1xz i1yz m1 xc1 yc1 zc1
syms i2xx i2yy i2zz i2xy i2xz i2yz m2 xc2 yc2 zc2
syms i3xx i3yy i3zz i3xy i3xz i3yz m3 xc3 yc3 zc3
syms i4xx i4yy i4zz i4xy i4xz i4yz m4 xc4 yc4 zc4
syms i5xx i5yy i5zz i5xy i5xz i5yz m5 xc5 yc5 zc5
syms i6xx i6yy i6zz i6xy i6xz i6yz m6 xc6 yc6 zc6
syms ddc1 ddc2 ddc3 ddc4 ddc5 ddc6 %角加速度 若是静止，变为0
syms dc1 dc2 dc3 dc4 dc5 dc6 g %角速度 和 重力
c1=cos(theta1);s1=sin(theta1);
c2=cos(theta2);s2=sin(theta2);
c3=cos(theta3);s3=sin(theta3);
c4=cos(theta4);s4=sin(theta4);
c5=cos(theta5);s5=sin(theta5);
c6=cos(theta6);s6=sin(theta6);
r1=[xc1 yc1 zc1 1]'; r2=[xc2 yc2 zc2 1]'; r3=[xc3 yc3 zc3 1]'; r4=[xc4 yc4 zc4 1]';r5=[xc5 yc5 zc5 1]';r6=[xc6 yc6 zc6 1]';
gg=[0 0 -g 0];
%坐标转换
T1=[c1 0 s1 0; s1 0 -c1 0; 0 1 0 d1; 0 0 0 1;];%1-2
T2=[c2 -s2 0 a2*c2; s2 c2 0 a2*s2; 0 0 1 0; 0 0 0 1;]; %2-3
T3=[c3 0 s3 a3*c3; s3 0 -c3 a3*s3; 0 1 0 0; 0 0 0 1;];%3-4
T4=[c4 0 -s4 0; s4 0 c4 0; 0 -1 0 d4; 0 0 0 1;];%4-5
T5=[c5 0 s5 0; s5 0 -c5 0; 0 1 0 0; 0 0 0 1;];%5-6
T6=[c6 -s6 0 0; s6 c6 0 0; 0 0 1 d6; 0 0 0 1;];%6-7
T12=T1*T2;
T13=T12*T3;
T14=T13*T4;
T15=T14*T5;
T16=T15*T6;
%%%%%%%%%%%%惯性矩阵
I1=[(-i1xx+i1yy+i1zz)/2 i1xy i1xz m1*xc1;
i1xy (i1xx-i1yy+i1zz)/2 i1yz m1*yc1;
i1xz i1yz (i1xx+i1yy-i1zz)/2 m1*zc1;
m1*xc1 m1*yc1 m1*zc1 m1; ];
I2=[(-i2xx+i2yy+i2zz)/2 i2xy i2xz m2*xc2;
i2xy (i2xx-i2yy+i2zz)/2 i2yz m2*yc2;
i2xz i2yz (i2xx+i2yy-i2zz)/2 m2*zc2;
m2*xc2 m2*yc2 m2*zc2 m2; ];
I3=[(-i3xx+i3yy+i3zz)/2 i3xy i3xz m3*xc3;
i3xy (i3xx-i3yy+i3zz)/2 i3yz m3*yc3;
i3xz i3yz (i3xx+i3yy-i3zz)/2 m3*zc3;
m3*xc3 m3*yc3 m3*zc3 m3; ];
I4=[(-i4xx+i4yy+i4zz)/2 i4xy i4xz m4*xc4;
i4xy (i4xx-i4yy+i4zz)/2 i4yz m4*yc4;
i4xz i4yz (i4xx+i4yy-i4zz)/2 m4*zc4;
m4*xc4 m4*yc4 m4*zc4 m4; ];
I5=[(-i5xx+i5yy+i5zz)/2 i5xy i5xz m5*xc5;
i5xy (i5xx-i5yy+i5zz)/2 i5yz m5*yc5;
i5xz i5yz (i5xx+i5yy-i5zz)/2 m5*zc5;
m5*xc5 m5*yc5 m5*zc5 m5; ];
I6=[(-i6xx+i6yy+i6zz)/2 i6xy i6xz m6*xc6;
i6xy (i6xx-i6yy+i6zz)/2 i6yz m6*yc6;
i6xz i6yz (i6xx+i6yy-i6zz)/2 m6*zc6;
m6*xc6 m6*yc6 m6*zc6 m6; ];
%%%%%%%%%%%%求速度的变换矩阵的导数
u11=diff(T1,'theta1');
u21=diff(T12,'theta1'); u22=diff(T12,'theta2');
u31=diff(T13,'theta1'); u32=diff(T13,'theta2'); u33=diff(T13,'theta3');
u41=diff(T14,'theta1'); u42=diff(T14,'theta2'); u43=diff(T14,'theta4');u44=diff(T14,'theta4');
u51=diff(T15,'theta1'); u52=diff(T15,'theta2'); u53=diff(T15,'theta4');u54=diff(T15,'theta4'); u55=diff(T15,'theta5');
u61=diff(T16,'theta1'); u62=diff(T16,'theta2'); u63=diff(T16,'theta4');u64=diff(T16,'theta4'); u65=diff(T16,'theta5');u66=diff(T16,'theta6');
%%%%%%%%%%%%%%%%%%%%求加速度
u111=diff(u11,'theta1');
%
u211=diff(u21,'theta1'); u212=diff(u21,'theta2');
u221=diff(u22,'theta2'); u222=diff(u22,'theta2');
%
u311=diff(u31,'theta1'); u312=diff(u31,'theta2'); u313=diff(u31,'theta3');
u321=diff(u32,'theta1'); u322=diff(u32,'theta2'); u323=diff(u32,'theta3');
u331=diff(u33,'theta1'); u332=diff(u33,'theta2'); u333=diff(u33,'theta3');
%
u411=diff(u41,'theta1'); u412=diff(u41,'theta2'); u413=diff(u41,'theta3'); u414=diff(u41,'theta4');
u421=diff(u42,'theta1'); u422=diff(u42,'theta2'); u423=diff(u42,'theta3'); u424=diff(u42,'theta4');
u431=diff(u43,'theta1'); u432=diff(u43,'theta2'); u433=diff(u43,'theta3'); u434=diff(u43,'theta4');
u441=diff(u44,'theta1'); u442=diff(u44,'theta2'); u443=diff(u44,'theta3'); u444=diff(u44,'theta4');
%
u511=diff(u51,'theta1'); u512=diff(u51,'theta2'); u513=diff(u51,'theta3'); u514=diff(u51,'theta4'); u515=diff(u51,'theta5');
u521=diff(u52,'theta1'); u522=diff(u52,'theta2'); u523=diff(u52,'theta3'); u524=diff(u52,'theta4'); u525=diff(u52,'theta5');
u531=diff(u53,'theta1'); u532=diff(u53,'theta2'); u533=diff(u53,'theta3'); u534=diff(u53,'theta4'); u535=diff(u53,'theta5');
u541=diff(u54,'theta1'); u542=diff(u54,'theta2'); u543=diff(u54,'theta3'); u544=diff(u54,'theta4'); u545=diff(u54,'theta5');
u551=diff(u54,'theta1'); u552=diff(u55,'theta2'); u553=diff(u55,'theta3'); u554=diff(u55,'theta4'); u555=diff(u55,'theta5');
%
u611=diff(u61,'theta1'); u612=diff(u61,'theta2'); u613=diff(u61,'theta3'); u614=diff(u61,'theta4'); u615=diff(u61,'theta5'); u616=diff(u61,'theta6');
u621=diff(u62,'theta1'); u622=diff(u62,'theta2'); u623=diff(u62,'theta3'); u624=diff(u62,'theta4'); u625=diff(u62,'theta5'); u626=diff(u62,'theta6');
u631=diff(u63,'theta1'); u632=diff(u63,'theta2'); u633=diff(u63,'theta3'); u634=diff(u63,'theta4'); u635=diff(u63,'theta5'); u636=diff(u63,'theta6');
u641=diff(u64,'theta1'); u642=diff(u64,'theta2'); u643=diff(u64,'theta3'); u644=diff(u64,'theta4'); u645=diff(u64,'theta5'); u646=diff(u64,'theta6');
u651=diff(u64,'theta1'); u652=diff(u65,'theta2'); u653=diff(u65,'theta3'); u654=diff(u65,'theta4'); u655=diff(u65,'theta5'); u656=diff(u65,'theta6');
u661=diff(u64,'theta1'); u662=diff(u65,'theta2'); u663=diff(u65,'theta3'); u664=diff(u65,'theta4'); u665=diff(u65,'theta5'); u666=diff(u65,'theta6');
%%%求Di
D1=-m1*gg*u11*r1-m2*gg*u21*r2-m3*gg*u31*r3-m4*gg*u41*r4-m5*gg*u51*r5-m6*gg*u61*r6;
D2=-m2*gg*u22*r2-m3*gg*u32*r3-m4*gg*u42*r4-m5*gg*u52*r5-m6*gg*u62*r6;
D3=-m3*gg*u33*r3--m4*gg*u43*r4-m5*gg*u53*r5-m6*gg*u63*r6;
D4=-m4*gg*u44*r4-m5*gg*u54*r5-m6*gg*u64*r6;
D5=-m5*gg*u55*r5-m6*gg*u65*r6;
D6=-m6*gg*u66*r6;
%%%求D1j
sd11=u11*I1*u11'+u21*I2*u21'+u31*I3*u31'+u41*I4*u41'+u51*I5*u51'+u61*I6*u61';
d11=0;
for i=1:4
trace=sd11(i,i);
d11=d11+trace;
end
sd12=u22*I2*u21'+u32*I3*u31'+u42*I4*u41'+u52*I5*u51'+u62*I6*u61';
d12=0;
for i=1:4
trace=sd12(i,i);
d12=d12+trace;
end
sd13=u33*I3*u31'+u43*I4*u41'+u53*I5*u51'+u63*I6*u61';
d13=0;
for i=1:4
trace=sd13(i,i);
d13=d13+trace;
end
sd14=u44*I4*u41'+u54*I5*u51'+u64*I6*u61';
d14=0;
for i=1:4
trace=sd14(i,i);
d14=d14+trace;
end
sd15=u55*I5*u51'+u65*I6*u61';
d15=0;
for i=1:4
trace=sd15(i,i);
d15=d15+trace;
end
sd16=u66*I6*u61';
d16=0;
for i=1:4
trace=sd16(i,i);
d16=d16+trace;
end
%%%求D2j
sd21=u21*I2*u22'+u31*I3*u32'+u41*I4*u42'+u51*I5*u52'+u61*I6*u61';
d21=0;
for i=1:4
trace=sd21(i,i);
d21=d121+trace;
end
sd22=u22*I2*u22'+u32*I3*u32'+u42*I4*u42'+u52*I5*u52'+u62*I6*u62';
d22=0;
for i=1:4
trace=sd22(i,i);
d22=d22+trace;
end
sd23=u33*I3*u32'+u43*I4*u42'+u53*I5*u52'+u63*I6*u62';
d23=0;
for i=1:4
trace=sd23(i,i);
d23=d23+trace;
end
sd24=u44*I4*u42'+u54*I5*u52'+u64*I6*u62';
d24=0;
for i=1:4
trace=sd24(i,i);
d24=d24+trace;
end
sd25=u55*I5*u52'+u65*I6*u62';
d25=0;
for i=1:4
trace=sd25(i,i);
d25=d25+trace;
end
sd26=u66*I6*u62';
d16=0;
for i=1:4
trace=sd16(i,i);
d16=d16+trace;
end
%%%求D3j
sd31=u31*I3*u33'+u41*I4*u43'+u51*I5*u53'+u61*I6*u63';
d31=0;
for i=1:4
trace=sd31(i,i);
d31=d31+trace;
end
sd32=u32*I3*u33'+u42*I4*u43'+u52*I5*u53'+u62*I6*u63';
d32=0;
for i=1:4
trace=sd32(i,i);
d32=d32+trace;
end
sd33=u33*I3*u33'+u43*I4*u43'+u53*I5*u53'+u63*I6*u63';
d33=0;
for i=1:4
trace=sd33(i,i);
d33=d33+trace;
end
sd34=u44*I4*u43'+u54*I5*u53'+u64*I6*u63';
d34=0;
for i=1:4
trace=sd34(i,i);
d34=d34+trace;
end
sd35=u55*I5*u53'+u65*I6*u63';
d35=0;
for i=1:4
trace=sd35(i,i);
d35=d35+trace;
end
sd36=u66*I6*u63;
d36=0;
for i=1:4
trace=sd36(i,i);
d36=d36+trace;
end
%%%求D4j
sd41=u41*I4*u44'+u51*I5*u54'+u61*I6*u64';
d41=0;
for i=1:4
trace=sd41(i,i);
d41=d41+trace;
end
sd42=u42*I4*u44'+u52*I5*u54'+u62*I6*u64';
d42=0;
for i=1:4
trace=sd42(i,i);
d42=d42+trace;
end
sd43=u43*I4*u44'+u53*I5*u54'+u63*I6*u64';
d43=0;
for i=1:4
trace=sd43(i,i);
d43=d43+trace;
end
sd44=u44*I4*u44'+u54*I5*u54'+u64*I6*u64';
d44=0;
for i=1:4
trace=sd34(i,i);
d44=d44+trace;
end
sd45=u55*I5*u54'+u65*I6*u64';
d45=0;
for i=1:4
trace=sd45(i,i);
d45=d45+trace;
end
sd46=u66*I6*u64';
d46=0;
for i=1:4
trace=sd46(i,i);
d46=d46+trace;
end
%%%求D5j
sd51=u51*I5*u55'+u61*I6*u65';
d51=0;
for i=1:4
trace=sd51(i,i);
d51=d51+trace;
end
sd52=u52*I5*u55'+u62*I6*u65';
d62=0;
for i=1:4
trace=sd52(i,i);
d52=d52+trace;
end
sd53=u53*I5*u55'+u63*I6*u65';
d53=0;
for i=1:4
trace=sd53(i,i);
d53=d53+trace;
end
sd54=u54*I5*u55'+u64*I6*u65';
d54=0;
for i=1:4
trace=sd54(i,i);
d54=d54+trace;
end
sd55=u55*I5*u55'+u65*I6*u65';
d55=0;
for i=1:4
trace=sd55(i,i);
d55=d55+trace;
end
sd56=u66*I6*u65';
d56=0;
for i=1:4
trace=sd56(i,i);
d56=d56+trace;
end
%%%求D5j
sd61=u61*I6*u66';
d61=0;
for i=1:4
trace=sd61(i,i);
d61=d61+trace;
end
sd62=u62*I6*u66';
d62=0;
for i=1:4
trace=sd62(i,i);
d62=d62+trace;
end
sd63=u63*I6*u66';
d63=0;
for i=1:4
trace=sd63(i,i);
d63=d63+trace;
end
sd64=u64*I6*u66';
d64=0;
for i=1:4
trace=sd64(i,i);
d64=d64+trace;
end
sd65=u65*I5*u66';
d65=0;
for i=1:4
trace=sd65(i,i);
d65=d65+trace;
end
sd66=u66*I6*u66';
d66=0;
for i=1:4
trace=sd56(i,i);
d66=d66+trace;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求Dijk
2866
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
