新手。用DH方法及暴力方法建立机器人正运动学坐标系

                                 

机械臂如上图

暴力方法：

1、坐标系的建立：

原点：确定各坐标系原点的方法与DH方法一致。

坐标轴方向：基座标系根据个人喜好进行建立，除基座标系系外，其他坐标系方向一致。{S}坐标系是为了描述方便，固定在1、2、3号轴相交处。

 

2、传递矩阵

以下为matlab中写出的运算代码

function [P05org]=matrix_data()
syms x1 x2 x3 x4 x5  d1 d2 d3 d4;
T0s=[-1 0 0 d2;
    0  0 -1 0;
    0 -1  0  d1;
    0 0  0  1];

Ts1=[ 1        0        0        0;
     0       cos(x1)  -sin(x1)  0;
     0       sin(x1)  cos(x1)   0;
     0        0        0        1];
 
 T12=[cos(x2)  0  sin(x2)   0;
      0       1  0         0;
     -sin(x2) 0  cos(x2)   0;
     0        0        0   1];
 
T23=[cos(x3)  -sin(x3)  0   0;
    sin(x3)  cos(x3)   0   0;
     0        0        1   0;
     0        0        0   1];
  
T34=[cos(x4)  -sin(x4)  0   -d3;
    sin(x4)  cos(x4)   0   0;
     0        0        1   0;
     0        0        0   1];
 
   T45=[1       0         0     -d4;
      0      cos(x5) -sin(x5)   0;
      0      sin(x5)  cos(x5)   0;
      0        0        0       1];
  
  P45org=[-d4;0;0;1];
  
  P05org=T0s*Ts1*T12*T23*T34*P45org;

 DH方法求解：

                               

1、坐标系的建立完全照搬机器人学导论第三版P53建立连杆坐标系的步骤

2、连杆参数的得出完全搬照P53连杆参数的求法，为了使得与暴力方法求的运动学方程一致，这里将每个轴的初始位置设为上图状态与暴力法一致。因此需要加上每个轴此时的初始角度。

3、建立传递矩阵。Ti-1，i的公式完全照搬P59(3-6)

function [T06]=matrix()
syms x1 x2 x3 x4 x5 d1 d2 d3 d4;
%    连杆转角    连杆长度   连杆偏距   关节角
sdh=[ pi/2       0            -d2       x1;
      -pi/2      0            0         -x2;
      -pi/2      0            0         x3;
      0         d3            0         x4;
      -pi/2     0             -d4       x5];

这里的连杆偏距与纸质版不同的原因是：在转换矩阵我已经将公式进行了三角转换，比如sin(x3+pi)=-sin(x3)，下面的转换矩阵也是经过转换后的；这里值得注意的是轴2的转角是负的，这里是由于暴力方法轴2是顺时针旋转，而DH都是绕旋转轴逆时针旋转，故为负值。暴力方法1到4号轴旋转的方向分别为，逆，顺，逆，逆，DH都为逆。

 T00=[0 1 0 0;
    -1 0 0  0;
    0  0 1  d1;
    0 0  0  1];

T00表示暴力方法中基座标系到DH方法基座标系的传递矩阵。

 T01=[cos(sdh(1,4))                -sin(sdh(1,4))                0               sdh(1,2);
     sin(sdh(1,4))*cos(sdh(1,1))  cos(sdh(1,4))*cos(sdh(1,1))   -sin(sdh(1,1))  -sin(sdh(1,1))*sdh(1,3);
     sin(sdh(1,4))*sin(sdh(1,1))  cos(sdh(1,4))*sin(sdh(1,1))   cos(sdh(1,1))   cos(sdh(1,1))*sdh(1,3);
     0                            0                             0                1];
 
 T12=[sin(sdh(2,4))                 cos(sdh(2,4))                0               sdh(2,2);
     -cos(sdh(2,4))*cos(sdh(2,1))  sin(sdh(2,4))*cos(sdh(2,1))   -sin(sdh(2,1))   -sin(sdh(2,1))*sdh(2,3);
     -cos(sdh(2,4))*sin(sdh(2,1))  sin(sdh(2,4))*sin(sdh(2,1))   cos(sdh(2,1))   cos(sdh(2,1))*sdh(2,3);
     0                            0                             0                1];

  T23=[-cos(sdh(3,4))               sin(sdh(3,4))                0               sdh(3,2);
     -sin(sdh(3,4))*cos(sdh(3,1))  -cos(sdh(3,4))*cos(sdh(3,1))   -sin(sdh(3,1))   -sin(sdh(3,1))*sdh(3,3);
     -sin(sdh(3,4))*sin(sdh(3,1))  -cos(sdh(3,4))*sin(sdh(3,1))   cos(sdh(3,1))   cos(sdh(3,1))*sdh(3,3);
     0                            0                             0                1];
 
   T34=[-sin(sdh(4,4))              -cos(sdh(4,4))                0               sdh(4,2);
     cos(sdh(4,4))*cos(sdh(4,1))  -sin(sdh(4,4))*cos(sdh(4,1))   -sin(sdh(4,1))   -sin(sdh(4,1))*sdh(4,3);
     cos(sdh(4,4))*sin(sdh(4,1))  -sin(sdh(4,4))*sin(sdh(4,1))   cos(sdh(4,1))   cos(sdh(4,1))*sdh(4,3);
     0                            0                             0                1];
 
 T44=[0 -1 0 0;
    1 0 0  0;
    0  0 1  0;
    0  0  0  1];

T44表示DH方法中4号坐标系到暴力方法中4号坐标系的传递矩阵。

 T04=T00*T01*T12*T23*T34*T44;
 P45org=[-d4;0;0;1];
 T06=T04* P45org;

结果如下，两者的出来的运动学方程是一致的。

 

总结：

1、正运动学是为了描述末端在基座标系中的位姿，坐标系的建立仅仅是为了描述的方便。因此坐标系的建立并不是一成不变的。

2、当基座标系与目标坐标系一致时，得出来的运动学方程时一致的。

3、暴力方法中轴的旋转方向有顺时针和逆时针之分，初始角度都为0。而DH方法中旋转方向都是逆时针，初始角度不一定为0。由此可见，各有优劣。

4、坐标系是绕电机轴旋转，而不是绕某个坐标系旋转。

      坐标系是绕电机轴旋转，而不是绕某个坐标系旋转。

      坐标系是绕电机轴旋转，而不是绕某个坐标系旋转。