新手。用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)  co