一、坐标系建立与D-H参数表

本文以irb1600机器人为分析对象，irb1600机器人坐标系建立如下图所示，DH参数表如表1所示，采用后置坐标系法建立。

初始角度是指，图片所示位姿，各个关节对应的角度值

二、位姿变换矩阵与正运动学计算

依据D-H参数表，计算变换矩阵如下：

当th1=0，th2=90°，th3=0，th4=180°，th5=180°，th6=0，计算得到机器人末端位姿为：

三、在Simulink中进行仿真验证

1.将urdf文件导入simulink中

在SolidWorks中将三维模型输出为urdf格式，命名为irb1600.urdf，注意导出时应注意机器人位姿最好为初始位姿，不要设置为随意姿态。
导入simulink的命令为：

smimport('irb1600.urdf')

增加Transform Sensor，用以在simulink中测量机器人末端的位置，最终建模如下。(如有细节看不懂，可文章下方留言~)

2.启动simulink进行验证

通过上述建立的D-H参数，计算得到的正运动学结果，作为simlink中机器人模型的关节角度输入，再通过simulink反馈的机器人末端位置，与理论计算结果进行对比，验证机器人运动学解算的正确性。

计算源码如下。这里使用的是robotics toolbox计算正运动学，也可以使用前文计算得到的变换矩阵来计算，两种方法都可以：

%使用前startup_rvc打开机器人工具箱
clear;
%theta，d， a ，alpha
L1 = Link('d', 481.5, 'a', 135, 'alpha', pi/2);
L2 = Link( 'd',0,    'a',475,   'alpha',0);
L3 = Link( 'd',0,    'a',0,      'alpha',pi/2);
L4 = Link( 'd',600,  'a',0,      'alpha',pi/2);
L5 = Link( 'd',0,    'a',0,      'alpha',pi/2);
L6 = Link( 'd',72.5,    'a',0,      'alpha',0);

L2.qlim=[0,pi/4];
L3.qlim=[-pi/2,pi/2];

robot = SerialLink([L1 L2 L3 L4 L5 L6],'name','irb1600');
%%
% % %定义关节角度，如果要改变输入，改这里就行
q1=0;
q2=pi/2;
q3=0;
q4=pi;
q5=pi;
q6=0;
% %该组关节角度理论计算的对应末端位置为807.5，0，956.5


robot.fkine([q1 q2 q3 q4 q5 q6])


%%
%使用如下命令将urdf文件导入simulink
% smimport('irb1600.urdf')

%统一初始位姿。
%因为机器人D-H坐标系的零位，和urdf文件的零位不统一，所以需要减去初始关节角度。
%也可以在导入urdf文件时，把模型调整到D-H坐标系认定的零位， 但是姿态会看起来很奇怪。这里还是采用减去初始关节角度这种方式。
q1_sm=q1;
q2_sm=q2-pi/2;
q3_sm=q3;
q4_sm=q4-pi;
q5_sm=q5-pi;
q6_sm=q6;

%启动simulink验证
robot_sm = importrobot('irb1600.urdf');
robot_sm.DataFormat = 'column';
open_system('irb1600_fk.slx')
sim('irb1600_fk.slx') 
%%

仿真结果如下（单位：米），可以验证得到正运动学计算是正确的的。Y方向0.39mm的误差，为三维模型误差，可近似为0。

此时机器人的位置和姿态如下，可以发现与我们建立D-H坐标时的初始姿态一致，再次证明建立的正运动学是正确的。

四、总结

本文分析IRB1600机器人的正运动学并在simulink中进行了验证。如有疑问，欢迎评论区讨论~