【机器人学】4-3.六自由度机器人动力学-拉格朗日方程【附MATLAB代码】

上一章用了牛顿欧拉递推式的动力学方程求解了6自由度机器人的各关节动力。具体可以看我的上一篇博客。

【机器人学】4-2.六自由度机器人动力学-牛顿欧拉递推式【附MATLAB代码】

这篇文章主要介绍拉格朗日方程求解机械臂的动力学。

               几乎所有的书上，在介绍拉格朗日方程时，都会有上述公式，不过在使用MATLAB建模时却异常困难，由于物理意义不太明确，比较牛顿欧拉递推式难度指数级上升。博主也是无意中在一本书上看到了较简单的建模方式。

                还记得【机器人学】4-1.六自由度机器人动力学-开篇-基础知识  中关于操作臂动力学方程的结构。

        任何机械臂动力学方程都可以写成如上式（4.11）方程结构。如果我们可以分别求出M，V，G那么动力学方程也就迎刃而解。

假设机械臂动力学参数如下：

假设有六轴机器人的DH参数如下所示：

	关节1	关节2	关节3	关节4	关节5	关节6
	0	90	0	0	-90	90
a	0	0	425	393	0	0
d	160.7	0	0	113.3	99	93.6
	0	90	0	-90	0	0
	0	0	0	0	0	0

MATLAB仿真验证

1. 求解解析参数

%求解解析参数
function [Jee,M,C,G] = myLagrange(dh,rob,Pc,Ic,g,m,q,dq)
alpha = dh(1,:);
a = dh(2,:);
d = dh(3,:);
theta = dh(4,:);
N = size(dh,2); 
for i=1:N
   pc{i}=Pc(:,i);
end
for i=1:N
    if alpha(i)==0
        Salpha(i)=0;
        Calpha(i)=1;
    elseif abs(alpha(i))==pi/2
        Calpha(i)=0;
        if alpha(i)==pi/2
            Salpha(i)=1;
        elseif alpha(i)==-pi/2
            Salpha(i)=-1;
        end
    elseif abs(alpha(i))==pi
        Salpha(i)=0;
        Calpha(i)=-1;
    else
        Salpha(i)=sin(alpha(i));
        Calpha(i)=cos(alpha(i));
    end
end

% R
for i=1:N
    if rob(i)==0    % Rotation matrix of revolute joints
        R{i}=[cos(q(i)) -sin(q(i)) 0;
             sin(q(i))*Calpha(i) cos(q(i))*Calpha(i) -Salpha(i);
             sin(q(i))*Salpha(i) cos(q(i))*Salpha(i)  Calpha(i)];
    elseif rob(i)==1    % Rotation matrix of prismatic joints
        R{i}=[cos(theta(i)) -sin(theta(i)) 0;
             sin(theta(i))*Calpha(i) cos(theta(i))*Calpha(i) -Salpha(i);
             sin(theta(i))*Salpha(i) cos(theta(i))*Salpha(i)  Calpha(i)];
    end
end
% P
for i=1:N
    if rob(i)==0    % Position vector of revolute joints
        p{i}=[a(i);-Salpha(i)*d(i);Calpha(i)*d(i)];
    elseif rob(i)==1    % Position vector of prismatic joints
        p{i}=[a(i);-Salpha(i)*q(i);Calpha(i)*q(i)];
    end
end
for i=1:N
    pic{i}=p{i}+R{i}*pc{i};     
end

poc{1}=pic{1};
for i=2:N
    nic=pic{i};      
    for j=i-1:-1:1
        nic=p{j}+R{j}*nic;
    end
    poc{i}=nic;
end
Pee = poc{N};

% R0{i} stand for 0--> ith framework rotation transition matrix
R0{1}=R{1};
for i=2:N
    R0{i}=simplify(R0{i-1}*R{i});
end
Ree = R0{N};

% linear part of jacobians
for i=1:N
    Jv{i}=simplify(jacobian(poc{i},q));
end

% angular part of jacobians
if rob(1)==0
    Jo{1}=simplify([R0{1}(:,3),zeros(3,N-1)]);
elseif rob(1)==1
    Jo{1}=zeros(3,N);
end

for i=2:N
    if rob(i)==1
        Jo{i}=zeros(3,N);
    elseif rob(i)==0
        Jo{i}=Jo{i-1};
        Jo{i}(:,i)=R0{i}(:,3);
    end
