与运动学建模不同的是，机械臂的刚体动力学建模首先需要建立其连体坐标系，该连体系可以不按D-H原则建立，然后在机械臂连体系下表示机械臂运动学量(位置、速度、加速度)，最后根据动力学普遍方程建立多体系统模型。
6维空间矢量代数包括刚体的移动量和转动量，相对于3维向量，以6维矢量表示的运动方程可以将牛顿方程和欧拉方程结合到一起。进而在表示动力学时更加简洁和方便。且易于编程实现。
% % clc;clear;close all;
q=[0 ;-90 ;0; 0 ;0; 0 ;0]*(pi/180);%第1:7代表1:7
q0=q;
dq=zeros(7,1);
h=0.01;
H=10;
i=0:h:H;
N=length(i);
bb(1:7,1)=dq;
aa(1:7,1)=zeros(7,1);
j=1;
for i=0:h:H
% tau=[3;2;1;0.5;0.3;0.2;0.1];
tau=[1*sin(i);0.5*cos(i);0.5*sin(i);0.1*cos(i);0.1*sin(i);0.05*sin(i);0.01*sin(i)]*0.1;
% tau=[0;0;0;0;0;1;0];
ddq=ABA_ddq(tau,q,dq);
cc(1:7,j)=ddq;
dq(1:7,1)=dq(1:7,1)+h*ddq(1:7,1);
q(1:7,1)=q(1:7,1)+dq(1:7,1)*h+ddq(1:7,1)*(h^2/2);
bb(j,1:7)=180/pi*dq(1:7,1);
aa(j,1:7)=(q(1:7,1)-q0)*180/pi;
j=j+1;
end
length(aa(1,:))
i=0:h:H;
figure(11);
plot(aa(:,1));
hold on
plot(a0(:,1),'r:')
figure(12);
plot(aa(:,2));
hold on
plot(a0(:,2),'r:')
figure(13);
plot(aa(:,3));
hold on
plot(a0(:,3),'r:')
figure(14);
plot(aa(:,4));
hold on
plot(a0(:,4),'r:')
figure(15);
plot(aa(:,5));
hold on
plot(a0(:,5),'r:')
figure(16);
plot(aa(:,6));
hold on
plot(a0(:,6),'r:')
figure(17);
plot(aa(:,7));
hold on
plot(a0(:,7),'r:')%ABA 2014/01/14
function ddq=ABA_ddq(tau,q,dq)
Ic3=dy_Icc();
s=[0;0;1;0;0;0];
m=[5 18 10 12 8.6 16 10];
Im3=[m(1)*eye(3,3),m(2)*eye(3,3),m(3)*eye(3,3),m(4)*eye(3,3),m(5)*eye(3,3),m(6)*eye(3,3),m(7)*eye(3,3)];
Ic6=[zeros(6,6),zeros(6,6),zeros(6,6),zeros(6,6),zeros(6,6),zeros(6,6),zeros(6,6)];%初始化
r=[[0;0;0],[0;0;150], [0;250;0], [0;0;150], [0;150;0], [0;0;135], [0;200;0]]*0.001; %坐标系原点位置
% r=[[0;0;0],[0;0;430], [0;0;430], [2080;0;297], [2080;0;0], [-430;0;0], [0;430;0]]*0.001; %坐标系原点位置
rc=[[0;0;75],[0;125;0], [0;0;75], [0;75;0], [0;0;75],[0;100;0], [0;0;50]]*0.001;% 质心位置
for i=1:7 %连体坐标系下的转动惯量
v0=rc(1:3,i);
rc_anti(1:3,3*i-2:3*i)=ABA_antisymmetric(v0);
m1=Ic3(1:3,3*i-2:3*i)-m(i)*rc_anti(1:3,3*i-2:3*i)*rc_anti(1:3,3*i-2:3*i);
m2=m(i)*rc_anti(1:3,3*i-2:3*i);
m3=(-1)*m(i)*rc_anti(1:3,3*i-2:3*i);
m4=m(i)*eye(3,3);
% Ic6(1:6,6*i-5:6*i)=[m1,m2;m3,m4];
% m=Ic6(1:6,6*i-5:6*i)
Ic6(1:3,6*i-5:6*i-3)=m1;
Ic6(1:3,6*i-2:6*i)=m2;
Ic6(4:6,6*i-5:6*i-3)=m3;
Ic6(4:6,6*i-2:6*i)=m4;
mm=[m1,m2;m3,m4];
end
s=[0;0;1;0;0;0];
for i=1:7 % pass 1
switch i
case 1
v(1:6,1)=s*dq(i);
ci(1:6,1)=zeros(6,1);
otherwise
XX=ABA_bianhuanjuzhen(i,q(i),1);% 从i-1到i
v(1:6,i)= XX*v(1:6,i-1)+s*dq(i);
crossM_v=ABA_crossM( v(1:6,i));
ci(1:6,i)=crossM_v*(s*dq(i));
end
crossF_v=ABA_crossF( v(1:6,i));
robotlink=Ic6(1:6,6*i-5:6*i);
p(1:6,i)=crossF_v*robotlink* v(1:6,i);
end
Ic6_A=Ic6;
for i=7:-1:1
i;
H(1:6,i)=Ic6_A(1:6,6*i-5:6*i)*s;
D(i)=s'*H(1:6,i);
uu(i)=tau(i)-s'*p(1:6,i);
if (i-1)~=0
Ima=Ic6_A(1:6,6*i-5:6*i)-(H(1:6,i)*H(1:6,i)'/D(i));
pma=p(1:6,i)+Ima*ci(1:6,i)+(H(1:6,i)*uu(i)/D(i));
XX=ABA_bianhuanjuzhen(i,q(i),1);% 从i-1到i
% % XT=ABA_bianhuanjuzhen(i,q(i),0);
Ic6_A(1:6,6*(i-1)-5:6*(i-1))= Ic6_A(1:6,6*(i-1)-5:6*(i-1))+XX'*Ima*XX;
p(1:6,i-1)=p(1:6,i-1)+XX'*pma;
end
end
ddq=zeros(7,1);
for i=1:7
switch i
case 1
aaa(1:6,1)=ci(1:6,1);
otherwise
X=ABA_bianhuanjuzhen(i,q(i),1);% 从i-1到i
aaa(1:6,i)=X*a(1:6,i-1)+ci(1:6,i);
end
ddq(i)=(uu(i)-H(1:6,i)'*aaa(1:6,i))/D(i);
a(1:6,i)=aaa(1:6,i)+s*ddq(i);
end
endfunction vcross = ABA_crossM( v )
% crossM MM6 cross-product tensor from M6 vector.
% crossM(v) calculates the MM6 cross-product tensor of motion vector v
% such that crossM(v) * m = v X m (cross-product of v and m) where m is any
% motion vector or any matrix or tensor mapping to M6.
vcross = [ 0 -v(3) v(2) 0 0 0 ;
v(3) 0 -v(1) 0 0 0 ;
-v(2) v(1) 0 0 0 0 ;
0 -v(6) v(5) 0 -v(3) v(2) ;
v(6) 0 -v(4) v(3) 0 -v(1) ;
-v(5) v(4) 0 -v(2) v(1) 0
];function vcross = ABA_crossF( v )
% crossF FF6 cross-product tensor from M6 vector.
% crossF(v) calculates the FF6 cross-product tensor of motion vector v
% such that crossF(v) * f = v X f (cross-product of v and f) where f is any
% force vector or any matrix or tensor mapping to F6.
vcross = -ABA_crossM(v)';ABA_bianhuanjuzhen.m的具体内容如下所示：
%预处理函数
%变换矩阵，使用函数
%此处需包含从i到i-1的变换，也包含i-1到i的变换
%i为当前需要变换的坐标，j=0为从i至i-1的变换，j=1为从i-1至i的变换，theta为对应的关节角度
function r=ABA_bianhuanjuzhen(i,theta,j)
c=cos(theta) ;s=sin(theta);
r=[[0;0;0],[0;0;150], [0;250;0], [0;0;150], [0;150;0], [0;0;135], [0;200;0]]*0.001; %坐标系原点位置
rot=zeros(6,6);
switch i
case 1
% m=[c -s 0; s c 0; 0 0 1 ]';%加了转置之后从i-1到i系
m=[c -s 0; s c 0;0 0 1]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 2
m=[c -s 0; 0 0 1 ; -s -c 0 ]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 3
m=[c -s 0; 0 0 1; -s -c 0]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 4
m=[c -s 0; 0 0 -1 ; s c 0 ]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 5
m=[c -s 0; 0 0 1; -s -c 0]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 6
m=[c -s 0; 0 0 -1 ; s c 0 ]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
case 7
m=[c -s 0; 0 0 1; -s -c 0]';
r_antis=ABA_antisymmetric(r(1:3,i));
rot1=[m,zeros(3,3);zeros(3,3),m];
rot2=[m',zeros(3,3);zeros(3,3),m'];
X1=rot1*[eye(3,3),zeros(3,3);(-1)*r_antis,eye(3,3)];
X2=[eye(3,3),r_antis;zeros(3,3),eye(3,3)]*rot2;
end
if j==1 %j=1为从i-1至i的变换
r=X1;
else if j==0 %j=0为从i至i-1的变换 *
r=X2;
end
end%此函数属于预处理函数
%求解向量的反对称阵
function antis=ABA_antisymmetric(v)
n=length(v);
if n==3
antis=[0 -v(3) v(2);
v(3) 0 -v(1);
-v(2) v(1) 0];
else
error('Wrong number of input arguments');
end
end%inertia
% ABA 这里的转动惯量和ＣＲＢＡ不一样
function Ic=dy_Icc()
Ic1=[0.15 0 0;
0 0.15 0;
0 0 0.08];
Ic2=[0.5 0 0;
0 0.2 0;
0 0 0.5];
Ic3=[0.35 0 0;
0 0.35 0;
0 0 0.15];
Ic4=[0.4 0 0;
0 0.15 0;
0 0 0.4];
Ic5=[0.38 0 0;
0 0.38 0;
0 0 0.16];
Ic6=[0.4 0 0;
0 0.16 0;
0 0 0.4];
Ic7=[0.23 0 0;
0 0.23 0;
0 0 0.17];
Ic8=[0 0 0 ;0 0 0 ;0 0 0];
Ic=[Ic1,Ic2,Ic3,Ic4,Ic5,Ic6,Ic7,Ic8];%元胞数组,相对于的转动惯量
end机械臂正向动力学算法进行相应的改进，可以得到空间漂浮基座机械臂的动力学，与SimMechanics中建立漂浮基座空间机械臂的模型进行对比仿真分析。结果均说明基于空间矢量的铰接体算法建立的空间机械臂的动力学，计算量小，计算结果精确，计算稳定性高。其可以用于全数值仿真或者半实物仿真中进行机械臂的仿真分析以及控制算法的建模分析。
正文完