机械臂顺向运动学的DH表达法

最近刚学了机器人学中关于机械臂顺向运动学的DH表达法，来这里做个笔记吧。写的是Craig方法而不是Standard法

首先是关于旋转矩阵和平移矩阵的公式

1 绕Z轴旋转，$R_{Z(\alpha )}=\left[\begin{array}{ccc}cos\alpha & -sin\alpha & 0\\ sin\alpha & cos\alpha & 0\\ 0& 0& 1\end{array}\right]$ 延Z轴平移 $P_X=\left[\begin{array}{c}0\\ 0\\ Z\end{array}\right]$

2 绕Y轴旋转，$R_{Y(\beta )}=\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]$ 延X轴平移 $P_Y=\left[\begin{array}{c}0\\ Y\\ 0\end{array}\right]$

3 绕X轴旋转，$R_{X(\gamma )}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & -sin\gamma \\ 0& sin\gamma & cos\gamma \end{array}\right]$ 延X轴平移 $P_X=\left[\begin{array}{c}X\\ 0\\ 0\end{array}\right]$

两个坐标系下的旋转矩阵 ${}_B^A\text{R}=\left[\begin{array}{ccc}{}^A{\text{X}}_B& {}^A{\text{Y}}_B& {}^A{\text{Z}}_B\end{array}\right]$其中${}^A{\text{X}}_B$是B坐标系X轴在A坐标系中的表达。
B坐标系的原点在A坐标系的位置为(X,Y,Z),则平移矩阵就为 ${}^A{\text{P}}_{{}_{Borg}}=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$

所以，点P在 B坐标系下的表示为${}^B\text{P}$,则在A坐标系中的表示为$\left[\begin{array}{c}{}^A\text{P}\\ 1\end{array}\right]=\left[\begin{array}{cc}{}^{{}_B^A\text{R}}& {}^A{\text{P}}_{{}_{Borg}}\\ 000& 1\end{array}\right]\left[\begin{array}{c}{}^B\text{P}\\ 1\end{array}\right]$

其次，建立坐标系

上图是用Craig法建立的坐标系，图中第i-1轴的连杆在轴的后面，而不是在前面。
建立坐标系的方法为：Z轴的方向是转轴的方向，$X_{i-1}$的方向是延轴$Z_{i-1}$和$Z_i$的公垂线的方向，$Y_{i-1}$的方向根据右手定则确定。
接着分析我们需要知道的4个参数
Ⅰ，${\alpha }_{i-1}$ :以$X_{i-1}$的方向看，$Z_{i-1}$和$Z_i$间的夹角。
Ⅱ ，$a_{i-1}$：沿着$X_{i-1}$的方向看，$Z_{i-1}$和$Z_i$间的距离。
Ⅲ，${\theta }_i$：以 Z i {Z _{i}}