机器人学回炉重造（1-2）：各种典型机械臂的正运动学建模（标准D-H法）

写在前面

本文所有机械臂均采用标准D-H建模法，该方法的建立过程可参考机器人学回炉重造（1）：正运动学、标准D-H法与改进D-H法的区别与应用（附ABB机械臂运动学建模matlab代码）

三连杆平面机械臂

坐标系建立如下图所示。其实这图有些误导人，不会出错的建模是在将各个关节变量角${\theta }_i$均置0 的情况下进行的，也就是图中红线画的那样。

D-H坐标参数如下表所示。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	$a_1$	0	0	${\theta }_1$
2	$a_2$	0	0	${\theta }_2$
3	$a_3$	0	0	${\theta }_3$

平行四边形操作臂

先讲闭链结构及其正运动学问题的解决办法。

闭链结构

大多数情况下讨论的基于D-H法的直接运动学方法都是利用了开链机械臂的固有递归特性，根据下面的技术可以将该方法扩展到包含闭链结构的机械手的情况。

考虑一个由n+1个连杆组成的闭链操作臂。由于闭环的存在，关节$l$的数量肯定会比$n$大；尤其是，能够理解得到闭环的数量等于$l-n$。

如上图所示，连杆0到i通过第一个$i$关节相继连接，如同开链运动结构一样。连杆$i$和连杆$i+1^{\prime }$通过关节$i+1^{\prime }$连接，连杆$i$和连杆$i+1^{\prime \prime }$通过关节$i+1^{\prime \prime }$连接；假定关节$i+1^{\prime }$和关节$i+1^{\prime \prime }$的轴线对齐。尽管未在图中表示，连杆$i+1^{\prime }$和连杆$i+1^{\prime \prime }$是闭链的组成结构。特别的，连杆$i+1^{\prime }$通过关节$i+2^{\prime }$进一步与连杆$i+2^{\prime }$连接，以此类推，直到连杆$j$连接关节$j$。同样地，连杆$i+1^{\prime \prime }$通过关节$i+2^{\prime \prime }$进一步与连杆$i+2^{\prime \prime }$连接，以此类推，直到连杆$k$连接关节$k$。最终，连杆$j$和连杆$k$通过关节$j+1$连接到一起并组成闭链。通常情况下，$j\ne k$。

为了将框架附加到各个连杆上并且运用D-H规则，要考虑一条封闭的运动链。假设闭合链可以在关节$j+1$处（即连杆$j$和连杆k之间的关节）断开。可以获得一条等效的树状运动学开链，因此可以如上图所示定义连杆坐标系。因为连杆0到$i$在树的两个分叉结构之前，因此它们在考虑范围之外。同样地，连杆$j+1$到$n$也被忽略了。注意到，坐标系$i$的$z_i$轴与关节$i+1^{\prime }$和关节$i+1^{\prime \prime }$的轴对齐。

由此可见，坐标系$j$相对于坐标系$i$的位姿可以通过齐次变换得到
$$A_j^i\left({\mathbf{q}}^{\prime }\right)={\mathbf{A}}_{i+1^{\prime }}^i\left(q_{i+1^{\prime }}\right)\dots {\mathbf{A}}_j^{j-1}\left(q_j\right)\dots \dots （1）$$
其中，${\mathbf{q}}^{\prime }={\left[\begin{array}{lll}{q}_{i+1\prime },& \dots & q_j\end{array}\right]}^T$。同样地，坐标系$k$相对于坐标系$i$的位姿可由以下得出
$$A_i^k\left({\mathbf{q}}^{\prime \prime }\right)={\mathbf{A}}_{i+1^{\prime \prime }}^i\left(q_{i+1^{\prime \prime }}\right)\dots {\mathbf{A}}_k^{k-1}\left(q_k\right)\dots \dots （2）$$
其中，${\mathbf{q}}^{\prime \prime }={\left[\begin{array}{lll}{q}_{i+1\prime \prime },& \dots & q_k\end{array}\right]}^T$。

因为连杆$j$和连杆$k$通过关节$j+1$和彼此连接，因此分析坐标系$j$和坐标系$k$之间的相互位姿关系是有价值的，如下图所示。注意到，因为连杆$j$和连杆$k$连接以形成一个闭链，轴$z_j$和轴$z_k$是对齐的。

