4-1 逆向运动学引言

正如第三章的引言所述：顺向运动学：就是知道了每个关节和连杆的夹角（${\theta }_1$, ${\theta }_2$, ${\theta }_3$），得到机械臂末端点到达的位置（${}^{W}P$），
逆向运动学：就是知道了机械臂末端点的位置（${}^{W}P$），反推每个关节需要呈现的角度（${\theta }_1$, ${\theta }_2$, ${\theta }_3$）。在实际的机械臂抓取任务执行中，往往首先知道物体的具体位置（$x$，$y$，$z$），这其实就是机械臂末端需要达到的位置，所以，要在知道机械臂末端位置的条件下，得到每个关节的角度$\theta$，这首先就要进行逆向运动学的计算；在得到每个关节角度之后，再通过顺向运动学操作机械臂达到物体的位置实现抓取。

假设机械臂有6自由度，分别是6个未知的关节角度（${\theta }_1$, ${\theta }_2$, ${\theta }_3$, ${\theta }_4$, ${\theta }_5$, ${\theta }_6$）,根据正向运动学的知识，能够有齐次变换矩阵使得坐标系{6}的点能够转换到坐标系{0}上进行表达，齐次变换矩阵记为${}_6^{0}T$，将6个未知的关节角度带入齐次变换矩阵${}_6^{0}T$后，齐次变换矩阵${}_6^{0}T$的未知元素有12个，即有12个非线性超越方程。矩阵当中有6个未知数，同时也有6个限制条件，所以，逆向运动学就是在解这些方程，从而得到关节角度。

Reachable workspace：手臂可以用一种以上的姿态到达的位置；
Dexterous workspace：手臂可以用任何的姿态到达的位置；
例如，以下幻灯片中，第一幅图当中，环形圆的全部是一个Reachable workspace，没有Dexterous workspace；而第二幅图当中，圆的全部是一个Reachable workspace，对于原点是一个Dexterous workspace。

Subspace：手臂在定义头尾的$T$所能达到的变动范围（机械臂在给定定义条件下（关节限制、结构），它的末端位姿（T）能够变化到什么范围），下面的例子说明了机械臂末端能到达的位置是有限的，因为旋转矩阵只跟($x$，$y$)有关，将机械臂能够达到的变动范围就称为Subspace。

4-2 多重解 01

齐次变换矩阵当中的元素是三角函数项，其$\theta$解的数目并不唯一，对于有6个旋转的关节的机械臂系统来说，其解的数目也跟偏置有关

Ex：PUMA（6旋转关节机械臂）
针对末端夹爪的特定工作点，整个机械臂有8组姿态到达这个特定工作点，也就是有8组解。前三轴就有4种姿态，并且这4种姿态的每一种姿态当中，又具有2组手腕转动姿态（总共8种姿态），但是若机械臂本身有几何限制（碰撞），并非每一种解都可以运作。

那既然有这么多组解，解的选择需要考虑以下几个因素，首先考虑离目标状态最近的解，比如说，中间那幅图，此刻夹爪在A点，需要到达B点的话还是选择上面那条路径（最快、最省能）；再如，如果有障碍物，那首要就是需要避开障碍物到达目标点B。

求解方法包括解析法和数值法，解析法当中有代数法和几何法，具有解析表达式；数值法由计算机求解出数值：

4-2 多重解 02

在讲述完逆向运动学的大概思路后，现在给出具体的例子说明这一求解过程。对于一个RRR的机械臂，IK的目的就是在给定了末端位姿$(x,y,\varphi )$的条件下去求解每个关节的角度$({\theta }_1,{\theta }_2,{\theta }_3)$，具体来说，当末端位姿已知时会得到坐标系{3}相对于坐标系{0}的变换矩阵:
$${}_3^{0}T=\left[\begin{array}{cccc}{c}_{\varphi }& -s_{\varphi }& 0.0& x\\ s_{\varphi }& c_{\varphi }& 0.0& y\\ 0.0& 0.0& 1.0& 0\\ 0& 0& 0& 1\end{array}\right]，$$
同时，根据FK，也会得到得到坐标系{3}相对于坐标系{0}的变换矩阵，不过矩阵当中的参数$({\theta }_1,{\theta }_2,{\theta }_3)$是未知的：
$${}_3^{0}T=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0.0& l_1{c}_1+l_2{c}_{12}\\ s_{123}& c_{123}& 0.0& l_1{s}_1+l_2{s}_{12}\\ 0.0& 0.0& 1.0& 0.0\\ 0& 0& 0& 1\end{array}\right]。$$
将两个矩阵当中对应的元素相等建立方程组进行求解，得到$({\theta }_1,{\theta }_2,{\theta }_3)$

