机器人轨迹规划

  • 关节空间轨迹

[q,qd,qdd] = jtraij(q0,qf,m)

该函数表示关节坐标从初始关节角度q0（1xN）到终止关节角度qf（1xN）变化。默认利用五次多项式与默认零边界条件计算轨迹。假定时间以M步从0到1变化。关节速度和加速度可以分别以qd（MxN）qdd（MxN）返回。轨迹q，qd和qdd是MxN矩阵，每个时间步长一行，每个关节一列。

  • 笛卡尔空间轨迹

Tc=ctraj（T0，T1，n）

该函数表示从姿态T0到T1的笛卡尔轨迹（4x4xN），沿路径具有梯形速度分布的N个点。Tc是齐次变换序列，最后一个下标是点索引，即T（：，：，i）是第i个点路径。如果T0或T1为[]，则视其为单位矩阵。

知道了上述轨迹规划函数之后，我们可以通过工具箱自带的PUMA560机器人模型进行试验。在matlab中输入 mdl_pima560 ,我们将获得机器人模型以及几个模型的初始状态参数。

mdl_puma560    %打开模型
qz             %零角度状态
qr             %就绪状态，机械臂伸直且垂直
qs             %伸展状态，机械臂伸直且水平
qn             %标准状态，机械臂灵巧工作状态
p560.plot(qn)  %画出标准状态的图形

接下来我将利用上述机器人模型来进行轨迹运动规划的仿真。

mdl_puma560%调出puma560 
t=[0:0.05:2];    %两秒完成轨迹，步长0.05
%产生位姿矩阵法1：直接给出关节角度
T1 = p560.fkine([0 0 0 0 0 0]);%生成一个位姿
T2 = p560.fkine([pi/2 pi/3 pi/6 0 0 0]);%生成一个位姿
q=p560.jtraj(T1,T2,t);%输入SE3 or 4*4变换矩阵，生成轨迹
%生成模型运动轨迹图
filename = 'demo.gif';
for i = 1:length(t)
    pause(0.01)
    p560.plot(q(i,:));
    f = getframe(gcf);  
    imind = frame2im(f);
    [imind,cm] = rgb2ind(imind,256);
    if i == 1
        imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',0.1);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
    end
end
  • 关节空间和笛卡尔空间的轨迹规划

关节空间规划是根据首位位姿，求出关节角度然后再进行规划，而笛卡尔空间规划是直接根据位姿进行规划。为了方便对比，接下来我将给出首位位姿矩阵，然后分别运用上述两种不同的方法进行规划，最后都转换为关节空间进行比较。

mdl_puma560%调出puma560 
t=[0:0.05:2];%两秒完成轨迹，步长0.05

T1 = [0     0    1    0.5963
      0     1    0   -0.1501
      -1    0    0   -0.01435
      0     0    0       1 ];
T2 = [0     0    1    0.8636
      0     1    0   -0.1501
      -1    0    0   -0.0203
      0     0    0       1];
%关节空间轨迹规划 
q1 = p560.ikine6s(T1);%根据末端位姿，求各关节
q2 = p560.ikine6s(T2);
qq=jtraj(q1,q2,t);%根据各关节，生成轨迹
Tqq=p560.fkine(qq);

%笛卡尔运动
%笛卡尔空间中直线运动,生成从SE3空间两点间直线的一系列中间位置，结果表达为4*4齐次换矩阵
Ts=ctraj(T1,T2,length(t));
qs=p560.ikine6s(Ts);

figure(1)%绘制各关节角度
for i=1:6
      subplot(2,3,i)
      plot(t, qq(:,i))
      hold on;
      plot(t, qs(:,i)) 
      legend('关节空间','笛卡尔空间')
      hold off;
end

figure(2)  %绘制空间运动轨
pq=transl(Tqq);%提取旋转矩阵中的位移部分
ps=transl(Ts);
%依次是 '关节空间','笛卡尔空间'
subplot(2,1,1)
plot3(pq(:,1),pq(:,2),pq(:,3))
subplot(2,1,2)
plot3(ps(:,1),ps(:,2),ps(:,3))
各关节的运动规划图
两种方法的运动轨迹图

观察可得：尽管机器臂最后的始末位置相同，但是在不同的轨迹规划下，机器臂的关节运动和末端执行器的运动轨迹确相差甚远。不同的方法各有自己的优缺点，在实际运用中，我们应该根据自己的实际需求来选择。