根据D-H表规定得到如下变换矩阵为：

由此可得机器人相邻两关节位姿分别为：

根据DH参数求解变换矩阵的函数trans：
%输入JD，即6个关节变量的值，求解正运动方程
function [ T ] = trans( theta, d, a, alpha )
T =[
cos(theta), -sin(theta)cos(alpha), sin(theta)sin(alpha), acos(theta);
sin(theta), cos(theta)cos(alpha), -cos(theta)sin(alpha), asin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1 ];
end
正向运动学：
function [ T04 ] = fkine(JD)
%JD为输入的16的数组，代表6个关节变量
% trans( theta, d, a, alpha )
if nargin<2; end
T01 =trans( JD(1)+pi/2, 0.0685, 0, pi/2 );
T12 =trans( JD(2)+pi/4, 0, 0.105, 0);
T23 =trans( JD(3), 0, 0, pi/2);
T34 =trans( JD(4), 0.1153, 0, pi/2 );
T45 =trans( JD(5)-pi, 0, 0, pi/2 );
T56 =trans( JD(6), 0.1288, 0, pi/2 );
T04 =T01T12T23T34T45T56
end

机器人模型建立
所设计的机器人由多个连杆机构组成，其关节类型包括旋转关节和移动关节两种。利用Matlab中机器人仿真工具箱Robotics Tool中Link和SerialLink两个函数可建立机器人模型[ ] 。其中，link函数表达式如下：
L=link([■(theta&d&alpha&a&sigma)])
式中，theta为关节变量；d为偏置距离；alpha为扭转角；a为连杆长度；sigma表示关节类型（0为旋转关节，1为移动关节）；前四个参数分别对应表1中的 ， ， ， 。
具体程序编制如下：

等等，其他见下载链接，如下图：

模型与matlab程序下载链接