逆运动学
本文我们研究机器人的逆运动学问题，即针对给定执行末端的位置和姿态，计算各个关节的转角或者位移。这是一个比正运动学复杂很多的问题。 对于正运动学问题，只要给定了各个关节的状态就可以通过齐次变换矩阵的相乘得到执行末端的位置和姿态；而对于逆运动学问题根据机械结构和环境约束， 可能无解，即使有解也未必是唯一的，往往需要根据一些优化目标选择一个合理地解。
逆运动学可以通过迭代的方法求得数值解，也可以通过一些几何方法得到解析解，但并不是所有的逆运动学问题都可以得到解析解的。 数值解的优势在于只要问题有解一般都可以通过搜索的方法得到解决，缺点就是慢。而解析解，无他，就是快！本文中关注更多的是解析解，关于数值解以后专门写一篇文章介绍。
在本文中，我们先形式化地描述逆运动学问题，然后介绍运动学解耦的原则，使得我们可以将这一问题分解为相对独立的位置和姿态两个子问题。 在此基础上分别介绍确定位置的一种几何解法，关于解决姿态问题的欧拉角的方法后续会专门写一篇文章进行介绍。最后做出总结。
1. 问题描述
逆运动学问题可以形式化描述如下：给定一个\(4 \times 4\)的齐次变换矩阵 $$ \begin{equation}\label{f1} H = \begin{bmatrix} R & \boldsymbol{d} \\ \boldsymbol{0} & 1 \end{bmatrix} \in SE(3) \end{equation} $$ 其中\(R \in SO(3)\)，找到如下方程的一个(或者所有)的解。 $$ \begin{equation}\label{f2} T_n^0(q_1,...,q_n) = H \end{equation} $$ 其中\(q_i\)描述的是各个关节的状态， 对于旋转关节则是转动的角度\(\theta_i\)，对于平动关节则是平移距离\(d_i\)。\(T_n^0\)具有如下的形式： $$ \begin{equation}\label{f3} T_n^0(q_1,...,q_n) = A_1(q_1)...A_n(q_n) \end{equation} $$ 这里，\(H\)表示执行末端期望的位置和姿态，我们的任务就是要找到一组关节状态\(q_1, ..., q_n\)，使得式(\(\ref{f2}\))成立。 由于齐次变换矩阵的最后一行总是\(0, 0, 0, 1\)，所以对于公式(\(\ref{f2}\))我们可以建立12个关于\(n\)个未知数的方程： $$ \begin{equation}\label{f4} T_{i,j}(q_1,...,q_n) = h_{i,j}; i=1,2,3; j=1,2,3,4 \end{equation} $$ 通常这是一个非线性的方程组，不能完全按照线性代数中简单的消元法得到解。 直接求解是比较困难的，一种可能的办法就是通过迭代或者搜索的方法得到一个接近的数值解。 在后文中会介绍一种解耦的策略，进而可以通过几何的方法得到逆运动学问题的解。
2. 运动学解耦
虽然我们很难得到逆运动学问题的一般解法，但是通过限定机械结构我们可以得到一些较为简单的结构的几何解法。 对于一个6自由度的机械臂，如果其最后三个关节为转动关节而且它们的转轴相交于一点， 那么我们就有可能将复杂的逆运动学问题拆分为两个较为简单的子问题， 逆位置运动(inverse position kinematics)和逆姿态运动(inverse orientation kinematics)，分别求解。 最后三个关节所组成的结构称为球腕(spherical wrist)， 转轴的交点称为球腕中心(wrist center)。逆位置运动则是求取确定球腕中心位置的前三个关节的状态， 逆姿态运动则求解组成球腕的三个关节的转角，使得执行末端满足期望。
对于一个有球腕结构的6自由度机械臂，设其球腕中心为\(o_c\)，我们有执行末端相对于基座坐标系的齐次转移矩阵： $$ \begin{equation}\label{f5} T_6^0(q_1,...,q_6) = \begin{bmatrix} R_6^0 & \boldsymbol{d_6^0} \\ \boldsymbol{0} & 1 \end{bmatrix} \end{equation} $$ 根据公式(\(\ref{f2}\))，我们有如下方程： $$ \begin{align}\label{f6} R_6^0(q_1,...,q_6) & = R \\ \boldsymbol{d_6^0}(q_1,...,q_6) & = \boldsymbol{d} \\ \end{align} $$ 其中，\(R\)和\(\boldsymbol{d}\)分别是执行末端的旋转矩阵和位置坐标。
我们在正向运动学与D-H坐标中提到的PUMA 260就是一种典型的具有球腕结构的机械臂， 其结构简图和D-H坐标描述如图1所示。从图中我们可以看到组成球腕的三个关节的转轴，\(z_3, z_4,z_5\)，相交于一点\(o_c\)。 所以当这些转动关节发生转动时，并不会改变\(o_c\)的位置，因此\(o_c\)的位置只由前三个关节决定。
 |