几何法求解： 运用余弦定理

$x^2+y^2=l_1^2+l_2^2-2l_1{l}_2\cos (180^{\circ }-{\theta }_2)$，根据三角函数的诱导公式可以得到$cos{\theta }_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}$
进一步，根据余弦定理有$\cos \psi =\frac{l_2^2-(x^2+y^2)-l_1^2}{-2l_1\sqrt{x^2+y^2}}$，${\theta }_2<0$（蓝色杆的情形），$\beta =atan2(y,x)$，此时，${\theta }_1=atan2(y,x)+\psi$；${\theta }_2>0$（绿色杆的情形），此时${\theta }_1=atan2(y,x)-\psi$；于是得到${\theta }_1$表达式：
$${\theta }_1=\{\begin{array}{ll}atan2(y,x)+\psi ,& {\theta }_2<0\\ atan2(y,x)-\psi ,& {\theta }_2>0\end{array}，$$
求${\theta }_3$ ，${\theta }_3=\varphi -{\theta }_1-{\theta }_2$。

给出具体数值，代入公式求解即可：

In-Video Quiz: 选B, 此时，${\theta }_2<0$，排除C和A，${\theta }_1=atan2(y,x)+\psi$，atan2(5, 3) = 59，所以${\theta }_1=78.4$，又有$\varphi ={\theta }_1+{\theta }_2+{\theta }_3$，可以得到${\theta }_3$的值。

代数法求解： 根据两个矩阵建立方程式
对于图中的平面三转动关节机械臂，末端位置 $(x,y)$ 可以写成：$x=l_1\cos {\theta }_1+l_2\cos ({\theta }_1+{\theta }_2)$，$y=l_1\sin {\theta }_1+l_2\sin ({\theta }_1+{\theta }_2)$，为了简化表达，有$c_1=\cos {\theta }_1,s_1=\sin {\theta }_1$，$c_{12}=\cos ({\theta }_1+{\theta }_2),s_{12}=\sin ({\theta }_1+{\theta }_2)$，所以末端位置重新写为$x=l_1{c}_1+l_2{c}_{12}$，$y=l_1{s}_1+l_2{s}_{12}$。分别对$x$和$y$平方：
$$x^2=(l_1{c}_1+l_2{c}_{12}{)}^2,y^2=(l_1{s}_1+l_2{s}_{12}{)}^2$$
展开得到：
$$x^2=l_1^2{c}_1^2+l_2^2{c}_{12}^2+2l_1{l}_2{c}_1{c}_{12}，y^2=l_1^2{s}_1^2+l_2^2{s}_{12}^2+2l_1{l}_2{s}_1{s}_{12}$$
将二者相加得到：
$$\begin{array}{rl}{x}^2+y^2=& l_1^2{c}_1^2+l_2^2{c}_{12}^2+2l_1{l}_2{c}_1{c}_{12}\\ & +l_1^2{s}_1^2+l_2^2{s}_{12}^2+2l_1{l}_2{s}_1{s}_{12}\end{array}$$
同类项整理有：
$$\begin{array}{rl}{x}^2+y^2=& l_1^2(c_1^2+s_1^2)+l_2^2(c_{12}^2+s_{12}^2)\\ & +2l_1{l}_2(c_1{c}_{12}+s_1{s}_{12})\end{array}$$
上式可以简化为：
$$x^2+y^2=l_1^2+l_2^2+2l_1{l}_2(c_1{c}_{12}+s_1{s}_{12})$$
令$$\alpha ={\theta }_1+{\theta }_2,\beta ={\theta }_1$$
上式变为：
$$\begin{array}{rl}{c}_1{c}_{12}+s_1{s}_{12}& =\cos {\theta }_1\cos ({\theta }_1+{\theta }_2)\\ & +\sin {\theta }_1\sin ({\theta }_1+{\theta }_2)\end{array}$$
根据余弦差角公式可得：
$$\begin{array}{rl}{c}_1{c}_{12}+s_1{s}_{12}& =\cos \left[({\theta }_1+{\theta }_2)-{\theta }_1\right]\\ & =\cos {\theta }_2\end{array}$$
所以得到$c_1{c}_{12}+s_1{s}_{12}=c_2$，也就是
$$(x^2)+(y^2)=l_1^2+l_2^2+2l_1{l}_2\cos {\theta }_2$$
求解${\theta }_2$
$${\theta }_2={\cos }^{-1}\left(\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}\right)$$

