学习笔记 – 基于空间矢量的机器人动力学：铰接体惯量法matlab程序

与运动学建模不同的是，机械臂的刚体动力学建模首先需要建立其连体坐标系，该连体系可以不按D-H原则建立，然后在机械臂连体系下表示机械臂运动学量(位置、速度、加速度)，最后根据动力学普遍方程建立多体系统模型。

6维空间矢量代数包括刚体的移动量和转动量，相对于3维向量，以6维矢量表示的运动方程可以将牛顿方程和欧拉方程结合到一起。进而在表示动力学时更加简洁和方便。且易于编程实现。

铰接体惯量法的具体流程

ABA_main.m的函数具体内容如下所示：

% % 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_ddq.m的具体内容如下：

%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
end

ABA_crossM.m的具体内容如下所示：

function 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
     ];

ABA_crossF.m的具体内容如下所示：

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

ABA_antisymmetric.m的具体内容如下所示：

%此函数属于预处理函数
%求解向量的反对称阵
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

dy_Icc.m的具体内容如下所示：

%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中空间七自由度机械臂虚拟样机模型
机械臂各关节角度曲线

机械臂正向动力学算法进行相应的改进，可以得到空间漂浮基座机械臂的动力学，与SimMechanics中建立漂浮基座空间机械臂的模型进行对比仿真分析。结果均说明基于空间矢量的铰接体算法建立的空间机械臂的动力学，计算量小，计算结果精确，计算稳定性高。其可以用于全数值仿真或者半实物仿真中进行机械臂的仿真分析以及控制算法的建模分析。

正文完
2022-05-22