Matlab Robotic Toolbox V9.10工具箱(二)：正/逆运动学

matlab机器人工具箱 robotic toolbox 做运动学分析非常方便，SerialLink 类中有现成的函数：SerialLink.fkine(theta)，可以直接对已经建立的机器人模型做运动学分析，同时可以使用SerialLink.ikine（T) 求逆运动学参数。

clear;
clc;
L1 = Link('d', 0, 'a', 0, 'alpha', pi/2);    %Link 类函数
L2 = Link('d', 0, 'a', 0.5, 'alpha', 0,'offset',pi/2);
L3 = Link('d', 0, 'a', 0, 'alpha', pi/2,'offset',pi/4);
L4 = Link('d', 1, 'a', 0, 'alpha', -pi/2);
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L6 = Link('d', 1, 'a', 0, 'alpha', 0);
b=isrevolute(L1);  %Link 类函数
robot=SerialLink([L1,L2,L3,L4,L5,L6]);   %SerialLink 类函数
robot.name='带球形腕的拟人臂';     %SerialLink 属性值
robot.manuf='飘零过客';     %SerialLink 属性值
robot.display();  %Link 类函数
theta=[0 0 0 0 0 0];
robot.plot(theta);   %SerialLink 类函数

theta1=[pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6];
p0=robot.fkine(theta);
p1=robot.fkine(theta1);
s=robot.A([4 5 6],theta);
cchain=robot.trchain;
q=robot.getpos();

q2=robot.ikine(p1);  %逆运动学
j0=robot.jacob0(q2);    %雅可比矩阵

p0 =

   -0.7071   -0.0000    0.7071    1.4142
    0.0000   -1.0000   -0.0000   -0.0000
    0.7071    0.0000    0.7071    1.9142
         0         0         0    1.0000


p1 =

    0.9874    0.1567    0.0206    1.0098
    0.0544   -0.4593    0.8866    1.8758
    0.1484   -0.8743   -0.4621    0.0467
         0         0         0    1.0000

>> s

s =

     1     0     0     0
     0     1     0     0
     0     0     1     2
     0     0     0     1


cchain =

Rz(q1)Rx(90)Rz(q2)Tx(0.5)Rz(q3)Rx(90)Rz(q4)Tz(1)Rx(-90)Rz(q5)Rx(90)Rz(q6)Tz(1)


q =

     0     0     0     0     0     0
q2 =

   1.0e+04 *

    0.0003    0.0180   -0.0399    1.1370    0.0002    0.0536


j0 =

   -0.1100    0.0707    0.3577   -0.0114    0.5092         0
   -0.8329   -0.0448   -0.2267   -0.6224    0.1813         0
   -0.0000    0.7623    0.3956   -0.1410   -0.8413         0
   -0.0000    0.5354    0.5354    0.3374   -0.0178   -0.8605
    0.0000    0.8446    0.8446   -0.2139   -0.9751    0.1275
    1.0000    0.0000    0.0000    0.9168   -0.2209   -0.4933

可以看出，逆解和原始的角度并不相同。
机器人工具箱中的比运动学函数并不精确，同时机器人通常有多组逆解，而ikine函数智能求出一组。
对满足pieper条件的机器人，最好自己求出他的解析解，利用解析解来求得多组逆解，不仅速度快，而且更精确。