将已经求得的${\theta }_2$代入，$x=l_1{c}_1+l_2{c}_{12}=l_1{c}_1+l_2(c_1{c}_2-s_1{s}_2)=(l_1+l_2{c}_2)c_1-l_2{s}_2{s}_1$
$y=l_1{s}_1+l_2{s}_{12}=l_1{s}_1+l_2(s_1{c}_2+c_1{s}_2)=(l_1+l_2{c}_2)s_1+l_2{s}_2{c}_1$，定义$k_1=l_1+l_2{c}_2,k_2=l_2{s}_2$，原位置方程变为：
$$\{\begin{array}{l}x=k_1\cos {\theta }_1-k_2\sin {\theta }_1\\ y=k_1\sin {\theta }_1+k_2\cos {\theta }_1\end{array}$$
对 $k_1$和 $k_2$进行极坐标变换，$k_1=r\cos \gamma$，$k_2=r\sin \gamma$，代入$x,y$中
$$\{\begin{array}{l}x=r\cos \gamma \cos {\theta }_1-r\sin \gamma \sin {\theta }_1,\\ y=r\cos \gamma \sin {\theta }_1+r\sin \gamma \cos {\theta }_1.\end{array}$$
两边同时除以 r：
$$\{\begin{array}{l}\frac{x}{r}=\cos \gamma \cos {\theta }_1-\sin \gamma \sin {\theta }_1=\cos (\gamma +{\theta }_1),\\ \frac{y}{r}=\cos \gamma \sin {\theta }_1+\sin \gamma \cos {\theta }_1=\sin (\gamma +{\theta }_1).\end{array}$$
求解${\theta }_1$，得到：
$$\gamma +{\theta }_1=atan2\left(\frac{y}{r},\frac{x}{r}\right)=atan2(y,x)$$
$$\boxed{{\theta }_1=atan2(y,x)-atan2(k_2,k_1)}$$
求解${\theta }_3$: ${\theta }_3=\varphi -{\theta }_1-{\theta }_2$

4-3 Pieper’s Solution - 1

若6自由度的机械臂有三个连续的轴交在同一点，则手臂有解析解，坐标系{6}的原点相对于坐标系{0}的表达等于坐标系{4}的原点相对于坐标系{0}的表达：

为了求解${\theta }_1$、${\theta }_2$、${\theta }_3$，让${\theta }_1$、${\theta }_2$、${\theta }_3$层层分离，根据顺向运动学的知识，首先标准的DH变换矩阵：
$${}_i^{i-1}T=\left[\begin{array}{cccc}{c}_i& -s_i& 0& a_{i-1}\\ s_i{c}_{{\alpha }_{i-1}}& c_i{c}_{{\alpha }_{i-1}}& -s_{{\alpha }_{i-1}}& -s_{{\alpha }_{i-1}}{d}_i\\ s_i{s}_{{\alpha }_{i-1}}& c_i{s}_{{\alpha }_{i-1}}& c_{{\alpha }_{i-1}}& c_{{\alpha }_{i-1}}{d}_i\\ 0& 0& 0& 1\end{array}\right]$$
对于末端坐标，可以写为：
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]={}^0{P}_{4,\mathrm{ORG}}={}_1^{0}T{}_2^{1}T{}_3^{2}T{}_4^{3}T{}^4{P}_{\mathrm{ORG}}.$$
由于$${}^4{P}_{\mathrm{ORG}}=\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right],$$于是得到,
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]={}_1^{0}T{}_2^{1}T{}_3^{2}T\left[\begin{array}{c}{a}_3\\ -d_4{s}_{{\alpha }_3}\\ d_4{c}_{{\alpha }_3}\\ 1\end{array}\right].$$
为了实现变量分离，定义关于${\theta }_3$的函数:
$$\left[\begin{array}{c}{f}_1({\theta }_3)\\ f_2({\theta }_3)\\ f_3({\theta }_3)\\ 1\end{array}\right]={}_3^{2}T\left[\begin{array}{c}{a}_3\\ -d_4{s}_{{\alpha }_3}\\ d_4{c}_{{\alpha }_3}\\ 1\end{array}\right].$$于是，整体可以写为：
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]={}_1^{0}T{}_2^{1}T\left[\begin{array}{c}{f}_1({\theta }_3)\\ f_2({\theta }_3)\\ f_3({\theta }_3)\\ 1\end{array}\right].$$
展开$f({\theta }_3)$，得到：
$$\begin{array}{rl}{f}_1({\theta }_3)& =a_3{c}_3+d_4{s}_{{\alpha }_3}{s}_3+a_2,\\ f_2({\theta }_3)& =a_3{c}_{{\alpha }_2}{s}_3-d_4{s}_{{\alpha }_3}{c}_{{\alpha }_2}{c}_3-d_3{s}_{{\alpha }_2},\\ f_3({\theta }_3)& =a_3{s}_{{\alpha }_2}{s}_3-d_4{s}_{{\alpha }_3}{s}_{{\alpha }_2}{c}_3+d_4{c}_{{\alpha }_2}{c}_{{\alpha }_3}+d_3{c}_{{\alpha }_2}.\end{array}$$

