机械手基础知识（2）之机械手的正运动学和逆运动学问题
开篇总结：
------机械手运动学是机器人控制中的重要研究内容，得知机械手各关节变量的大小，可以计算出机械手末端的位姿，这个过程叫做机械手的正向运动学；
------获得机械手末端在笛卡尔空间中的位姿，可以计算机械手的各个关节变量的大小，这种逆过程叫做机械手逆向运动学。
------给定机械手各关节大小以及 D-H 模型，则可以唯一计算出机械手末端在笛卡尔空间中的位姿，但是，其逆过程可能对应多个解。
机械手的正运动学问题和机械手的逆运动学问题
------关于机械手的一些知识，我之前介绍了关于基于视觉伺服技术的机械手控制的两种方法，即基于图像的视觉伺服和基于位置的视觉伺服，在这一部分，我将介绍通过举例介绍什么事机械手的正运动学问题，什么是机械手的逆运动学问题。
1. 机械臂系统构型描述
------一个串联的机械臂的整个链路由一组被称为连杆的刚体组成，这些连杆由关节（运动副）连接起来。每个关节都有一个自由度，可以是滑动的或移动的，也可以是转动的(转动关节)。关节的运动改变了其相邻连杆的相对角度或位置。
------一个机器人的关节结构可以用一个字符串来描述，例如 Puma 为‘RRRRRR’，斯坦福臂为‘RRPRRR’，其中每个字符代表了相应关节的类型，R 是转动副，P 是移动副。在 1955 年，迪拉维特（Denavit）和哈滕贝格（Hartenberg）提出了一种系统的描述串联式链路上连杆和关节的几何形状的方法，这就是如今熟知的 D-H 参数法。
------有 N 个关节（编号从 1 到 N）的机械臂有 N +1个连杆(编号从 0 到 N)。连杆0 是机械臂的基座，连杆 N则固定连接末端执行器或者工具。关节 j 将连杆 j +1接到连杆 j ，因此关节 j 带动连杆 j 。连杆被看作一个刚体，它确定了相邻两个关节轴线的空间关系。一个连杆由长度 a i a_{i} ai和扭转角 α i \alpha_{i} αi两个参数来描述。同样，一个关节也由两个参数来描述，其中连杆偏移 d i d_{i} di是指沿着关节轴线从一个连杆坐标系到另一个连杆坐标系的距离，而关节角 θ i \theta_{i} θi则是指一个连杆相当于另一个连杆关于关节轴线的转动角度。
-----------------------------------------------------------------图1 ----------------------------------------------
------图1对这种参数法进行了图示说明。深红色和蓝色分别表示与连杆和连杆关联的所有参数。而圆圈中的数字表示进行基本变换的顺序，圆圈中的数字表示基本变换的顺序。{j}坐标系固接于连杆 j 的远端或末梢。关节 j 的轴线即为该坐标系的 z 轴，这些关节和连杆的参数既我们所熟知的 D-H 参数，总结与表 1 中。
------在这个约定之后，第一个关节 1，连接连杆 0 和连杆 1。连杆 0 是机器人的基础。通常对于第一个连杆， d i = α i = 0 d_{i}=\alpha_{i}=0 di=αi=0，但我们可以假设 d i d_{i} di来表示基座上的肩关节的高度超出基座。
----------------------表1. D-H参数，包括它们的物理意义，符号和定义 ------------------------------
------最后一个关节 N ，连接连杆 N -1连杆和 N ，连杆 N 是机器人的工具，它的相关参数 d N d_{N} dN 和 a N a_{N} aN 分别代表了该工具的长度和 x 轴偏移量。
------从连杆坐标系{j -1} 到坐标系{j}的变换被定义为基本的旋转和平移，其形式如下：
----------------------------------------------------------------------------------（1）
它可以展开为
--------------------------------------------------------------------------------------------（2）
------参数 a j a_{j} aj 和 α j \alpha_{j} αj 始终是常量，对于转动关节 θ j \theta_{j} θj是关节变量且 d j d_{j} dj是常量，然而对于移动关节 d j d_{j} dj 是变量， θ j \theta_{j} θj是常量，且 α j = 0 \alpha_{j}=0 αj=0。使用如下的广义关节坐标：
------对于一个 N 轴的机器人，其广义关节坐标为 q ∈ l q\in l q∈l ，其中 l ⊂ R N l\subset \mathbb{R}^{N} l⊂RN被称为关节空间或者位形空间。一般情况下，一个全旋转机器人的关节坐标被称为关节角度。这种关节坐标同时也称为机械臂姿态，它不属于笛卡尔姿态的末端执行器的姿态。
------使用matlab中 Robot 工具箱中的 Link 对象来描述一个机器人连杆，其创建如下；
>> L = Link([0, 0.1, 0.2, pi/2, 0])
L=
theta=q d=0.1 a=0.2 alpha=1.571（R，stdDH）
------其中，输入向量的元素按顺序分别是 θ j \theta_{j} θj， d j d_{j} dj， a j a_{j} aj ， α j \alpha_{j} αj 。而可选的第 5 个元素 σ j \sigma_{j} σj则表明该关节是转动的 σ j = 0 \sigma_{j}=0 σj=0 ，还是移动的 σ j = 1 \sigma_{j}=1 σj=1，如果未指定该值则默认为转动关节。
------该 Link 对象的显示值不仅给出了其运动学参数，还表示了其他的一些状态，例如它是一个转动关节（标签“R”），以及使用了标准的 D-H 参数表达（标签“stdDH”）。
2. 机械臂的正运动学分析
------机械手正运动学问题也称为直接问题，指的是：对于一个给定的机械手，在已知机械手杆件参数和关节角矢量的情况下，求机械手末端工具相对于世界坐标系（或称参考坐标系、基坐标系）的位姿pose。
上句话是什么意思呢？意思就是，在在知道机械手各杆件参数的情况下，机械手底层控制算法驱动机械手关节和杆件运动，让机械手各关节杆件满足这些参数，这时，机械手末端工具就可以到达期望的位姿。
------机械臂的正运动学通常表述为如下的函数形式：
--------------------------------------------------------------（3）
------它表明末端执行器的位姿是基于关节坐标的一个函数。若用齐次变换，其表达式将由（2）式所给的单个连杆变换矩阵的简单乘积。对于 N 个机械臂则有
----------------------------------------------------------------------------------------（4）
------对于任何一个机械臂，无论其关节的数量和关节如何，都可以计算出其正向运动学解。通常真正实用的机械臂的任务空间是三维的，控制机械臂运动，就是要给机械臂每个关节一个特定的电压控制信号，使他们各自运动一个特定的角度，得到期望的位姿状态，对于一个 6轴机械臂，其总的矩阵变换通常被写作 T 6 T_{6} T6 。这个过程称为机械臂的正运动学，本文中将以 Puma560 机械臂作为所有全旋转六轴机械臂来在 MATLAB 仿真。
------使用如下指令来定义一个 Puma560 机械臂的实例：
>> mdl_puma560
它将在工作区中创建一个名为 p560 的 Serial Link（串行接口、串行链路）
------指令“mdl_puma560”还在工作区中创建了大量关节坐标向量，代表了一些典型的机械臂位形：
机器人的正向运动学计算如下：
它返回一个对应末端执行器位姿的齐次变换 T 6 T_{6} T6
至此，在已知机械手参数的情况下，机械手末端位姿求得。
3. 机械臂的逆运动学分析
------机械手运动学逆问题亦称解臂形问题, 它是指在已知手臂各杆件几何参数及手臂末端执行器相对于参考系坐标系的期望位姿为已知条件下, 求手臂能否使其末端执行器到达这个预期的位姿? 若能到达, 那么手臂有几种不同的解?
------在上一小节的中，讨论了机械臂的正运动学求解方法，即在给定关节坐标以及可选工具和基座变换的条件下，如何确定末端执行器的位姿。实际应用中的另一个问题是机械臂运动学的逆问题：给定期望的末端执行器位姿 ξ E \xi _{E} ξE ，求取所需的关节坐标。 对机械臂逆运动学的求解可分为：基于解析式的封闭解和基于迭代的数值解。由于数值解析法比相应的封闭解法的求解速度要慢的多。
------对六自由度机械臂来说，只有在特殊情况下才有解析解（封闭解）：存在几个正交关节轴或有多个 a i a_{i} ai为 0 或 ± 9 0 o \pm 90^{o} ±90o。研究表明，具有 6 个旋转关节的机器人存在封闭解的充分条件是相邻的三个关节轴线相交于一点。Puma560 机器人也满足这个条件。
------如上节利用 Puma560 机械臂的实例来讨论逆运动学问题。机器人的标准状态的关节坐标为
对应的末端执行器位姿是：
------Puma560 是一种包含一个球形腕关节的六轴臂形机器人，因此使用 ikine6s 方法（ikine6s 方法会检查机器人的 D-H 参数，以确定它是否满足这些标准）来计算其逆运动学的封闭解。要实现位姿T，所需的关节坐标为
但令人惊讶的是，这些解与我们采用的标准状态关节坐标完全不同。但进一步探究：
------可以发现，两组不同的关节坐标得到了相同的末端执行器位姿。因此逆运动学求解的结果一般不是唯一的，实际应用要根据机械臂关节的转动范围，逆运动学的求解结果一般选择使关节移动量最少的解。
4. 总结
关于机械手的正逆运动学分析，我们需要结合实际的应用来分析，例如，在基于位置的视觉伺服系统中，我们可以这样分析机械手的正逆运动学关系。
简单而言，对于一个七个关节全是旋转关节的KUKA机械手来讲，只有一个参数θ。而机械手逆运动学要做的就是：在机械手末端工具的位姿已知的情况下，估计每个关节的旋转角度，而机械手正运动学问题则是根据这些角度驱动机械手各关节的旋转，则机械手末端工具最终会到达期望的位姿。
下面用一幅图来显示正逆运动学以及机械手位姿估计算法之间的关系。
如上图，首先，计算机根据相机内部参数，目标的三维模型估算出相机相对于目标物体的相对位姿。然后，机械手逆运动学根据估算出来的位姿计算出机械手各关节的旋转角度；最后，机械手关节控制器控制机械手个关节运动，让机械手末端到达期望位姿。
但是，有一点需要提出的是，一般来讲，机械手逆运动学求解是的结果不是唯一的。也就是说机械臂可能会以不同的状态控制末端工具到达指定地点，然而这些不同的解中，肯定是有不符合条件的解，比如没有绕开障碍物等，因此，我们需要对齐进行约束。