因此，可得到坐标系$j$和坐标系$k$之间的方向约束如下公式所示
$$z_j^i\left({\mathbf{q}}^{\prime }\right)={\mathbf{z}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\dots \dots （3）$$
其中，两个轴的单位向量均相对于坐标系$i$。

此外，如果关节$j+1$是平移关节，轴$x_j$和轴$x_k$之间形成的关节角${\theta }_{jk}$是固定不变的，根据式（3），可得到如下约束：
$${\mathbf{x}}_j^{iT}\left({\mathbf{q}}^{\prime }\right){\mathbf{x}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)=\cos {\theta }_{jk}\dots \dots （4）$$
显然，没有必要在轴$y_j$和轴$y_k$上施加类似的约束，因为这是多余的。

至于坐标系$j$和坐标系$k$之间的位置约束，让$p_j^i$和$p_k^i$分别表示坐标系$j$和坐标系$k$的原点相对于坐标系$i$的位置。通过将坐标系$k$到坐标系$j$的距离向量投影到坐标系$j$上，可以得到如下约束：
$${\mathbf{R}}_i^j\left({\mathbf{q}}^{\prime }\right)\left({\mathbf{p}}_j^i\left({\mathbf{q}}^{\prime }\right)-{\mathbf{p}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\right)={\left[\begin{array}{lll}0& 0& d_{jk}\end{array}\right]}^T\dots \dots （5）$$
其中，${\mathbf{R}}_i^j={\mathbf{R}}_j^{iT}$表示坐标系$i$相对于坐标系$j$的方向余弦矩阵。此时，如果关节$j+1$是旋转关节，那么$d_{jk}$是沿着轴$z_j$的固定偏移量；因此，式（5）中的三个等式可以充分描述位置约束。但是，如果关节$j+1$是移动关节，那么$d_{jk}$是变量。因此，只有式（5）中的前两个等式可以描述位置约束，即
$$\left[\begin{array}{c}{x}_j^{iT}\left({\mathbf{q}}^{\prime }\right)\\ {\mathbf{y}}_j^{iT}\left({\mathbf{q}}^{\prime }\right)\end{array}\right]\left({\mathbf{p}}_j^i\left({\mathbf{q}}^{\prime }\right)-{\mathbf{p}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\right)=\left[\begin{array}{l}0\\ 0\end{array}\right]\dots \dots （6）$$
其中，${\mathbf{R}}_{\mathbf{j}}^{\mathbf{i}}=[x_j^i,y_j^i,z_j^i]$。

综上，如果关节$j+1$是旋转关节，则约束可表示为
$$\{\begin{array}{l}{\mathbf{R}}_i^j\left({\mathbf{q}}^{\prime }\right)\left({\mathbf{p}}_j^i\left({\mathbf{q}}^{\prime }\right)-{\mathbf{p}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\right)={\left[\begin{array}{lll}0& 0& d_{jk}\end{array}\right]}^T\\ {\mathbf{z}}_j^i\left({\mathbf{q}}^{\prime }\right)={\mathbf{z}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\end{array}\dots \dots （7）$$
如果关节$j+1$是移动关节，则约束可表示为
$$\{\begin{array}{l}\left[\begin{array}{c}{\mathbf{x}}_j^{iT}\left({\mathbf{q}}^{\prime }\right)\\ {\mathbf{y}}_j^{iT}\left({\mathbf{q}}^{\prime }\right)\end{array}\right]\left({\mathbf{p}}_j^i\left({\mathbf{q}}^{\prime }\right)-{\mathbf{p}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\right)=\left[\begin{array}{l}0\\ 0\end{array}\right]\\ {\mathbf{z}}_j^i\left({\mathbf{q}}^{\prime }\right)={\mathbf{z}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)\\ {\mathbf{x}}_j^{iT}\left({\mathbf{q}}^{\prime }\right){\mathbf{x}}_k^i\left({\mathbf{q}}^{\prime \prime }\right)=\cos {\vartheta }_{jk}\end{array}\dots \dots （8）$$
无论哪种情况，都必须满足六个等式条件。应该解决这些问题，以便从${\mathbf{q}}^{\prime }$和${\mathbf{q}}^{\prime \prime }$组成部分（闭链的自由度特征）中敏锐地选择数量减少独立关节变量。自然而然地，这些变量是被激活的关节，而该链上的其他关节（包括切割关节）通常是未被激活的。这些独立变量与以上分析中未涉及的其余关节变量一起构成了关节向量$\mathbf{q}$，其可由正运动学公式计算得到
$${\mathbf{T}}_n^0(\mathbf{q})={\mathbf{A}}_i^0{\mathbf{A}}_j^i{\mathbf{A}}_n^j\dots \dots （9）$$
这是从坐标系$j$恢复链关闭后的连续转换序列。

通常，除非机械臂具有简单的运动学结构，否则无法保证在封闭形式下解决约束问题。换句话说，对于具有特定集合形状（例如平面结构）的给定机械臂，上述某些等式特性可能变得具有依赖性。因此，独立等式的数量少于六个，这应该会很容易解决。

总而言之，有必要勾勒出使用D-H准则为闭链机械臂计算正运动学函数过程的操作步骤。

1. 在闭链中，选择一个未激活的关节。假设将关节切开，以便在树形结构中获得开链；
2. 根据D-H准则计算几次变换矩阵；
3. 找到通过切割关节连接的两个坐标系之间的等式约束；
4. 解出减少关节变量个数的约束；
5. 通过上述关节变量计算得到齐次变换矩阵，并通过组合从基座标系到末端执行器坐标系的各种变换来计算得到正运动学函数。

例：平行四边形操作臂

考虑如上图所示的平行四边形机械臂。在闭合链中，前两个关节分别将连杆$1^{\prime }$和连杆$1^{\prime \prime }$连接到连杆$0$。选择第4个关节作为切割关节，并相应地建立了连杆坐标系。D-H参数如下表所示，其中根据平行四边形结构可以看出$a_{1^{\prime }}=a_{3^{\prime }}$，$a_{2^{\prime }}=a_{1^{\prime \prime }}$。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1‘	$a_{1^{\prime }}$	0	0	${\theta }_{1^{\prime }}$
2’	$a_{2^{\prime }}$	0	0	${\theta }_{2^{\prime }}$
3‘	$a_{3^{\prime }}$	0	0	${\theta }_{3^{\prime }}$
1’‘	$a_{1^{\prime \prime }}$	0	0	${\theta }_{1^{\prime \prime }}$
4	$a_4$	0	0	0

经过上文闭链结构的分析可知，可分别得到树状结构两个分支的坐标变换如下
$${\mathbf{A}}_{3^{\prime }}^0\left({\mathbf{q}}^{\prime }\right)={\mathbf{A}}_{1^{\prime }}^0{\mathbf{A}}_{2^{\prime }}^{1^{\prime }}{\mathbf{A}}_{3^{\prime }}^{2^{\prime }}=\left[\begin{array}{cccc}{c}_{1^{\prime }{2}^{\prime }{3}^{\prime }}& -s_{1^{\prime }{2}^{\prime }{3}^{\prime }}& 0& a_{1^{\prime }{c}_1^{\prime }}+a_{2^{\prime }}{c}_{1^{\prime }{2}^{\prime }}+a_3^{\prime }{c}_{1^{\prime }{2}^{\prime }{3}^{\prime }}\\ s_{1^{\prime }{2}^{\prime }{3}^{\prime }}& c_{1^{\prime }{2}^{\prime }{3}^{\prime }}& 0& a_{1^{\prime }}{s}_{1^{\prime }}+a_{2^{\prime }}{s}_{1^{\prime }{2}^{\prime }}+a_{3^{\prime }}{s}_{1^{\prime }{2}^{\prime }{3}^{\prime }}\\ 0& 0& 1& 0\\ 0& 0& p& 1\end{array}\right]\dots \dots （10）$$
其中，$\mathbf{q}’=[{\theta }_{1^{\prime }},{\theta }_{2^{\prime }},{\theta }_{3^{\prime }}]$，并且
$${\mathbf{A}}_{1^n}^0\left(q^{\prime \prime }\right)=\left[\begin{array}{cccc}{c}_{1\prime \prime }& -s_{1\prime \prime }& 0& a_{1\prime \prime }{c}_{1\prime \prime }\\ s_{1\prime \prime }& c_{1\prime \prime }& 0& a_{1\prime \prime }{s}_{1\prime \prime }\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （11）$$
其中，$q^{\prime \prime }={\theta }_{1^{\prime \prime }}$。最后一个连杆的齐次变换矩阵为
$$A_4^{3^{\prime }}=\left[\begin{array}{llll}1& 0& 0& a_4\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （12）$$
根据式（7）可以得到位置约束为（$d_{3^{\prime }{1}^{\prime \prime }}=0$）
$${\mathbf{R}}_0^{3^{\prime }}\left({\mathbf{q}}^{\prime }\right)\left({\mathbf{p}}_{3^{\prime }}^0\left({\mathbf{q}}^{\prime }\right)-{\mathbf{p}}_{1^{\prime \prime }}^0\left({\mathbf{q}}^{\prime \prime }\right)\right)={\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\dots \dots （13）$$
因为$a_{1^{\prime }}=a_{3^{\prime }}$，$a_{2^{\prime }}=a_{1^{\prime \prime }}$，因此可以提取两个独立变量
$$\begin{array}{rl}& a_{1^{\prime }}\left(c_{1^{\prime }}+c_{1^{\prime }{2}^{\prime }{3}^{\prime }}\right)+a_{1^{\prime \prime }}\left(c_{1^{\prime }{2}^{\prime }}-c_{1^{\prime \prime }}\right)=0\\ & a_{1^{\prime }}\left(s_{1^{\prime }}+s_{1^{\prime }{2}^{\prime }{3}^{\prime }}\right)+a_{1^{\prime \prime }}\left(s_{1^{\prime }{2}^{\prime }}-s_{1^{\prime \prime }}\right)=0\end{array}$$
为了满足$a_{1^{\prime }}$和$a_{1^{\prime \prime }}$的任意性，必须有
$$\begin{array}{rl}& {\theta }_{2^{\prime }}={\theta }_{1^{\prime \prime }}-{\theta }_{1^{\prime }}\\ & {\theta }_{3^{\prime }}=\pi -{\theta }_{2^{\prime }}=\pi -{\theta }_{1^{\prime \prime }}+{\theta }_{1^{\prime }}\end{array}\dots \dots （14）$$
因此，可以得到关节变量为$\mathbf{q}=[{\theta }_{1^{\prime }},{\theta }_{1^{\prime \prime }}{]}^T$。这些关节是被激活关节的自然选择。将${\theta }_{2^{\prime }}$和${\theta }_{3^{\prime }}$的表达式带入齐次变换矩阵${\mathbf{A}}_{3^{\prime }}^0$中并计算正运动学函数，如下
$${\mathbf{T}}_4^0(\mathbf{q})={\mathbf{A}}_{3^{\prime }}^0(\mathbf{q}){\mathbf{A}}_4^{3^{\prime }}={\left[\begin{array}{cccc}-c_{1^{\prime }}& s_{1^{\prime }}& 0& a_{1^{\prime \prime }}{c}_{1^{\prime \prime }}-a_4{c}_{1^{\prime }}\\ -s_{1^{\prime }}& -c_{1^{\prime }}& 0& a_{1^{\prime \prime }}{s}_{1^{\prime \prime }}-a_4{s}_{1^{\prime }}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}^{\dots }\dots （15）$$
可以看出，平行四边形机械臂在运动学上等效于二连杆平面臂。然而，值得注意的区别是，两个驱动关节——提供结构的自由度——位于基座上。对于该结构的动力学模型而言，上述过程能够带来很大的简化。

球形臂

考虑上图所示的球形臂，其连杆坐标系如上图所示。注意到，坐标系{0}原点在轴$z_0$和轴$z_1$的交叉点处，因此$d_1=0$，类似地，坐标系{2}的原点在轴$z_1$和轴$z_2$的交叉点处。D-H参数如下表所示。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	0	$-\pi /2$	0	${\theta }_1$
2	0	$\pi /2$	$d_2$	${\theta }_2$
3	0	0	$d_3$	0

可计算得到正运动学公式如下
$${\mathbf{T}}_3^0(q)={\mathbf{A}}_0^1{\mathbf{A}}_1^2{\mathbf{A}}_2^3=\left[\begin{array}{cccc}{c}_1{c}_2& -s_1& c_1{s}_2& c_1{s}_2{d}_3-s_1{d}_2\\ s_1{c}_2& c_1& s_1{s}_2& s_1{s}_2{d}_3+c_1{d}_2\\ -s_2& 0& c_2& c_2{d}_3\\ 0& 0& 0& 1\end{array}\right]\dots \dots （16）$$
其中变量$\mathbf{q}=[{\theta }_1{\theta }_2{d}_3{]}^T$。注意到，第三个关节对旋转矩阵的影响并不明显。另外，单位向量${\mathbf{y}}_3^0$的方向由第一个关节唯一决定，这是因为第二个关节的旋转轴$z_1$与轴$y_3$平行。与之前的结构不同，在球形臂的例子中，坐标系{3}能够表示末端执行器坐标系的单位方向向量$(n_e{o}_e{a}_e)$，也就是说${\mathbf{T}}_e^3={\mathbf{I}}_4$。

拟人臂

考虑如上图所示的拟人化机械臂，可以注意到这个机械臂相当于一个二连杆平面机械臂附加了一个绕平面轴的旋转。从上文分析可以知道，平行四边形机械臂可以用来替代平面二连杆机械臂，这种方式在一些拟人化的工业机械臂结构中可以看到。

上图已经画出了机械臂的连杆坐标系，与前面的结构类似，该机械臂坐标系{0}的原点在轴$z_0$和轴$z_1$的交叉点处（$d_1=0 $）；另外，轴$z_1$和轴$z_2$平行，轴$x_1$和轴$x_2$的方向是根据平面二连杆机械臂进行选择的（也可以参考本文第的三连杆平面机械臂）。D-H参数表如下。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	0	$\pi /2$	0	${\theta }_1$
2	$a_2$	0	0	${\theta }_2$
3	$a_3$	0	0	${\theta }_3$

可计算得到正运动学公式如下
$${\mathbf{T}}_3^0(q)={\mathbf{A}}_1^0{\mathbf{A}}_2^1{\mathbf{A}}_3^2=\left[\begin{array}{cccc}{c}_1{c}_{23}& -c_1{s}_{23}& s_1& c_1(a_2{c}_2+a_3{c}_{23})\\ s_1{c}_{23}& -s_1{s}_{23}& -c_1& s_1(a_2{c}_2+a_3{c}_{23})\\ s_{23}& c_{23}& 0& a_2{c}_2+a_3{s}_{23}\\ 0& 0& 0& 1\end{array}\right]\dots \dots （17）$$
其中关节变量$\mathbf{q}=[{\theta }_1{\theta }_2{\theta }_3{]}^T$。因为$z_2$和$z_3$是对齐的，故坐标系{3}与可能的末端执行器坐标系不一致，需要左乘一个固定的姿态变换矩阵。

球腕

如上图所示，考虑一种典型结构，该结构仅由腕部组成。由于该腕部结构通常被认为安装在六自由度机械手的三自由度机械臂上，故其关节变量从4开始命名。值得注意的是，由于所有旋转轴均交于一点，故该腕部是球形的。一旦$z_3$,$z_4$,$z_5$以及$x_3$方向确定后，$x_4$和$x_5$的方向则会存在不确定性。参考上图建立的连杆坐标系，可以得到D-H参数表如下。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
4	0	$-\pi /2$	0	${\theta }_4$
5	0	$\pi /2$	0	${\theta }_5$
6	0	0	$d_6$	${\theta }_6$

可计算得到正运动学公式如下
$${\mathbf{T}}_6^3(q)={\mathbf{A}}_4^3{\mathbf{A}}_5^4{\mathbf{A}}_6^5=\left[\begin{array}{cccc}{c}_4{c}_5{c}_6-s_4{s}_6& -c_4{c}_5{s}_6-s_4{c}_6& c_4{s}_5& c_4{s}_5{d}_6\\ s_4{c}_5{c}_6+c_4{s}_6& -s_4{c}_5{s}_6+c_4{c}_6& s_4{s}_5& s_4{s}_5{d}_6\\ -s_5{c}_6& s_5{s}_6& c_5& c_5{d}_6\\ 0& 0& 0& 1\end{array}\right]\dots \dots （18）$$
其中关节变量$\mathbf{q}=[{\theta }_4{\theta }_5{\theta }_6{]}^T$。注意，作为对坐标系选择的结果，从齐次变换矩阵${\mathbf{T}}_6^3$中提取的块矩阵（旋转矩阵）${\mathbf{R}}_6^3$与推导得到的欧拉角的旋转矩阵相同，即${\theta }_4$,${\theta }_5$,${\theta }_5$构成了相对于坐标系$O_3-x_3{y}_3{z}_3$的ZYZ角的角度集合。此外，坐标系{6}的单位向量与可能的末端执行器坐标系的单位向量一致。

斯坦福机械臂

斯坦福机械臂是由球形臂和球腕组成的，如上图所示。因为球形臂中的坐标系{3}和球腕中的坐标系{3}相同，因此通过简单地对公式（16）和（18）进行组合即可得到该机械臂的正运动学方程，如下
$${\mathbf{T}}_6^0={\mathbf{T}}_3^0{\mathbf{T}}_6^3=\left[\begin{array}{cccc}{\mathbf{n}}_6^0& {\mathbf{o}}_6^0& {\mathbf{a}}_6^0& {\mathbf{p}}_6^0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （19）$$
可计算得到
$${\mathbf{p}}_6^0=\left[\begin{array}{c}{c}_1{s}_2{d}_3-s_1{d}_2+\left(c_1\left(c_2{c}_4{s}_5+s_2{c}_5\right)-s_1{s}_4{s}_5\right)d_6\\ s_1{s}_2{d}_3+c_1{d}_2+\left(s_1\left(c_2{c}_4{s}_5+s_2{c}_5\right)+c_1{s}_4{s}_5\right)d_6\\ c_2{d}_3+\left(-s_2{c}_4{s}_5+c_2{c}_5\right)d_6\end{array}\right]\dots \dots （20）$$
这是末端执行器的位置；
$$\begin{gathered} {\mathbf{n}}_6^0=\left[\begin{array}{c}{c}_1\left(c_2\left(c_4{c}_5{c}_6-s_4{s}_6\right)-s_2{s}_5{c}_6\right)-s_1\left(s_4{c}_5{c}_6+c_4{s}_6\right)\\ s_1\left(c_2\left(c_4{c}_5{c}_6-s_4{s}_6\right)-s_2{s}_5{c}_6\right)+c_1\left(s_4{c}_5{c}_6+c_4{s}_6\right)\\ -s_2\left(c_4{c}_5{c}_6-s_4{s}_6\right)-c_2{s}_5{c}_6\end{array}\right]\dots \dots （21） \\ {\mathbf{s}}_6^0=\left[\begin{array}{c}{c}_1\left(-c_2\left(c_4{c}_5{s}_6+s_4{c}_6\right)+s_2{s}_5{s}_6\right)-s_1\left(-s_4{c}_5{s}_6+c_4{c}_6\right)\\ s_1\left(-c_2\left(c_4{c}_5{s}_6+s_4{c}_6\right)+s_2{s}_5{s}_6\right)+c_1\left(-s_4{c}_5{s}_6+c_4{c}_6\right)\\ s_2\left(c_4{c}_5{s}_6+s_4{c}_6\right)+c_2{s}_5{s}_6\end{array}\right] \end{gathered}$$

$${\mathbf{a}}_6^0=\left[\begin{array}{c}{c}_1\left(c_2{c}_4{s}_5+s_2{c}_5\right)-s_1{s}_4{s}_5\\ s_1\left(c_2{c}_4{s}_5+s_2{c}_5\right)+c_1{s}_4{s}_5\\ -s_2{c}_4{s}_5+c_2{c}_5\end{array}\right]\dots \dots （22）$$

这是末端执行器的方向。

将${\mathbf{p}}_6^0$与单独球形臂的${\mathbf{p}}_3^0$进行对比可以看出，两者的不同之处在于末端执行器的坐标系原点选定在距离坐标系{3}$d_6$且沿着方向${\mathbf{a}}_6^0$的地方。换句话说，如果$d_6=0$，${\mathbf{p}}_6^0$和${\mathbf{p}}_3^0$将是相同的。这是解决该机械臂逆运动学问题的关键所在。

带球形手腕的拟人化机械臂

因为拟人臂中的坐标系{3}与球腕中的坐标系{3}不一致，因此带球形手腕的拟人化机械臂的正运动学方程不能简单地用${\mathbf{T}}_3^0$乘以${\mathbf{T}}_6^3$。D-H参数表如下所示。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	0	$\pi /2$	0	${\theta }_1$
2	$a_2$	0	0	${\theta }_2$
3	0	$\pi /2$	0	${\theta }_3$
4	0	$-\pi /2$	$d_4$	${\theta }_4$
5	0	$\pi /2$	0	${\theta }_5$
6	0	0	$d_6$	${\theta }_6$

可得到
$${\mathbf{T}}_6^0={\mathbf{T}}_3^0{\mathbf{T}}_6^3=\left[\begin{array}{cccc}{\mathbf{n}}_6^0& {\mathbf{o}}_6^0& {\mathbf{a}}_6^0& {\mathbf{p}}_6^0\\ 0& 0& 0& 1\end{array}\right]\dots \dots （23）$$
末端执行器的位置为
$${\mathbf{p}}_6^0=\left[\begin{array}{c}{a}_2{c}_1{c}_2+d_4{c}_1{s}_{23}+d_6\left(c_1\left(c_{23}{c}_4{s}_5+s_{23}{c}_5\right)+s_1{s}_4{s}_5\right)\\ a_2{s}_1{c}_2+d_4{s}_1{s}_{23}+d_6\left(s_1\left(c_{23}{c}_4{s}_5+s_{23}{c}_5\right)-c_1{s}_4{s}_5\right)\\ a_2{s}_2-d_4{c}_{23}+d_6\left(s_{23}{c}_4{s}_5-c_{23}{c}_5\right)\end{array}\right]\dots \dots （24）$$
末端执行器的姿态为
$${\mathbf{n}}_6^0=\left[\begin{array}{c}{c}_1\left(c_{23}\left(c_4{c}_5{c}_6-s_4{s}_6\right)-s_{23}{s}_5{c}_6\right)+s_1\left(s_4{c}_5{c}_6+c_4{s}_6\right)\\ s_1\left(c_{23}\left(c_4{c}_5{c}_6-s_4{s}_6\right)-s_{23}{s}_5{c}_6\right)-c_1\left(s_4{c}_5{c}_6+c_4{s}_6\right)\\ s_{23}\left(c_4{c}_5{c}_6-s_4{s}_6\right)+c_{23}{s}_5{c}_6\end{array}\right]\dots \dots （25）$$

$${\mathbf{s}}_6^0=\left[\begin{array}{c}{c}_1\left(-c_{23}\left(c_4{c}_5{s}_6+s_4{c}_6\right)+s_{23}{s}_5{s}_6\right)+s_1\left(-s_4{c}_5{s}_6+c_4{c}_6\right)\\ s_1\left(-c_{23}\left(c_4{c}_5{s}_6+s_4{c}_6\right)+s_{23}{s}_5{s}_6\right)-c_1\left(-s_4{c}_5{s}_6+c_4{c}_6\right)\\ -s_{23}\left(c_4{c}_5{s}_6+s_4{c}_6\right)-c_{23}{s}_5{s}_6\end{array}\right]\dots \dots （26）$$

$${\mathbf{a}}_6^0=\left[\begin{array}{c}{c}_1\left(c_{23}{c}_4{s}_5+s_{23}{c}_5\right)+s_1{s}_4{s}_5\\ s_1\left(c_{23}{c}_4{s}_5+s_{23}{c}_5\right)-c_1{s}_4{s}_5\\ s_{23}{c}_4{s}_5-c_{23}{c}_5\end{array}\right]\dots \dots （27）$$

当$d_6=0$时，可得到腕部轴线的交点位置。在这种情况下，式（24）中的位置向量${\mathbf{p}}_6^0$和式（17）中拟人臂的位置向量${\mathbf{p}}_3^0$相同，这是因为$d_4$即为前臂的长度$a_3$，并且上图（带球形手腕的拟人化机械臂）中的轴$x_3$相对于图（拟人臂）中的轴$x_3$旋转了$\pi /2$。

DLR机械臂

考虑上图所示的DLR机械臂，该机械臂有七个自由度，就其本身而言多出来的自由度是多余的。对于腕部结构来说，其有两种可能的构型。参考上文提到的球腕结构，可建立连杆坐标系如上图所示。D-H参数表如下。

Link	$a_i$	${\alpha }_i$	$d_i$	${\theta }_i$
1	0	$\pi /2$	0	${\theta }_1$
2	0	$\pi /2$	0	${\theta }_2$
3	0	$\pi /2$	$d_3$	${\theta }_3$
4	0	$\pi /2$	0	${\theta }_4$
5	0	$\pi /2$	$d_5$	${\theta }_5$
6	0	$\pi /2$	0	${\theta }_6$
7	0	0	$d_7$	${\theta }_7$

计算得到末端执行器的位置为
$${\mathbf{p}}_7^0=\left[\begin{array}{c}{d}_3{x}_{d_3}+d_5{x}_{d_5}+d_7{x}_{d_7}\\ d_3{y}_{d_3}+d_5{y}_{d_5}+d_7{y}_{d_7}\\ d_3{z}_{d_3}+d_5{z}_{d_5}+d_7{z}_{d_7}\end{array}\right]\dots \dots （28）$$
其中，

$x_{d_3}=c_1{s}_2$
$x_{d_5}=c_1\left(c_2{c}_3{s}_4-s_2{c}_4\right)+s_1{s}_3{s}_4$
$x_{d_7}=c_1\left(c_2{k}_1+s_2{k}_2\right)+s_1{k}_3$
$y_{d_3}=s_1{s}_2$
$y_{d_5}=s_1\left(c_2{c}_3{s}_4-s_2{c}_4\right)-c_1{s}_3{s}_4$
$y_{d_7}=s_1(c_2{k}_1+s_2{k}_2)-c_1{k}_3$
$z_{d_3}=-c_2$
$z_{d_5}=-c_2{c}_4+s_2{c}_3{s}_4$
$z_{d_7}=s_2\left(c_3\left(c_4{c}_5{s}_6-s_4{c}_6\right)+s_3{s}_5{s}_6\right)-c_2{k}_2$

$k_1=c_3\left(c_4{c}_5{s}_6-s_4{c}_6\right)+s_3{s}_5{s}_6$
$k_2=s_4{c}_5{s}_6+c_4{c}_6$
$k_3=s_3\left(c_4{c}_5{s}_6-s_4{c}_6\right)-c_3{s}_5{s}_6$

计算得到末端执行器的方向为
$$n_7^0=\left[\begin{array}{c}\left(\left(x_a{c}_5+x_c{s}_5\right)c_6+x_b{s}_6\right)c_7+\left(x_a{s}_5-x_c{c}_5\right)s_7\\ \left(\left(y_a{c}_5+y_c{s}_5\right)c_6+y_b{s}_6\right)c_7+\left(y_a{s}_5-y_c{c}_5\right)s_7\\ \left(z_a{c}_6+z_c{s}_6\right)c_7+z_b{s}_7\end{array}\right]\dots \dots （29）$$

$$s_7^0=\left[\begin{array}{c}-\left(\left(x_a{c}_5+x_c{s}_5\right)c_6+x_b{s}_6\right)s_7+\left(x_a{s}_5-x_c{c}_5\right)c_7\\ -\left(\left(y_a{c}_5+y_c{s}_5\right)c_6+y_b{s}_6\right)s_7+\left(y_a{s}_5-y_c{c}_5\right)c_7\\ -\left(z_a{c}_6+z_c{s}_6\right)s_7+z_b{c}_7\end{array}\right]\dots \dots （30）$$

$${\mathbf{a}}_7^0=\left[\begin{array}{c}\left(x_a{c}_5+x_c{s}_5\right)s_6-x_b{c}_6\\ \left(y_a{c}_5+y_c{s}_5\right)s_6-y_b{c}_6\\ z_a{s}_6-z_c{c}_6\end{array}\right]\dots \dots （31）$$

其中，

$x_a=\left(c_1{c}_2{c}_3+s_1{s}_3\right)c_4+c_1{s}_2{s}_4$
$x_b=\left(c_1{c}_2{c}_3+s_1{s}_3\right)s_4-c_1{s}_2{c}_4$
$x_c=c_1{c}_2{s}_3-s_1{c}_3$
$y_a=\left(s_1{c}_2{c}_3-c_1{s}_3\right)c_4+s_1{s}_2{s}_4$
$y_b=(s_1{c}_2{c}_3-c_1{s}_3)s_4-s_1{s}_2{c}_4$
$y_c=s_1{c}_2{s}_3+c_1{c}_3$

$z_a=(s_2{c}_3{c}_4-c_2{s}_4)c_5+s_2{s}_3{s}_5$

$z_b=\left(s_2{c}_3{c}_4-c_2{s}_4\right)s_5-s_2{s}_3{c}_5$
$z_c=s_2{c}_3{s}_4+c_2{c}_4$

---

参考文献

Robotics - Modelling, Planning and Control