Matlab Robotics Toolbox系列—使用篇（4）

Chapter 4 Inverse Kinematics

% 逆运动学方法适合笛卡尔坐标系下的关节角解算，这里仍以puma560为例。

>> mdl_puma560

 

% 在正式计算之前，先验证下正运动学方法计算出来的齐次变换矩阵，并以此为例，求解逆运动学的关节角

% 输入关节角

>> q = [0 -pi/4 -pi/4 0 pi/8 0]

 

% 正运动学计算

>>T = p560.fkine(q)

T =

 

    0.3827    0.0000    0.9239    0.7371

   -0.0000    1.0000   -0.0000   -0.1501

   -0.9239   -0.0000    0.3827   -0.3256

         0         0         0    1.0000

 

% 求解逆运动学参数

>> qi = p560.ikine(T);

>> qi

qi =

   -0.0000   -0.7854   -0.7854   -0.0000    0.3927    0.0000

 

% pinv指采用伪逆方法求解

>> qi = p560.ikine(T, 'pinv');

qi =

   -0.0000   -0.7854   -0.7854   -0.0000    0.3927    0.0000 

 

% 通常情况下，对于特定的末端位姿，不止有一组解，对于6自由度机械臂而言，通常采用解析式法求解各组关节角，在RTB中，该函数为ikine6s()，除上述情况以外，还应该注意奇异解

 

>> qi = p560.ikine6s(T)

qi =

2.7400   -3.0950   -0.7854    2.7547    1.2768    0.2787

 

% 逆运动学还可以用于求解路径，下面将以笛卡尔坐标系下的直线路径举例说明

% 定义初始坐标点

>> T1 = transl(0.6, -0.5, 0.0);

% 定义末端坐标点

>> T2 = transl(0.4, 0.5, 0.2) ;

 

% 采用ctraj方法计算笛卡尔坐标下的路径，ctraj方法使用的是梯形速度控制方法计算路径，使用方法与jtraj类似（jtraj用于计算关节空间轨迹，ctraj用于计算笛卡尔坐标系下的路径）

>> T = ctraj(T1, T2, 50);

 

% 求解逆运动学

>> q = p560.ikine6s(T);

 

% 检验结果

>> subplot(3,1,1); plot(q(:,1)); xlabel('Time (s)'); ylabel('Joint 1 (rad)');

>> subplot(3,1,2); plot(q(:,2)); xlabel('Time (s)'); ylabel('Joint 2 (rad)');

>> subplot(3,1,3); plot(q(:,3)); xlabel('Time (s)'); ylabel('Joint 3 (rad)');

 

% 清空表格&动画演示

>> clf

>> p560.plot(q)