小伙伴们大家好！我来了^_^//

今天我们将继续上一次的话题，如何使用MATLAB来实现机器人的建模仿真与控制。在matlab要实现机器人的仿真与控制首先要知道机器人的DH参数，根据机器人的DH参数来推导机器人的正逆运动学，最后编程实现我们想要的仿真控制。

上一篇博客中，我们谈到了通过GUI实现了正逆运动学的计算结果显示与机器人可视化三维模型的运动显示。并未对机器人模型的可视化做过多的说明【实际上PUMA762的可视化在demo中的function loaddata中有说明——将Pro/E中的机器人连杆三维模型STL，通过cad2matdemo.m转换成matlab可读的mat文件，再通过每一个连杆相对于基坐标的变换实现运动的可视化】。但是，这种方式效率低且容易出错。

所以今天再来说说matlab环境中可视化机器人的三维模型的一些问题。

1、从机器人工具箱谈起

机器人工具箱（Robotics Toolbox）由Peter Corke教授编写，十分强大。专门用于机器人仿真，在机器人建模、轨迹规划、控制、可视化方面使用非常方便。主要通过DH建模完成机器人模型并进行相关运动学或动力学运动控制仿真。其中最主要的两个函数为：

Link类中主要是完成机器人建模，如机器人的关节和连杆的运动学动力学参数。

SerialLink类中包含机器人的属性、可视化显示函数；运动学方法(如正运动函数fkine，逆运动学函数ikine，速度雅可比)；动力学方法（加速度，正动力学，重力，摩擦力，牛顿欧拉逆动力等）。

先通过机器人工具箱建立模型【首先还是要知道机器人的DH参数，以PUMA560为例】。先用Link建立连杆，再用SerialLink建立机器人。以下为机器人的两种DH参数信息：

一知半解|MATLAB机器人建模与仿真控制(2)

分别使用标准DH模型和改进DH模型显示的机器人可视化结果。通过teach的控制面板随意改变两个figure中的机器人关节角度，其末端位姿都是一致的，证明不管是标准DH还是改进DH，其结果都是一样的。如下：

一知半解|MATLAB机器人建模与仿真控制(2)

所以，从结果来说这种可视化是能接受的。但是如果能更好的在仿真中观察到真实机器人三维模型，效果会更好。下面就来说说….

2、CAD模型的机器人在MATLAB环境下可视化

可视化的方式有很多【下面给出有参考连接】，这里只介绍使用机器人的URDF（Unified Robotics Description Format，机器人统一描述格式）文件。关于URDF的介绍在古月居上也有，这里就不再说了。【URDF文件的获取：可以直接在github上找你需要的机器人URDF文件；可以通过三维软件如Pro/E或Solidworks导出；或者在机器人生产商的官方网站中查找，一般都会提供】

如果我们有机器人的URDF文件，那么如何在matlab环境下做到可视化和仿真呢？下面就以PUMA560来说明。

一知半解|MATLAB机器人建模与仿真控制(2)

如上，将puma560机器人的描述文件包括URDF文件和meshes中三维模型的stl部件添加到路径中后。

在matlab中输入：robot=importrobot(‘puma560.urdf’)；show(robot)；便可以轻易的显示机器人三维模型。

一知半解|MATLAB机器人建模与仿真控制(2)

机器人的RigidBodyTree（刚体结构树）属性如下：

一知半解|MATLAB机器人建模与仿真控制(2)

3、运动仿真

现在我们知道了机器人模型的两种可视化方式，如果我们不希望花费时间自己编写机器人的正逆运动学函数，或者你希望验证自己编写的正逆运动学函数的正确性，那么可以直接利用机器人工具箱【Robotics Toolbox，Peter教授编写】或机器人系统工具箱【Robotics System Toolbox，Mathworks官方工具箱】里面的函数来实现对比验证或运动仿真。

如下为分别使用机器人工具箱和机器人系统工具箱，完成笛卡尔末端圆弧路径跟踪的运动仿真演示。

一知半解|MATLAB机器人建模与仿真控制(2)

从图中可以观察到机器人末端基本上完成了圆弧的跟踪，但是由于两种方式在逆解过程中的迭代方向和收敛情况不同【两种方式的逆解只给出一个解】，导致逆解有所不同，在可视化动画中，机器人系统工具箱要稳定些，机器人工具箱明显要差很多。

4、核心代码

（1）以下是先使用机器人工具箱完成机器人建模的，包括使用标准DH建立的模型和改进DH建立的机器人模型，最后通过使用工具箱逆解函数ikine()实现对定义好的圆弧路径的跟踪。

