Matlab机器人工具箱（Robotics Toolbox）学习笔记
目录
- 下载安装与基础知识
-
- 机器人工具箱下载安装
- 基本函数使用方法
-
- 位姿描述
- 建立机器人模型
- 机器人运动学
-
- 正运动学
- 逆运动学
-
- 关于正运动学的分析
- 关于逆运动学的分析
- 雅克比矩阵
- 带有移动关节的机器人
-
- 机器人工具箱自带的模型
- 建立自己的移动关节机器人模型
- 蒙特卡罗法的工作域分析
- 机器人动力学
-
- 查看模型动力学参数 R.dyn()
- 逆动力学 R.rne()
- 动力学方程
-
- 重力载荷 R.gravload（）
- 关节空间惯性矩阵 R.inertia()
- 科氏力和向心力耦合矩阵 R.coriolis()
- 计算有效载荷 R.payload()
- 正向动力学
-
- 赋予机器人动力学参数
机器人工具箱其实以前上课的时候接触过，跟着老师的课件一步步的做过实验，但是课程结束后就没有再用过。。。
现在由于将来课题的原因，再加上老师要求学习的MBD课程要用到，所以来仔细学习一下。
❤ 2020.5.9 ❤
上了这么多年学，都没怎么学习过matlab，感觉血亏。不过该来的总是要来的，毕业课题加上MBD，让我不得不认真学一学matlab，虽然基本操作现在马马虎虎会了一点，可是各种工具箱才是matlab的精髓。
matlab最简洁的零基础教程：Matlab入门教程【基础向】
当然，这里讲的真的是太零基础了，以至于学过任何一种编程语言的人都会觉得简单，当然用来了解一下matlab的基础语法格式还是可以的。
下载安装与基础知识
机器人工具箱下载安装
matlab的机器人工具箱是个第三方工具箱，我试了在matlab的附加功能资源管理器里是没法直接下载的，只能去作者的官网下载。
作者名叫Peter Corke，其个人网站及机器人工具箱的官网为：
Peter Corke
作者针对工具箱还编写了一本书，可以算是工具箱的说明书了，在网站上可以直接下载英文版，还有中文官方版，只是官网上没有下载，当然除了买之外也有其他神奇的方法可以得到。。。
在作者的个人网站或者说机器人工具箱的官方网站下载好工具包后，解压复制到matlab的toolbox文件夹里
将路径复制到matlab的主窗口中，回车进入
双击打开startup_rvc.m，点击运行
命令窗口提示如下，安装成功。
然后添加路径
在命令窗口输入 ver ，检测机器人工具箱是否安装成功
这样就是成功了。
还有一步不是必须的，但是MBD课程的文件里提到了，就是把机器人工具箱的功能块添加到simulink的library browser里，首先在MATLAB\R2018b\toolbox\rvctools\simulink下建立一个脚本文件slblocks.m，然后添加如下内容
function blkStruct = slblocks
%SLBLOCKS Defines a block library.
% Library's name. The name appears in the Library Browser's
% contents pane.
blkStruct.Name = ['Robot' sprintf('\n') 'ToolBox'];
% The function that will be called when the user double-clicks on
% the library's name. ;
blkStruct.OpenFcn = 'roblocks';
% The argument to be set as the Mask Display for the subsystem. You
% may comment this line out if no specific mask is desired.
% Example: blkStruct.MaskDisplay = 'plot([0:2*pi],sin([0:2*pi]));';
% No display for now.
% blkStruct.MaskDisplay = '';
% End of blocks
运行，显示如下内容应该就是成功了
这个文件的作用可以参照matlab的帮助文件，“将库添加到 Library Browser 中”字段，但是这个脚本的内容和帮助文件还是有很大差别的，毕竟这个脚本是mbd课程提供的软件包里的机器人工具箱里提供的，反正能用
好了，启动simulink，然后就可以在library browser里面找到robot toolbox的内容了
除了下载安装包之外，还可以在官网下载 RTB.mltbx 文件，然后直接打开或在matlab中打开就可以安装工具箱，只是这样安装之后怎么调用simulink功能块就不知道了。
❤ 2020.5.10 ❤
下面记录一下机器人工具箱的基本功能使用方法，内容来源见
→→→劉海濤LHT
这是系列的第一集，剩下的我就不贴了
基本函数使用方法
位姿描述
〇 二维空间位姿描述
SE2()函数的旋转角使用角度制，如pi/3这样的数据。注意SE要大写（至少我的小写是不行的），当需要使用角度制数据时要写成T=SE2（1,2,60,‘deg’）的格式。
trplot2()函数的参数为SE2()函数生成的旋转矩阵，生成旋转后的坐标。
※ 如果需要用角度制表示则写成这样
〇 三维空间位姿描述
rotx/roty/rotz是绕轴转动指定角度生成三维变换矩阵，如果用角度制要加’deg’。
trplot是三维空间的绘图指令。
tranimate()是把变换动作用动画显示出来。
transl()是三维空间平移。
trotx/torty/trotz是三维齐次变换矩阵，输出的是4x4的矩阵，除了矩阵中除了表示旋转角度的3x3矩阵之外还有一个1。
建立机器人模型
〇 Link类
Link()函数的作用是在matlab中建立机器人的DH参数，首先需要对机器人的DH参数进行建模，当然这就是机器人学的内容了，等我有机会再详细写一个机器人学的学习笔记。
单个关节的link函数效果如下。注意使用改进DH方法时必须加’modified’，不然默认使用标准DH。
建立link之后可以查询link的属性，比如查询关节类型
可以查询的属性参见图片。sigma是干嘛的我还不知道，教程里没说
〇 SerialLink类
用来把定义好的连杆整合成一个机器人。
比如运行这样一个脚本，先定义6个连杆，再将连杆整合
然后可以通过指令查询Six_Link的属性，比如
属性的类型从上图查询。
属性可以再定义之后修改，也可以在定义的时候添加，比如
然后再查询
关于.plot(theta)，这个是一个重要的绘图指令，通过制定各个关节角的角度来显示机器人的位置，比如
可以得到图像
不要在意这些扭曲的关节。
关于.display，就是把这个机器人的参数全展示出来
关于.teach，就是示教模式，图形示教器，输入
之后，会显示这么一个东西，可以在里面改变各个关节的角度。
机器人运动学
正运动学
关于.fkine()命令是运动学正解，生成对应关节角度的末端其次变换矩阵，比如
逆运动学
关于.ikine6s()yu.ikine()，这两个都是求运动学的逆解，有6s的是封闭解，这个指令只能应用于标准型DH建模方法建立的模型，如果用的改进DH会报错，不知道为什么。。。
然后用.ikine()，可是。。。
应该是模型建立的问题，不过视频里说这个函数就是有问题的，如果想实现反解运算还需要自己写函数。。。我去。。。
先继续吧。
☆ 一个赞破
以matlab内置的puma560模型为例
首先打开puma560
如图所示是puma560的一些参数和指令
可以用绘图指令画一画，比如
运动学正解
运动学反解封闭解
运动学反解数值解
震惊，居然不一样。。。
关于正运动学的分析
根据论文《协作机器人零力控制与碰撞检测技术研究》_陈赛旋
〇 根据表达式判断所建立的DH模型是标准型（Standard DH）还是改进型（Modified DH）
第三四行的首元素为0的是标准型
长成这样的是改进型
〇 机器人逆解脚本程序编写
1、把推导出来的变换矩阵在脚本文件中表示出来
syms a2 a3 d1 d4 d5 d6 theta1 theta2 theta3 theta4 theta5 theta6
T01 = [ cos(theta1) 0 sin(theta1) 0;
sin(theta1) 0 -cos(theta1) 0;
0 1 0 d1;
0 0 0 1];
T12 =[ cos(theta2) -sin(theta2) 0 a2*cos(theta2);
sin(theta2) cos(theta2) 0 a2*sin(theta2);
0 0 1 0;
0 0 0 1];
T23 =[ cos(theta3) -sin(theta3) 0 a3*cos(theta3);
sin(theta3) cos(theta3) 0 a3*sin(theta3);
0 0 1 0;
0 0 0 1];
T34 = [ cos(theta4) 0 sin(theta4) 0;
sin(theta4) 0 -cos(theta4) 0;
0 1 0 d4;
0 0 0 1];
T45=[ cos(theta5) 0 -sin(theta5) 0;
sin(theta5) 0 cos(theta5) 0;
0 -1 0 d5;
0 0 0 1];
T56=[ cos(theta6) -sin(theta6) 0 0;
sin(theta6) cos(theta6) 0 0;
0 0 1 d6;
0 0 0 1];
T=T01*T12*T23*T34*T45*T56
2、将矩阵化简
用simplify()函数
关于逆运动学的分析
通过左乘T01的逆，构造4x4个方程
syms a2 a3 d1 d4 d5 d6 theta1 theta2 theta3 theta4 theta5 theta6 nx ny nz ox oy oz ax ay az px py pz
T01 = [ cos(theta1) 0 sin(theta1) 0;
sin(theta1) 0 -cos(theta1) 0;
0 1 0 d1;
0 0 0 1];
T12 =[ cos(theta2) -sin(theta2) 0 a2*cos(theta2);
sin(theta2) cos(theta2) 0 a2*sin(theta2);
0 0 1 0;
0 0 0 1];
T23 =[ cos(theta3) -sin(theta3) 0 a3*cos(theta3);
sin(theta3) cos(theta3) 0 a3*sin(theta3);
0 0 1 0;
0 0 0 1];
T34 = [ cos(theta4) 0 sin(theta4) 0;
sin(theta4) 0 -cos(theta4) 0;
0 1 0 d4;
0 0 0 1];
T45=[ cos(theta5) 0 -sin(theta5) 0;
sin(theta5) 0 cos(theta5) 0;
0 -1 0 d5;
0 0 0 1];
T56=[ cos(theta6) -sin(theta6) 0 0;
sin(theta6) cos(theta6) 0 0;
0 0 1 d6;
0 0 0 1];
TT=[ nx oz ax pz;
ny oy ay py;
nz oz az pz;
0 0 0 1];
T1=inv(T01)*TT
T2=T01*T12*T23*T34*T45*T56
化简T1、T2
（以下全是论文原文）
求解关节角
注意atan2（）是个很重要的函数，在这里求解必须用这个，老师说的我也没太明白
雅克比矩阵
先建立机器人模型然后
.jacob0()：针对基坐标
.jacobn()：针对工具坐标
两者可以转化
❤ 2022.6.8 ❤
带有移动关节的机器人
机器人工具箱自带的模型
可以查看该机器人的动画
建立自己的移动关节机器人模型
※ 注意：在建立模型的过程中，Link（）函数里面“d”和“theta”谁是变量就不写谁。
%关节角、连杆偏距、连杆长度、连杆转角
L(1) = Link('d', 0, 'a',0, 'alpha',0, 'qlim',[-pi,pi], 'modified');
L(2) = Link('theta',0, 'a',0, 'alpha',pi/2, 'qlim',[0,10], 'modified');
L(3) = Link('d', 0.1, 'a',0, 'alpha',0, 'qlim',[-pi,pi], 'modified');
RPR_robot = SerialLink(L,'name','RPR');
RPR_robot.plotopt={'workspace',[-10,10,-10,10,-10,10]};
RPR_robot.teach
蒙特卡罗法的工作域分析
〇 蒙特卡罗法
代码
%定义D-H参数
a2= 0.420;
a3= 0.375;
d2= 0.138+0.024;
d3=-0.127-0.024;
d4= 0.114+0.021;
d5= 0.114+0.021;
d6= 0.090+0.021;
%定义出XYZ的坐标值。
for i=1:100000
theta1=-pi +2*pi*rand;
theta2=0 +2*pi*rand;
theta3=-(5/6)*pi+(5/3)*pi*rand;
theta4=-pi +2*pi*rand;
theta5=-pi +2*pi*rand;
theta6=-pi +2*pi*rand;
x(i)= a2*cos(theta1)*cos(theta2)+a3*cos(theta1)*cos(theta2+theta3)-d5*cos(theta1)*sin(theta2+theta3+theta4)-sin(theta1)*(d2+d3+d4)-d6*(cos(theta5)*sin(theta1)-cos(theta1)*cos(theta2+theta3+theta4)*sin(theta5));
y(i)=d6*(cos(theta1)*cos(theta5)+cos(theta2+theta3+theta4)*sin(theta1)*sin(theta5))+a3*sin(theta1)*cos(theta2+theta3)-d5*sin(theta1)*sin(theta2+theta3+theta4)+cos(theta1)*(d2+d3+d4)+a2*cos(theta2)*sin(theta1);
z(i)=-a3*sin(theta2+theta3)-a2*sin(theta2)-d5*cos(theta2+theta3+theta4)-d6*sin(theta5)*sin(theta2+theta3+theta4);
end
plot3(x,y,z,'b.','MarkerSize',0.5)
运行结果
机器人动力学
〇 工具箱中与动力学相关的方程
查看模型动力学参数 R.dyn()
然后就会显示出每个关节的参数
2-6略
也可以显示单个关节的参数
或
逆动力学 R.rne()
〇 举例
动力学方程
重力载荷 R.gravload（）
计算重力载荷，参数为所求关节位置，结果是个向量
关节空间惯性矩阵 R.inertia()
科氏力和向心力耦合矩阵 R.coriolis()
先给定一个速度
qn是模型里的当前位置
计算有效载荷 R.payload()
先计算没有加载时的惯性矩阵
加载有效载荷
再计算惯性矩阵，发现数值改变了
正向动力学
〇 例子
mdl_puma560;
torqfun = [1 2 3 4 5 6];
p560 = p560.nofriction();%没有摩擦的动力学模型，加快求解速度
[T,q,qd] = p560.fdyn(1,torqfun)
for kk = 1:65
qdd(kk,:) = p560.accel(q(kk,:),qd(kk,:),torqfun);
end
正常可以算出关节的加速度
但是报错了
查了一下好像新版本的fdyn（）函数有点不一样
没看太懂，等需要用的到的时候再说吧。
赋予机器人动力学参数
首先建立DH模型
L(1)= Link([0 0 0 0],'modified');
L(2)= Link([0 0.138+0.024 0 -pi/2],'modified');
L(3)= Link([0 -0.127-0.024 0.420 0], 'modified');
L(4)= Link([0 0.114+0.021 0.375 0],'modified');
L(5)= Link([0 0.114+0.021 0 -pi/2],'modified');
L(6)= Link([0 0.090+0.021 0 pi/2],'modified');
Six_Link=SerialLink(L,'name','sixlink');
然后按照文档添加动力学参数
L(1).m = 0.77;
L(2).m = 0.99;
L(1).qlim = [1,3];
然后再查看动力学参数
