【现代机器人学】学习笔记五：逆运动学（Inverse kinematics）

这节的内容是根据末端位姿，计算关节角度，也就是所谓的IK。IK其实是一个比较复杂的问题，远不止本节内容所述的这么简单。这节个人觉得还是偏向基本的概念了。

逆运动学解析法

6R PUMA机器人 

 这节文章中大力描述了一些肩关节，肘关节，腕关节，这个需要事先联想一下人的胳膊的构造。拿右边胳膊来说，比如上图中，可以把z0想象成从你头顶射出向上方的轴线，然后你的胳膊就可以绕着身体左右摆动，即这里的θ1。还可以沿着身体右侧抬起来，即θ2。那么3轴就相当于你的肘部的关节，可以弯曲，控制小臂（图里是a3）。末端456轴图里面没有画出来，但实际上就是一个可以朝三个方向旋转的轴，交于一点，类似于你的手腕。

它的解析法求解，仔细想想也不困难：

py和px可以算θ1。（使用atan2(py,px)来算）

再根据图里的r和pz算最长的那个斜边。然后余弦定理，把α和β算出来，再求解θ2和θ3就是了。

注意，解不只有一种，比如把θ1先转180度，然后2，3关节从空中翻过来（想象一下），依然能达到同一个末端。

能计算出末端位置以后，再算姿态，因为后面三个关节轴互相垂直，所以螺旋轴的w分别为（0，0，1），（0，1，0），（1，0，0），套入FK公式，用PoE公式来看，左边一堆指数积，等于右边的FK结果。然后移项完就是左边三个旋转，等于右边一个已经得到的R。用ZYX欧拉角和R的转换公式即可。

书中还提到了另外一种复杂的情况，即含肩部偏距，就是1关节和2关节不是交于一点的，而是有个偏距。这种通常情况，如果不考虑机械臂的关节限位的话，是有4种解的。

这种的解析法实际上还是有些复杂，网页上也说不清楚（其实我认为也没有说的必要），根据一个老同事的意见，一个构型的机械臂的解析法逆解公式，找个推导的论文，直接用结论就好了。我深以为然...

Stanford机械臂

这种就是把肘关节替换成了平动副

 解法这里借用滕瀚哲老师的ppt内容：

同样，这种解法也可以像PUMA机器人一样，1轴转180度到另一边去，然后2轴再从上方180度翻回来。。

顺便在这里提一下书中没有提到的机器人的pieper准则：

Pieper准则：如果机器人满足两个充分条件中的一个，就会得到封闭解，这两个条件是：

• 三个相邻关节轴相交于一点；

例如上面提到的PUMA和Stanford机器人，末端3轴交于一点。

• 三个相邻关节轴相互平行；

• 例如ur5机器人：

逆运动学的数值求解 

这部分开始说的一点很重要，即计算有解析解，也可以用数值法来改善求解精度。例如PUMA机器人，关节如果不能绝对精确交于一点，那么也不用摒弃解析法，而是可以用它当作数值法的初值，来迭代结果。

牛顿拉弗森法

相比我们熟悉的梯度下降法，这个方法的好处似乎它不用指定迭代步长：

假如我们想求得蓝线和横轴的交点，只需要确定一个初值，例如xn。然后过曲线上对应点的斜率，让它交于横轴，xn减去这个这个三角形的底，就可以得到xn+1，一直下去就能逐步逼近蓝线和横轴的交点了。。

根据这个式子，f'x很简单是高比底，我们拿高，再比一个（高比底），就得到了底，用xn减去这个底，继续逼近就是了。

同样，如果是求最低点，也可以说让斜率为0，那么也是相同的道理（用泰勒展开）：

逆运动学的数值算法

对于机器人这里，做法也是类似：

这时候我们可能不是想让f函数的结果等于0，而是想让它等于某个值，例如xd。因为在这里，f函数实际上是前向运动学FK的函数，它是关于关节角θ的函数，希望得到的当然是末端位姿x了。

这时候就要重新构造一个关于θ的函数gθ，让它等于0。

这时候原先的：

把θ套进去，就是：

而，继续带入就是：

即：

 注意：

1.FK函数对θ求导，就是雅可比矩阵。

我们可以看到，不同列是不同的关节变量，同一列里面的不同行则是f结果中的各项对于同一个关节的偏导数。

2.这个公式可以这样辅助记忆：

每次修改的步长，是误差左乘雅可比矩阵的逆。

ps：不过这种方法和梯度下降法比看似很吊，因为不用人为指定梯度下降里面的步长了，但是也存在一个问题，就是当落在最高点或者最高点附近6的时候，这时候梯度是0，用牛顿拉弗森法算出来的步长是无限大，根本就没法收敛。

另外中文翻译版的书里，图6.7，最后一句话，当初始值在xd-f(θ)高点之上或附近，结果可能导致更大的... 这个地方翻译的就很烂。译者画蛇添足的加了一个“之上”，搞得好像是在那个高点上方。但是实际看了英文版原版就知道，这里人家用的介词是"at"，而不是“up”，所以翻译成“落在高点”即可。

