MATLAB机械臂入门

需要安装rotbot tool工具箱，然后也可以装一个这个插件，在matlab里面双击即可打开

[RTB.mltbx]

本笔记参考b站教程

https://www.bilibili.com/video/BV1q44y1x7WC/?spm_id_from=333.337.search-card.all.click

基础入门

示例代码

1clear;
2clc;
3L(1)=Link('revolute','d',0.216,'a',0,'alpha',pi/2);
4L(2)=Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);
5L(3)=Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0,'offset',-atan(427.46/145));
6L(4)=Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',atan(427.46/145));
7L(5)=Link('revolute','d',0.258,'a',0,'alpha',0);
8Five_dof=SerialLink(L,'name','5-dof');
9Five_dof.base=transl(0,0,0.28);
10Five_dof.teach;

本代码可以打印出一个根据DH参数表建模的一个模型，并使用teach函数打开示教

一些函数

可以在matlab中使用doc Seriallink命令查看串联机械臂的一些函数

plot3d 三维模型展示，（在有模型文件的时候可以展示）

1mdl_puma560
2p560.plot3d(qz,'view',[0 0]);

Fkine 正向运动学

1q0 = [pi/2 pi/2 0 0 0];
2T = Five_dof.fkine(q0);

这里，q0 是一个包含五个关节角度的向量。每个元素代表一个关节的角度（以弧度为单位）。

输出

1T = 
2    0    1    0    0
3   -1    0    0    -0.645
4    0    0    1    1.181
5    0    0    0    1

fkine 是Robotics System Toolbox中的一个函数，用于计算机械臂的正向运动学。Five_dof 是一个 SerialLink 对象，表示一个五自由度的机械臂模型。q0 是输入的关节角度向量。一个4x4的齐次变换矩阵，表示机械臂末端执行器的位置和姿态。矩阵的前3x3部分是旋转矩阵，表示末端执行器的姿态；最后一列的前三个元素是位置向量，表示末端执行器的位置。

逆向运动学（数值解，解析解）

1% 逆向运动学
2q1 = Five_dof.ikine(T,'mask',[1 1 1 1 1 0]); 
3q2 = Five_dof.ikunc(T);

1>> q1
2
3q1 =
4
5    1.5708    1.5708   -0.0000    0.0000         0
6
7>> q2
8
9q2 =
10
11    1.5708    1.5708    0.0000    0.0000   -0.0000

工作空间可视化

1% 随机寻找工作空间
2num = 30000;% 迭代次数
3P = zeros(num,3);% 0矩阵,用来存末端点的位置
4for i=1:num
5    q1 = L(1).qlim(1) + rand*( L(1).qlim(2) - L(1).qlim(1) );
6    q2 = L(2).qlim(1) + rand*( L(2).qlim(2) - L(2).qlim(1) );
7    q3 = L(3).qlim(1) + rand*( L(3).qlim(2) - L(3).qlim(1) );
8    q4 = L(4).qlim(1) + rand*( L(4).qlim(2) - L(4).qlim(1) );
9    q5 = L(5).qlim(1) + rand*( L(5).qlim(2) - L(5).qlim(1) );
10    
11    q=[q1 q2 q3 q4 q5];
12    T=Five_dof.fkine(q);
13    P(i,:)=transl(T);
14end
15plot3(P(:,1),P(:,2),P(:,3),'b.','MarkerSize',1);
16hold on
17grid on
18daspect([1 1 1]);
19view([45 45]);
20Five_dof.plot([0 0 0 0 0]);

轨迹规划

1% 五次多项式轨迹tploy
2t = linspace(0,2,51);% 0-2,51次插值 等效于0：0.04：2
3[P,dP,ddP] = tpoly(0,3,t);

1% 画图
2% 绘制位置、速度和加速度曲线
3figure;
4% 绘制位置 P
5subplot(3, 1, 1); % 3行1列的子图，第1个
6plot(t, P, 'b', 'LineWidth', 1.5);
7xlabel('时间 t (s)');
8ylabel('位置 P');
9title('位置 vs 时间');
10grid on;
11% 绘制速度 dP
12subplot(3, 1, 2); % 3行1列的子图，第2个
13plot(t, dP, 'r', 'LineWidth', 1.5);
14xlabel('时间 t (s)');
15ylabel('速度 dP');
16title('速度 vs 时间');
17grid on;
18% 绘制加速度 ddP
19subplot(3, 1, 3); % 3行1列的子图，第3个
20plot(t, ddP, 'g', 'LineWidth', 1.5);
21xlabel('时间 t (s)');
22ylabel('加速度 ddP');
23title('加速度 vs 时间');
24grid on;

混合可以让运动过程中的最大速度持续时间长，从而节省时间。

1% 混合曲线lspb
2[P,dP,ddP]=lspb(0,3,51,0.1); %可以指定最大速度

给定位置：

1% 给定位置
2T1 = transl(0.7,-0.5,0) * trotx(180);% 初位置 180度是为了调整末端姿态
3T2 = transl(0.7,0.5,0.5) * trotx(180);% 末位置
4q1 = Five_dof.ikunc(T1);% 求逆解
5q2 = Five_dof.ikunc(T2);
6Five_dof.plot(q1);
7pause
8Five_dof.plot(q2);

1% 规划和动画
2P1 = [0.7,-0.5,0];
3P2 = [0.7,0.5,0.5];
4t = linspace(0,2,51);
5Traj = mtraj(@tpoly, P1,P2,t);% 规划出一条三维线轨
6n = size(Traj,1);
7T = zeros(4,4,n);% 定义n个4*4零矩阵
8for i= 1:n
9    T(:,:,i) = transl(Traj(i,:)) * trotx(180);
10end
11Qtraj = Five_dof.ikunc(T);
12Five_dof.plot(Qtraj);
13% Five_dof.plot(Qtraj,'trail','b');% 画出轨迹
14% Five_dof.plot(Qtraj,'movie','trail.gif');% 保存gif

1% 画出曲线
2hold on
3plot(t,Traj(:,1),'.-','LineWidth',1);
4plot(t,Traj(:,2),'.-','LineWidth',1);
5plot(t,Traj(:,3),'.-','LineWidth',1);
6grid on
7legend('x','y','z');
8xlabel('time');
9ylabel('position');