end
Jee = [Jv{N};Jo{N}];
% M
for i = 1:size(rob,2)
    TEEA{i} = Ic(:,:,i);
end

for i=1:N
    Mass{i}=simplify(m(i)*Jv{i}.'*Jv{i}+Jo{i}.'*R0{i}*TEEA{i}*R0{i}.'*Jo{i});
end

M=0;
for i=1:N
    M=simplify(Mass{i}+M);
end

% C
for k=1:N
   for s=1:N
       c(1)=.5*((diff(M(k,s),q(1))+diff(M(k,1),q(s))-diff(M(1,s),q(k)))*dq(1));
      for i=2:N
       c(i)=.5*((diff(M(k,s),q(i))+diff(M(k,i),q(s))-diff(M(i,s),q(k)))*dq(i))+c(i-1);
      end
      C(k,s)=simplify(c(N));
   end
end
% G
P(1)=m(1)*[0,0,g]*poc{1};
for i=2:N
    P(i)=P(i-1)+m(i)*[0,0,g]*poc{i};
end
P=simplify(P(N));
for i=1:N
    G(i,:)=simplify(diff(P,q(i)));
end
end

2.输入参数生成解析表达式

%输入参数生成解析表达式的参数
clear
clc

alpha=[0,     pi/2,  0,           0,         -pi/2,      pi/2];
a=    [0,     0,     0.425,      0.393,    0,         0];
d=    [0.1607, 0,     0,           0.1133,    0.099,     0.0936];
theta=[0,     pi/2,  0,           -pi/2,     0,         0];

dh = [alpha; a; d; theta];
rob=[0,0,0,0,0,0];%0 standard for rotation, 1 standard for translation
N = size(rob);
g = sym('g');
assume(g,'real');
m = sym('m',N); 
assume(m, 'real')
Pc = sym('Pc%d%d',[3,6]);
assume(Pc,'real');
Ic = sym('Ic_%d%d_%d',[3,3,6]);
assume(Ic,'real');
for i = 1:size(Ic,3)
    Ic(2,1,i)=Ic(1,2,i);
    Ic(3,1,i)=Ic(1,3,i);
    Ic(3,2,i)=Ic(2,3,i);
end
q = sym('q',N);       % 1st group of state variables (joints' angle or position)
assume(q, 'real')
dq = sym('dq',N);     % 2nd group of state variables (joints' velocity)
assume(dq, 'real')

[J_L,M_L,C_L,G_L] = myLagrange(dh,rob,Pc,Ic,g,m,q,dq);
%将生成的参数保存至Result_L_test.mat文件中
save ('Result_L_test.mat','J_L','M_L','C_L','G_L');

3.拉格朗日方程求解动力学

%输入参数
%inertia_tensor_list 机器人的惯性矩阵
%mass_center_list 机器人质心矩阵
%thetaOffset 角度偏移
%theta  角度
%qd 角速度
%qdd 角加速度
%mass_list 关节质量
function tau_L = LagCom(inertia_tensor_list,mass_center_list,thetaOffset,theta,qd,qdd,mass_list)
rob=[0,0,0,0,0,0];
N = size(rob);
g = sym('g');
assume(g,'real');
m = sym('m',N); 
assume(m, 'real')
Pc = sym('Pc%d%d',[3,6]);
assume(Pc,'real');
Ic = sym('Ic_%d%d_%d',[3,3,6]);
assume(Ic,'real');

for i = 1:size(Ic,3)
    Ic(2,1,i)=Ic(1,2,i);
    Ic(3,1,i)=Ic(1,3,i);
    Ic(3,2,i)=Ic(2,3,i);
end
q = sym('q',N);       % 1st group of state variables (joints' angle or position)
assume(q, 'real')
dq = sym('dq',N);     % 2nd group of state variables (joints' velocity)
assume(dq, 'real')

load('Result_L_test.mat');
sym_sub = [];
num_sub = [];
for i = 1:size(Ic,3)
    sym_sub=[sym_sub,m(i)];
    num_sub=[num_sub,mass_list(i)];
    for j = 1:size(Pc,1)
        sym_sub=[sym_sub,Pc(j,i)];
        num_sub=[num_sub,mass_center_list(j,i)];
    end
    for j = 1:size(Ic,1)
        for k = 1:size(Ic,2)
            sym_sub=[sym_sub,Ic(j,k,i)];
            num_sub=[num_sub,inertia_tensor_list(j,k,i)];
        end
    end
end

G = subs(G_L,[g,sym_sub],[9.8,num_sub]);
M = subs(M_L,sym_sub,num_sub);
C = subs(C_L,sym_sub,num_sub);
J=subs(J_L,Pc,mass_center_list);
save('MCG.mat','M','C','G','J');
tau_L = (double(subs(M,q,(theta'+thetaOffset')'))*qdd'  + double(subs(C,[q,dq],[(theta'+thetaOffset')',qd]))*qd' + double(subs(G,q,(theta+thetaOffset))))';
end

4.机器人工具箱验证

clear;
clc;
m=[0 10 10 10 10 10];
% DH parameters  th     d    a    alpha  sigma
L1=Link([  0       0.1607       0        0       ],'modified');
L2=Link([  0       0        0       pi/2    ],'modified');L2.offset = pi/2;
L3=Link([  0       0        0.425       0       ],'modified');
L4=Link([  0       0.1133       0.393       0    ],'modified');L4.offset = -pi/2;
L5=Link([  0       0.099        0       -pi/2    ],'modified');
L6=Link([  0       0.0936        0        pi/2    ],'modified');
thetaOffset=[0,     pi/2,  0,           -pi/2,     0,         0];
L1.Jm = 0; L2.Jm = 0; L3.Jm = 0; L4.Jm = 0; L5.Jm = 0; L6.Jm = 0;

L1.m = m(1); L2.m = m(2); L3.m = m(3);
L4.m = m(4); L5.m = m(5); L6.m = m(6);

% L1.r = 
L1.I = [0 0 0; 0 0 0; 0 0 3.7033];
L2.I = [-2.782 0.086 -0.8413; 0.086 0 -0.1364; -0.8413 -0.1364 2.4741];
L3.I = [-0.6741 -0.3658 0.0373; -0.3658 0 0.0377; 0.0373 0.0377 1.0102];
L4.I = [0.4964 -0.3109 0.1658; -0.3109 0 0.2139; 0.1658 0.2139 -0.1213];
L5.I = [0.0893 -0.0252 -0.0699; -0.0252 0 0.0325; -0.0699 0.0325 0.151];
L6.I = [-0.2332 0.0203 -0.0601; 0.0203 0 0.0477; -0.0601 0.0477 0.0763];
% 质心位置
p10 = [0;0;0];p21 = [3.5349;0.0939;0]; p32 = [1.8417; 0.0746; 0];
p43 = [-0.0475;0.1624;0]; p54 = [-0.0202;-0.0844;0];  
p65 = [0.0006; 0.03;0];
L1.r = p10; L2.r = p21; L3.r = p32;
L4.r = p43; L5.r = p54; L6.r = p65;

robot = SerialLink([L1, L2, L3, L4, L5, L6]); 
robot.name='My_Robot';

inertia_tensor_list = zeros(3,3,6);
inertia_tensor_list(:,:,1)  = L1.I;
inertia_tensor_list(:,:,2)  = L2.I;
inertia_tensor_list(:,:,3)  = L3.I;
inertia_tensor_list(:,:,4)  = L4.I;
inertia_tensor_list(:,:,5)  = L5.I;
inertia_tensor_list(:,:,6)  = L6.I;
mass_center_list = [p10.';p21.';p32.';p43.';p54.';p65.']';

theta = [pi, 0, pi, 0, 0, pi]*pi/180;
qd = [1 1 pi 1 1 1]; qdd = [1 1 1 pi 1 1];
tau_L = LagCom(inertia_tensor_list,mass_center_list,thetaOffset,theta,qd,qdd,m');
tau_N = myNewtonEuler(theta*180/pi, qd', qdd',m);
tau = robot.rne(theta, qd, qdd, [0 0 9.8]);
[tau_L;tau_N;tau]

注意：使用机器人工具箱验证之前，先调用第二个matlab代码生成拉格朗日方程的解析形式，时间比较长请耐心等待，博主跑了大概30分钟。

  5.MATLAB计算结果

当theta = [pi, 0, pi, 0, 0, pi]*pi/180; qd = [1 1 pi 1 1 1]; qdd = [1 1 1 pi 1 1];时结果如下，第一行为拉格朗日方程计算的结果，第二行为牛顿欧拉递推式的结果，第三行为机器人工具箱的结果。

当theta = [pi, 0, pi/2, 0, pi/2, pi]*pi/180; qd = [1 1 pi 1 pi/2 1]; qdd = [1 pi/2 1 pi 1 1];时结果如下。

下一章：5-1：六自由度机器人轨迹规划