RoboticsToolbox实例学习汇总

PUMA560 的MATLAB 仿真
要建立PUMA560的机器人对象，首先我们要了解PUMA560的D-H参数, 之后我们可以利用Robotics Toolbox工具箱中的link和robot函数来建立PUMA560的机器人对象。

其中link函数的调用格式：
L = LINK([alpha A theta D])
L =LINK([alpha A theta D sigma])
L =LINK([alpha A theta D sigma offset])
L =LINK([alpha A theta D], CONVENTION)
L =LINK([alpha A theta D sigma], CONVENTION)
L =LINK([alpha A theta D sigma offset], CONVENTION)
参数CONVENTION 可以取’standard 和’modified '，其中’standarcT 代表采用标准的D-H 参数，‘ modified '代表采用改进的D-H参数。

参数’alpha' 代表扭转角，参数‘ A '代表杆件长度，参数‘theta'代表关节角，参数‘ D' 代表横距，参数‘sigma'代表关节类型：0代表旋转关节，非0代表移动关节。

另外LINK还有一些数据域：
LINK.alpha %返回扭转角
LINK.A %返回杆件长度
LINK.theta %返回关节角
LINK.D %返回横距
LINK.sigma %返回关节类型
LINK.RP %返回‘ R'(旋转)或‘ P'(移动)
LINK.mdh %若为标准D-H参数返回0,否则返回1
LINK.offset %返回关节变量偏移
LINK.qlim %返回关节变量的上下限[min max]
LINK.islimit(q) %如果关节变量超限，返回-1,0, +1
LINK.I %返回一个3X 3对称惯性矩阵
LINK.m %返回关节质量
LINK.r %返回3X 1的关节齿轮向量
LINK.G %返回齿轮的传动比
LINK.Jm %返回电机惯性
LINK.B %返回粘性摩擦
LINK.Tc %返回库仑摩擦
LINK.dh return legacy DH row
LINK.d yn return legacy DYN row
其中robot函数的调用格式：
ROBOT %创建一个空的机器人对象
ROBOT(robot) %创建robot 的一个副本
ROBOT(robot, LINK) %用LINK来创建新机器人对象来代替robot ROBOT(LINK,…)%用LINK 来创建一个机器人对象
ROBOT(DH,…)％用D-H矩阵来创建一个机器人对象
ROBOT(DYN,…)%用DYN矩阵来创建一个机器人对象
利用MATLAB 中Robotics Toolbox 工具箱中的transi、rotx、roty 和rotz 可以实现用齐次变换矩阵表示平移变换和旋转变换。

下面举例来说明：
A机器人在x轴方向平移了0.5米，那么我们可以用下面的方法来求取平移变换
后的齐次矩阵：
>> tran sl(0.5,0,0)
ans =
1.0000 0 0 0.5000
0 1.0000 0 0
0 0 1.0000 0
0 0 0 1.0000
B机器人绕x轴旋转45度，那么可以用rotx来求取旋转后的齐次矩阵:
>> rotx(pi/4)
ans =
1.0000 0 0 0
0 0.7071 -0.7071 0
0 0.7071 0.7071 0
0 0 0 1.0000
C机器人绕y轴旋转90度，那么可以用roty来求取旋转后的齐次矩阵:
>> roty(pi/2)
ans =
0.0000 0 1.0000 0
0 1.0000 0 0
-1.0000 0 0.0000 0
0 0 0 1.0000
D机器人绕z轴旋转-90度:，那么可以用rotz来求取旋转后的齐次矩阵:
>> rotz(-pi/2)
ans =
0.0000 1.0000 0 0
-1.0000 0.0000 0 0
0 0 1.0000 0
0 0 0 1.0000
当然，如果有多次旋转和平移变换，我们只需要多次调用函数在组合就可以了。

另外，可以和我们学习的平移矩阵和旋转矩阵做个对比，相信是一致的。

3轨迹规划
利用Robotics Toolbox提供的ct⑻、jt⑻和trinterp函数可以实现笛卡尔规划、关节空间规划和变换插值。

其中ct⑻函数的调用格式：
TC = CTRAJ(T0, T1, N)
TC = CTRAJ(T0, T1, R)
参数TC为从TO到T1的笛卡尔规划轨迹，N为点的数量，R为给定路径距离向量，R的每个值必须在0到1之间。

其中jt⑻函数的调用格式：
[Q QD QDD] = JTRAJ(QO, Q1, N)
[Q QD QDD] = JTRAJ(QO, Q1, N, QDO, QD1)
[Q QD QDD] = JTRAJ(QO, Q1, T)
[Q QD QDD] = JTRAJ(QO, Q1, T, QDO, QD1)
参数Q为从状态QO到Q1的关节空间规划轨迹，N为规划的点数，T为给定的时间向量的长度，速度非零边界可以用QDO和QD1来指定。

QD和QDD 为返回的规划轨迹的速度和加速度。

其中trinterp函数的调用格式：
TR = TRINTERP(TO, T1, R)
参数TR为在TO和T1之间的坐标变化插值，R需在0和1之间。

要实现轨迹规划，首先我们要创建一个时间向量，假设在两秒内完成某个动作，采样间隔是56ms,那么可以用如下的命令来实现多项式轨迹规划：t=0:0.056:2; [q,qd,qdd]=jt raj(q z,qr,t);
其中t为时间向量，qz为机器人的初始位姿，qr为机器人的最终位姿，q为经过的路径点，qd为运动的速度，qdd为运动的加速度。

