正逆运动

在上一篇我们利用DH法得到了6轴机械臂的几何关系模型。根据这个模型，我们将对机械臂进行运动学分析。

Forward kinematics (for a robot arm) takes as input joint angles, and calculates the Cartesian position and orientation of the end effector. Inverse kinematics takes as input the Cartesian end effector position and orientation, and calculates joint angles.

根据描述，正运动（Forward Kinematics, FK）描述的是给定关节角度（Joint Angles），则计算出末端位姿；逆运动（Inverse Kinematics, IK）描述的是给定末端位姿，计算各关节角度。

在实际应用当中，正运动一般用在主动控制，例如遥控器、操控设备、机器人示教等应用。而逆运动更符合我们生活中的事件，例如知道了地上的垃圾位置后捡起来，这时就用到了逆运动了。所以逆运动分析在机器人学是很重要的一部分。

那么我们将利用下面的DH模型，试着分别求一下正运动解和逆运动解。

Gluon-6L3模型

正运动解算

根据上一篇的模型矩阵，我们可计算得6个变换矩阵，连乘后得到矩阵模型：

根据位姿T矩阵的形式，可知：

此处我们尝试令关节转动角为  度，代入上式，可得到末端位姿：（参数太多，用了下matlab）：

q1=pi/12;q2=pi/12-pi/2;q3=pi/12;q4=pi/12-pi/2;q5=pi/12;q6=pi/12;
d1=105.03;d4=75.66;d5=80.09;d6=44.36;
a2=-174.42;a3=-174.42;
A1 = [cos(q1) 0 sin(q1) 0;
      sin(q1) 0 -cos(q1) 0;
      0 1 0 d1;
      0 0 0 1];
A2 = [cos(q2) -sin(q2) 0 a2*cos(q2);
      sin(q2) cos(q2)  0 a2*sin(q2);
      0 0 1 0;
      0 0 0 1];
A3 = [cos(q3) -sin(q3) 0 a3*cos(q3);
      sin(q3) cos(q3)  0 a3*sin(q3);
      0 0 1 0;
      0 0 0 1];
A4 = [cos(q4) 0 sin(q4) 0;
      sin(q4) 0 -cos(q4) 0;
      0 1 0 d4;
      0 0 0 1];
A5 = [cos(q5) 0 -sin(q5) 0;
      sin(q5) 0 cos(q5) 0;
      0 -1 0 d5;
      0 0 0 1];
A6 = [cos(q6) -sin(q6) 0 0;
      sin(q6) cos(q6) 0 0;
      0 0 1 d6;
      0 0 0 1];
T1 = A1*A2*A3*A4*A5*A6

可以了解到，根据模型，我们得到的位姿只有上式一个解。

下面根据模型和末端位姿，我们来求关节转动角。

逆运动解算

机器人逆运动结算方式可分为两种：（1）解析解法（(Analytical Solutions）（2）数值解法（Numerical Solutions）。直接用公式解算的解析解法比用数值迭代的数值解法要快很多。

对于解析解法，也分为两种 ：（1）代数法（2）几何法。这两种方法其实区别比较模糊。代数法意味着我得到了T矩阵后，对T矩阵进行代数求解；几何法意味着，对机械臂一边建立几何关系，一边计算当前的关节位置。

本篇既然已经得到了T矩阵的代数形式，则选择使用代数法对T矩阵求解析解。

首先我们要意识到，我们有6个关节角度，所以有6个未知量要求。然而根据T矩阵，我们有12个方程式，而且都是带三角函数的非线性方程。所以要找个好的方式去求解。

有一种方法是对T矩阵中每一个关节的变换矩阵A进行反变换，再根据反变换后的矩阵进行等式匹配。过程如下，

*此处插播一条变换矩阵逆的公式

1）我们先对第一个矩阵A1进行反变换：

仔细观察一下，对于上面矩阵做等式匹配，看看有没有可以求解的地方。

嗯...我好像发现了可以用的几个等式。首先矩阵[3,3]元素可知

 ，

再将（1）代入矩阵[3,4]元素等式

由于  ，我们可以求出  有两个解  和。根据式（1）相应的得到的解  和。因为有些解不符合要求，所以在计算的过程中我们要去做解的筛选，比如三角函数的结果应该符合  ，不符合要求的解就应该舍弃，相应地根据这个解计算的其他角的解都要舍弃。

可得到

 和

 和

矩阵[3,1]元素和[3,2]元素相除，可得

所以可求得

 或 

代入确定值时，可以用得到的q1,q5,q6用矩阵[3,1]元素和矩阵[3,2]验证一下，在本篇中可排除掉q1,q5,q6的各4个解中的3个解。

矩阵[2,3]元素可知

 或 

2）由于关节2、3、4都是平行的，左乘A2、A3的逆不会产生什么有用的结果。则进行下一步逆运算。

再根据矩阵等式的对应关系，求出q34和q3，接着求出q4和q2。具体步骤就省略了，感兴趣的朋友可以试试。

至此我们6个角全求完了。（可累死我了）

我们用matlab的rtb（robotics-toolbox）整体演算一下。

close all;
%% 建立机器人DH参数,初始姿态为竖直。
L(1)=Link('d',105.03,'a',0,'alpha',pi/2,'standard'); 
L(2)=Link('d',0,'a',-174.42,'alpha',0,'offset',-pi/2,'standard');
L(3)=Link('d',0,'a',-174.42,'alpha',0,'standard');
L(4)=Link('d',75.66,'a',0,'alpha',pi/2,'offset',-pi/2,'standard');
L(5)=Link('d',80.09,'a',0,'alpha',-pi/2,'standard');
L(6)=Link('d',44.36,'a',0,'alpha',0,'standard');  
robot=SerialLink(L(1:6),'name','Gluon_6L3', 'manufacturer','innfos')
%% 正解，给定关节角，求末端位姿
Theta=15*ones(1,6);
%% 验证正逆解的结果
Theta=Theta/180*pi;                         %换算成弧度
T=robot.fkine(Theta)             %求正解的齐次变换矩阵
q1=robot.ikine(T)*180/pi      %求逆解验证关节角

其中robot.fkine()是用来求正解的方法，robot.ikine()是用来求逆解的方法。最后可得到结果：

matlab中用到求逆解的方法是利用运动学的雅可比矩阵进行数值迭代计算出来的。根据本人的惨痛经验，对比于手撕解析解求逆解法，让计算机来数值迭代可能更方便一点。

关于机器人正逆运动就暂时告一段落。

以上如有错误或者理解不对的地方，尽管指正和探讨。