Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）

轨迹规划可以分为关节空间的轨迹规划和笛卡尔空间的轨迹规划

• 关节空间规划：用时小，计算量少
  [q,qt,qtt]=jtraj(q1,q2,t);
  返回关节的位置、速度、加速度
  使用五次多项式插值

• 笛卡尔空间规划：对于直线、圆形等末端轨迹形状要求严格的场合
  Ts=ctraj(T1,T2,length(t));
  返回末端位姿
  使用带抛物线过渡的直线规划（对位姿矩阵的3个位置量）

• 所用函数：
  p = transl(Ts);
  轨迹的位移部分

01 关节空间轨迹规划

绘制图为：

1. 机械臂位置及其运动过程
2. 关节 位置&速度&加速度 随时间变化曲线
3. 末端执行器轨迹(x-y视图)
4. 带轨迹标记的运动过程

% RoboticToolbox v10
% 关节空间轨迹规划
clc
 clear
%模型导入
    mdl_5dof    
    du = pi/180;   

%% 运动学轨迹
    % 改
    t=[0:0.1:8];%8秒完成轨迹，步长0.05
    
    %产生位姿矩阵法1：直接给出关节角度
    T1 = bot.fkine([20 50 -30 -25 -10]*du);%生成一个位姿
    T2 = bot.fkine([70 10 -60 -50 30]*du);%生成一个位姿
    
    %产生位姿矩阵法2：描述位置
    %T1 = transl(0.2,0.2,0.2)*trotx(pi/4);%位移*旋转,创建齐次变换
    %T2 = transl(0.2,-0.1,0.1)*trotx(pi/2);
    %T1 = transl(300)*trotz(pi/4);%位移*旋转,创建齐次变换
    %T2 = transl(200)*trotz(pi/2);    
    
    %关节角度
    q1 = bot.ikine(T1,'mask',[1 1 1 1 0 1]); %如果是[1 1 1 1 1 0]，则最后一个关节角度一直是0
    q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1); 
    
    %关节空间运动规划
    [q,qt,qtt]=jtraj(q1,q2,t);
   
    %笛卡尔运动规划
    %Ts=ctraj(T1,T2,length(t));
    
    bot.plot(q)%绘制轨迹
 %% 画图
    figure('name','关节随时间变化')
    subplot(3, 1, 1);
    plot(t, q,'LineWidth',1.5) %绘制关节角随时间的变化
    grid on;
    xlabel('时间(s)');ylabel('关节角度(rad)')
    set(gca,'YLim',[-3 2]);
    set(gca,'YTick',[-3,-2:2:2]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

    subplot(3,1,2);
    plot(t, qt,'LineWidth',1.5) %绘制关节角速度随时间的变化
    grid on;
    xlabel('时间(s)');ylabel('角速度(rad/s)')
    set(gca,'YLim',[-0.3 0.3]);
    set(gca,'YTick',[-0.3:0.15:0.3]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

    subplot(3, 1, 3);
    plot(t, qtt,'LineWidth',1.5) %绘制关节角加速度随时间的变化，如图2
    grid on;
    xlabel('时间(s)');ylabel('角加速度(rad/s^2)')
    set(gca,'YLim',[-0.12 0.12]);
    set(gca,'YTick',[-0.12:0.06:0.12]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

%%    
    %末端执行器轨迹(x-y视图)
    figure('name','末端执行器轨迹(x-y视图)')
    T = bot.fkine(q); %得到笛卡尔轨迹
    p = transl(T);%轨迹的位移部分
    plot(p(:,1),p(:,2),'LineWidth',1.5) %绘制xy平面内的末端轨迹，如图3
    xlabel('X轴(mm)');ylabel('Y轴(mm)')
    %title('末端执行器轨迹(x-y视图)')
    
    
    %带轨迹标记的运动过程
    figure('name',