MATLAB机器人工具箱1-位姿描述及轨迹

1. 位姿描述

1.1 二维空间位姿描述

%开启机器人工具箱
startup_rvc;

代码：

clc;
clear;

%开启机器人工具箱
startup_rvc;

%%二维平面内的位姿描述
T1=SE2(1,2,30*pi/180);                  %建立齐次坐标变换，代表（1，2）的平移和30度的theta变换
trplot2(T1, 'frame', '1', 'color', 'r');   %绘制变换坐标系T1,名字1，颜色r
axis([0 5 0 5]);
T2=SE2(2, 1, 0);
hold on;
trplot(T2, 'frame', '2', 'color', 'b');
T3=T1*T2;
trplot(T3, 'frame', '3', 'color', 'g');
T4=T2*T1;
trplot(T4, 'frame', '4', 'color', 'c');

P=[2;3];                  %在世界坐标系建立点P
plot_point(P, '*');      %在世界坐标系中绘制点P
P1=double(inv(T1))* [P; 1];             %P点相对于坐标系{1}的坐标
P2=homtrans(double( inv(T2)), P);  %P点相对于坐标系{2}的坐标

 1.2 三维空间位姿描述

%%三维平面内的位姿描述
%rotx(angle),输入角度制的值
R1=rotx(60)    %表示X轴旋转；
R2=roty(90)    %表示Y轴旋转；
R3=rotz(90)    %表示Z轴旋转；
trplot(R2,'frame', '1', 'color', 'b');
grid on;
tranimate(R2)   %旋转动画，展示世界坐标系旋转到指定坐标系的过程

%trotx(angle),输入角度制的值
T1=trotx(90)   %表示X轴旋转
T2=transl(1,2,3) %平移
T3=T1*T2;
tranimate(T3);

2. 轨迹

2.1 平滑一维轨迹

clc;
clear;

%%轨迹生成
%tpoly(S0,SF,M) s a scalar trajectory (Mx1) that varies smoothly from S0 to SF in M steps using a quintic (5th order) polynomial.
s1=tpoly(0,1,50);
plot(s1); xlabel('time'); ylabel('s1');

%不设定初始速度时
%sd速度，sdd加速度。Velocity and acceleration can be optionally returned as SD (Mx1) and SDD (Mx1) respectively.
[s2,sd2,sdd2]=tpoly(0,1,50);
figure;
subplot(3,1,1)
plot(s2); xlabel('time'); ylabel('s2');
subplot(3,1,2)
plot(sd2); xlabel('time'); ylabel('sd2');
subplot(3,1,3)
plot(sdd2); xlabel('time'); ylabel('sdd2');

%设定初始和最终速度
[s3,sd3,sdd3]=tpoly(0, 1, 50, 0.5, 0);   %初始速度为0.5，终止速度为0
figure;
subplot(3,1,1)
plot(s3); xlabel('time'); ylabel('s3');
subplot(3,1,2)
plot(sd3); xlabel('time'); ylabel('sd3');
subplot(3,1,3)
plot(sdd3); xlabel('time'); ylabel('sdd3');

 2.2 抛物线轨迹规划

%%抛物线轨迹规划
s=lspb(0,1,50)
[s1,sd1,sdd1]=lspb(0,1,50);       
subplot(3,1,1) 		
plot(s1);xlabel('time'); ylabel('s1');
subplot(3,1,2)
plot(sd1);xlabel('time'); ylabel('sd1');
subplot(3,1,3)
plot(sdd1);xlabel('time'); ylabel('sdd1');

%指定速度时
[s2,sd2,sdd2]=lspb(0,1,50, 0.035);       %设定速度为0.035
figure;
subplot(3,1,1) 		
plot(s2);xlabel('time'); ylabel('s2');
subplot(3,1,2)
plot(sd2);xlabel('time'); ylabel('sd2');
subplot(3,1,3)
plot(sdd2);xlabel('time'); ylabel('sdd2');