(8.2)利用Newton-Euler公式求解二连杆的动力学方程：

最新推荐文章于 2024-01-08 19:20:26 发布

wllrc

最新推荐文章于 2024-01-08 19:20:26 发布

阅读量3.1k

点赞数 5

分类专栏：机械臂文章标签：几何学

本文链接：https://blog.csdn.net/rouyu308/article/details/125226150

版权

机械臂专栏收录该内容

16 篇文章 88 订阅

订阅专栏

前面的两章已经介绍了基于标准DH和修正DH的Newton-Euler动力学公式，下面通过一个二连杆模型来实际验证一下。

这里假设连杆的质量都集中在连杆的末端，即将其看成是在连杆末端的质点。

一、修正DH参数建模：

由图可知，连杆1的坐标系为x1y1z1，连杆2的坐标系为x2y2z2；

由于认为连杆的质量都集中在末端即看做了一个质点则：

另外，连杆末端是自由的没有受力，故：

连杆的底座不旋转，因此：

考虑重力则：

连杆质心在连杆坐标系下的描述为：

1、公式说明：

2、MATLAB验证代码：

%%MDH下的NewtonEuler动力学验证
clear,clc
syms q1 q2 qp1 qp2 qpp1 qpp2 a1 a2 m1 m2 g real;
%%DH参数表：alpha    a    theta    d
DH = [0  0    q1  0;
         0   a1  q2  0;
         0   a2   0   0];
%关节变量
q    =[q1;q2];
qp  =[qp1;qp2];
qpp=[qpp1;qpp2];

%基座速度：
w{1}=[0 0 0]';
dotw{1}=[0 0 0]';
dotv{1}=[0;g;0];
f{3}=[0;0;0];
n_f{3}=[0;0;0];
%质量
m=[m1;m2];

%惯性矩
Ic{1}=[0 0 0;
         0 0 0;
         0 0 0];
Ic{2}=[0 0 0;
         0 0 0;
         0 0 0];

%质心位置相对于连杆坐标系
Tcm{1}=[1,0,0,a1;
             0,1,0,0;
             0,0,1,0;
             0,0,0,1];

 Tcm{2}=[1,0,0,a2;
             0,1,0,0;
             0,0,1,0;
             0,0,0,1];

n=size(q,1)+1;  %末端坐标系只有平移没有转动
z=[0 0 1]';
         
for i=1:n-1
   T{i}=[cos(DH(i,3)),                     -sin(DH(i,3)),                     0,                   DH(i,2);
          sin(DH(i,3))*cos(DH(i,1)),    cos(DH(i,1))*cos(DH(i,3)),    -sin(DH(i,1)),    -DH(i,4)*sin(DH(i,1));
          sin(DH(i,3))*sin(DH(i,1)),    sin(DH(i,1))*cos(DH(i,3)),      cos(DH(i,1)),    DH(i,4)*cos(DH(i,1));
          0,                                   0,                                      0,                   1];
%       display(T{i});
end
T{n}=[1,  0,  0,  a2;
        0,  1,  0,  0;
        0,  0,  1,  0;
        0,  0,  0,  1];
%坐标系之间的距离:在MDH下后一个坐标系{i+1}原点在前一个坐标系{i}的表示就是（i）T（i+1）的位置坐标；
for i=1:n-1
    r{i}=T{i}(1:3,4);
    %display(r{i});
end
r{n}=T{n}(1:3,4);
% display(T{n});
%  tr2rt(T{n})
%%速度的正向递推
for i=1:n-1
    w{i+1}=(tr2rt(T{i}))'*w{i}+qp(i)*z; %连杆角速度
   %display(w{i+1});
   dotw{i+1}=(tr2rt(T{i}))'*dotw{i}+(tr2rt(T{i}))'*skew(w{i})*qp(i)*z+qpp(i)*z;  %连杆角加速度
   %display(dotw{i+1});
   dotv{i+1}=tr2rt(T{i})'*(skew(dotw{i})*r{i}+skew(w{i})*skew(w{i})*r{i}+dotv{i}); %连杆线加速度
   %display(dotv{i+1});
   dotvc{i+1}=skew(dotw{i+1})*Tcm{i}(1:3,4)+skew(w{i+1})*skew(w{i+1})*Tcm{i}(1:3,4)+dotv{i+1};
%    display(dotvc{i+1});
end

%%连杆的受力
for i=1:n-1
    F{i}=m(i)*dotvc{i+1};
    %display(F{i});
    N{i}=Ic{i}*dotw{i+1}+skew(dotw{i+1})*Ic{i}*dotw{i+1};
%     display(N{i});
end

%%连杆的力的反向递推
for i=n-1:-1:1
    f{i}=tr2rt(T{i})*f{i+1}+F{i};
    %display(f{i});
%     N{i}
%     tr2rt(T{i})*n_f{i+1}
%     skew(Tcm{i}(1:3,4))*F{i}
%     skew(r{i+1})*tr2rt(T{i+1})*f{i+1}
    n_f{i}=N{i}+tr2rt(T{i})*n_f{i+1}+skew(Tcm{i}(1:3,4))*F{i}+skew(r{i+1})*tr2rt(T{i+1})*f{i+1};
    n_f{i}=simplify(n_f{i});
    display(n_f{i});
end

二、标准DH参数建模：