Matlab RoboticToolBox（一）Link参数、三自由度/四自由度逆运动学

（一）Link参数

参考文章：MATLAB-Robot（2）：标准DH和改进DH的区别

这是我看了这么多博文对于Link函数参数讲解得最明白的，为了备份一下，不得已转载该博主的核心图片。

对于标准DH矩阵：

theta：绕Zi轴，从Xi旋转到Xi+1的角度

D：沿Zi轴，从Xi移动到Xi+1的距离

A：沿Xi轴，从Zi移动到Zi+1的距离

alpha：绕Xi+1轴，从Zi旋转到Zi+1的角度

（二）关于三轴（三自由度）/四轴（四自由度）的逆运动学

文章推荐：请问三自由度机器人在MATLAB机器人工具箱里面仿真，其反解是不是应该只能得到坐标，而得不到位姿呢？

有些讽刺的是，这不是一篇文章，而是一个问题。

找遍了各种博客，关于RoboticToolBox的逆运动学的文章基本都是从同一篇文章转载过来的，而这片文章中提到关于欠驱动的函数只提到只言片语：

其中ikine函数的调用格式：

     Q = IKINE(ROBOT, T)

     Q = IKINE(ROBOT, T, Q)

     Q = IKINE(ROBOT, T, Q, M)

参数ROBOT为一个机器人对象，Q为初始猜测点（默认为0），T为要反解的变换矩阵。当反解的机器人对象的自由度少于6时，要用M进行忽略某个关节自由度。

这里面的“初始猜测点”是什么东西？M又是怎么使用？找了很久都没找到。真不知道一篇文章转来转去有什么意思？是想播点阅读量赢点下载积分吗？不真正去解决问题，把时间浪费在转载烂大街的文章上，百度一搜全都是这篇文章！

在matlab里面使用help命令（help ikine），再点击重载函数，找到如下线索：

  In this case we specify the 'mask' option where the mask
  vector (1x6) specifies the Cartesian DOF (in the wrist coordinate
  frame) that will be ignored in reaching a solution.  The mask vector
  has six elements that correspond to translation in X, Y and Z, and rotation
  about X, Y and Z respectively.  The value should be 0 (for ignore) or 1.
  The number of non-zero elements should equal the number of manipulator DOF.
 
  For example when using a 3 DOF manipulator rotation orientation might be
  unimportant in which case use the option: 'mask', [1 1 1 0 0 0].
 
  For robots with 4 or 5 DOF this method is very difficult to use since
  orientation is specified by T in world coordinates and the achievable
  orientations are a function of the tool position.

原来mask是掩码，几轴就用几个1去掩住。

那么Q又是个什么东西？没找到。

无奈只好在百度不断地看这些重复来重复去的文章，终于在一个问题里面找到一些线索：robotics toolbox 里的逆解函数q = ikine(robot, T, q0, M)用法问题

按这里的来看Q似乎就是一个1x4的矩阵，对应于4-axis，意思是类似于数值计算逼近解的初始值吗？

我按照这个试了一下，依然不行，仍然报错。（这里可能是版本问题，以前的参数应该是这个，但我用的是10.2.0，可能改了）

然后找啊找，终于找到一开始推荐的那篇文章，里面的Q竟然是一个"mask"字符串？！

不对，应该是版本改了，本身这个函数原型就不是IKINE(ROBOT, T, Q, M)了！

源码

三轴

clear;
clc;
%建立机器人模型
%       theta	d       a       alpha       offset
L1=Link([0      0.2       0        pi/2        0     ]); %定义连杆的D-H参数
L2=Link([0      0       0.2        0           0     ]);
L3=Link([0      0       0.2     0           0     ]);
L4=Link([0      0     0.2       0           0     ]);
% L5=Link([pi      0        0        pi/2      0     ]);
% L6=Link([0       0.08     0        0         0     ]);
% robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
robot=SerialLink([L1 L2 L3],'name','manman');
%robot.plot([0,pi/4,-pi/2,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
%robot.display();
% teach(robot);
theta=[0,pi/6,pi/3]
figure(1)
robot.plot(theta);  %输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();

p=robot.fkine(theta)    %fkine正解函数，根据关节角theta，求解出末端位姿p

mask = [1 1 0 0 0 0];
q=ikine(robot,p,'mask',mask)    %ikine逆解函数，根据末端位姿p，求解出关节角q

figure(2)
robot.plot(q);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();

输出：

theta =
         0    0.5236    1.0472
robot = 
manman:: 3 axis, RRR, stdDH, slowRNE                             
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|        0.2|          0|     1.5708|          0|
|  2|         q2|          0|        0.2|          0|          0|
|  3|         q3|          0|        0.2|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

p = 
         0        -1         0    0.1732
         0         0        -1         0
         1         0         0       0.5
         0         0         0         1
q =
    0.0000    0.5236    1.0472

theta和q完全相同。

四轴

clear;
clc;
%建立机器人模型
%       theta	d       a       alpha       offset
L1=Link([0      0.2       0        pi/2        0     ]); %定义连杆的D-H参数
L2=Link([0      0       0.2        0           0     ]);
L3=Link([0      0       0.2     0           0     ]);
L4=Link([0      0     0.2       0           0     ]);
% L5=Link([pi      0        0        pi/2      0     ]);
% L6=Link([0       0.08     0        0         0     ]);
% robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','manman'); %连接连杆，机器人取名manman
robot=SerialLink([L1 L2 L3 L4],'name','manman');
%robot.plot([0,pi/4,-pi/2,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
%robot.display();
% teach(robot);
theta=[0,pi/6,-pi/4,2*pi/3]
figure(1)
robot.plot(theta);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();

p=robot.fkine(theta)    %fkine正解函数，根据关节角theta，求解出末端位姿p

mask = [1 1 1 1 0 0];
q=ikine(robot,p,'mask',mask)    %ikine逆解函数，根据末端位姿p，求解出关节角q

figure(2)
robot.plot(q);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();

输出：

theta =
         0    0.5236   -0.7854    2.0944
robot = 
manman:: 4 axis, RRRR, stdDH, slowRNE                            
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|        0.2|          0|     1.5708|          0|
|  2|         q2|          0|        0.2|          0|          0|
|  3|         q3|          0|        0.2|          0|          0|
|  4|         q4|          0|        0.2|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+

p = 
   -0.2588   -0.9659         0    0.3146
         0         0        -1         0
    0.9659   -0.2588         0    0.4414
         0         0         0         1
q =
   -0.0000   -0.2618    0.7854    1.3090

运行以下输出图像就可以看见，输出的姿态反了过来，这里涉及了一个多解的问题，三轴只有一个解，但四轴可以有两个解，五轴四个、六轴八个，但这些都是不涉及姿态的前提下，涉及姿态理应只有一个解。这可能跟逆解的掩码算法有什么关系，具体没有深入了解。

问题

需要注意的是，非完全自由机械臂（六轴以下）的数值解由于算法问题，很多角度是解不出来的，比如四轴用[0,pi/6,-pi/3,pi/6]，迭代超过100次，视作解失败，输出空矩阵。