4. 使用Matlab建立机器人模型并进行正逆运动学分析

4.1 建立机器人模型

（Maltab版本为R2015a）
使用Link和SerialLink函数建立机器人的模型，Link函数为L=Link([theta d a alpha])，其中theta，d，a和alpha为D-H参数，可在命令行窗口输入help Link寻求帮助。建模程序示例如下。（theta角初始都为0）

clc;
clear;
%建立机器人模型
%L=Link([theta d a alpha])
L1=Link([0 0.1065 0      pi/2],'modified');%定义连杆1
L2=Link([0 0      -0.408 0],'modified');%定义连杆2
L3=Link([0 0      -0.382 0],'modified');%定义连杆3
L4=Link([0 0.1109 0      pi/2],'modified');%定义连杆4
L5=Link([0 0.1109 0      -pi/2],'modified');%定义连杆5
L6=Link([0 0.0841 0      0],'modified');%定义连杆6
six_robot=SerialLink([L1 L2 L3 L4 L5 L6]);
theta=[pi/2 -2*pi/3 -2*pi/3 0 2*pi/3 0];
six_robot.plot(theta);
%six_robot.display();
%six_robot.teach;%teach函数
%T=fkine(six_robot,theta);
%Q=six_robot.ikine(T);

使用plot(theta)函数绘制机器人,theta为机器人各关节的角度。建模结果如下。

使用display函数，如程序，可以查看机器人的各项参数。

可以使用teach函数，如程序，进行机器人的实时展示，调节左下角q1-q6的值可以改变机器人的位姿，机器人位姿在左上角显示，如下图。

4.2 正逆运动学分析

使用fkine和ikine函数进行机器人的正逆运动学计算，函数如下。
已知角度求力矩，T=fkine(Six_robot,theta)，theta为各个关节角度。
已知力矩求角度，Q=Six_robot.ikine(T)，T为各个关节的力矩。
函数执行结果如图。


可以看出Q和theta并不相等。