机器人动力学
复习资料-故障模式
2023年3月17日发(作者：复文)机器⼈动⼒学⽅程（⼀）：⽜顿-欧拉法
1、⽜顿-欧拉法
⽜顿欧拉法分两步，⾸先向外迭代，计算出各个杆的⾓速度，⾓加速度，质⼼线加速度，进⽽计算出每个连杆的合外⼒（矩）；再向内迭代，计算
出每个连杆的内⼒，进⽽得到关节⼒矩。迭代过程如下：
⽜顿-欧拉迭代法
2、Matlab代码
由于⽜顿-欧拉法迭代的特性，使其对于编程来说⽐较⽅便实现。基于算法，笔者编写了Matlab函数NewtonEulerDynamics⽤于求解各个关节⼒
矩的解析表达式：
functiontorque_list=NewtonEulerDynamics(dh_list,mass_list,mass_center_list,inertia_tensor_list,f_external)
%输⼊参数：
%dh_list：机器⼈DH参数
%mass_list:连杆的质量，单位kg
%mass_center_list：连杆质⼼在连杆坐标系下的位置，单位：m
%inertia_tensor_list：连杆关于质⼼坐标系的惯性张量，质⼼坐标系与连杆坐标系⽅位⼀致
%f_external：施加在末端连杆的外⼒和外⼒矩
%输出参数：
%torque_list：⽤q,dq,ddq（关节位置、速度、加速度）表达的关节⼒矩
[rows,columns]=size(dh_list);
number_of_links=rows-1;
ifcolumns~=4
error('wrongDHparameters!')
end
T=sym([]);
R=sym([]);
a=sym([]);
d=sym([]);
alpha=sym([]);
theta=sym([]);
theta=sym([]);
fori=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
fori=1:rows
dh=dh_list(i,:);
alpha(i)=dh(1);
a(i)=dh(2);
d(i)=dh(3);
theta(i)=dh(4);
ifi==rows
q(i)=0;
end
T(:,:,i)=[cos(q(i)),-sin(q(i)),0,a(i);
sin(q(i))*cos(alpha(i)),cos(q(i))*cos(alpha(i)),-sin(alpha(i)),-sin(alpha(i))*d(i);
sin(q(i))*sin(alpha(i)),cos(q(i))*sin(alpha(i)),cos(alpha(i)),cos(alpha(i))*d(i);
0,0,0,1];
T=T(:,:,i);
%提取旋转矩阵并求逆
R(:,:,i)=simplify(inv(T(1:3,1:3)));
P(:,:,i)=T(1:3,4:4);
end
%关节轴
z=[0,0,1]';
%重⼒加速度符号
symsgreal
%外推--->
fori=0:number_of_links-1
ifi==0
wi=[0,0,0]';
dwi=[0,0,0]';
dvi=[0,g,0]';
else
wi=w(:,i);
dwi=dw(:,i);
dvi=dv(:,i);
end
w(:,:,i+1)=R(:,:,i+1)*wi+dq(i+1)*z;
dw(:,:,i+1)=R(:,:,i+1)*dwi+cross(R(:,:,i+1)*wi,dq(i+1)*z)+ddq(i+1)*z;
dv(:,:,i+1)=R(:,:,i+1)*(cross(dwi,P(:,:,i+1))+cross(wi,cross(wi,P(:,:,i+1)))+dvi);
dvc(:,:,i+1)=cross(dw(:,:,i+1),mass_center_list(i+1,:)')...
+cross(w(:,:,i+1),cross(w(:,:,i+1),mass_center_list(i+1,:)'))...
+dv(:,:,i+1);
F(:,:,i+1)=mass_list(i+1)*dvc(:,:,i+1);
N(:,:,i+1)=inertia_tensor_list(:,:,i+1)*dw(:,:,i+1)+cross(w(:,:,i+1),inertia_tensor_list(:,:,i+1)*w(:,:,i+1));
end
f=sym([]);
n=sym([]);
%内推<---
fori=number_of_links:-1:1
ifi==number_of_links
f(:,:,i+1)=f_external(1,:)';
n(:,:,i+1)=f_external(2,:)';
end
f(:,:,i)=R(:,:,i+1)f(:,:,i+1)+F(:,:,i);
f(:,:,i)=simplify(f(:,:,i));
n(:,:,i)=N(:,:,i)+R(:,:,i+1)n(:,:,i+1)+cross(mass_center_list(i,:)',F(:,:,i))...
+cross(P(:,:,i+1),R(:,:,i+1)f(:,:,i+1));
n(:,:,i)=simplify(n(:,:,i));
torque_list(i)=dot(n(:,:,i),z);
end
torque_list=torque_list';
以上代码适⽤于修改的DH模型，如果是标准的DH模型需要做⼀点修改。代码运⾏后将给出各个关节扭矩的封闭解。
下⾯的代码给出⼀个⽤例：
clc;
clear;
symsq1q2m1m2L1L2real
dh_params=[0,0,0,q1;
0,L1,0,q2;
0,L2,0,0];
mass_center=[L1,0,0;
L2,0,0;];
mass=[m1,m2];
inertia_1=[0,0,0;
0,0,0;
0,0,0];
inertia_2=[0,0,0;
0,0,0;
0,0,0];
inertia_tensor(:,:,1)=inertia_1;
inertia_tensor(:,:,2)=inertia_2;
f_ext=[0,0,0;
0,0,0];
torque=NewtonEulerDynamics(dh_params,mass,mass_center,inertia_tensor,f_ext);
注意dh_params第三⾏给出的是末端执⾏器相对于最后⼀根连杆的坐标表达。上述代码求解的问题是书中的例题6.7，求解平⾯⼆连杆的动⼒学
⽅程：
平⾯⼆连杆
运⾏后输出：
torque=
L1^2*ddq1*m1+L1^2*ddq1*m2+L2^2*ddq1*m2+L2^2*ddq2*m2+L2*g*m2*cos(q1+q2)+L1*g*m1*cos(q1)+L1*g*m2*cos(q1)-L1*L2*dq2^2*m2*sin(q2)+2*L1*L2*d
L2*m2*(cos(q2)*(L1*ddq1+g*cos(q1))+sin(q2)*(L1*dq1^2-g*sin(q1))+L2*(ddq1+ddq2))
书中的求解结果：
关节⼒矩
对⽐可以表明两者⼀致，证明计算是正确的。当然，编写的代码本⾝对于关节数⽬是没有限制的，可⽤于更多关节机器⼈的动⼒学⽅程计算。