机器人学中的正运动学

本文对机器人学基本知识中的正运动学（Forward Kinematics）进行介绍，适合入门学习机器人学，如果你发现本文有疏漏、错误之处，欢迎读者批评指正。如果你喜欢本文，可以收藏本文，也可将本文分享给你的朋友、同学，但是转载不被允许

一、位置$(Position)$表示

• 笛卡尔坐标
  $$P=\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$$

• 柱坐标
  变量：$variables=\left(\begin{array}{c}\rho \\ \theta \\ z\end{array}\right)$
  因此，位置 $P$ 可以表示为：
  $$P=\left(\begin{array}{c}\rho \cos \theta \\ \rho \sin \theta \\ z\end{array}\right)$$
  

• 球坐标
  变量：$variables=\left(\begin{array}{c}r\\ \theta \\ \varphi \end{array}\right)$
  因此，位置 $P$ 可以表示为：
  $$P=\left(\begin{array}{c}r\sin \theta \cos \varphi \\ r\sin \theta \sin \varphi \\ r\cos \theta \end{array}\right)$$
  

二、方向$(Orientation)$表示

2.1 基本公式

绕$X$轴旋转角$\gamma$，旋转矩阵为：
$$R_{\gamma }=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & -\sin \gamma \\ 0& \sin \gamma & \cos \gamma \end{array}\right]$$
绕$Y$轴旋转角$\beta$，旋转矩阵为：
$$R_{\beta }=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]$$
绕$Z$轴旋转角$\alpha$，旋转矩阵为：
$$R_{\alpha }=\left[\begin{array}{ccc}\cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]$$
一个坐标系$\{B\}$相对另一个坐标系$\{A\}$
$${}_B^{A}R=\left[\begin{array}{ccc}\overrightarrow{n}& \overrightarrow{o}& \overrightarrow{a}\end{array}\right]=\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]\text{\hspace{1em}(1)}$$

我们容易知道三维空间中的方向只有3个自由度，而上述表达方式含有9个变量，因此我们需要添加6个约束如下：

• $\overrightarrow{\mathbf{n}}\cdot \overrightarrow{\mathbf{o}}=0$
• $\overrightarrow{\mathbf{n}}\cdot \overrightarrow{\mathbf{a}}=0$
• $\overrightarrow{\mathbf{a}}\cdot \overrightarrow{\mathbf{o}}=0$
• $\left|\overrightarrow{n}\right|=1$
• $\left|\overrightarrow{o}\right|=1$
• $\left|\overrightarrow{a}\right|=1$

当然我们也可以根据它们的几何关系将上述约束简化为：

• $\overrightarrow{\mathbf{n}}\cdot \overrightarrow{\mathbf{o}}=0$
• $\overrightarrow{\mathbf{a}}=\overrightarrow{\mathbf{n}}\times \overrightarrow{\mathbf{o}}$
• $\left|\overrightarrow{n}\right|=1$
• $\left|\overrightarrow{o}\right|=1$
• $\left|\overrightarrow{a}\right|=1$

2.2 RPY角

假设坐标系$\{B\}$初始时与参考坐标系$\{A\}$重合，先绕坐标系$\{A\}$的$X_A$轴旋转角$\gamma$，再绕坐标系$\{A\}$的$Y_A$轴旋转角$\beta$，最后绕坐标系$\{A\}$的$Z_A$轴旋转角$\alpha$。在上述过程中，各个旋转量表达如下：

则$\{B\}$相对于$\{A\}$最终的姿态为：
$${}_B^{A}R=R_Z(\alpha )R_Y(\beta )R_X(\gamma )\text{\hspace{1em}(2)}$$
具体计算为：
$$\begin{array}{rl}{}_B^{A}R& =R_Z(\alpha )R_Y(\beta )R_X(\gamma )\\ & =\left[\begin{array}{ccc}\cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]\times \left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right]\times \left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & -\sin \gamma \\ 0& \sin \gamma & \cos \gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}c\alpha & -s\alpha & 0\\ s\alpha & c\alpha & 0\\ 0& 0& 1\end{array}\right]\times \left[\begin{array}{ccc}c\beta & 0& s\beta \\ 0& 1& 0\\ -s\beta & 0& c\beta \end{array}\right]\times \left[\begin{array}{ccc}1& 0& 0\\ 0& c\gamma & -s\gamma \\ 0& s\gamma & c\gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\gamma s\beta c\alpha -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]\\ & =\left[\begin{array}{ccc}{n}_x& o_x& a_x\\ n_y& o_y& a_y\\ n_z& o_z& a_z\end{array}\right]\end{array}\text{\hspace{1em}(3)}$$

