KUKA 正运动学计算

采用DH参数模型进行Kuka机器人的运动学计算，参考古月居的文章，利用matlab第三方工具箱ROBOTICS TOOLBOX 建立Kuka机器人的运动学模型。

这里对KUKA KR10 R1100-2 机器人进行建模， 机器人的DH参数可以通过roboDK获取（找人帮忙获取的参数） 

利用该参数，可以建立机器人DH模型：

         %a    alpha     d     theta 
dhparams =[0      0      400      0
           25    -pi/2   0        0
          560    0       0       -pi/2
           25    -pi/2   515        0
            0     pi/2    0       0
            0    -pi/2   90       pi];

for i = 1 : 6
    % 采用 modified DH模型， 不能用 std-DH
    L(i) = Link('revolute', 'd', dhparams(i,3), 'a', dhparams(i,1), 'alpha', dhparams(i,2),'modified');
end

myrobot= SerialLink(L, 'name', 'sixlink'); 
myrobot.display
  
offset = dhparams(:,end)';   %  关节轴的偏移量，在关节坐标上做加法
theta = [11.642420, -88.638112, 90.964350, 35.732042, -19.629808, -36.885212]*pi/180;

Trans2poseABBKUKA(myrobot.fkine(theta + offset).T)

 函数 Trans2poseABBKUKA 把正运动学计算得到的变换矩阵转换为KUKA机器人的姿态坐标

function pose = Trans2poseABBKUKA(Tmat)
pose(1:3) = Tmat(1:3,end);
pose(4:6) = rotm2eul(Tmat(1:3,1:3),'ZYX')*180/pi; 

这里用roboDK来验证该程序，将工具坐标建立在法兰盘上，调整roboDK的机器人姿态如下：

将机器人的各轴姿态复制到前面程序的theta变量，并计算

 计算结果和roboDK中显示的工具坐标 [   672.381404,  -248.235703,   650.015017,   -27.603525,    76.456172,     0.049594 ] 完全一样。

采用该方法可以建立机器人的正运动学模型。

这里需要注意，建立关节模型的时候需要采用modified DH模型，就是前面代码中的 'modified' 参数。