MATLAB机器人工具箱 机械臂仿真
学习自B站：Nino_FM
采用 Standard DH 建模法
旋转算子
R = rotx(pi/2)
R =
1.0000000 0.9996 -0.02740 0.0274 0.9996 % 显然结果有问题
rotx(pi/2/pi*180)
ans =
1 0 00 0 -10 1 0
rotx(90)
ans =
1 0 00 0 -10 1 0
机械臂建模
Standard DH
clc;clear% Standard DHL(1) = Link('revolute','d',0.216,'a',0,'alpha',pi/2);L(2) = Link('revolute','d',0,'a',0.5,'alpha',0,'offset',pi/2);L(3) = Link('revolute','d',0,'a',sqrt(0.145^2+0.42746^2),'alpha',0,'offset',-atan(427.46/145));L(4) = Link('revolute','d',0,'a',0,'alpha',pi/2,'offset',atan(427.46/145));L(5) = Link('revolute','d',0.258,'a',0,'alpha',0);Five_dof = SerialLink(L,'name','5-dof');Five_dof.base = transl(0,0,0.28);Five_dof.teach
代码显示
机器人工具箱 常用函数
rotx() roty() rotz() eul2r() tr2eul() rpy2r() tr2rpy()
欧拉角
例子: r1 = eul2r(90,60,30) 与 r2 = rotz(90) * roty(60) * rotz(30) 等价
r1 =
-0.5000 -0.866000.4330 -0.2500 0.8660-0.7500 0.4330 0.5000
r2 =
-0.5000 -0.866000.4330 -0.2500 0.8660-0.7500 0.4330 0.5000
例子: r3 = rpy2r(90,60,30) 与 r4 = rotz(30)roty(60)rotx(90) 等价
r3 =
0.4330 0.7500 0.50000.2500 0.4330 -0.8660-0.8660 0.50000
r4 =
0.4330 0.7500 0.50000.2500 0.4330 -0.8660-0.8660 0.50000
旋转<------------------>变换矩阵
trotx() troty() trotz() eul2tr() tr2eul() rpy2tr() tr2rpy()
例子: T1 = eul2tr(90,60,30) 与 T2 = trotz(90) * troty(60) * trotz(30) 等价
T1 =
-0.5000 -0.8660000.4330 -0.2500 0.86600-0.7500 0.4330 0.50000000 1.0000
T2 =
-0.5000 -0.8660000.4330 -0.2500 0.86600-0.7500 0.4330 0.50000000 1.0000
例子: T3 = rpy2tr(90,60,30) 与 T4 = trotz(30) * troty(60) * trotx(90) 等价
T3 =
0.4330 0.7500 0.500000.2500 0.4330 -0.86600-0.8660 0.500000000 1.0000
T4 =
0.4330 0.7500 0.500000.2500 0.4330 -0.86600-0.8660 0.500000000 1.0000
位移、旋转矩阵<------------------>变换矩阵
transl() t2r() r2t()
例子: T = transl(1.5,1,0.5) * trotx(30) * trotz(60)
P = transl(T)
R = t2r(T)
T =
0.5000 -0.86600 1.50000.7500 0.4330 -0.5000 1.00000.4330 0.2500 0.8660 0.5000000 1.0000
P =
1.50001.00000.5000
R =
0.5000 -0.866000.7500 0.4330 -0.50000.4330 0.2500 0.8660
帮助文档
doc SerialLink
创建一个SerialLink机器人对象
SerialLink.teach
模型展示（SerialLink.plot）
q = [0 0 0 0 0]; % 初始位姿Five_dof = SerialLink(L,'name','5-dof');Five_dof.plot(q)
三维模型展示（plot3d）
mdl_puma560;p560.plot3d(qz,'view',[0 0]);
正向运动学（fkine）
q0 = [pi/2 pi/2 0 0 0];T = Five_dof.fkine(q0)
T =
0100
-100 -0.645
001 0.9015
0001
逆向运动学（ikine、ikunc......）
q1 = Five_dof.ikine(T,'mask',[1 1 1 1 1 0])q2 = Five_dof.ikunc(T)
q1 =
1.5708 1.5708 -0.0000 0.00000
q2 =
1.5708 1.5708 0.0000 0.0000 -0.0000
工作空间可视化
关节空间随机生成变量------(fkine)------>变换矩阵------(transl)------>三维坐标
rand函数
在[m,n]内随机生成一个数字：m + rand * (m-n)
随机关节空间变量：q= q_min + rand * (q_max - q_min)
L(1).qlim = [-150,150]/180*pi;L(2).qlim = [-100,90]/180*pi;L(3).qlim = [-90,90]/180*pi;L(4).qlim = [-100,100]/180*pi;L(5).qlim = [-180,180]/180*pi;num = 30000; % 迭代次数P = zeros(num, 3); % 初始化Pfor i =1:numq1 =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));q5 =L(5) .qlim(1) + rand * (L(5).qlim(2) - L(5).qlim(1));q = [q1 q2 q3 q4 q5];T = Five_dof.fkine(q);P(i, :) = transl(T);endplot3(P(:,1),P(:,2),P(:,3),'b.','markersize',1);hold ongrid ondaspect([1 1 1]);view([45 45]);Five_dof.plot([0 0 0 0 0]);
可视化显示
轨迹规划
五次多项式轨迹（tpoly）
t = linspace(0,2,51);[P,dP,ddP] = tpoly(0,3,t);% 指定初末速度%[P,dP,ddP] = tpoly(0,3,51,0.02,0.01);
混合曲线轨迹（lspb）
t = linspace(0,2,51);[P,dP,ddP] = lspb(0,3,t);% 指定最大速度%[P,dP,ddP] = tpoly(0,3,51,0.1);
多维轨迹（mtraj）
% 例子: (0,0)--------->(3,4)t = linspace(0,2,51);[P,dP,ddP] = mtraj(@tpoly,[0 0],[3 4],t);
多维多段轨迹（mstraj）
%例子: (0,0)----(2s)---->(3,4)----(1s)---->(1,2)% TRAJ = mstraj(WP,QDMAX,TSEG,Q0,DT,TACC,OPTIONS)WP = [0,0;3,4;1,2];P1 = mstraj(wp,[],[2,1],[],0.04,0);P2 = mstraj(wp,[],[2,1],[],0.04,0.5);
实例演示
给定位置（0.7,-0.5,0）--------------------->（0.7,0.5,0.5）
T1 = transl(0.7,-0.5,0)*trotx(180);T2 = transl(0.7,0.5,0)*trotx(180);q1 = Five_dof.ikunc(T1);q2 = Five_dof.ikunc(T2);Five_dof.plot(q1);pause;Five_dof.plot(q2);
P1/P2 -----------(mtraj)----------->Traj-----------(transl)----------->T-----------(ikunc)----------->Qtraj
轨迹：'trail' , 'b'
保存动画：'movie' , 'trail.gif'
P1 = [0.7,-0.5,0];P2 = [0.7,0.5,0.5];t = linspace(0,2,51);Traj = mtraj(@tpoly,P1,P2,t);n = size(Traj,1);T = zeros(4,4,n);for i = 1:nT(:, :, i) = transl(Traj(i,:))*trotx(180);endQtraj = Five_dof.ikunc(T);Five_dof.plot(Qtraj);% 绘制末端轨迹%Five_dof.plot(Qtraj,'trail','b')% 自动保存动画%Five_dof.plot(Qtraj,'movie','trail.gif');
绘制图像
hold onplot(t,Traj(:,1),'.-','linewidth',1);plot(t,Traj(:,2),'.-','linewidth',1);plot(t,Traj(:,3),'.-','linewidth',1);grid onlegend('x','y','z');xlabel('time');ylabel('position');
% 线性插值T_liner = trinterp(T1,T2,51);P_liner = transl(T_liner);t1 = linspace(0,2,51);subplot(1,2,1);hold on plot(t1,P_liner(:,1),'.-','linewidth',1);plot(t1,P_liner(:,2),'.-','linewidth',1);plot(t1,P_liner(:,3),'.-','linewidth',1);grid on legend('x','y','z');xlabel('time');ylabel('position');% 五次多项式插值T_tpoly = trinterp(T1,T2,tpoly(0,2,50)/2);P_tpoly = transl(T_tpoly);t2 = linspace(0,2,50);subplot(1,2,2);hold onplot(t2,P_tpoly(:,1),'.-','linewidth',1);plot(t2,P_tpoly(:,2),'.-','linewidth',1);plot(t2,P_tpoly(:,3),'.-','linewidth',1);grid on legend('x','y','z');xlabel('time');ylabel('position');
笛卡尔轨迹ctraj
% 笛卡尔轨迹ctrajT = ctraj(T1,T2,51);t1 = linspace(0,2,51);P1 = transl(T);hold on plot(t1,P1(:,1),'.-','linewidth',1);plot(t1,P1(:,2),'.-','linewidth',1);plot(t1,P1(:,3),'.-','linewidth',1);T = ctraj(T1,T2,tpoly(0,2,50)/2);t2 = linspace(0,2,50);P2 = transl(T);hold on plot(t2,P2(:,1),'.-','linewidth',1);plot(t2,P2(:,2),'.-','linewidth',1);plot(t2,P2(:,3),'.-','linewidth',1);grid on legend('x1','y1','z1','x2','y2','z2');xlabel('time');ylabel('position');
给定位置和姿态
（0.7,-0.5,0）* troty(150)--------------->（0.7,0.5,0.5）* trotx(200)
T1 = transl(0.7,-0.5,0)*troty(150);T2 = transl(0.7,0.5,0.5)*trotx(200);q1 = Five_dof.ikunc(T1);q2 = Five_dof.ikunc(T2);Five_dof.plot(q1);pauseFive_dof.plot(q2);
T1、T2------(tr2rpy)----->rpy1、rpy2------(mtraj)----->rpy_traj------(tr2rpy)----->T_raj_rot
rpy1 = [0,150,0]; %绕y轴转150度rpy2 = [200,0,0]; %绕x轴转200度t = linspace(0,2,51);rpy_traj = mtraj(@tpoly,rpy1,rpy2,t);T_traj_rot = rpy2tr(rpy_traj);P1 = transl(T1);P2 = transl(T2);P_traj = mtraj(@tpoly,P1',P2',t);T_traj_transl = transl(P_traj);n = length(t);T_traj = zeros(4,4,n);for i = 1:nT_traj(:,:,i) = T_traj_transl(:,:,i)*T_traj_rot(:,:,i);endq_traj = Five_dof.ikunc(T_traj);Five_dof.plot(q_traj,'trail','r')
上面的轨迹效果不太好，可用下面的代码替换
线性插值
% 线性插值t = linspace(0,2,51);T_traj = trinterp(T1,T2,t/2);q = Five_dof.ikunc(T_traj);Five_dof.plot(q,'trail','r')
梯形速度图像插值
% 梯形速度图像插值t = linspace(0,2,51);T_traj = ctraj(T1,T2,t/2);q = Five_dof.ikunc(T_traj);Five_dof.plot(q,'trail','r')
jtraj
% jtrajt = linspace(0,2,51);[q,dq,ddq] = jtraj(q1,q2,t);Five_dof.plot(q,'trail','r')
关节变量、速度图像、加速度图像
% 关节变量、速度图像、加速度图像subplot(1,3,1)plot(t,q)grid on xlabel('time')ylabel('q')legend('joint1','joint2','joint3','joint4','joint5')subplot(1,3,2)plot(t,dq)grid on xlabel('time')ylabel('dq')legend('joint1','joint2','joint3','joint4','joint5')subplot(1,3,3)plot(t,ddq)grid on xlabel('time')ylabel('ddq')legend('joint1','joint2','joint3','joint4','joint5')