伪逆

我们回到知识点：

前面的方法，有一个前提是雅可比可逆。如果雅可比不可逆，它的逆矩阵就得不到，那后面就不好算了。

所以这时候可以采用伪逆的概念来代替求解逆矩阵。（前提是机器人不能处于奇异位形，长宽不一致的雅可比矩阵的满秩为小的那个数）

假设一个矩阵是m行n列，反正伪逆算出来的大小一定是n行m列。

如果关节n比末端变量m多，那么这个矩阵就是粗矮（当然也意味着这是一个冗余机器人），求解方法就是：

 辅助记忆法：从下面的 入手，因为冗余，所以雅可比可以光明正大的写在前面，伪逆跟在后面。所以在 部分也是逆在后面。把握“伪逆算出来的大小一定是n行m列”，这样就很容易得知这个设置的结构了，只有这样安排才能满足这个要求。

同理，如果末端关节n比m少，则它是个欠驱动的机器人，那只能让人家伪逆在前面：

同样右侧也是逆的部分在前面，根据计算出的伪逆大小是n行m列，也可以很快写出这个公式来。

逆运动学求解示例（末端到达3d点）

 做法很简单，一直迭代即可：

 雅可比则是：

 逆运动学求解示例（末端到达6d pose）

 

这个要注意几点：

1.误差是FK得到的末端位姿和期望位姿，这俩矩阵的差异不能直接减，而是必须用逆矩阵计算差异，这个在机器人其他领域也很常见了。

2.得到误差后，求log，让它变成一个旋量，那么这个旋量就是末端b到期望d的旋量（因为是计算Tbd，即相对于末端坐标系的预期位形）

3.左乘物体坐标系的雅可比，加到上次的关节角上。

物体雅可比的概念参见【现代机器人学】学习笔记四：一阶运动学与静力学_zkk9527的博客-CSDN博客

这点中文版书中翻译仍然有误，可见这一节译者自己的水平确实不太行。

如中文书中140页底下，"的伪逆应该作为物体速度旋量..."

 这显然是翻译错误，因为雅可比的绝对不可能是旋量，这里原书中是这样写的：

“the pseudoinverse of should act on a body twist  "

这里的act on 应该翻译为 “作用于” 更合适，而不是翻译成“作为”。

逆向速度运动学

 在上一节的内容中，表面上叫一阶运动学于静力学，但是实际上在一阶运动学的部分只有前向运动学的概念，而没有逆向的内容。这节课才进行了补充：

显然期望的结果是，根据末端速度，计算出关节速度。

上一节的内容提到，关节角速度，左乘雅可比矩阵，即可得到末端速度。

那么在这里，结合上面提到的雅可比矩阵的伪逆的概念，则是末端速度左乘雅可比矩阵的伪逆，即可得到关节角速度。

逆向速度运动学的内容其实就这些。

预热内容：

但是书里面还乱七八糟夹杂了一些其他内容，这些内容实际上后面动力学部分（此处占坑）是要整体细致的讲的，这里可能只是想先预热一下，先做了解：

即冗余机器人，IK反解可能无数个，因此需要做一些限制，比如赋予关节速度不同的权重。

这样，其实目标函数是，希望让动能和势能的变化最小 ，不过同时还要满足一个约束，即关节速度左乘雅可比为末端旋量。

这种求解目标函数，还附带一个约束的题目，一律可以用拉格朗日乘子法求解，有一套固定的求解方法，套公式即可。这个其实也是大学高等数学的内容。这个公式我就不再罗列了，需要就去查附录D，后续在动力学部分会深入介绍这个内容。

讲这个的目的是：

1.回顾静力学部分概念，关节力矩等于雅可比逆矩阵左乘力旋量。

2.拉格朗日乘子可以理解为一个动态力产生的末端速度，与平衡重量的静态力相加的结果。（为后面动力学预热）

3.告诉你，最小动能方程中，也用到了伪逆的概念（当势能函数为0或者不定的时候），强调了伪逆的重要性。

最后再强调两点：

1.对于冗余机器人来说，末端闭环（从一个点出发回到该点），不意味着关节角度闭环，可能起点和终点对应的关节角是不同的。

2. 当中，表示1秒内把转换到Tsd的速度旋量。

ps：J的伪逆里面的J是物体雅可比，这里的V就是Vb，就是对Tbd求对数，这个代表目前从Tsb到Tsd之间的误差。

另外不管是原英文书，还是翻译的中文书，在最后的本章小结中，都写错了，误把 写成了，其实这个上面是没有dot的，加上了dot意味着它是角速度了。原书写错了，翻译的时候也没发现。。等我总结完以后，我会整理一下这本书已经发现的一些错误，把它汇总起来，反馈给读者邮箱。

配套的代码实现，可以参考：【现代机器人学】学习笔记十三：配套代码解析