机器人学——3.3-逆运动学

上一节已经展示了机械臂的正运动学求解方法，即在给定关节坐标以及可选工具和基座变换的条件下，如何确定末端执行器的位姿。
实际应用中的另一个问题是运动学的逆问题：给定期望的末端执行器位姿 $xi_E$ ，如何求取所需的关节坐标？例如，如果我们知道一个物体的笛卡儿坐标位姿，那么机器人需要什么样的关节坐标才能接近这个物体？这就是逆运动学问题，写成函数形式如下：
$q=mathcal{K}^{-1}(xi)$ 一般而言这个函数的解不是唯一的，而且对于某些类型的机械臂甚至没有解析解存在，只能求出一个数值解。

解析解

对于一个六轴机器人，其逆运动学具有解析解的一个必要条件是：三个腕关节的轴相交于一个点。这意味着腕关节的运动只改变末端执行器的姿态，而不改变其位置。这种机构被称为球腕，而且几乎所有的工业机器人都具有这样的腕关节。
我们将继续使用Puma机器人模型来探讨逆运动学问题。机器人标准状态的关节坐标为

>> mdl_puma560
>> qn
qn =
         0    0.7854    3.1416         0    0.7854         0

对应的末端执行器位姿是

>> T = p560.fkine(qn)
T =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1501
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000

Puma560 是一种包含一个球腕的六轴机器人，因此我们使用ikine6s方法来计算其逆运动学的封闭解（ikine6s方法会检查机器人的 D-H 参数，以确定它是否满足这些标准）。要实现位姿T，所需的关节坐标为

>> qi = p560.ikine6s(T)
qi =
    2.6486   -3.9270    0.0940    2.5326    0.9743    0.3734

奇怪的是，这些解与我们开始采用的标准状态关节坐标完全不同。但是，如果我们进一步探究一下：

>> p560.fkine(qi)
ans =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1500
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000

我们发现，两组不同的关节坐标值得到了相同的末端执行器位姿，而这两组坐标值对应的机器人位形如图所示。该 Puma 机器人的肩关节相对于腰部有个水平偏移，所以一组坐标解对应的机械臂在腰部的左侧，而另一组则对应在右侧，它们分别被称为左手运动学位形和右手运动学位形。一般来说，有 $8$ 组不同的关节坐标值可以得到相同的末端执行器位姿——正如前面所提到的，逆解不是唯一的。
在这里插入图片描述

我们可以人为指定获得右手位形解：

>> qi = p560.ikine6s(T, 'ru')
qi =
   -0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000

它就是我们最初的那组关节角度，并且是肘关节在上的右手位形。
除了左手位形和右手位形的解，还有一些解对应肘关节在上或在下，以及手腕翻转或不翻转。对于 Puma560 机器人的腕关节 $heta_4$ ，具有一个较大的旋转范围，因此其可以采用两个相差 $pi$ 的关节角度。又由于大多数机器人夹持器只需两个手指，因此相差 $pi$ 并不会改变它抓取物体的效果。

由ikine6s返回的运动学位形由一个或多个字来控制

左手或右手	`'l'`，`'r'`
肘部在上或在下	`'u'`，`'d'`
手腕是否翻转	`'f'`，`'n'`

由于关节转角上的机械限制以及连杆之间可能发生的碰撞，并不是所有 $8$ 种位形解在物理上都可以实现，有时甚至可能不存在可以实现的解。例如：

>> p560.ikine6s(transl(3, 0, 0))
警告: point not reachable 
> In SerialLink/ikine6s (line 210) 
ans =
   NaN   NaN   NaN   NaN   NaN   NaN

以上位姿就没有对应的解，这仅仅是因为手臂的长度不足以达到这个位姿。另外，由于奇异位形的存在，即关节轴线重合而减少了有效自由度（再次出现万向节锁问题），某个姿态也可能是无法实现的。Puma560 有一个奇异位形，即当 $q_5$ 等于零时，关节 $4$ 和关节 $6$ 之间的轴线重合了。在这种情况下，ikine6s可以做到最好的是限制 $q_4+q_6$ ，但它们各自的数值却是任意的。例如，考虑位形：

>> q = [0 pi/4 pi 0.1 0 0.2];

其中 $q_4+q_6=0.3$ ，逆运动学的解为

>> qi = p560.ikine6s(p560.fkine(q), 'ru')
qi =
   -0.0000    0.7854    3.1416   -3.0243    0.0000   -2.9589

