一、机器人的正逆运动学

　　假设有一个构型已知的机器人，即它的所有连杆长度和关节角度都是已知的，则计算机器人手的位姿就称为正运动学分析。即已知所有机器人的关节变量，用正运动学方程就能计算出任一瞬间机器人的位姿。如果想要将机器人手放在期望的位姿，就必须知道机器人的每一个连杆的长度和关节的角度，才能将手定位在期望的位姿，这就称为逆运动学分析。

二、姿态的正逆运动学方程

　　假设固连在机器人手上的运动坐标系在参考坐标系中已经运动到期望的位置上，但它仍然平行于参考坐标系，所以下一步要在不改变位置的情况下，适当的旋转坐标系从而达到所预期的姿态。这时只能绕当前坐标系而不能绕参考坐标系旋转，因为绕参考坐标系旋转将会改变当前坐标系原点的位置。　

2.1 滚动角、俯仰角和偏航角

　　滚动角、俯仰角和偏航角分别绕当前$a、o$和$n$轴的3个顺序旋转所得，能够把机器人手调整到所期望的姿态。此时当前的坐标系一定要平行于参考坐标系，且RPY（$Roll$-滚动角，$Pitch$-俯仰角，$Yaw$-偏航角）的旋转运动都是相对于当前的运动轴的。所以，需要右乘所有由RPY和其他旋转所产生的与姿态改变相关的矩阵。

• 绕$a$轴（运动坐标系的$z$轴）旋转$\phi_a$，称为滚动
• 绕$o$轴（运动坐标系的$y$轴）旋转$\phi_o$，称为俯仰
• 绕$n$轴（运动坐标系的$x$轴）旋转$\phi_n$，称为偏航
  
  　　表示PRY姿态变换的矩阵为
  $$PRY(\phi_a,\phi_o,\phi_n)=Rot(a,\phi_a)Rot(o,\phi_o)Rot(n,\phi_n)=$$

$$\left[ \begin{matrix} cos\phi_acos\phi_o & cos\phi_asin\phi_osin\phi_n -sin\phi_acos\phi_n& cos\phi_asin\phi_ocos\phi_n+sin\phi_asin\phi_n &0\\ sin\phi_acos\phi_o & sin\phi_asin\phi_osin\phi_n +cos\phi_acos\phi_n &sin\phi_asin\phi_ocos\phi_n-cos\phi_asin\phi_n &0\\ -sin\phi_o & cos\phi_osin\phi_n & cos\phi_ocos\phi_n &0 \\ 0 & 0&0 &1 \end{matrix} \right]$$
　　该矩阵表示了仅由RPY引起的姿态变化。该坐标系相对于参考坐标系的最终位姿是表示位置变换的矩阵和RPY矩阵的乘积。
　　RPY的逆运动学方程的解：
$$\begin{cases} \phi_a = ATAN2(n_y,n_x)\\ \phi_o = ATAN2[-n_z,(n_xcos\phi_a+n_ysin\phi_a)]\\ \phi_n=ATAN2[(-a_ycos\phi_a+a_xsin\phi_a),(o_ycos\phi_a-o_xsin\phi_a)] \end{cases}$$
　　ATAN2返回角度的反正切值，默认返回弧度值。在python中调用math中的atan2()函数即可。
　　例1：下面给出了一个直角坐标-PRY型机器人手所期望的最终位姿，求所需的PRY角和位移。
$$^RT_p= \left[ \begin{matrix} n_x & o_x& a_x &p_x\\ n_y & o_y& a_y&p_y\\ n_z& o_z& a_z &p_z \\ 0 & 0&0 &1 \end{matrix} \right] = \left[ \begin{matrix} 0.354& -0.674& 0.649&4.33\\ 0.505& 0.722& 0.475&2.50\\ -0.788& 0.160& 0.595&8 \\ 0 & 0&0 &1 \end{matrix} \right]$$ 　
　　解：根据逆运动学的解的公式，可得：
$$\begin{cases} \phi_a = ATAN2(n_y,n_x)= ATAN2(0.505,0.354)=55^o\\ \phi_o=ATAN2(-n_z,(n_xcos\phi_a+n_ysin\phi_a))=ATAN2(0.788,0.616)=52^o\\ \phi_n=ATAN2((-a_ycos\phi_a+a_xsin\phi_a),(o_ycos\phi_a-o_xsin\phi_a))=ATAN2(0.259,0.966)=15^o\\ p_x=4.33,p_y=2.5,p_z=8\\ \end{cases}$$
　　注：计算ATAN2的python代码　　

import math
math.atan2(0.505,0.354)*180/math.pi

2.2 欧拉角

　　除了最后的旋转是绕当前的$a$轴外，欧拉角的其他方面均与PRY相似。　　　

• 绕$a$轴（运动坐标系的$z$轴）旋转$\phi$
• 接着绕$o$轴（运动坐标系的$y$轴）旋转$\theta$
• 最后再绕$a$轴（运动坐标系的$z$轴）旋转$\psi$
  　　
  　　表示欧拉角姿态变化的矩阵是
  　　 $$Euler(\phi,\theta,\psi)=Rot(a,\phi)Rot(o,\theta)Rot(n,\psi)=$$

$$\left[ \begin{matrix} cos\phi cos\theta cos\psi -sin\phi sin\psi&-cos\phi cos\theta sin\psi - sin\phi cos\psi&cos\phi sin\theta &0\\ sin\phi cos\theta cos\psi +cos\phi sin\psi&-sin\phi cos\theta sin\psi + cos\phi cos\psi&sin\phi sin\theta &0\\ -sin\theta cos\psi&sin\theta sin\psi&cos\theta &0\\ 0 & 0&0 &1 \end{matrix} \right]$$
　　该矩阵表示了仅由欧拉角引起的姿态变化。该坐标系相对于参考坐标系的最终位姿是表示位置变换的矩阵和欧拉角矩阵的乘积。
　　欧拉角的逆运动学方程的解：
$$\begin{cases} \phi = ATAN2(a_y,a_x)\\ \psi = ATAN2[(-n_xsin\phi+n_ycos\phi),(-o_xsin\phi+o_ycos\phi)]\\ \theta=ATAN2[(a_xcos\phi),a_z] \end{cases}$$