2.3 欧拉角

$\text{Z-Y-X}$​欧拉角

假设坐标系$\{B\}$初始时与参考坐标系$\{A\}$重合，先绕坐标系$\{B\}$的$Z_B$轴旋转角$\alpha$，再绕坐标系$\{B\}$的$Y_B$轴旋转角$\beta$，最后绕坐标系$\{B\}$的$X_B$轴旋转角$\gamma$。则$\{B\}$相对于$\{A\}$最终的姿态为：
$$\begin{gathered} {}_B^{A}R=R_Z(\alpha )R_Y(\beta )R_X(\gamma ) \\ =\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]\text{\hspace{1em}(4)} \end{gathered}$$
$\text{Z-Y-Z}$欧拉角

假设坐标系$\{B\}$初始时与参考坐标系$\{A\}$重合，先绕坐标系$\{B\}$的$Z_B$轴旋转角$\alpha$，再绕坐标系$\{B\}$的$Y_B$轴旋转角$\beta$，最后绕坐标系$\{B\}$的$Z_B$轴旋转角$\gamma$。则$\{B\}$相对于$\{A\}$最终的姿态为：
$$\begin{gathered} {}_B^{A}R=R_Z(\alpha )R_Y(\beta )R_Z(\gamma ) \\ =\left[\begin{array}{ccc}c\alpha c\beta c\gamma -s\alpha s\gamma & -c\alpha c\beta s\gamma -s\alpha c\gamma & c\alpha s\beta \\ s\alpha c\beta c\gamma +c\alpha s\gamma & -s\alpha c\beta s\gamma +c\alpha c\gamma & s\alpha s\beta \\ -s\beta c\gamma & s\beta s\gamma & c\beta \end{array}\right]\text{\hspace{1em}(5)} \end{gathered}$$

三、平移运动$(Displacement)$

