动力学主要分为牛顿-欧拉法拉格朗日法

  • 牛顿-欧拉法
    向外递推速度与角速度,向内迭代计算力与力矩

  • 拉格朗日方程:
    根据能量思想,从标量(拉格朗日方程)得到动力学方程
    先计算动能与势能,再构造拉格朗日方程,最后对广义变量(关节角度)求导,得到力矩

单位的说明:

  • sw得到的
    惯性张量单位是 千克·平方毫米
    位置是 毫米
    质量是 kg
  • 如果加速度单位为 m·s-2,即 N/kg
  • 那么最后计算出的力矩的单位是 N·mm

参考:

01 准备工作:为机械臂模型中加入动力学参数

需要得到每个关节的 质量,质心位置,惯性张量
以上数据通常在SolidWorks中获得(指定材质→测量-质量属性)
(可以先在关节角处创建一个坐标系,在质量属性的测量中指定输出坐标系)
参考:https://blog.csdn.net/weixin_43455581/article/details/103579030
加入动力学参数后的机械臂模型,并保存为 mdl_Dyn_5dof.m

% mdl_Dyn_5dof.m
% 单臂动力学结构参数
d=[        0,     0,        0,        0,       0];
a=[        0,    13,   233.24,   175.64,       0];%/1000
alpha=[    0,  pi/2,        0,        0,    pi/2];

%使用offset
L(1)=Link('d',d(1),'a',a(1),'alpha',alpha(1),'modified'); 
L(2)=Link('d',d(2),'a',a(2),'alpha',alpha(2),'offset',pi/2,'modified');
L(3)=Link('d',d(3),'a',a(3),'alpha',alpha(3),'modified');
L(4)=Link('d',d(4),'a',a(4),'alpha',alpha(4),'offset',pi/2,'modified');
L(5)=Link('d',d(5),'a',a(5),'alpha',alpha(5),'modified');

du=pi/180;
ra=180/pi;
%定义关节范围
L(1).qlim =[-170, 170]*du;
L(2).qlim =[60-70, 60+70]*du;%-10,130
L(3).qlim =[-70-70,-70+70]*du;%-140,0
L(4).qlim =[-70,70]*du;
L(5).qlim =[-170, 170]*du;
bot=SerialLink(L,'name','五自由度机械臂');
%bot.tool= transl(0, 0, tool)

% 动力学参数
data=[
    %     Ixx,    Iyy,      Izz,        Ixy,        Ixz,        Iyz,         xc,         yc,        zc,       m
       47.316,  51.601,   77.113,     -0.003,     -2.549,     -0.016,     -0.598,      0.016,   -23.413,   0.076;
       62.746, 651.130,  704.486,     29.632,     -0.001,     -0.003,    104.910,    -31.512,     0.001,   0.151;
        6.264, 224.674,  228.590,    -14.345,     -0.006,          0,     69.863,      8.061,     0.015,   0.065;
        1.502,   1.800,    2.241,      0.455,          0,          0,      4.498,    -12.503,         0,   0.008;
       13.735,  14.594,   15.321,          0,      0.004,          0,      0.046,          0,    43.571,   0.036
    ];

% data(:,1:6)=data(:,1:6)./1000000;
% data(:,7:9)=data(:,7:9)./1000;

% 惯性张量
data(:,[5 6])=data(:,[6 5]);%交换Ixz和Iyz
for i=1:5
   %I = [L_xx, L_yy, L_zz, L_xy, L_yz, L_xz]
   %放入是6个数字,但存储是矩阵形式的9个数字
   bot.links(i).I=data(i,1:6); 
end

%质心
for i=1:5
   bot.links(i).r=data(i,7:9); 
end

% 质量
for i=1:5
   bot.links(i).m=data(i,10); 
end

% 对于空中机械臂,重力与坐标系方向一致,所以为正
% 这与matlab自带的重力系统相反,所以matlab自带函数为负
% 重力单位是m·s-2,也是N/kg,考虑到第二种意义,这里不改变数量值
bot.gravity=[0;0;-9.81];

02 机器人工具箱函数

2.1 正动力学

  • fdyn()
  • accel()

在这里插入图片描述

在这里插入图片描述

% torqfun = [0,30,6,0,0,0];%设定一组关节力

bot_nf=bot.nofriction();
[T,q,qd] = bot_nf.fdyn(1, torqfun)
for i=1:65,
    qdd = bot_nf.accel(q(i,:),qd(i,:),torqfun)
end

2.2 逆动力学

