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

20 篇文章 18 订阅

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

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

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

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

 关节1 关节2 关节3 关节4 关节5 关节6 Ixx 0 -2.782 -0.6741 0.4964 0.0893 -0.2332 Ixy 0 0. 086 -0.3658 -0.3109 -0.0252 0. 0203 Ixz 0 -0.8413 0.0373 0.1658 -0.0699 -0.0601 Iyy 0 0 0 0 0 0 Iyz 3.7033 -0.1364 0.0377 0.2139 0.0325 0.0477 Izz 0 2.4741 1.0102 -0.1213 0. 151 0.0763 lx 0 3. 5349 1.8417 -0.0475 -0.0202 0.0006 ly 0 0. 0939 0.0746 0.1624 -0.0844 0.03 lz 0 0 0 0 0 0 m 0 10 10 10 10 10

 关节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')

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]


5.MATLAB计算结果

11-10 5466
10-10
06-06
03-15 8760
03-29 6523

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。