| 图1 PUMA-260的D-H参数 |
所谓的运动学解耦，实际上就是借助球腕结构把逆运动学问题拆分为两个较为简单的子问题， 先根据公式(\(\ref{f8}\))确定球腕中心的位置，求解从基座到球腕中心涉及到的各个关节状态； 再根据公式(\(\ref{f10}\))确定执行末端相对于球腕中心的姿态，借助欧拉角方法求解球腕结构中各个关节的转角。 接下来介绍求取确定球腕中心位置的各个关节状态的一种几何解法。
3. 逆位置运动的一种几何解法
逆位置运动几何解法的核心思想就是，求解关节\(i\)的运动参数\(q_i\)时，就把机械臂投影到\(x_{i-1}-y_{i-1}\)所确定的平面上， 然后通过简单的三角函数关系确定\(q_i\)。我们仍然以PUMA 260为例进行介绍，图2(1)中是逆位置运动几何解法的3D图示，其中只涉及了前三个关节的转角。 整个求解过程可以分为三步：
首先，把机械臂投影到基座所在坐标系的\(x_0-y_0\)平面内，求解第一个关节的转角\(\theta_1\)， 然后投影到\(x_1-y_1\)平面上求取第二个关节的转角\(\theta_2\)，最后投影到\(x_2-y_2\)平面上求取第三个关节转角\(\theta_3\)。
 |
 |
| 图2(2) 机械臂在\(x_0-y_0\)平面上的投影 | |
 |
|
| 图2(1) 机械臂逆位置运动几何解法3D图示 | 图2(3) 机械臂在\(x_1-y_1\)平面和\(x_2-y_2\)平面上的投影 |
图2(2)中显示了机械臂在\(x_0-y_0\)平面内的投影，从中我们可以看出第一个关节的转角\(\theta_1\)需要根据两个辅助角求得： $$ \begin{equation}\label{f12} \theta_1 = \phi - \alpha \end{equation} $$ 对于给定的球腕中心我们有: $$ \begin{equation}\label{f13} \phi = atan2(x_c, y_c) \end{equation} $$ 而\(\alpha\)则需要根据具体的机械结构确定： $$ \begin{equation}\label{f14} \alpha = atan2(\sqrt{r^2 + (d_2 + d_3)^2}, d_2 + d_3) \end{equation} $$ 其中，\(r^2 = x_c^2 + y_c^2\)。实际上\(\phi\)还有一个解\(atan2(x_c, y_c) + \pi\)，这将导致转角\(\theta_1\)具有第二个解， 其求解过程可以参考第一种解的方法简单分析几何关系得到，这里不再赘述。
图2(3)描述了求解\(\theta_2, \theta_3\)的方法，图中\(s=z_c-d_1\)是球腕中心在\(y_1\)轴上的投影，有如下几何关系： $$ \begin{align} \theta_2 & = \frac{\pi}{2} - \gamma - \beta \\ \theta_3 &= \arcsin{\left(\frac{s - a_2\sin{\theta_2}}{d_4}\right)} = \arcsin{\left(\frac{z_c-d_1 - a_2 \times \sin{\theta_2}}{d_4}\right)} \\ \gamma & = \arccos{(s / r)} = \arccos{\left(\frac{z_c - d_1}{\sqrt{x_c^2 + y_c^2}}\right)} \\ \beta & = \arccos{\left(\frac{r^2 + a_2^2 - d_4^2}{2a_2r}\right)} = \arccos{\left(\frac{x_c^2 + y_c^2 + a_2^2 - d_4^2}{2a_2r}\right)} \\ \end{align} $$
以上是对PUMA 260的逆位置运动的分析，对于一些简单的机械结构我们都可以通过几何方法就得到各个关节的状态。 对于D-H坐标系，非零的参数越小进行几何分析的复杂度越低。大多数机械中的关节参数\(a_i, d_i\)为0，\(\alpha_i\)则取0或者\(±pi/2\)，这样可以大大简化分析过程。
4. 总结
对于拥有球腕结构的机械臂，我们可以将复杂的逆运动学问题拆分为逆位置运动和逆姿态运动两个较为简单的子问题分别求解。 逆位置运动几何解法的核心思想就是，求解关节\(i\)的运动参数\(q_i\)时，就把机械臂投影到\(x_{i-1}-y_{i-1}\)所确定的平面上， 然后通过简单的三角函数关系确定\(q_i\)。 逆姿态运动可以通过欧拉角的方法予以解决，关于欧拉角的方法后续会专门写一篇文章介绍。