为了在已知机器人各关节转角时得到末端执行器的位姿，可以首先给每个关节指定各自的关节坐标系，然后确定从一个关节到其相邻的下一个关节坐标系间的变换矩阵，该矩阵描述关节坐标系间的变换关系，通过依次变换可最终推导出末端执行器相对于基座坐标系的位姿，从而建立机器人的运动学方程。
机器人的末端执行器位姿T和各关节坐标组成的关节向量q的关系可以表述为如下函数形式：
由6.2节可知，相邻两关节的坐标系间的变换可以由基本旋转和平移得到，可表述如下：
若采用齐次变换，则末端执行器位姿可由单个关节间变换矩阵的连续乘积得到，如下式：
基于这一矩阵变换关系，利用MATLAB强大的矩阵运算能力，可以在已知D-H参数和各关节转角时快速求解得到末端执行器的位姿。由于末端执行器的位姿可由6个自由度——3个移动自由度和3个转动自由度来描述，故为了使末端执行器可以到达任意姿态，工业机器人的机械臂往往具有6个关节，也即6个自由度。本节即以6个关节的工业机器人Staubli TX200为例，使用MATLAB机器人工具箱，通过标准D-H参数建立机器人各关节的关节坐标系，进而构建机器人模型进行机器人的正运动学计算。
Staubli TX200六轴重负载机器人能适应各种工作环境，能代替人工完成大多数简单重复性工作。如图6.10（a）所示，该机器人主要由基座、回转主体、肩部、肘部、腕部等几部分构成。其中腕部由3个转动关节组成，以实现腕部的俯仰、翻滚与偏转。整个结构共有6个杆件和6个关节，具有6个自由度。该机器人的D-H参数见表6.4。
图6.10 Staubli TX200六轴重负载机器人
（a）实物图；（b）从基座到末端执行器的各关节坐标系
表6.4 Staubli TX200机器人的连杆机械臂的标准D-H参数
首先通过D-H参数分别创建各关节对应的Link向量，Link向量用来存储与机器人关节连接相关的参数、如运动学参数、刚体惯性参数、电动机和传动参数等，并采用默认的标准D-H参数。
将以上6个Link向量传递给SerialLink类的构造函数，并把机器人命名为“Staubli TX200”，将其作为绘图时的展示名称。
(www.xing528.com)
构造得到SerialLink对象，其详细信息如下：
用SerialLink对象的plot函数方法构建Staubli TX200机器人三维模型展示图，如图6.11所示。Staubli TX200机器人在标准D-H参数描述下从基座到末端执行器的各关节坐标系变换如图6.10（b）所示，其中q1～q6分别表示机器人的关节变量，分别对应图6.11中轴1～6的转角。
SerialLink对象的fkine函数可进行机器人的正运动学计算，该函数调用形式如下：
该函数接受的输入参数q为各关节转角组成的一维行向量，默认采用弧度制输入，也可以添加参数options为‘deg’来设置采用角度制输入，函数返回一个4×4的末端执行器位姿的齐次变换矩阵T。给定3个机器人的典型位形（初始位形qz、展开位形qr和一般位形qn）输入，分别如下：
qz＝［0-pi／2 pi 0 0 0］ qr＝［0-pi／2 pi／2 0 0 0］ qn＝［0-pi／2-3∗pi／4 pi／4 pi／4 0］
图6.11 Staubli TX200机器人的三维模型
分别对该机器人初始位形qz、展开位形qr和一般位形qn对应的正运动学进行计算：
可得末端执行器位姿的齐次变换矩阵分别为：
分别对应末端执行器位姿如图6.12（a）～图6.12（c）所示。
图6.12 不同关节坐标下对应的末端执行器的位姿
（a）零角度初始状态位姿；（b）伸展状态的位姿；（c）一般状态的位姿
免责声明：以上内容源自网络，版权归原作者所有，如有侵犯您的原创版权请告知，我们将尽快删除相关内容。