不妨假设坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵为${}_B^{A}T$：
$${}_B^{A}T=\left[\begin{array}{cccc}{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{array}\right]=\left[\begin{array}{cccc}\vec{n}& \vec{o}& \vec{a}& \vec{p}\end{array}\right]$$

第一种情况：坐标系$\{B\}$沿着坐标系$\{A\}$的坐标轴平移${}^A\Delta \vec{p}=\begin{array}{cccc}[\Delta p_x& \Delta p_x& \Delta p_x& 0{]}^T\end{array}$。

那么平移后坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵为${}_B^A{T}^{\prime }$：
$$\begin{array}{rl}{}_B^A{T}^{\prime }& =T_{disp}{\times }_B^{A}T\\ & =\left[\begin{array}{cccc}1& 0& 0& \Delta p_x\\ 0& 1& 0& \Delta p_y\\ 0& 0& 1& \Delta p_z\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}{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{array}\right]\\ & =\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x+\Delta p_x\\ n_y& o_y& a_y& p_y+\Delta p_y\\ n_z& o_z& a_z& p_z+\Delta p_z\\ 0& 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{cccc}\vec{n}& \vec{o}& \vec{a}& \vec{p}{+}^A\Delta \vec{p}\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$
第二种情况：坐标系$\{B\}$沿着坐标系$\{B\}$的坐标轴平移${}^B\Delta \vec{p}=\begin{array}{ccc}[\Delta p_x^{\prime }& \Delta p_y^{\prime }& \Delta p_z^{\prime }{]}^T\end{array}$。

那么平移后坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵为${}_B^A{T}^{\prime }$。

显然，平移运动不会改变坐标系中轴的方向（$Orientation$），因此我们只需要关心$\begin{array}{cccc}[p_x& p_y& p_z& 0{]}^T\end{array}$的变化。

因此，为了得到$\begin{array}{cccc}[p_x& p_y& p_z& 0{]}^T\end{array}$的变化，我们需要将$\left[\begin{array}{c}{}^B\Delta \vec{p}\\ 0\end{array}\right]$转化为$\left[\begin{array}{c}{}^A\Delta \vec{p}\\ 0\end{array}\right]$，而我们知道$\left[\begin{array}{c}{}^B\Delta \vec{p}\\ 0\end{array}\right]$可以在${}_B^{A}T$的作用下得到$\left[\begin{array}{c}{}^A\Delta \vec{p}\\ 0\end{array}\right]$：
$$\begin{array}{rl}\left[\begin{array}{c}{}^B\Delta \vec{p}\\ 0\end{array}\right]& {=}_B^{A}T\times \left[\begin{array}{c}{}^B\Delta \vec{p}\\ 0\end{array}\right]\\ & =\left[\begin{array}{cccc}{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{array}\right]\times \left[\begin{array}{c}\Delta p_x^{\prime }\\ \Delta p_y^{\prime }\\ \Delta p_z^{\prime }\\ 0\end{array}\right]\\ & =\Delta p_x^{\prime }\times \vec{n}+\Delta p_y^{\prime }\times \vec{o}+\Delta p_z^{\prime }\times \vec{a}\end{array}\text{\hspace{1em}(7)}$$
最终齐次变换矩阵${}_B^A{T}^{\prime }$为：
$$\begin{array}{rl}{}_B^A{T}^{\prime }& =\begin{array}{cccc}[\vec{n}& \vec{o}& \vec{a}& \vec{p}+\Delta p_x^{\prime }\times \vec{n}+\Delta p_y^{\prime }\times \vec{o}+\Delta p_z^{\prime }\times \vec{a}]\end{array}\\ & =\begin{array}{cccc}[\vec{n}& \vec{o}& \vec{a}& \vec{p}]\end{array}\times \left[\begin{array}{cc}{I}_3& {}^B\Delta \vec{p}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]\\ & {=}_B^{A}T\times \left[\begin{array}{cc}{I}_3& {}^B\Delta \vec{p}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]\end{array}\text{\hspace{1em}(8)}$$
多次平移可由重复上述表达式得到。

四、旋转运动$(Rotation)$

不妨假设坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵为${}_B^{A}T$。

第一种情况：坐标系$\{B\}$依次绕绝对坐标系$\{A\}$的坐标轴$M_i$旋转${\theta }_i$角（$i=1,2,3,\cdots ,n$），每次旋转的齐次变换矩阵为$T_i$（$T_i=\left[\begin{array}{cc}{R}_{M_i}({\theta }_i)& {\vec{0}}_{3\times 1}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$，$R_{M_i}({\theta }_i)$可由2.1节得到），最终旋转后坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵${}_B^A{T}^{\prime }$为：
$$\begin{array}{rl}{}_B^A{T}^{\prime }& =T_n\times T_{n-1}\times \cdots T_i\times \cdots T_1\times {}_B^{A}T\end{array}\text{\hspace{1em}(9)}$$
第二种情况：坐标系$\{B\}$依次绕相对坐标系$\{B\}$的坐标轴$M_i$旋转${\theta }_i$角（$i=1,2,3,\cdots ,n$），每次旋转的齐次变换矩阵为$T_i$（$T_i=\left[\begin{array}{cc}{R}_{M_i}({\theta }_i)& {\vec{0}}_{3\times 1}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$，$R_{M_i}({\theta }_i)$可由2.1节得到），最终旋转后坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵${}_B^A{T}^{\prime }$为：
$$\begin{array}{rl}{}_B^A{T}^{\prime }& ={}_B^{A}T\times T_1\times T_2\times \cdots T_i\times \cdots T_n\end{array}\text{\hspace{1em}(10)}$$

五、复合运动$(Compound)$

综合3&4节我们可以得到：

不妨假设坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵为${}_B^{A}T$。

将坐标系$\{B\}$依次相对于坐标系$\{A/B\}$的轴$M$旋转$\theta$角/平移$d$位移，$\cdots$。

对上述过程进行合理排序，得到如下描述：

将坐标系$\{B\}$依次相对于坐标系$\{A\}$运动（旋转/平移）$o_i$，每次运动对应的齐次变换矩阵为$T_i,i=1,2,3,\cdots ,m$；将坐标系$\{B\}$依次相对于坐标系$\{B\}$运动（旋转/平移）$o_j^{\prime }$，每次运动对应的齐次变换矩阵为$T_j,j=1,2,3,\cdots ,n$。

依据已知知识，我们不难得到：

若$o_i$为平移量，那么$T_i=\left[\begin{array}{cc}{I}_{3\times 3}& \Delta {\vec{p}}_i\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$；如果$o_i$为旋转量，那么$T_i=\left[\begin{array}{cc}{R}_{M_i}({\theta }_i)& {\vec{0}}_{3\times 1}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$。

若$o_j^{\prime }$为平移量，那么$T_j=\left[\begin{array}{cc}{I}_3& {}^B\Delta {\vec{p}}_j\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$；如果$o_j^{\prime }$为旋转量，那么$T_j=\left[\begin{array}{cc}{R}_{M_j}({\theta }_j)& {\vec{0}}_{3\times 1}\\ {\vec{0}}_{1\times 3}& 1\end{array}\right]$。

运动后坐标系$\{B\}$相对于坐标系$\{A\}$的齐次变换矩阵${}_B^A{T}^{\prime }$为：
$$\begin{array}{rl}{}_B^A{T}^{\prime }& =T_m\times T_{m-1}\times \cdots T_i\times \cdots T_1\times {}_B^{A}T\times T_1\times T_2\times \cdots T_j\times \cdots T_n\end{array}\text{\hspace{1em}(11)}$$

六、改进$\mathbf{DH}$法

6.1 $\mathbf{DH}$定义与建模

DH法：Denavit-Hartenberg法

改进DH建模：

上述各个量分别为：

• $a_{i-1}$：连杆长度（$Linklength$），沿着${\overrightarrow{X}}_{i-1}$，从${\overrightarrow{Z}}_{i-1}$移动到${\overrightarrow{Z}}_i$的距离；
• ${\alpha }_{i-1}$：连杆转角（$Linktwist$），绕着${\overrightarrow{X}}_{i-1}$，从${\overrightarrow{Z}}_{i-1}$移动到${\overrightarrow{Z}}_i$的距离；
• $d_i$：连杆偏距（$Linkoffset$），沿着${\overrightarrow{Z}}_i$，从${\overrightarrow{X}}_{i-1}$移动到${\overrightarrow{X}}_i$的距离；
• ${\theta }_i$：关节角（$Jointangle$），绕着${\overrightarrow{Z}}_i$，从${\overrightarrow{X}}_{i-1}$移动到${\overrightarrow{X}}_i$的距离；

改进DH法建模步骤：

1. 确定每个轴的方向和相邻轴之间的公共法线；
2. 将轴与公共法线的交点当做所在坐标系的原点；
3. 将轴的方向确定为${\overrightarrow{Z}}_i$，将轴${\overrightarrow{Z}}_i$与轴${\overrightarrow{Z}}_{i+1}$之间公共法线确定为${\overrightarrow{X}}_i$;
4. 根据尽可能将参数设置为0的原则，确定坐标系$\{0\}$的${\overrightarrow{X}}_0$和坐标系$\{n\}$的${\overrightarrow{X}}_n$；
5. 根据${\overrightarrow{X}}_i$和${\overrightarrow{Z}}_i$，利用右手定则确定${\overrightarrow{Y}}_i$
6. 将参数填入DH表中

DH表格式：

6.2 改进$\mathbf{DH}$法下的齐次变换矩阵

显然，DH法下的齐次变换矩阵是从坐标系$\{i-1\}$到坐标系$\{i\}$，且一切运动都是相对于相对坐标系进行，因此齐次变换矩阵${}^{i-1}{T}_i$：
$$\begin{array}{rl}{}^{i-1}{T}_i& =R_X({\alpha }_{i-1})\times D_X(a_{i-1})\times R_Z({\theta }_i)\times D_Z(d_i)\\ & =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos {\alpha }_{i-1}& -\sin {\alpha }_{i-1}& 0\\ 0& \sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& a_{i-1}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i& 0& 0\\ \sin {\theta }_i& \cos {\theta }_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& 0& a_{i-1}\\ s{\theta }_{i}c{\alpha }_{i-1}& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{d}_i\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$$
因此，对于整个机械臂系统来说，坐标系$\{i\}$相对于坐标系$\{0\}$的齐次变换矩阵为：
$${}^0{T}_i{=}^0{T}_1{}^1{T}_2{\cdots }^{i-1}{T}_i,i\in \{1,2,3,\cdots ,n\}\text{\hspace{1em}(12)}$$

6.3 工具箱$\mathbf{Toolbox}$的部分使用

• 安装Toolbox

1. 下载链接（如果嫌麻烦，本文最后将提供相关资料下载的途径）

2. 将下载的文件夹放置进Matlab的安装路径下

3. 将文件夹中的$rvctools$文件夹加入到路径中

   

4. 打开Matlab，在命令行中运行startup_rvc，然后再运行rtbdemo

   

• toolbox关于正运动学的基本使用

  该工具箱与理论的吻合例证

  设置：$\theta =\pi /8;d=3.5;a=4.7;\alpha =-3\pi /7$

例证如下：

theta=pi/8;d=3.5;a=4.7;alpha=-3*pi/7;
T1=[cos(theta) -sin(theta) 0 a;
     sin(theta)*cos(alpha) cos(theta)*cos(alpha) -sin(alpha) -sin(alpha)*d;
     sin(theta)*sin(alpha) cos(theta)*sin(alpha) cos(alpha) cos(alpha)*d;
     0 0 0 1];
L=Link([theta,d,a,alpha 0],'modified');%0代表旋转关节
T2=L.A(theta)
T1-T2

创建$Link$

L=Link([theta,d,a,alpha])%默认为标准DH和旋转关节
L=Link([theta,d,a,alpha,sigma])%sigma代表关节类型，0为旋转关节，1为移动关节
%也可通过外文注明来确定关节类型
L=Link('revolute','d',1.2,'a',0.3,'alpha',pi/2)
L=Link('prismatic','theta',pi/2,'a',0.3,'alpha',pi/2)
%若要使用改进DH法，则可通过下列方式
L=Link([0 0 1 pi/2 1],'modified')

连接$Links$

%方法1
L(1)=Link([0,0,1,pi/2],'modified');L(2)=Link([0,0,2,pi/2],'modified');
twolink=SerialLink(L,'name','twolink')
%方法2
DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink')

连接$Robot$

DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
%方法1
R=SerialLink([R1,R2])
%方法2
R=R1*R2

模型作图

DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
R=SerialLink([R1,R2]);
R.plot([pi/2,-pi/2,pi/2,-pi/2])

改参作图

DH=[0,0,1,pi/2;0,0,2,pi/2];twolink=SerialLink(DH,'name','twolink');
R1=SerialLink(twolink);R2=SerialLink(twolink);
R=SerialLink([R1,R2]);
R.teach()

6.4 一个简单例子

为了检验学习效果，我将提供一个简单例子，该例子为六自由度驱动机器人，表示如下：

我们对其DH建模，首先对各个坐标系进行标定：

DH表：

因此我们可以得到相邻坐标系的齐次变换矩阵：
$$\begin{array}{rl}& {}^0{T}_1=\left[\begin{array}{cccc}c{\theta }_1& -s{\theta }_1& 0& 0\\ s{\theta }_1& c{\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{,}^1{T}_2=\left[\begin{array}{cccc}c{\theta }_2& -s{\theta }_2& 0& 0\\ 0& 0& -1& 0\\ s{\theta }_2& c{\theta }_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]{,}^2{T}_3=\left[\begin{array}{cccc}c{\theta }_3& -s{\theta }_3& 0& a_2\\ s{\theta }_3& c{\theta }_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\\ & {}^3{T}_4=\left[\begin{array}{cccc}c{\theta }_4& -s{\theta }_4& 0& a_3\\ s{\theta }_4& c{\theta }_4& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{,}^4{T}_5=\left[\begin{array}{cccc}c{\theta }_5& -s{\theta }_5& 0& a_4\\ 0& 0& 1& 0\\ -s{\theta }_5& -c{\theta }_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]{,}^5{T}_6=\left[\begin{array}{cccc}c{\theta }_5& -s{\theta }_5& 0& 0\\ 0& 0& -1& 0\\ s{\theta }_5& c{\theta }_5& 0& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$$
因此，该机器人的手（末端执行器）所在坐标系$\{H\}$相对于机器人基座$\{R\}$的齐次变换矩阵${}^R{T}_H$为：
$${}^R{T}_H{=}^0{T}_1{\times }^1{T}_2{\times }^2{T}_3{\times }^3{T}_4{\times }^4{T}_5{\times }^5{T}_6\text{\hspace{1em}(13)}$$

七、下载资源

链接：百度网盘下载资源
提取码：noqa

参考目录

[1]Niku, Saeed B . Introduction to robotics : analysis, control, applications[J]. 2011.