将${}_2^{1}T$作用到上式，可定义：
$$\left[\begin{array}{c}{g}_1({\theta }_2,{\theta }_3)\\ g_2({\theta }_2,{\theta }_3)\\ g_3({\theta }_2,{\theta }_3)\\ 1\end{array}\right]={}_2^{1}T\left[\begin{array}{c}{f}_1({\theta }_3)\\ f_2({\theta }_3)\\ f_3({\theta }_3)\\ 1\end{array}\right].$$
于是，
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]={}_1^{0}T\left[\begin{array}{c}{g}_1({\theta }_2,{\theta }_3)\\ g_2({\theta }_2,{\theta }_3)\\ g_3({\theta }_2,{\theta }_3)\\ 1\end{array}\right].$$
再作用${}_1^{0}T$，得到：
$$\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}{c}_1{g}_1-s_1{g}_2\\ s_1{g}_1+c_1{g}_2\\ g_3\\ 1\end{array}\right].$$
这时，展开$g_1$，$g_2$，$g_3$，可以得到：
$$\begin{array}{rl}{g}_1({\theta }_2,{\theta }_3)& =c_2{f}_1-s_2{f}_2+a_1,\\ g_2({\theta }_2,{\theta }_3)& =s_2{c}_{{\alpha }_1}{f}_1+c_2{c}_{{\alpha }_1}{f}_2-s_{{\alpha }_1}{f}_3-d_2{s}_{{\alpha }_1},\\ g_3({\theta }_2,{\theta }_3)& =s_2{s}_{{\alpha }_1}{f}_1+c_2{s}_{{\alpha }_1}{f}_2+c_{{\alpha }_1}{f}_3+d_2{c}_{{\alpha }_1}.\end{array}$$
构造平方和消去${\theta }_1$
$$r=x^2+y^2+z^2=g_1^2+g_2^2+g_3^2（r仅为{\theta }_2和{\theta }_3的函数）.$$将$g_1$，$g_2$，$g_3$展开得到：
$$\begin{array}{rl}r& =f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2{f}_3+2a_1(c_2{f}_1-s_2{f}_2).\end{array}$$
定义定义辅助变量:
$$\begin{array}{rl}{k}_1({\theta }_3)& =f_1,\\ k_2({\theta }_3)& =-f_2,\\ k_3({\theta }_3)& =f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2{f}_3.\end{array}$$
$r$最终写为：
$$r=2a_1(k_1{c}_2+k_2{s}_2)+k_3（r仅为{\theta }_2和{\theta }_3的函数）.$$