其中q、qd、qdd都是六列的矩阵，每列代表每个关节的位置、速度和加速度。

如q(:,3)代表关节3的位置, qd(:,3)代表关节3的速度，qdd(:,3)代表关节3的加速度。

4运动学的正问题
利用Robotics Toolbox中的fkine函数可以实现机器人运动学正问题的求解。

其中fkine函数的调用格式：
TR = FKINE(ROBOT, Q)
参数ROBOT为一个机器人对象，TR为由Q定义的每个前向运动学的正解。

以PUMA560为例，定义关节坐标系的零点qz=[0 0 0 0 0 0],那么fkine(p560,qz) 将返回最后一个关节的平移的齐次变换矩阵。

如果有了关节的轨迹规划之后，我们也可以用fkine来进行运动学的正解。

比如：
t=0:0.056:2; q=jt ⑻(qz, qr,t); T=fki ne(p560,q);
返回的矩阵T是一个三维的矩阵，前两维是4X4的矩阵代表坐标变化，第三维是时间。

5运动学的逆问题
利用Robotics Toolbox中的ikine函数可以实现机器人运动学逆问题的求解。

其中ikine函数的调用格式：
Q = IKINE(ROBOT, T)
Q = IKINE(ROBOT, T, Q)
Q = IKINE(ROBOT, T, Q, M)
参数ROBOT为一个机器人对象，Q为初始猜测点(默认为0), T为要反解的变换矩阵。

当反解的机器人对象的自由度少于6时，要用M进行忽略某个关节自由度。

有了关节的轨迹规划之后，我们也可以用ikine函数来进行运动学逆问题的求解。

比如：
t=0:0.056:2; T1=transl(0.6,-0.5,0); T2=transl(0.4,0.5,0.2); T=ctraj(T1,T2,length(t)); q=ik in e(p560,T);
我们也可以尝试先进行正解，再进行逆解，看看能否还原。

Q=[0 -pi/4 -pi/4 0 pi/8 0]; T=fkine(p560,q); qi=ikine(p560,T);
6动画演示
有了机器人的轨迹规划之后，我们就可以利用Robotics Toolbox中的plot函数来实现对规划路径的仿真。

puma560;T=0:0.056:2; q=jtraj(qz,qr,T); plot(p560,q);
当然，我们也可以来调节PUMA560的六个旋转角，来实现动画演示。

drivebot(p560)
%旋转矩阵用欧拉角表示，
R = rotz(a)*roty(b)*rotz(c)
(4)eul = tr2eul(R); %旋转矩阵用 roll-pitch-yaw 角表示， R = rotx(r)*roty(p)*rotz(y)
(5)rpy = tr2rpy(R);
%旋转矩阵用四元数表示
则描述该转动的四元数可以表示成：
e , e _
q — cos — sin — n
2 2 e e e e
-cos — sin — cos m d + sin —cos /7 ・ j + sin —
cos / - k
一个有固定点的刚体通过绕该点的某个轴转
过特定角度可达到任何姿态
转轴的方向可以表示成一个单位矢量:
n — cos tz * / + cos 卩、j + k
四元数的表示:
2 片耳P.
g = 2 + PJ + PJ +
z “…标量部分
片2 PJ占吋•…矢量部分
包括一个实数单位1和三个虚数单位i,j, k
另一种表示法：9 =仇，尸)，P代表矢量部分(6)q = Quaternion(R);
%将四元数转化为旋转矩阵
(7)q.R；
%界面,可以是“ rpy,” “ elue角度单位为度。

(8)triplea ngle('rpy');
General/Transformations
%沿x轴平移0.5,绕y轴旋转pi/2，绕z轴旋转-pi/2 (1)t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2) %将齐次变换矩阵转化为欧拉角
(2)tr2eul(t)
%将齐次变换矩阵转化为roll、pitch、yaw角
(3)tr2rpy(t)
General/Trajectory
clear;
clc;
p0 = -1;%定义初始点及终点位置
p1 = 2;
p = tpoly(pO, p1,50);% 取步长为50
figure(1);
plot(p);%绘图，可以看到在初始点及终点的一、二阶导均为零
[p,pd,pdd] = tpoly(p0, p1,50);% 得到位置、速度、加速度
%p为五阶多项式，速度、加速度均在一定范围内
figure(2);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');
subplot(3,1,3); plot(pdd); xlabel('Time');
ylabel('pdd');
%另外一种方法：
[p,pd,pdd] = lspb(p0, p1,50);
figure(3);
subplot(3,1,1); plot(p); xlabel('Time'); ylabel('p');
subplot(3,1,2); plot(pd); xlabel('Time'); ylabel('pd');% 可以看到速度是呈梯形subplot(3,1,3); plot(pdd); xlabel('Time'); ylabel('pdd');
%三维的情况：
p = mtraj(@tpoly, [0 1 2], [2 1 0], 50); figure(4); plot(p)
%对于齐次变换矩阵的情况
TO = tran sl(0.4, 0.2, 0) * trotx(pi);% 定义初始点和目标点的位姿T1 = tran sl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-pi/2);
T = ctraj(T0, T1,50);
first=T(:,:,1);%初始位姿矩阵
tenth=T(:,:,10);%第十个位姿矩阵
figure(5);
tranimate(T);%动画演示坐标系自初始点运动到目标点的过程。