matlab动力学方程,机器人动力学方程（二）：拉格朗日法

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(:,:,