MATLAB机器人工具箱学习（一）

一. 下载地址：

Robotics Toolbox - Peter Corke

在matlab中打开安装包：RTB.mltbx 将自动下载

注：最好下载R2021a或以上的最新版，R2016a示教会报错。

二. 基本命令和常用函数：

在命令行窗口输入rtbdemo 可打开自带的demo

>> rtbdemo

输入doc rotx，可查看旋转矩阵matrix函数的文档。旋转矩阵R=rotx(arg)中要填角度而不是弧度。

>> doc rotx

1.旋转——旋转矩阵 

 rotx(), roty(), rotz(),给定一个旋转角度就可得到一个旋转矩阵。

r = rotx(90)
%输出结果为
r =
 
     1     0     0
     0     0    -1
     0     1     0

 eul2r() 为由欧拉角得到一个旋转矩阵，tr2eul() 为由旋转矩阵得欧拉角

r1 = eul2r(90,60,30)  %分别对应于绕自身坐标系Z、Y、Z轴的旋转。
输出结果为
r =
 
   -0.5000   -0.8660         0
    0.4330   -0.2500    0.8660
   -0.7500    0.4330    0.5000
%等价于r1 = rotz(90)*roty(60)*rotz(30)，旋转矩阵的右乘

 rpy2r() 为由横滚-俯仰-偏航角得到旋转矩阵，tr2rpy() 为由旋转矩阵得到横滚-俯仰-偏航角

r2 = rpy2r(90,60,30)  %分别绕固定坐标系的XYZ旋转
输出结果为
r2 =
 
    0.4330    0.7500    0.5000
    0.2500    0.4330   -0.8660
   -0.8660    0.5000         0
%等价于 r2 = rotz(30)*roty(60)*rotx(90) 旋转矩阵的左乘

2.旋转——变换矩阵 

 trotx(), troty(), trotz(),给定一个旋转角度就可得到一个变换矩阵。

t = trotx(90)
%输出结果为
t =
 
     1     0     0     0
     0     0    -1     0
     0     1     0     0
     0     0     0     1

eul2tr() ，tr2eul()      rpy2tr() ，tr2rpy() 与上相似

3.位移加旋转——变换矩阵,旋转矩阵——变换矩阵

T = transl(1.5，1，0.5)*trotx(30)*trotz(60);
P = transl(T);
R = t2r(T);
输出结果为
T =
 
    0.5000   -0.8660         0    1.5000
    0.7500    0.4330   -0.5000    1.0000
    0.4330    0.2500    0.8660    0.5000
         0         0         0    1.0000 %最右一列为为一体，左上角3×3为旋转矩阵
 
P =
 
    1.5000
    1.0000
    0.5000 %位移
 
R =
 
    0.5000   -0.8660         0
    0.7500    0.4330   -0.5000
    0.4330    0.2500    0.8660 %旋转矩阵

 三.Link类

类属性和类方法的详细介绍引用 http://t.csdn.cn/NP1N5

‘ revolute ‘为旋转关节，’d‘为连杆偏移量，’a'为连杆长度，‘alpha’为连杆扭角。

L = Link('revolute','d','0.216','a',0,'alpha',pi/2);

串联机械臂要用到SerialLink，SerialLink.teach为示教函数。

bot = SerialLink(L, 'name', '5-dof')  %将SerialLink赋值，定义给bot变量
bot.teach %示教

 SerialLink.plot动画，可给定一个m*n的矩阵，n为机械臂所含关节个数

q = [0 0 0 0 0]; 1*n
bot.plot(q);  五个关节都是初始位置
 
若给定m*n矩阵，则运行结果为一个动画

 SerialLink.fkine正向运动学, 给一个关节变量，可以求出变换矩阵

q0 = [pi/2 pi/2 0 0 0]; %5关节机器人的关节变量
T = bot.fkine(q0);
输出结果为
T = 
         0         1         0         0
        -1         0         0    -0.645
         0         0         1     1.181
         0         0         0         1

  SerialLink.ikine和SerialLink.ikunc逆向运动学，给变换矩阵，不考虑关节限制求关节变量

q1 = bot.ikine(T,'mask',[1 1 1 1 1 0]);  %当关节数量小于6时，要用mask向量
q2 = bot.ikunc(T); %不需考虑
输出结果（1.5708为pi/2）
q1 =
 
    1.5708    1.5708   -0.0000    0.0000         0
 
q2 =
 
    1.5708    1.5708    0.0000    0.0000   -0.0000

四.建立一个简单四关节机器人

建立一个只在xoz平面运动的四关节机器人。其中Link.qlim给关节增加限制，让第一关节锁死。

clear;
clc;
L(1) = Link('standard','d', 100, 'a', 0, 'alpha', pi/2,'offset',0,'qlim',[0,0]/180*pi);
L(2) = Link('revolute','d', 0, 'a', 420, 'alpha', 0,'offset',pi/2,'qlim',[-90,90]/180*pi);
L(3) = Link('revolute','d', 0, 'a', 480, 'alpha', 0,'offset',-pi/2,'qlim',[-90,130]/180*pi);
L(4) = Link('revolute','d', 0, 'a', 840, 'alpha', 0,'offset',pi/2,'qlim',[-120,90]/180*pi);
 
bot = SerialLink(L, 'name', '4-dof'); %
bot.base = transl(0,0,1); %世界坐标系定为（0，0，1）
bot.display(); %可查看D-H参数
 
q = [0 0 0 0];  %初始关节角为0
bot.plot(q);  %SerialLink.plot动画，可给定一个m*n的矩阵，n为机械臂所含关节个数
bot.teach  %示教
 
输出结果为：
bot = 
 
4-dof:: 4 axis, RRRR, stdDH, slowRNE                             
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|        100|          0|     1.5708|          0|
|  2|         q2|          0|        420|          0|     1.5708|
|  3|         q3|          0|        480|          0|    -1.5708|
|  4|         q4|          0|        840|          0|     1.5708|
+---+-----------+-----------+-----------+-----------+-----------+
base:    t = (0, 0, 1), RPY/xyz = (0, 0, 0) deg     

运行结果如图 

 

 接下来我们让它动个位置。

T1 = bot.fkine(q);  %初始位置
T2 = transl(0,0,1840);  %变换位置
 
q1 = bot.ikunc(T1);
q2 = bot.ikunc(T2);
 
pause  %按下空格后继续运行
bot.plot(q1);
pause
bot.plot(q2);

五.工作空间可视化

 在之前的代码中添加以下代码，利用rand随机绘制30000个点。

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

运行结果如图：

 六.轨迹规划

从当前位置移动到（0，0，1841），用末端执行器位置来规划会出现一些bug，采用关节角较为简单，关节角为[0,0,pi/2,-pi/2]。

T1 = [0,0,0,0];
T2 = [0,0,pi/2,-pi/2];
step = 50; %插入50个值
[q,qd,qdd] = jtraj(T1,T2,step); %五次多多项式关节空间轨迹规划
bot.plot(q,'trail','b');  %运行后在命令行窗口再复制运行一次，trail轨迹，b蓝色

 轨迹运行结果如图。引用链接http://t.csdn.cn/Wi0ce