%% 标准DH建立puma560机器人模型
clear;
clc;
%建立机器人模型
% theta d a alpha offset
L1=Link([0 0 0 pi/2 0 ],'standard'); %连杆的D-H参数
L2=Link([0 0 0.4318 0 0 ],'standard');
L3=Link([0 0.1501 0.0203 -pi/2 0 ],'standard');
L4=Link([0 0.4318 0 pi/2 0 ],'standard');
L5=Link([0 0 0 -pi/2 0 ],'standard');
L6=Link([0 0 0 0 0 ],'standard');
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','puma560','base' , ...
transl(0, 0, 0.62)* trotz(0)); %连接连杆，机器人取名puma560
% robot.plot3d([0,pi/2,0,0,pi,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
figure(1)
robot.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。
%% 改进DH模型又是怎么样的呢？
rmdh =robot.MDH()%将标准DH转换成改进DH
%%
%将转换后的DH代入Link中，standard改为modified。
clc;
%建立机器人模型
% theta d a alpha offset
L11=Link([0 0 0 0 0 ],'modified'); %连杆的D-H参数
L21=Link([0 0 0 pi/2 0 ],'modified');
L31=Link([0 0.1501 0.4318 0 0 ],'modified');
L41=Link([0 0.4318 0.0203 -pi/2 0 ],'modified');
L51=Link([0 0 0 pi/2 0 ],'modified');
L61=Link([0 0 0 -pi/2 0 ],'modified');
robot1=SerialLink([L11 L21 L31 L41 L51 L61],'name','M_puma560'); %连接连杆，机器人取名M_puma560
% robot1.plot([0,pi/2,0,0,pi,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
figure(2)
robot1.teach() %teach可视化模型并可以单轴驱动，可以查看模型与实际机器人的关节运动是否一致。
%%
%% 定义圆路径
t = (0:0.2:20)'; count = length(t);center = [-0.25 0.6 0.46];radius = 0.1;%圆心和半径
theta = t*(2*pi/t(end));
points =(center + radius*[cos(theta) sin(theta) zeros(size(theta))])';
plot3(points(1,:),points(2,:),points(3,:),'r')
%% 使用Peter工具箱逆解进行轨迹重现
hold on
% axis([-1 1.2 -1 1.2 -1 1.2])
title("工具箱机器人模型末端运动轨迹");
xlabel('x/米','FontSize',12);
ylabel('y/米','FontSize',12);
zlabel('z/米','FontSize',12);
for i = 1:size(points,2)
pause(0.01)
bx = points(1,i);
by = points(2,i);
bz =points(3,i);
tform = rpy2tr(136,-180,-180; %欧拉角转姿态齐次矩阵
targetPos = [bx by bz]; %末端位置向量
% targetOr = [0.02 -0.14 0.99 -0.01] %四元数
% tform = quat2tform(targetOr) %四元数转姿态齐次矩阵
TR=transl(targetPos)*tform; %位姿齐次矩阵
hold on
grid on
plot3(bx,by,bz,'*','LineWidth',1);
q=robot.ikine(TR)
pause(0.01)
robot.plot(q);%动画演示
end

（2）以下是使用URDF可视化机器人模型，并调用机器人系统工具箱逆解函数ik实现的圆弧路径跟踪。

%% 导入机器人
clear
clc
robot=importrobot('puma560.urdf');
show(robot);
axes.CameraPositionMode = 'auto';
%% Define the trajectory as a circle with a radius of 0.15
t = (0:0.2:20)';
count = length(t);
center = [-0.25 0.6 0.46];
radius = 0.1;
theta = t*(2*pi/t(end));
points =(center + radius*[cos(theta) sin(theta) zeros(size(theta))])';
%% Draw the defined trajectory and inverse kinematics solution
hold on
plot3(points(1,:),points(2,:),points(3,:),'r')
eeOffset = 0.01;
eeBody = robotics.RigidBody('end_effector');%定义末端执行器，并再后面添加到机器人的Link5上
setFixedTransform(eeBody.Joint,trvec2tform([eeOffset 0 0]));
addBody(robot,eeBody,'puma_link_5');
ik = robotics.InverseKinematics('RigidBodyTree',robot);%逆解解算器
weights = [0.1 0.1 0 1 1 1]; %权重
qInitial = robot.homeConfiguration;
%%
%通过点的轨迹循环来跟踪圆。调用每个点的ik对象以生成实现末端位置的关节配置，存储要稍后使用的逆解结果。
for i = 1:size(points,2)
% Solve for the configuration satisfying the desired end effector
tform = rpy2tr(136,-180,-180);%姿态齐次矩阵
tform = trvec2tform(points(:,i)')*tform ;%末端位姿齐次矩阵
qSol(i,:) = ik('end_effector',tform,weights,qInitial);%求解各关节角度
% Start from prior solution
qInitial = qSol(i,:);
end
%% 动画显示
title('robot move follow the trajectory')
hold on
axis([-0.8 0.8 -0.8 0.85 0 1.3]);
for i = 1:size(points,2)
show(robot,qSol(i,:)','PreservePlot',false);%false改为true时，留下重影。
pause(0.3)
plot3(points(1,i),points(2,i),points(3,i),'.','LineWidth',1);
end
hold off

总结：由于作者能力有限，文中难免有描述不足和错误，希望同大家相互交流学习^_^//

（1）关于STL的说明：

https://baike.baidu.com/item/stl%E6%A0%BC%E5%BC%8F/3511640?fr=aladdin

（2）如何导入机器人模型到matlab中：

https://www.mathworks.com/help/physmod/sm/cad-import.html?s_tid=srchtitle