1、拉格朗日法
拉格朗日法是一种基于能量的动力学方法，从拉格朗日函数L（系统动能和势能的差值）出发来建立机器人动力学方程：
Lagrange 方程
应用于机器人动力学模型推导（详细过程可参考霍伟编写的《机器人动力学与控制》），最终可得出如下的形式：
式中各项H,C,G的求解算法如下，本文将该算法用Matlab实现。
各项的求解算法
2、Matlab代码
function [H,C,G] = LagrangianDynamics(dh_list, mass_list, mass_center_list, inertia_tensor_list)
[rows, columns] = size(dh_list);
number_of_links = rows;
if columns ~= 4
error('wrong DH parameters!')
end
for i = 1:rows
% 定义关节位置，速度，加速度符号
eval(['syms ','q',num2str(i),' real;']);
eval(['syms ','dq',num2str(i),' real;']);
eval(['syms ','ddq',num2str(i),' real;']);
eval(['q(i)=','q',num2str(i),';']);
eval(['dq(i)=','dq',num2str(i),';']);
eval(['ddq(i)=','ddq',num2str(i),';']);
end
A = sym([]);
for i = 1:number_of_links
dh = dh_list(i,:);
alpha(i) = dh(1);
a(i) = dh(2);
d(i) = dh(3);
q(i) = dh(4);
A(:,:,i) = [cos(q(i)), -sin(q(i))*cos(alpha(i)), sin(alpha(i))*sin(q(i)), a(i)*cos(q(i));
sin(q(i)), cos(q(i))*cos(alpha(i)), -sin(alpha(i))*cos(q(i)), a(i)*sin(q(i));
0, sin(alpha(i)), cos(alpha(i)), d(i);
0, 0, 0, 1];
end
A = simplify(A);
% 计算每个连杆坐标系在{0}系下的表达
A0 = sym([]);
for i = 1:number_of_links
A0(:,:,i) = eye(4,4);
for j = 1:i
A0(:,:,i) = A0(:,:,i)*A(:,:,j);
end
end
A0 = simplify(A0);
J = sym([]);
for i = 1:number_of_links
J(:,:,i) = JMatrix(mass_list(i),mass_center_list(1,:),inertia_tensor_list(:,:,i));
end
% 计算H(q),由H(q)对称性，只需计算上三角部分
syms tr
for i = 1:number_of_links
for j = i:number_of_links
tr = 0;
for k = j:number_of_links
tr = tr + trace(eval(['diff(A0(:,:,k),q',num2str(i),')'])*J(:,:,k)*...
eval(['diff(transpose(A0(:,:,k)),q',num2str(j),')']));
end
H(i,j) = simplify(tr);
H(j,i) = H(i,j);
end
end
% 计算C(q)
for i = 1:number_of_links
for j = 1:number_of_links
c = 0;
for k = 1:number_of_links
c = c + 1/2*(eval(['diff(H(i,j),q',num2str(k),')'])...
+ eval(['diff(H(i,k),q',num2str(j),')'])...
- eval(['diff(H(j,k),q',num2str(i),')']))*eval(['dq',num2str(k)]);
end
C(i,j) = simplify(c);
end
end
syms gc
g = [0,0,-gc,0]';
% 计算G(q)
for i = 1:number_of_links
gi = 0;
for j = 1:number_of_links
gi = gi - mass_list(j)*g'...
*eval(['diff(A0(:,:,j),q',num2str(i),')'])...
*[mass_center_list(j,:),1]';
end
G(i) = simplify(gi);
end
G = G';
end
代码中的齐次矩阵A(:,:,i)计算对应的是标准DH模型，如果杆件坐标系是用修改的DH模型建立，则只需修改A(:,:,i)的计算即可。下面的用例可测试编写的代码：
% 拉格朗日动力学方程求解测试
clc;
clear;
syms q1 q2 q3 m1 m2 m3 d2 real
syms Ix1 Iy1 Iz1 Ixy1 Iyz1 Ixz1 real
syms Ix2 Iy2 Iz2 Ixy2 Iyz2 Ixz2 real
syms Ix3 Iy3 Iz3 Ixy3 Iyz3 Ixz3 real
syms xc1 yc1 zc1 xc2 yc2 zc2 xc3 yc3 zc3 real
dh_params = [-pi/2, 0, 0, q1;
pi/2, 0, d2, q2;
0, 0, 0, q3];
mass_center = [xc1, yc1, zc1;
xc2, yc2, zc2;
xc3, yc3, zc3];
mass = [m1,m2,m3];
inertia_1 = [Ix1, Ixy1, Ixz1;
Ixy1, Iy1, Iyz1;
Ixz1, Iyz1, Iz1];
inertia_2 = [Ix2, Ixy2, Ixz2;
Ixy2, Iy2, Iyz2;
Ixz2, Iyz2, Iz2];
inertia_3 = [Ix3, Ixy3, Ixz3;
Ixy3, Iy3, Iyz3;
Ixz3, Iyz3, Iz3];
inertia_tensor(:,:,1) = inertia_1;
inertia_tensor(:,:,2) = inertia_2;
inertia_tensor(:,:,3) = inertia_3;
[h,c,g] = LagrangianDynamics(dh_params, mass, mass_center, inertia_tensor)
上面的程序基于编写的LagrangianDynamics函数求解书中的算例2-1：
算例
代码运行后输出动力学方程中的各个系数矩阵：
h =
[ Ix2 + Iy1 + Iy3 + d2^2*m2 + d2^2*m3 - Ix2*cos(q2)^2 + Ix3*cos(q3)^2 - Iy3*cos(q2)^2 - Iy3*cos(q3)^2 + Iz2*cos(q2)^2 + Iz3*cos(q2)^2 + Ixy3*sin(2*q3) + Ixz2*sin(2*q2) - Ix3*cos(q2)^2*cos(q3)^2 + Iy3*cos(q2)^2*cos(q3)^2 + 2*d2*m2*yc1 + 2*Ixz3*cos(q2)*cos(q3)*sin(q2) - 2*Iyz3*cos(q2)*sin(q2)*sin(q3) + 2*d2*m3*yc1*cos(q3) + 2*d2*m3*xc1*sin(q3) - 2*Ixy3*cos(q2)^2*cos(q3)*sin(q3), Ixy2*sin(q2) - Iyz2*cos(q2) - Ixy3*sin(q2) - Iyz3*cos(q2)*cos(q3) - Ixz3*cos(q2)*sin(q3) + 2*Ixy3*cos(q3)^2*sin(q2) - Ix3*cos(q3)*sin(q2)*sin(q3) + Iy3*cos(q3)*sin(q2)*sin(q3) - d2*m2*zc1*cos(q2) - d2*m3*zc1*cos(q2) + d2*m2*xc1*sin(q2) + d2*m3*xc1*cos(q3)*sin(q2) - d2*m3*yc1*sin(q2)*sin(q3), Iz3*cos(q2) + Ixz3*cos(q3)*sin(q2) - Iyz3*sin(q2)*sin(q3) + d2*m3*yc1*cos(q2)*cos(q3) + d2*m3*xc1*cos(q2)*sin(q3)]
[ Ixy2*sin(q2) - Iyz2*cos(q2) - Ixy3*sin(q2) - Iyz3*cos(q2)*cos(q3) - Ixz3*cos(q2)*sin(q3) + 2*Ixy3*cos(q3)^2*sin(q2) - Ix3*cos(q3)*sin(q2)*sin(q3) + Iy3*cos(q3)*sin(q2)*sin(q3) - d2*m2*zc1*cos(q2) - d2*m3*zc1*cos(q2) + d2*m2*xc1*sin(q2) + d2*m3*xc1*cos(q3)*sin(q2) - d2*m3*yc1*sin(q2)*sin(q3), Ix3/2 + Iy2 + Iy3/2 - (Ix3*cos(2*q3))/2 + (Iy3*cos(2*q3))/2 - Ixy3*sin(2*q3), - Iyz3*cos(q3) - Ixz3*sin(q3)]
[ Iz3*cos(q2) + Ixz3*cos(q3)*sin(q2) - Iyz3*sin(q2)*sin(q3) + d2*m3*yc1*cos(q2)*cos(q3) + d2*m3*xc1*cos(q2)*sin(q3), - Iyz3*cos(q3) - Ixz3*sin(q3), Iz3]
c =
[ dq2*(Iyz3*sin(q3) - Ixz3*cos(q3) - Ixz2 + 2*Ixz2*cos(q2)^2 + (Ix2*sin(2*q2))/2 + (Iy3*sin(2*q2))/2 - (Iz2*sin(2*q2))/2 - (Iz3*sin(2*q2))/2 + 2*Ixz3*cos(q2)^2*cos(q3) - 2*Iyz3*cos(q2)^2*sin(q3) + Ix3*cos(q2)*cos(q3)^2*sin(q2) - Iy3*cos(q2)*cos(q3)^2*sin(q2) + 2*Ixy3*cos(q2)*cos(q3)*sin(q2)*sin(q3)) - dq3*(Ixy3 - Ixy3*cos(q2)^2 - 2*Ixy3*cos(q3)^2 + (Ix3*sin(2*q3))/2 - (Iy3*sin(2*q3))/2 + 2*Ixy3*cos(q2)^2*cos(q3)^2 + Iyz3*cos(q2)*cos(q3)*sin(q2) + Ixz3*cos(q2)*sin(q2)*sin(q3) - d2*m3*xc1*cos(q3) + d2*m3*yc1*sin(q3) - Ix3*cos(q2)^2*cos(q3)*sin(q3) + Iy3*cos(q2)^2*cos(q3)*sin(q3)), dq1*(Iyz3*sin(q3) - Ixz3*cos(q3) - Ixz2 + 2*Ixz2*cos(q2)^2 + (Ix2*sin(2*q2))/2 + (Iy3*sin(2*q2))/2 - (Iz2*sin(2*q2))/2 - (Iz3*sin(2*q2))/2 + 2*Ixz3*cos(q2)^2*cos(q3) - 2*Iyz3*cos(q2)^2*sin(q3) + Ix3*cos(q2)*cos(q3)^2*sin(q2) - Iy3*cos(q2)*cos(q3)^2*sin(q2) + 2*Ixy3*cos(q2)*cos(q3)*sin(q2)*sin(q3)) + dq2*(Ixy2*cos(q2) - Ixy3*cos(q2) + Iyz2*sin(q2) + Iyz3*cos(q3)*sin(q2) + Ixz3*sin(q2)*sin(q3) + 2*Ixy3*cos(q2)*cos(q3)^2 - Ix3*cos(q2)*cos(q3)*sin(q3) + Iy3*cos(q2)*cos(q3)*sin(q3) + d2*m2*xc1*cos(q2) + d2*m2*zc1*sin(q2) + d2*m3*zc1*sin(q2) + d2*m3*xc1*cos(q2)*cos(q3) - d2*m3*yc1*cos(q2)*sin(q3)) - (dq3*sin(q2)*(Iz3 + Ix3*(2*cos(q3)^2 - 1) - Iy3*(2*cos(q3)^2 - 1) + 4*Ixy3*cos(q3)*sin(q3) + 2*d2*m3*yc1*cos(q3) + 2*d2*m3*xc1*sin(q3)))/2, - dq3*(Iyz3*cos(q3)*sin(q2) + Ixz3*sin(q2)*sin(q3) - d2*m3*xc1*cos(q2)*cos(q3) + d2*m3*yc1*cos(q2)*sin(q3)) - dq1*(Ixy3 - Ixy3*cos(q2)^2 - 2*Ixy3*cos(q3)^2 + (Ix3*sin(2*q3))/2 - (Iy3*sin(2*q3))/2 + 2*Ixy3*cos(q2)^2*cos(q3)^2 + Iyz3*cos(q2)*cos(q3)*sin(q2) + Ixz3*cos(q2)*sin(q2)*sin(q3) - d2*m3*xc1*cos(q3) + d2*m3*yc1*sin(q3) - Ix3*cos(q2)^2*cos(q3)*sin(q3) + Iy3*cos(q2)^2*cos(q3)*sin(q3)) - (dq2*sin(q2)*(Iz3 + Ix3*(2*cos(q3)^2 - 1) - Iy3*(2*cos(q3)^2 - 1) + 4*Ixy3*cos(q3)*sin(q3) + 2*d2*m3*yc1*cos(q3) + 2*d2*m3*xc1*sin(q3)))/2]
[ dq3*((Ix3*sin(q2))/2 - (Iy3*sin(q2))/2 + (Iz3*sin(q2))/2 - Ixz3*cos(q2)*cos(q3) + Iyz3*cos(q2)*sin(q3) - Ix3*cos(q3)^2*sin(q2) + Iy3*cos(q3)^2*sin(q2) - 2*Ixy3*cos(q3)*sin(q2)*sin(q3)) - dq1*(Iyz3*sin(q3) - Ixz3*cos(q3) - Ixz2 + 2*Ixz2*cos(q2)^2 + (Ix2*sin(2*q2))/2 + (Iy3*sin(2*q2))/2 - (Iz2*sin(2*q2))/2 - (Iz3*sin(2*q2))/2 + 2*Ixz3*cos(q2)^2*cos(q3) - 2*Iyz3*cos(q2)^2*sin(q3) + Ix3*cos(q2)*cos(q3)^2*sin(q2) - Iy3*cos(q2)*cos(q3)^2*sin(q2) + 2*Ixy3*cos(q2)*cos(q3)*sin(q2)*sin(q3)), -dq3*(Ixy3*cos(2*q3) - (Ix3*sin(2*q3))/2 + (Iy3*sin(2*q3))/2), Ixy3*dq2 - 2*Ixy3*dq2*cos(q3)^2 + (Ix3*dq2*sin(2*q3))/2 - (Iy3*dq2*sin(2*q3))/2 - Ixz3*dq3*cos(q3) + (Ix3*dq1*sin(q2))/2 - (Iy3*dq1*sin(q2))/2 + Iyz3*dq3*sin(q3) + (Iz3*dq1*sin(q2))/2 - Ix3*dq1*cos(q3)^2*sin(q2) + Iy3*dq1*cos(q3)^2*sin(q2) - Ixz3*dq1*cos(q2)*cos(q3) + Iyz3*dq1*cos(q2)*sin(q3) - 2*Ixy3*dq1*cos(q3)*sin(q2)*sin(q3)]
[ dq1*(Ixy3 - Ixy3*cos(q2)^2 - 2*Ixy3*cos(q3)^2 + (Ix3*sin(2*q3))/2 - (Iy3*sin(2*q3))/2 + 2*Ixy3*cos(q2)^2*cos(q3)^2 + Iyz3*cos(q2)*cos(q3)*sin(q2) + Ixz3*cos(q2)*sin(q2)*sin(q3) - d2*m3*xc1*cos(q3) + d2*m3*yc1*sin(q3) - Ix3*cos(q2)^2*cos(q3)*sin(q3) + Iy3*cos(q2)^2*cos(q3)*sin(q3)) - dq2*((Ix3*sin(q2))/2 - (Iy3*sin(q2))/2 + (Iz3*sin(q2))/2 - Ixz3*cos(q2)*cos(q3) + Iyz3*cos(q2)*sin(q3) - Ix3*cos(q3)^2*sin(q2) + Iy3*cos(q3)^2*sin(q2) - 2*Ixy3*cos(q3)*sin(q2)*sin(q3)), dq2*(Ixy3*(2*cos(q3)^2 - 1) - Ix3*cos(q3)*sin(q3) + Iy3*cos(q3)*sin(q3)) - dq1*((Ix3*sin(q2))/2 - (Iy3*sin(q2))/2 + (Iz3*sin(q2))/2 - Ixz3*cos(q2)*cos(q3) + Iyz3*cos(q2)*sin(q3) - Ix3*cos(q3)^2*sin(q2) + Iy3*cos(q3)^2*sin(q2) - 2*Ixy3*cos(q3)*sin(q2)*sin(q3)), 0]
g =
0
-gc*(m2*xc2*cos(q2) + m2*zc2*sin(q2) + m3*zc3*sin(q2) + m3*xc3*cos(q2)*cos(q3) - m3*yc3*cos(q2)*sin(q3))
gc*m3*sin(q2)*(yc3*cos(q3) + xc3*sin(q3))
对比书中给出的结果，经过适当变形，可以看出是一致的，代入到文章开头的运动方程，即可获得该机械臂显式的动力学方程。
1
2
3
4