$q_4$ 和 $q_6$ 的值完全不一样，但是它们的和始终等于 $0.3$ （在一个周期内）

>> qi(4)+qi(6)+2*pi
ans =
    0.3000

数值解

对于不具有 $6$ 个关节和球形腕关节的机器人，我们需要使用一种迭代数值解。继续使用上面的示例，我们使用ikine方法来计算一般逆运动学解（机器人工具箱9.x的高版本的函数有bug，建议使用低版本或者10.x版本，10.x版本的参数略有不同）：

>> T = p560.fkine(qn)
T =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1501
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000
>> qi = p560.ikine(T)
qi =
    0.0000   -0.8335    0.0940   -0.0000   -0.8312  0.0000

它们与最初的关节数值不同：

>> qn
qn =
         0    0.7854    3.1416         0    0.7854         0

但它们仍能得到正确的工具位姿：

>> p560.fkine(qi)
ans =
    0.0000   -0.0000    1.0000    0.5963
   -0.0000    1.0000    0.0000   -0.1500
   -1.0000   -0.0000    0.0000   -0.0144
         0         0         0    1.0000

绘制该位姿的图形：

>> p560.plot(qi)

结果清楚地显示ikine方法找到了肘关节在下的位形。
在这里插入图片描述

这个通用数值解法的局限性是：它不像解析法能提供对手臂运动位形的明显控制，只有通过设定关节坐标初始值（默认为零）的隐含控制。如果指定初始关节坐标：

>> qi = p560.ikine(T, [0 0 3 0 0 0])
qi =
    0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000

这时其数值解收敛于肘关节在上的位形。
在这里插入图片描述
通用数值方法ikine要比解析方法ikine6s慢很多。然而，它也具有很大的优势，它能够求解处于奇异位形以及非 $6$ 关节型的机械臂。

欠驱动机械臂

一个欠驱动机械臂是指关节数少于 $6$ 个的机械臂，因此其末端执行器可以达到的位姿是受限制的。它们通常有一个 $x-y-z- heta$ 的任务空间 $mathcal{T}subsetmathbb{R}^3 imesmathbb{S}$ ，以及一个位形空间 $mathcal{C}subsetmathbb{S}^3 imesmathbb{R}$ 。

我们以两连杆机械臂为例，首先定义该机器人：

>> mdl_twolink

然后定义所期望的末端执行器的位姿：

>> T = transl(0.4, 0.5, 0.6);

然而此位姿对于该两连杆机器人是过约束的，因为工具既不能满足在 $hat{x}_2$ 、 $hat{y}_2$ 方向上的姿态约束，也不能实现在 $z$ 轴上除零之外的任何平移量。因此，我们要求ikine方法只考虑在 $x$ 轴和 $y$ 轴平移方向上的误差，而忽略其他所有笛卡儿坐标的自由度。为了达到这个目的，我们指定一个mask vector作为第四个参数：

>> q = twolink.ikine(T, [0 0], [1 1 0 0 0 0])
q =
   -0.3488    2.4898

该mask vector的元素分别对应末端执行器坐标系中的 $3$ 个移动和 $3$ 个转动： $t_x$ ， $t_y$ ， $t_z$ ， $r_x$ ， $r_y$ ， $r_z$ 。
在这个示例中，我们规定只考虑在 $x$ 轴和 $y$ 轴平移方向上的误差（向量中的非零元素，mask vector的值只能为 $0$ 或 $1$ ）。产生的关节角度值对应于某个末端位姿。求解其正运动学解：

>> twolink.fkine(q)
ans =
   -0.5398   -0.8418         0    0.4000
    0.8418   -0.5398         0    0.5000
         0         0    1.0000         0
         0         0         0    1.0000

得到我们所期望的 $x$ 轴和 $y$ 轴上的平移量，但姿态方向和 $z$ 轴的平移是不正确的。

冗余机械臂

冗余机械臂是指具有多于 $6$ 个关节的机器人。正如前面提到的， $6$ 个关节在理论上就足以达到任何笛卡儿任务空间 $mathcal{T}subset SE(3)$ 中所期望的位姿。然而，由于关节限制和奇异位形等实际问题的存在，并非所有机器人可达空间内的位姿都可以实现。增加额外的关节是解决这个问题的一种方法。为了说明这一点，我们将创建一个 Puma560 冗余机械臂。把它放在一个能在 $xy$ 平面移动的平台上，用以模仿一个安装在车辆上的机器人。这个新的机器人具有和 Puma 机器人相同的任务空间 $mathcal{T}subset SE(3)$ ，但其位形空间变为 $mathcal{C}subsetmathbb{R}^2 imesmathbb{S}^6$ 。位形空间的维数超过任务空间的维数。
此机器人基座的 D-H 参数如下：

