matlab仿真建立机器臂模型并得到其D-H参数表

最近老师要求我们在matlab中构造下图机器人的模型

其初步构型长这样

想要实现图示关节的构型，必须在正确的坐标位置绘制出圆柱体（为圆柱指定位置和z轴方向），那么就必须先获得机器臂各个关节的位姿（位置和姿态），引入D-H参数，利用D-H参数表构建各个连杆的齐次变换矩阵，便能够表示机器臂各个关节的位姿，画上圆柱，实现此机器臂的构型。

D-H法是建立机器人各个关节间的齐次变换矩阵的通用方法，规则简单、通用性强。

那么问题来了，如何获得此机器臂的D-H参数信息呢？

D-H法一般有两步走

一、先确定每个关节处坐标系的z轴

二、再确定每个关节处坐标系的x轴

关节x轴的确定由相邻关节z轴和自身的z轴决定，此时分为三种情况

1. 两z轴异面
   
2. 两z轴相交
   
3. 两z轴平行（此时公垂线有无数条，可以有多种选择）
   
   

 有了以上知识，便能直接在机器臂的构型中画出每个关节的坐标

根据坐标系绘制D-H参数表

      将机器人的基座建立在世界坐标系的原点处，其x、y、z轴的朝向也和世界坐标系的重合，link1表示世界坐标系到基座的变换，故link1的4个参数均为0。Link2将dz设置为56是为了将第一个连杆沿着第一个圆柱（圆柱也是依托在此关节处的坐标画出来的）的z轴移动56mm；将alf设置为90是为了让第一个圆柱处的坐标系的姿态绕x轴旋转90度，实现第一个圆柱到第二个圆柱的形态变化。

       同样道理，link3处先绕z轴顺时针旋转90度，使x轴竖直向上，再让其沿着x轴平移43mm，实现第二个圆柱到第三个圆柱的形态变化。

        同理link4。

        要将第4个关节变换到第五个关节的那种状态要先绕z轴旋转再绕z轴旋转然后再沿着z轴平移，但是D-H参数的顺序是有严格限制的，必须是“先绕z轴旋转，再沿z轴平移，再沿着x轴平移，再沿着x轴旋转”，故仅靠一次D-H变换无法实现。所以我在此引入多一个关节，让它实现先从前一个关节到过渡关节进行绕z轴旋转再绕x轴旋转，再从过渡关节到后一个关节进行沿z轴平移45.5mm，以此解决顺序问题。

此处引入过渡关节帮助变化，但在实际的机器人构型中并不想表示出来，所以在后续的绘图代码中只需在循环画图中不画出此关节即可。

for i=2:7
      
      Link(i).A=Link(i-1).A*Link(i).A;        %计算第i个关节的位姿根据上一关节的位姿计算下一关节的位姿
      Link(i).p= Link(i).A(:,4);
      Link(i).n= Link(i).A(:,1);
      Link(i).o= Link(i).A(:,2);
      Link(i).a= Link(i).A(:,3);
      Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
      Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
      if i~=6
      DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;
      end
end

实现机器人构型，大功告成

如果将if条件注释掉，机器臂构型将长这样