Matlab 机器人工具箱例程：运动学+动力学+路径规划+可视化

文章目录

1 创建机器人
2 机器人显示
3 机器人示教
4 机器人路径规划：给定关节角路径
5 机器人路径规划：给定末端位姿，求关节角路径
6 工作空间可视化
参考链接

1 创建机器人

clc;clear;close all;
deg = pi/180;L1= Revolute('d', 0, 'a', 0, 'alpha', 0,'modified', ...'I', [0.1183 -0.0001 0.0001;-0.0001 0.1182 0.0001;0.0001 0.0001 0.0140], ...'r', [0.0002 0.0002 0.1264], ...'m', 5.6431, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 1.48e-3, ...'Tc', [0.395 -0.435], ...'qlim', [-180 180]*deg );L2 = Revolute('d', 0.06, 'a', 0, 'alpha', -pi/2,'modified', ...'I', [0.0723,0.0000,-0.0051;0.0000,0.0784,0.0000;-0.0051,0.0000,0.0169;], ...'r', [-0.0062,0.0001,0.1080], ...'m', 5.0478, ...'Jm', 2.2e-4, ...'G', 121, ...'B', .817e-3, ...'Tc', [0.126 -0.071], ...'qlim', [-105 105]*deg );
L3 = Revolute('d', -0.004, 'a', 0.332, 'alpha', 0, 'modified', ...'I', [0.4263,0.0000,-0.0072;0.0000,0.4334,0.0001;-0.0072,0.0001,0.0191], ...'r', [-0.0131,0.0001,0.2402], ...'m', 5.7542, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 1.38e-3, ...'Tc', [0.132, -0.105], ...'qlim', [-225 45]*deg );L4 = Revolute('d', -0.056, 'a', 0, 'alpha', pi/2, 'modified', ...'I', [0.0821,0.0000,-0.0314;0.0000,0.1257,0.0001;-0.0314,0.0001,0.0451], ...'r', [-0.0850,0.0003,0.1540], ...'m', 3.0870, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 71.2e-6, ...'Tc', [11.2e-3, -16.9e-3], ...'qlim', [-110 110]*deg);
L5 = Revolute('d', 0.050, 'a', 0, 'alpha', -pi/2, 'modified', ...'I', [0.0235,0.0000,-0.0002;0.0000,0.0253,0.0000;-0.0002,0.0000,0.0045], ...'r', [0.0001,0.0002,0.0982], ...'m', 2.0459, ...'Jm', 2.2e-4, ...'G', 81, ...'B', 82.6e-6, ...'Tc', [9.26e-3, -14.5e-3], ...'qlim', [-115 115]*deg );
L6 = Revolute('d', -0.050, 'a', 0, 'alpha', pi/2, 'modified', ...'I', [0.0684,0.0000,0.0001;0.0000,0.0696,-0.0001;0.0001,-0.0001,0.0047], ...'r', [-0.0111,-0.0003,0.1366], ...'m', 2.6317, ...'Jm', 2.2e-4, ...'G', 51, ...'B', 36.7e-6, ...'Tc', [3.96e-3, -10.5e-3], ...'qlim', [-180 180]*deg );
% SerialLink类函数
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','VIPER7','comment','LL');
% Link类函数，显示建立机器人DH参数 
robot.display();

输出：

robot = 
VIPER7:: 6 axis, RRRRRR, modDH, slowRNE                          - LL;                                                           
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|       0.06|          0|    -1.5708|          0|
|  3|         q3|     -0.004|      0.332|          0|          0|
|  4|         q4|     -0.056|          0|     1.5708|          0|
|  5|         q5|       0.05|          0|    -1.5708|          0|
|  6|         q6|      -0.05|          0|     1.5708|          0|
+---+-----------+-----------+-----------+-----------+-----------+

2 机器人显示

%通过手动输入各个连杆转角，模型会自动运动到相应位置
theta1=[0 -pi/2 -pi/2 0 0 0];
% SerialLink类函数,显示机器人图像
robot.plot(theta1);
title('机器人plot显示')

在这里插入图片描述

3 机器人示教

robot.teach();
title('机器人teach示教')