tau = R.rne(q, qd, qdd, grav, fext)

    在这里插入图片描述

    在这里插入图片描述

    2.3 重力载荷

    bot.gravload(q)

    在这里插入图片描述

    在这里插入图片描述

    % RoboticToolbox v10
    % 动力学:使用原有工具箱函数
    % 绘制静止状态关节2/3的重力载荷图
    mdl_Dyn_5dof
    du=pi/180;
    ra=180/pi;
    %% test:比较重力正负的影响
    bot.gravity=[0;0;9.81];
    tau1=bot.gravload([30,30,-30,30,30]*du);
    % 上述两句等价于
    % tau3=bot.rne([30,30,-30,30,30]*du,[0 0 0 0 0],[0 0 0 0 0],[0 0 9.8])
    bot.gravity=[0;0;-9.81];
    tau2=bot.gravload([30,30,-30,30,30]*du);


    %% 数据
    % gravload = p560.gravload(qn); %计算重力负荷
    bot.gravity %查看重力
    % 重力负荷随关节位型的变换
    [q2_st,q2_end]=deal(bot.links(2).qlim(1),bot.links(2).qlim(2));%关节2坐标范围
    [q3_st,q3_end]=deal(bot.links(3).qlim(1),bot.links(3).qlim(2));%关节3坐标范围
    [Q2 Q3] = meshgrid(q2_st:0.1:q2_end, q3_st:0.1:q3_end);
    % [Q2 Q3] = meshgrid(-pi:0.1:pi, -pi:0.1:pi);
    for i = 1:numcols(Q2)
        for j = 1:numcols(Q3)
            g = bot.gravload([0 Q2(i,j) Q3(i,j) 0 0]);%关节2/3设置角度,其余设置为0
            g2(i,j) = g(2);
            g3(i,j) = g(3);
        end
    end

    %% 绘图
    figure('name','肩关节重力载荷')
    % 单位deg
    Q2du=Q2*ra;Q3du=Q3*ra;
    surfl(Q2du,Q3du,g2);
    xlabel('\theta_2(deg)');ylabel('\theta_3(deg)');zlabel('关节2重力载荷');
    % 单位rad
    % surfl(Q2,Q3,g2);
    % xlabel('\theta_2(rad)');ylabel('\theta_3(rad)');zlabel('关节2重力载荷');

    figure('name','肘关节重力载荷')
    surfl(Q2du,Q3du,g3)
    xlabel('\theta_2(deg)');ylabel('\theta_3(deg)');zlabel('关节3重力载荷');
    % surfl(Q2,Q3,g3)
    % xlabel('\theta_2(rad)');ylabel('\theta_3(rad)');zlabel('关节3重力载荷');

    2.4 惯性矩阵&科氏矩阵

    在这里插入图片描述

    在这里插入图片描述

    03 自己写的 拉格朗日方程 的函数

    可参考论文

    • 《仿生四足-轮复合移动机构设计与多运动模式步态规划研究》
    • 《六自由度工业机械臂动力学模型简化分析》

    在这里插入图片描述

    其中

    在这里插入图片描述

    在这里插入图片描述

    • 拉格朗日方程求解力矩的函数为:

    % 逆动力学求解函数
    % 输入 机械臂名称,位置、速度、加速度矩阵
    % 输出关节扭矩
    % 是MDH_Dy.m的改进版,使用offset
    % 改进体现在直接使用机器人的某些参数
    function [ t ] = MDH_Dyn( robot,Q,DQ,DDQ )
        global T_cell;
        %% 关节角度
    %     syms q1 q2 q3 q4 q5;
    %     syms dq1 dq2 dq3 dq4 dq5;
    %     syms ddq1 ddq2 ddq3 ddq4 ddq5;
        [q1,q2,q3,q4,q5]=deal(Q(1),Q(2),Q(3),Q(4),Q(5));
        [dq1,dq2,dq3,dq4,dq5]=deal(DQ(1),DQ(2),DQ(3),DQ(4),DQ(5));
        [ddq1,ddq2,ddq3,ddq4,ddq5]=deal(DDQ(1),DDQ(2),DDQ(3),DDQ(4),DDQ(5));
        %% 计算伪惯量矩阵
        J_cell=cell(5,1);
        %Ixx,Iyy,Izz,  Ixy,Ixz,Iyz,  xc,yc,zc,m
        %长度单位mm,惯性张量kg*mm,
        data=[
        %     Ixx,    Iyy,      Izz,        Ixy,        Ixz,        Iyz,         xc,         yc,        zc,       m
           47.316,  51.601,   77.113,     -0.003,     -2.549,     -0.016,     -0.598,      0.016,   -23.413,   0.076;
           62.746, 651.130,  704.486,     29.632,     -0.001,     -0.003,    104.910,    -31.512,     0.001,   0.151;
            6.264, 224.674,  228.590,    -14.345,     -0.006,          0,     69.863,      8.061,     0.015,   0.065;
            1.502,   1.800,    2.241,      0.455,          0,          0,      4.498,    -12.503,         0,   0.008;
           13.735,  14.594,   15.321,          0,      0.004,          0,      0.046,          0,    43.571,   0.036
        ];

    %     data=[
    %         22.134, 30.762, 24.755, -2.241, -0.00, 0, -2.546, -21.352, 0.302, -2.948;
    %         85.387, 822.001, 893.708, 48.758, 0, 0, 102.348, -34.530, 0, 0.223;
    %         5.440, 281.010, 283.608,  -17.983, 0, 0, 68.088, 7.699, 0, 0.084;
    %         1.541, 1.748, 2.151, 0.404, 0, 0, 3.601, -12.855, 0, 0.009;
    %         8.962, 9.856, 8.307, 0, 0, 0, 0, 0, -2.222, 0.025
    %         ];
        for i=1:5
            Ixx=data(i,1);Iyy=data(i,2);Izz=data(i,3);
            Ixy=data(i,4);Ixz=data(i,5);Iyz=data(i,6);
            xc=data(i,7);yc=data(i,8);zc=data(i,9);
            m=data(i,10);

            J=[(-Ixx+Iyy+Izz)/2,Ixy,Ixz,m*xc;
                Ixy,(Ixx-Iyy+Izz)/2,Iyz,m*yc;
                Ixz,Iyz,(Ixx+Iyy-Izz)/2,m*zc;
                m*xc,m*yc,m*zc,m];

            J_cell{i}=J;   
        end
        %%
        T_cell=cell(5,1);
        q=[q1,q2,q3,q4,q5];
        for i=1:5
            T_cell{i}=robot.links(i).A(q(i)).T;
        end
        %%
        % t=zeros(5,1);
        % Dij=zeros(5,5);
        % Dijj=zeros(5,5);
        % Dijk=zeros(5,10);
        % Di=zeros(5,1);
        q=[q1;q2;q3;q4;q5];
        dq=[dq1;dq2;dq3;dq4;dq5];
        ddq=[ddq1;ddq2;ddq3;ddq4;ddq5];
        dqdq=[dq1*dq2;dq1*dq3;dq1*dq4;dq1*dq5;
            dq2*dq3;dq2*dq4;dq2*dq5;
            dq3*dq4;dq3*dq5;
            dq4*dq5];
        %% Dij
        %行
        for i=1:5
            %列
            for j=1:5
                p=max(i,j);
                %累加
                D=0;
                for pp=p:5          
                    Upj=Uij(pp,j);
                    Upi=Uij(pp,i);
                    D=D+trace(Upj*J_cell{pp}*Upi.');           
                end
                Dij(i,j)=D;
            end
        end
        %% Dijj
        for i=1:5
            for j=1:5
                p=max(i,j);
                %累加
                D=0;
                for pp=p:5          
                    Upjj=Uijk(pp,j,j);
                    Upi=Uij(pp,i);
                    D=D+trace(Upjj*J_cell{pp}*Upi.');           
                end
                Dijj(i,j)=D;
            end
        end
        %% Dijk
        %标记标号j和k,for循环记录不太方便,所以直接写下来
        dijk_j=[1,1,1,1,2,2,2,3,3,4];
        dijk_k=[2,3,4,5,3,4,5,4,5,5];
        %行
        for i=1:5
            %列循环
            for s=1:10
                %列内标号循坏
                j=dijk_j(1,s);
                k=dijk_k(1,s);
                %p=max(i,j,k)
                p=max(i,j);
                p=max(p,k);
                %累加
                D=0;
                for pp=p:5
                  Upjk=Uijk(pp,j,k);
                  Upi=Uij(pp,i);
                  D=D+trace(Upjk*J_cell{pp}*Upi.');  
                end
                Dijk(i,s)=D;
            end
        end
        %% Di
        for i=1:5
            D=0;
            %累加
            for p=i:5
               m_p=data(p,10);
               %位置和加速度都是齐次
               gT=[0,0,9.81,0];%空中机械臂重力与坐标系方向一致,所以为正
               Upi=Uij(p,i);
               %位置是在p坐标系下 r是1×4的列矩阵
               r_cp=[data(p,7);data(p,8);data(p,9);1];
               D=D+m_p*gT*Upi*r_cp;%前两个关节力矩都为0,Up2只在后两行为0,g只在第三行不为0
            end
            Di(i,1)=-D;
        end
        %% 计算
        t= Dij*ddq + Dijj*(dq.^2) + 2*Dijk*dqdq +Di; 
        %%
        t=t';
    end

    • 其中Uij为

    % 计算拉格朗日动力学参数Uij
    % 在MDH_Dy中使用
    function [ U ] = Uij( i,j )
        global T_cell;
    %旋转矩阵对角度求导
       Q=[0, -1, 0, 0;
          1,  0, 0, 0;
          0,  0, 0, 0;
          0,  0, 0, 0];
       U=1;
       for kk=1:j
            U=U*T_cell{kk};
        end
        
        U=U*Q;
        
        for kk=j+1:i
            U=U*T_cell{kk};
        end

    end

    • 其中Uijk为

    % 计算拉格朗日动力学参数Uij
    % 在MDH_Dy中使用
    function [ U ] = Uijk(  i,j,k )
       global T_cell;
       Q=[0, -1, 0, 0;
           1,  0, 0, 0;
           0,  0, 0, 0;
           0,  0, 0, 0];
       U=1;
       for p=1:j-1
            U=U*T_cell{p};
       end
        
       U=U*Q;
       %for先判断再循坏 
       for p=j:k-1
            U=U*T_cell{p};
       end
        
       U=U*Q;
        
       for p=k:i
            U=U*T_cell{p};
       end

    end