本文给出基于matlab机器人工具箱和Simmechanics的机器人运动控制仿真系统。该仿真系统可以根据机器人的DH参数，建立机器人的模型，并且利用机器人工具箱计算雅可比矩阵，利用Simulink搭建机器人的运动控制仿真系统。

1 运动控制仿真原理

机器人运动控制系统完整的包含了轨迹规划、逆向运动学、逆向动力学、正向动力学和正向运动学等模块。具体如下所示

冗余机械臂位置控制系统原理图

在上述仿真系统中，由于系统的逆向动力学主要用于控制，正向动力学用于机器人的仿真，而在一般运动控制仿真中默认机器人的可以进行有效的位置伺服跟踪，因而可以忽略系统的动态响应。因而上图中的机器人位置控制系统等效图如下所示：

运动控制仿真完整框图

运动控制仿真简易框图

2 基于simulink运动控制仿真系统

在Simulink中搭建机器人的运动控制仿真系统，由于在运动控制仿真系统中可以忽略机器人的动力学，因而仿真系统中核心模块是机器人的轨迹规划+逆向运动学+正向运动学。

Simulink中搭建机器人的运动控制仿真系统

上述仿真系统中，借用了两个“interpreted MATLAB Fcn”，分别调用如下两个matlab函数：Robot_iiwa.m和FK_Robot_iiwa.m

% 机器人工具箱的函数
function dq = Robot_iiwa(theta)
%UNTITLED 此处显示有关此函数的摘要
%   此处显示详细说明
%syms q1 q2 q3 q4 q5 q6
% theta=zeros(1,8);
q1 = theta(1);
q2 = theta(2)-pi/2;
q3 = theta(3);
q4 = theta(4)-pi/2;
q5 = theta(5);
q6 = theta(6);
q7 = theta(7);
time=theta(8);
%             theta  d   a  alpha
L(1) = Link([0,  340,   0,  -pi/2],'standard');
L(2) = Link([0,  0,     0,    pi/2],'standard');
L(3) = Link([0,  400,  0,   pi/2],'standard');
L(4) = Link([0,  0,     0,  -pi/2],'standard');
L(5) = Link([0,  400,   0,  -pi/2],'standard');
L(6) = Link([0,  0 ,     0,     pi/2],'standard');
L(7) = Link([0,  141 ,  0,     0],'standard');
iwa = SerialLink(L, 'name', 'Robot');
% teach(iwa)
% J = iwa.jacob0([0 0 0 0 0 0 0]);
J = iwa.jacob0([q1 q2 q3 q4 q5 q6 q7]);
% T = iwa.fkine([q1 q2 q3 q4 q5 q6]);
% dq=[0 0 0 0 0 0 0];
dx=[-30 25 -20 0 0 0]';
dx=zeros(6,1);
if time<=10
    dx(1)=80*cos(1*time);
    dx(2)=80*sin(1*time);
else 
    dx(1)=0;
    dx(2)=0;
end
dq=pinv(J)*dx;
end

FK_Robot_iiwa.m函数具体如下所示：

% 机器人工具箱的函数
function y= FK_Robot_iiwa(in)
%UNTITLED 此处显示有关此函数的摘要
%   此处显示详细说明
%syms q1 q2 q3 q4 q5 q6
% theta=zeros(1,8);
q1 = in(1);
q2 = in(2)-pi/2;
q3 = in(3);
q4 = in(4)-pi/2;
q5 = in(5);
q6 = in(6);
q7 = in(7);
%%%
dq1 = in(8);
dq2 = in(9);
dq3 = in(10);
dq4 = in(11);
dq5 = in(12);
dq6 = in(13);
dq7 = in(14);
%             theta  d   a  alpha
L(1) = Link([0,  340,   0,  -pi/2],'standard');
L(2) = Link([0,  0,     0,    pi/2],'standard');
L(3) = Link([0,  400,  0,   pi/2],'standard');
L(4) = Link([0,  0,     0,  -pi/2],'standard');
L(5) = Link([0,  400,   0,  -pi/2],'standard');
L(6) = Link([0,  0 ,     0,     pi/2],'standard');
L(7) = Link([0,  141 ,  0,     0],'standard');
iwa = SerialLink(L, 'name', 'Robot');
% teach(iwa)
% J = iwa.jacob0([0 0 0 0 0 0 0]);
J = iwa.jacob0([q1 q2 q3 q4 q5 q6 q7]);
T = iwa.fkine([q1 q2 q3 q4 q5 q6 q7]);

dx=J*[dq1 dq2 dq3 dq4 dq5 dq6 dq7]';
y(1)=T(1,4);
y(2)=T(2,4);
y(3)=T(3,4);
y(4)=dx(1);
y(5)=dx(2);
y(6)=dx(3);
end

上述系统完成了机器人运动控制的原理仿真：

程序Robot_iiwa中包含了机器人建模，轨迹规划和逆向运动学；程序FK_Robot_iiwa包含了机器人的建模和正向运动学等的计算。

3 机器人LBR iiwa

认机协作中的iiwa

LBR iiwa

在Matlab/Simmechanics中建立LBR IIWA的具体动力学模型

LBR iiwa 14 R820
轴数：7
工作范围：820mm
机器重量：29.5Kg
重复定位精度：0.15mm

7DOF manipulator simulation system

连杆部分建模

link1

link2

link3

link4

link5

link6

link7

机器人stl模型

4 仿真演示

动画放示意图

仿真操作视频1

仿真操作视频2

仿真最终视频

SimMechanics机器人动力学建模；机械臂动力学建模；机械臂仿真；机械臂运动控制；机器人工具箱；多刚体动力学

机器人运动控制仿真：Matlab机器人工具箱和Simmechanics

1 运动控制仿真原理

2 基于simulink运动控制仿真系统

3 机器人LBR iiwa

4 仿真演示