1、前记： 此篇说说利用如何在Matlab中进行机器人轨迹规划（Trajectory Planning）。先利用机器人工具箱（Robotics Toolbox）函数jtraj和ctraj作演示，来初步认识机器人轨迹规划如何在任务空间（Task Space）和关节空间（Joint Space）实现的，并再之前的系列中更新GUI的演示功能。 2、问题描述 轨迹规划是整个导航或运动规划问题的子集。典型的运动规划层次结构如下：
对于机器人的任务规划可以是自主的，也可以是在线人机交互的，也可以是预先定义的。路径规划也由上层的任务规划决定，要求机器人在运动过程不不能碰到障碍物。本文假定任务规划器中已经有一组路径点，我们的目的是生成一个轨迹，以实现随时间的推移在规定的约束（最大速度、加速度、动力学约束等）下来跟随这些路径点【路径的空间属性，轨迹的时间属性】。轨迹的跟踪需要实际的电机作为驱动源，通过底层的伺服控制来满足轨迹跟踪所需要的位置命令、力矩命令。 【针对如何从起点到目标点的路径规划问题，在古月居中已经有部分介绍https://www.guyuehome.com/tag/98，基本上属于路径规划系统的形式。对于工业机器人来讲，用于确定任务路径的一种常用方式是使用示教器或者物理性的引导（拖动示教）机器人达到所期望的运动，在工作任务中将示教的运动进行重新播放，即示教和运动重现。在某些情况下，如静态环境中多次执行相同的路径，使用示教的方式可能比一个路径规划系统更加有效。但在一些变结构复杂动态环境下，仅仅使用示教又会出现很多问题。例如，在需要人机协同工作的情况下，示教重现的方式会大大减低人机共融的协调性；在一些机器人分拣工作中不可能对每个物体进行抓取点的示教。所以不管是使用示教的方式完成任务还是使用路径规划系统完成任务，需依实际应用场景而定】 3、笛卡尔任务空间vs关节空间轨迹规划 机械臂的轨迹规划一般在机器人任务空间展开或者在关节空间展开。其中任务空间轨迹规划是指路径点和插补位于机械手特定位置的笛卡尔位姿(位置和方向)上，通常为末端执行器。这表明每个路径点和插补点都需要进行一次逆运动学计算，因此容易出现奇异位姿问题。关节空间轨迹规划是指路径点和插补点直接位于关节位置上(角度或位移，取决于关节类型)。也就是关节空间轨迹规划只需要在路径点上进行一次逆运动学求解，然后在关节空间中进行空间插值，因此易于实现对关节运动的约束，可以避免奇异位姿的问题。下图简单说明两者的优缺点。
现在我们以之前的GUI为例子，一方面来完善GUI的功能。另一方面来看看使用机器人工具箱中的关节空间轨迹规划函数jtraj与笛卡尔任务空间轨迹规划函数ctraj的进行轨迹规划的表现。【轨迹规划面板】
（1）关节空间规划jtraj函数 如上GUI所示，在这里主要是通过正运动学控制面板获得机器人的起始构型，并通过正运动计算获得末端位姿，在关节空间面板上设置输入机器人的终点位置（姿态与起始姿态一致）。【关节控件面板上的静态文本控件和可编辑文本设置就不多说了，静态文本作为输入位置的标识，用户在可编辑文本中输入终点位置和时间步】点击下面的Go按钮（Tag为JointMove），回调JointMove函数。 JointMove回调函数：
function JointMove_Callback(hObject, eventdata, handles)
Robot= evalin('base','Robot');%从基本工作空间导入机器人模型到GUI空间中
theta1=get(handles.Joint1,'value');
theta2=get(handles.Joint2,'value');
theta3=get(handles.Joint3,'value');
theta4=get(handles.Joint4,'value');
theta5=get(handles.Joint5,'value');
theta6=get(handles.Joint6,'value');
q0=[theta1 theta2 theta3 theta4 theta5 theta6];
T0=Robot.fkine(q0*pi/180); %通过正运动学控制面板获得初始位姿T0
R=[T0.n T0.o T0.a];%提取T0相同的姿态，se3--->3by3,下面操作构成齐次矩阵
numMats = size(R,3);H = zeros(4,4,numMats,'like',R);
H(1:3,1:3,:) = R;
H(4,4,:) = ones(1,1,numMats,'like',R);
X = str2double(get(handles.editX,'String'));
Y = str2double(get(handles.editY,'String'));
Z = str2double(get(handles.editZ,'String'));
time = str2double(get(handles.editT,'String'));
T1 = transl(X,Y,Z)*H;%关节空间面板获取的终点位姿
t = 0:0.05:time;%时间步0.05
traj = Robot.jtraj(T0,T1,t');%jtraj调用格式，规划的结果保存到traj中
axes(handles.JointTrajectory);%指定的关节变化坐标区
drawnow;
cla reset
qplot(t,traj);%画出规划的角度变化
title('关节角');
axes(handles.plot);%在plot区显示机器人运动
for i=1:size(traj,1)
q=traj(i,:);
Robot.plot(q);
theta1=q(1)*180/pi;theta2=q(2)*180/pi;theta3=q(3)*180/pi;
theta4=q(4)*180/pi;theta5=q(5)*180/pi;theta6=q(6)*180/pi;
ModelName = 'TestPuma560';%传递给Simulink的关节驱动中
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
T=Robot.fkine(q);
hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','c');%画出轨迹
X=T.t(1,1);Y=T.t(2,1);Z=T.t(3,1);
set(handles.X,'string',num2str(X));
set(handles.Y,'string',num2str(Y));
set(handles.Z,'string',num2str(Z));
set(handles.Joint1,'value',theta1);set(handles.Joint2,'value',theta2);
set(handles.Joint3,'value',theta3); set(handles.Joint4,'value',theta4);
set(handles.Joint5,'value',theta5); set(handles.Joint6,'value',theta6);
set(handles.edit1,'string',num2str(theta1)); set(handles.edit2,'string',num2str(theta2));
set(handles.edit3,'string',num2str(theta3)); set(handles.edit4,'string',num2str(theta4));
set(handles.edit5,'string',num2str(theta5)); set(handles.edit6,'string',num2str(theta6));
end 关节空间执行效果： 
如上,通过关节空间规划，机器人在运动过程中末端执行器在显示区留下青色轨迹为圆弧。 （2）任务空间规划ctraj函数 通过在笛卡尔空间控制面板获得机器人的起始位置和终点位置以及两个位置在笛卡尔空间的插补次数，点击下面的Go按钮（Tag为EndeffectMove），回调EndeffectMove函数。 EndeffectMove回调函数：
function EndeffectMove_Callback(hObject, eventdata, handles)
Robot= evalin('base','Robot');%从基础空间导入机器人模型到GUI空间
X0 = str2double(get(handles.editX0,'String'));
Y0 = str2double(get(handles.editY0,'String'));
Z0 = str2double(get(handles.editZ0,'String'));
STEP = str2double(get(handles.editStep,'String'));%获取机器人末端起始位置和插补次数
t = 0 : 1 : STEP;%步长
X1 = str2double(get(handles.editX1,'String'));
Y1 = str2double(get(handles.editY1,'String'));
Z1 = str2double(get(handles.editZ1,'String'));%获取机器人末端终点位置（姿态也可以类似定义输入）
T0 = transl(X0,Y0,Z0)*trotx(41*pi/180)*troty(132*pi/180)*trotz(133*pi/180); %初始点位姿（姿态固定）
q0 = Robot.ikine6s(T0);%逆解得起始点的关节构型
plot(Robot,q0);%显示起始点机器人构型状态
T1 = transl(X1,Y1,Z1)*trotx(41*pi/180)*troty(132*pi/180)*trotz(133*pi/180); %终点位姿（姿态固定）
T = ctraj(T0,T1,STEP); % 末端T0与T1之间进行插值
q1 = Robot.ikine6s(T);% 每个插值点进行逆运动学求解，q1为50by6的数组
drawnow;
axes(handles.JointTrajectory)%指定对象操作的坐标区
cla reset
qplot(q1);%在角度变化区画出角度变化
title('关节角')
axes(handles.plot)%机器人显示区显示动态变化
for i=1:size(q1,1)
q=q1(i,:);
Robot.plot(q);
theta1=q(1)*180/pi;theta2=q(2)*180/pi;theta3=q(3)*180/pi;
theta4=q(4)*180/pi;theta5=q(5)*180/pi;theta6=q(6)*180/pi;
ModelName = 'TestPuma560';%角度传给Simulink多体模型中的各关节驱动接口
set_param([ModelName '/Slider Gain'],'Gain',num2str(theta1));
set_param([ModelName '/Slider Gain1'],'Gain',num2str(theta2));
set_param([ModelName '/Slider Gain2'],'Gain',num2str(theta3));
set_param([ModelName '/Slider Gain3'],'Gain',num2str(theta4));
set_param([ModelName '/Slider Gain5'],'Gain',num2str(theta5));
set_param([ModelName '/Slider Gain6'],'Gain',num2str(theta6));
T=Robot.fkine(q);%正运动学计算获得末端位置
hold on
plot3(T.t(1,1),T.t(2,1),T.t(3,1),'.','LineWidth',3,'color','k');%显示规划的轨迹
X=T.t(1,1);Y=T.t(2,1);Z=T.t(3,1);%将结果显示到末端位姿面板和正运动学控制面板
set(handles.X,'string',num2str(X));
set(handles.Y,'string',num2str(Y));
set(handles.Z,'string',num2str(Z));
set(handles.Joint1,'value',theta1);set(handles.Joint2,'value',theta2);
set(handles.Joint3,'value',theta3); set(handles.Joint4,'value',theta4);
set(handles.Joint5,'value',theta5); set(handles.Joint6,'value',theta6);
set(handles.edit1,'string',num2str(theta1)); set(handles.edit2,'string',num2str(theta2));
set(handles.edit3,'string',num2str(theta3)); set(handles.edit4,'string',num2str(theta4));
set(handles.edit5,'string',num2str(theta5)); set(handles.edit6,'string',num2str(theta6));
end 任务笛卡尔空间执行效果： 
如上机器人显示区的黑色轨迹为任务空间轨迹规划的结果，轨迹为直线。 通过以上两个运动的比较，由于关节空间规划只考虑了机器人末端起始位置和末端目标位置，没考虑在运动过程中的末端的运动，所以可能导致机器人与附近的物体发生碰撞，而笛卡尔空间的规划机器人末端严格按照插补的路径点进行运动，所以对空间中的运动路径可控性要好些。 4、如何执行任务仿真 抛开上面的GUI演示功能。这里以关节空间轨迹规划函数jtraj为例，假如需要让机器人经过一些固定位置，那么我们就需要将路径点信息代入到关节空间轨迹规划函数中即可。如下scara机器人的打孔\拧螺丝动作和puma机器人对物体进行抓取和放置动作。 （1）Scara打孔、拧螺丝动作仿真
clc
clear
L1=Link([0 0 0 0 0],'modified');
L2=Link([0 0 200 pi 0],'modified');
L3=Link([0 0 150 0 1],'modified');%移动关节最后一个参数为1
L4=Link([0 0.20 0 0 0],'modified');
Robot=SerialLink([L1 L2 L3 L4],'name','Scara');
Robot.qlim=[-125*pi/180 125*pi/180;-140*pi/180 140*pi/180;0 200;-3*pi 3*pi];%关节限制
Robot.plot([0 0 0 0], 'workspace',[-400 400 -400 400 -300 300]);%空间范围定义
% Robot.teach()
q0=[0 0 0 0];%固定关节构型0
q1b=[pi/2 0 100 -3*pi];
q1=[pi/2 0 200 3*pi];
q2=[pi/2 0 0 pi];
q3=[0 0 0 0];
t=0:.04:2;
sqtraj1=jtraj(q0,q1b,t);%关节空间规划出的轨迹1
sqtraj2=jtraj(q1b,q1,t);%关节空间规划出的轨迹2
sqtraj3=jtraj(q1,q2,t);
sqtraj4=jtraj(q2,q3,t);
hold on
atj=zeros(4,4);
view(3)
hold on
for i=1:1:51 %运动显示
atj=Robot.fkine(sqtraj1(i,:));
jta=transpose(atj);
JTA(i,:)=jta.t;
jta=JTA;
plot2(jta(i,:),'r.')
Robot.plot(sqtraj1(i,:))
plot2(JTA,'b')
end
for i=1:1:51
atj=Robot.fkine(sqtraj2(i,:));
jta=transpose(atj);
JTA(i,:)=jta.t;
jta=JTA;
plot2(jta(i,:),'r.')
Robot.plot(sqtraj2(i,:))
plot2(JTA,'b')
end
for i=1:1:51
atj=Robot.fkine(sqtraj3(i,:));
jta=transpose(atj);
JTA(i,:)=jta.t;
jta=JTA;
plot2(jta(i,:),'r.')
Robot.plot(sqtraj3(i,:))
plot2(JTA,'b')
end
for i=1:1:51
atj=Robot.fkine(sqtraj4(i,:));
jta=transpose(atj);
JTA(i,:)=jta.t;
jta=JTA;
plot2(jta(i,:),'r.')
Robot.plot(sqtraj4(i,:))
plot2(JTA,'b')
end
（2）Puma560抓取和放置仿真
%% This code was tweaked by Mr Peter Corke，Love And Respect >_<
%%
mdl_puma560 %puma model
p = [0.8 0 -0.2];% ball position
T = transl(p) * troty(pi/2);%the pick pose
q1= [0.2,0.6,-2.6,0.47,0.46,1.2];%start point
qqr = p560.ikine6s(T, 'ru');% Pick pose configration
qqr(6)=1.2;
qrt = jtraj(q1, qqr,20); % path from qr' to T
ae = [-132 24];
p(1) = 1;
clf
plot_sphere(p, 0.045, 'b');%position of the ball
p560.plot3d(qrt, 'view', ae, 'nowrist'); % animate motion to the ball,no arrow on wrist frane
%% For pick and place
qr =[1.6,0.48,-2.4,0.5,0.4,1.2];
qrt = jtraj(qqr, qr, 20); % path from T to qr'
clf
for i=1:length(qrt)
T = p560.fkine(qrt(i,:)); % get EE pose
Tball = T * SE3(0,0,0.2); %tool transfrom
P = Tball.t;
clf
plot_sphere(P, 0.045, 'b');%plot moving sphere
p560.plot3d(qrt(i,:), 'view', ae, 'nowrist');
pause(0.01)
end
qrr=[1.6,0.8,-2.2,0.5,-0.6,1.2];
qrt1 = jtraj(qr, qrr, 15); % path from T to qr'
pause(0.5)
for i=1:length(qrt1)
T = p560.fkine(qrt1(i,:)); % get EE pose
clf
plot_sphere(P, 0.045, 'b');%plot moving sphere
p560.plot3d(qrt1(i,:), 'view', ae, 'nowrist');
pause(0.01)
end
对于笛卡尔空间轨迹规划去实现任务的过程也差不多，无非是对执行任务过程中的笛卡尔空间路径点之间进行插值，再在每个插值点进行逆解去跟踪路径，应用的场合多在激光切割、涂胶、焊接、3D打印等作业中，这里就不在举仿真例子了。 总结： （1）利用工具箱函数进行轨迹规划上面的演示只是冰山一角，古月居上有很多帖子也写过，如古月老师很久一篇帖子https://blog.75271.com/44985.html （2）关于轨迹规划Mathworks在官网blog中有介绍：https://blogs.mathworks.com/racing-lounge/2019/11/06/robot-manipulator-trajectory/?s_tid=srchtitle利用机器人系统工具箱函数（Robotics System Toolbox）进行轨迹规划的技术博客。 （3）如果官网的英文读得不满意，在Mathworks中国区的公众号上也有相关内容：https://mp.weixin.qq.com/s/uNW7Kbsr1rxFT6jqI-rCiA
