（6）六轴机械臂的运动学正、逆解

下面在前面的ur5机械臂的DH参数基础是对其正逆解进行求解，为了后面能在MATLAB中利用stl文件进行实际显示，这里以标准DH参数为例进行讲解。（修正DH参数在用plot3d函数是显示失败，不知道是不是这个函数只能显示标准dh参数的机械臂模型，有知道的网友可以在评论里告知一下，谢谢。）

一、运动学正解：

机器人正运动学是在已知各连杆相对位置关系（关节角）的情况下，得到末端执行器的位姿。在标准DH参数下相邻坐标系之间的齐次变换矩阵为：

 则，正解代码如下：

function    [T06,Pos]=ForwardSolver_MDH(theta)

    DH_JXB =[90    0      144  0; 
             0     264    0    90;
             0     236    0    0; 
             -90   0      106  -90; 
             90    0      114  0; 
             0     0      67   0];
    d=DH_JXB(1:6,3); 
    a=DH_JXB(1:6,2); 
    DH_JXB(1:6,1)=DH_JXB(1:6,1)/180*pi; %度数转化为弧度
    alp=DH_JXB(1:6,1); 
    offset=[0 90 0 -90 0 0];
    theta=(theta+offset)*pi/180;

for i=1:6
        T{i}=[     cos(theta(i)),       -sin(theta(i))*cos(alp(i)),                  sin(theta(i))*sin(alp(i)),     a(i)*cos(theta(i));
                   sin(theta(i)),       cos(theta(i))*cos(alp(i)),       
-cos(theta(i))*sin(alp(i)),    a(i)*sin(theta(i));
0,             sin(alp(i)),                      cos(alp(i)),                  d(i);
0,                   0,                               0,                          1
             ]

    end

    disp('Homogeneous transformation matrix T06:')
    T06=T{1}*T{2}*T{3}*T{4}*T{5}*T{6}
    %% 求末端位置
    X=T06(1,4);Y=T06(2,4);Z=T06(3,4);
    %% 求末端姿态Rotations about X, Y, Z axes (for a robot gripper)
    R=T06;
    if abs(abs(R(1,3)) - 1) < eps  % when |R13| == 1
        % singularity
        rpy(1) = 0;  % roll is zero
        if R(1,3) > 0
        rpy(3) = atan2( R(3,2), R(2,2));   % R+Y
        else
             rpy(3) = -atan2( R(2,1), R(3,1));   % R-Y
        end
        rpy(2) = asin(R(1,3));
    else
        rpy(1) = -atan2(R(1,2), R(1,1));
        rpy(3) = -atan2(R(2,3), R(3,3));

        rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
    end
    RPY=rpy*180/pi;
    Rall=RPY(1);Pitch=RPY(2);Yaw=RPY(3);
    Pos=[X,Y,Z,Rall,Pitch,Yaw];
end

这里对姿态的描述进行说明：在MATLAB中RPY欧拉角是世界坐标系下的XYZ欧拉角；

RPY角的定义如下：

 输入端位姿形式为：

MATLAB中对应的RPY旋转矩阵如下：

 这是个旋转矩阵，与齐次矩阵中的旋转矩阵等价，所以根据齐次矩阵中的旋转矩阵便可以得到末端的姿态RPY。

二、运动学逆解：

机器人逆运动学是已知机器人末端执行器的位姿，通过变换矩阵T得到机器人各关节的角度。求解逆运动学有解析法、几何法、迭代法。这里介绍解析法。如果机器人末端三轴的轴线始终交于一点则该机器人必有解析解。

1、得到齐次变换矩阵：

利用MATLAB求解其齐次变换矩阵：

syms a1 a2 a3 a4 a5 a6 d1 d2 d3 d4 d5 d6 a1p1 a1p2 a1p3 a1p4 a1p5 a1p6 th1 th2 th3 th4 th5 th6;
a=[0 a2 a3 0 0 0];
d=[d1 0 0 d4 d5 d6];
alp=[90 0 0 -90 90 0]*pi/180;
theta=[th1 th2 th3 th4 th5 th6];
% theta(1)=t1;theta(2)=t2;theta(3)=t3;theta(4)=t4;theta(5)=t5;theta(6)=t6;

 T01=[         cos(theta(1)),       0,      sin(theta(1)),     a(1)*cos(theta(1));
               sin(theta(1)),       0,      -cos(theta(1)),    a(1)*sin(theta(1));
               0,                   1,      0,                 d(1);
               0,                   0,      0,                 1
         ];

 T12=[         cos(theta(2)),       -sin(theta(2)),      0,     a(2)*cos(theta(2));
               sin(theta(2)),       cos(theta(2)),       0,     a(2)*sin(theta(2));
               0,                   0,                   1,     d(2);
               0,