在这里插入图片描述

4 机器人路径规划：给定关节角路径

% 初始角
theta1=[0 -pi/2 -pi/2 0 0 0];
% 目标关节角
theta2=[-pi/2 0 -pi/4 -pi/3 pi/4 pi/2];
% 时间
t=[0:0.01:2];
% 关节空间路径规划
% 相当于具有tpoly插值的mtraj，但是对多轴情况进行了优化，还允许使用额外参数设置初始和最终速度
g=jtraj(theta1,theta2,t);
% 通过可选的输出参数，获得随时间变化的关节速度加速度向量
[q,qd,qdd]=jtraj(theta1,theta2,t);
figure
i=1:6;
%绘制动画
subplot(2,3,1);
robot.plot(g)
%绘制每个关节位置
subplot(2,3,2);
qplot(q(:,i));
grid on;title('位置');
%绘制每个关节速度
subplot(2,3,3);
qplot(qd(:,i));
grid on;title('速度');
%绘制每个关节加速度
subplot(2,3,4);
qplot(qdd(:,i));
grid on;title('加速度');
% 获得每个时间点所需要的关节力矩
Q = robot.rne(q,qd,qdd);
%绘制每个关节的力矩
subplot(2,3,5)
qplot(t,Q);
grid on;title('关节力矩');

在这里插入图片描述

5 机器人路径规划：给定末端位姿，求关节角路径

%% 已知机械臂初始和目标的关节角度，利用五次多项式进行轨迹规划
T1=transl(0.3,0.1,0);						%根据给定起始点，得到起始点位姿
T2=transl(0,0.3,0.1);						%根据给定终止点，得到终止点位姿
init_ang=robot.ikine(T1);				%根据起始点位姿，得到起始点关节角
targ_ang=robot.ikine(T2);				%根据终止点位姿，得到终止点关节角
step = 20;%轨迹规划方法
figure
%关节空间轨迹规划
%五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
[q, qd, qdd]=jtraj(init_ang,targ_ang,step); 
grid on
%根据插值，得到末端执行器位姿
T=robot.fkine(q);		
% 
nT=T.T;
%输出末端轨迹
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));
title('根据运动学求正解得到目标轨迹');
%动画演示 
robot.plot(q);	
%robot.plot(q,'trail','b');  %运行后在命令行窗口再复制运行一次，trail轨迹，b蓝色

在这里插入图片描述

%% 求解上述运行过程中的位置、速度、加速度的变化曲线
figure
subplot(3,2,[1,3]); 					%subplot 对画面分区 三行两列 占用1到3的位置
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot.plot(q);							%动画演示subplot(3, 2, 2);
i = 1:6;
plot(q(:,i));
title('位置');
grid on;subplot(3, 2, 4);
i = 1:6;
plot(qd(:,i));
title('速度');
grid on;subplot(3, 2, 6);
i = 1:6;
plot(qdd(:,i));
title('加速度');
grid on;

在这里插入图片描述

6 工作空间可视化

figure
num = 30000;
p = zeros(num,3);%先声明0矩阵可加快运行速度
for i=1:numq1 = L1.qlim(1) + rand * (L1.qlim(2) - L1.qlim(1));q2 = L2.qlim(1) + rand * (L2.qlim(2) - L2.qlim(1));q3 = L3.qlim(1) + rand * (L3.qlim(2) - L3.qlim(1));q4 = L4.qlim(1) + rand * (L4.qlim(2) - L4.qlim(1));q5 = L5.qlim(1) + rand * (L5.qlim(2) - L5.qlim(1));q6 = L6.qlim(1) + rand * (L6.qlim(2) - L6.qlim(1));q = [q1 q2 q3 q4 q5 q6];Ts = robot.fkine(q);  % SerialLink.fkine正向运动学, 给一个关节变量，可以求出变换矩阵P(i,:) = transl(Ts);
end
plot3( P(:,1), P(:,2), P(:,3),'b.','markersize',1); %在三维空间内绘制30000个点hold on; %添加新绘图的时候保留当前绘图
grid on; %在画图的时候添加网格线view([45 45]);
robot.plot([0 0 0 0 0 0]);