连杆	$heta_i$	$d_i$	$a_i$	$alpha_i$	$sigma_i$
1	0	$q_1$	0	$-dfrac{pi}{2}$	1
2	$-dfrac{pi}{2}$	$q_2$	0	$dfrac{pi}{2}$	1

然后我们从D-H参数中创建serialLink对象

>> platform = SerialLink([0 0 0 -pi/2 1; -pi/2 0 0 pi/2 1], 'base', troty(pi/2), 'name', 'platform')

D-H 参数要求移动关节产生沿局部坐标 $z$ 轴方向的平移，基座变换则将该 $z$ 轴旋转为世界坐标的 $x$ 轴方向。这个机器人的关节坐标即是其 $x$ 和 $y$ 坐标位置。我们可以通过测试来证明我们使用的 D-H 参数是正确的。

>> platform.fkine([1, 2])
ans =
    1.0000    0.0000    0.0000    1.0000
   -0.0000    1.0000    0.0000    2.0000
         0   -0.0000    1.0000    0.0000
         0         0         0    1.0000

我们看到，旋转子矩阵为单位矩阵，这意味着该平台的坐标系与世界坐标系相平行，且 $x$ 和 $y$ 方向上的位移正是我们所要求的。
我们通过将两个机器人串联起来从而将 Puma 机器人安装在此平台上。在工具箱中，我们可以用两种不同的方式来表达这种串联。一种是通过乘法：

>> p8 = platform * p560;

另一种是通过串联：

>> p8 = SerialLink([platform, p560]);

串联方式还允许指定所创建 serialLink对象的其他属性。

然而，机器人的串联也存在一个小的问题。我们想要把 Puma 手臂安装在平台顶部的高大底座上。在此之前，我们通过基座变换增加了底座，但基座变换只能存在于一个运动链的开始，而现在这个变换是在八轴机器人的关节 $2$ 和关节 $3$ 之间。因此这里就直接通过设置 Puma 的 $d_1$ 为底座高度来实现该底座添加：

>> p560.links(1).d = 30 * 0.0254;

现在可以将这两个机器人结合起来了：

>> p8 = SerialLink([platform, p560], 'name', 'p8')
p8 = 
p8 (8 axis, PPRRRRRR, stdDH)                         
+---+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |
+---+-----------+-----------+-----------+-----------+
|  1|          0|         q1|          0|     -1.571|
|  2|     -1.571|         q2|          0|      1.571|
|  3|         q3|      0.762|          0|      1.571|
|  4|         q4|          0|     0.4318|          0|
|  5|         q5|       0.15|     0.0203|     -1.571|
|  6|         q6|     0.4318|          0|      1.571|
|  7|         q7|          0|          0|     -1.571|
|  8|         q8|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+
                                                     
grav =    0  base =  0  0  1  0   tool =  1  0  0  0 
          0          0  1  0  0           0  1  0  0 
       9.81         -1  0  0  0           0  0  1  0 
                     0  0  0  1           0  0  0  1

其结果即是命名为P8的一个新的八轴机器人。我们注意到，新的机器人继承了该平台的基座变换，且机器人底座位移以 $d_3$ 参数出现。
我们使用通用的数值解方法可以找到这个新冗余机械臂的逆运动学解。例如，将末端执行器移动到 $(0.5,1.0,0.7)$ ，并且使工具在 $xz$ 平面中指向下方，其笛卡儿位姿为

>> T = transl(0.5, 1.0, 0.7) * rpy2tr(0, 3*pi/4, 0);

所需的关节坐标为（不唯一）

>> qi = p8.ikine(T)
qi=
	-0.2854	1.0528	0.1246	-0.5377	-0.6091	-0.0939 -1.2147 0.1212

其中前两个元素是移动关节在基平面内的位移，后面六个是机器人手臂的关节角度。我们看到，这个解很好地利用了机器人的第二个关节，即平台沿y轴的移动，使机械臂的底座靠近所期望的点。我们可以绘制出机器人的该位形图。
在这里插入图片描述