在上面的讨论当中，已经得到了$r$的表达式，$r=x^2+y^2+z^2$是已知的，$z$也是已知的且可以表述为：
$$z=g_3=(k_1{s}_2-k_2{c}_2)s_{{\alpha }_1}+k_4(z也仅为{\theta }_2和{\theta }_3的函数)$$
其中$$\begin{array}{rl}{k}_1({\theta }_3)& =f_1,\\ k_2({\theta }_3)& =-f_2,\\ k_4({\theta }_3)& =f_3{c}_{{\alpha }_1}+d_2{c}_{{\alpha }_1}.\end{array}$$
联立$r$和$z$，得到一个关于${\theta }_2$和${\theta }_3$的二元一次方程组：
$$\{\begin{array}{l}r=2a_1(k_1{c}_2+k_2{s}_2)+k_3,\\ z=(k_1{s}_2-k_2{c}_2)s_{{\alpha }_1}+k_4.\end{array}$$这时，求解${\theta }_3$：

1. 若$a_1=0,$则$r=k_3({\theta }_3)=f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2{f}_3.$
2. 若$s_{{\alpha }_1}=0,$ 则$z=k_4({\theta }_3)=f_3{c}_{{\alpha }_1}+d_2{c}_{{\alpha }_1}.$
3. 若上述两个特殊条件均不满足，则将两个方程平方相加，可得到$\frac{(r-k_3{)}^2}{4a_1^2}+\frac{(z-k_4{)}^2}{s_{{\alpha }_1}^2}=k_1^2+k_2^2.$
   
   求解${\theta }_2$和${\theta }_1$：
   在求得${\theta }_3$后，将其代入$r$，求解${\theta }_2$：$r=2a_1\left(k_1{c}_2+k_2{s}_2\right)+k_3,$；随后，将已经求得的${\theta }_2$和${\theta }_3$代入$x$中，求解${\theta }_1$：$x=c_1{g}_1({\theta }_2,{\theta }_3)-s_1{g}_2({\theta }_2,{\theta }_3)$。
   至此，Pieper 方法的位置逆解推导完成。整个推导采用了变量分离(Variable Separation)的思想。首先利用齐次变换矩阵 ${}_4^{3}T$ 的第四列，将末端位置表示为仅与关节变量 ${\theta }_3$ 有关的中间变量；随后进一步构造关于 ${\theta }_2$ 和 ${\theta }_3$ 的中间变量，并利用 $r=x^2+y^2+z^2$ 消去关节变量 ${\theta }_1$，再结合末端高度坐标 $z$ 进一步消去 ${\theta }_2$，最终得到仅含未知量 ${\theta }_3$ 的非线性方程。求得 ${\theta }_3$ 后，再依次回代即可求解 ${\theta }_2$ 和 ${\theta }_1$，从而完成位置逆运动学的解析求解。

4-3 Pieper’s Solution - 2

在 4-3 Pieper’s Solution - 1中已经将${\theta }_3$、${\theta }_2$和${\theta }_1$求解出来了，现在需要求解${\theta }_4$、${\theta }_5$和${\theta }_6$。
由之前所述，后三个关节的三个转轴相交于一点（球腕），这三个关节的旋转角度可以通过Z-Y-Z Euler Angles（林沛群机器人之运动学笔记—2）进行求解，现在需要找到DH 定义与欧拉角定义对应关系。由于两种方法建立坐标系的方式不同，导致第 4、6 个关节角相差 180度，而第 5 个关节角保持不变。因此，在利用 Z-Y-Z 欧拉角分解姿态矩阵求解腕部关节时，需要先完成这种角度映射，才能得到符合机器人 DH 模型定义的 ${\theta }_4$、${\theta }_5$和${\theta }_6$​。最后得到：
$$\begin{array}{rl}{\theta }_4^{\prime }& ={\theta }_4+\pi ,\\ {\theta }_5^{\prime }& ={\theta }_5,\\ {\theta }_6^{\prime }& ={\theta }_6+\pi .\end{array}$$
其中${\theta }_4,{\theta }_5,{\theta }_6$由DH定义，$({\theta }_4^{\prime },{\theta }_5^{\prime },{\theta }_6^{\prime })$由Z-Y-Z 欧拉角定义，后续利用$${}_6^{3}R=R_z\left({\theta }_4^{\prime }\right)R_y\left({\theta }_5^{\prime }\right)R_z\left({\theta }_6^{\prime }\right)$$
即可求出欧拉角，再根据上述关系恢复机器人的关节角。