机械臂Matlab仿真——路径规划之三阶多项式路径点规划以及五阶多项式路径点规划

仿真平台：Matlab 2019b 实时编辑器（旧的版本好像没有这几个路径规划函数）
机械臂路径点跟踪有几个基本方法：三阶多项式拟合，五阶多项式拟合以及抛物线拟合（理论来自于《机器人学导论》作者：Jhon J.Craig，这里不再赘述）

原理都不难，编程难度应该也不大，但是刚好新版本的matlab更新了这几个函数（能拿来用为什么要自己写）因此本节简单介绍这两个路径规划函数的使用：

1. 三阶路径规划：cubicpolytraj
2. 五阶路径规划：quinticpolytraj

1.三阶多项式规划函数

%假设是个二维机器人，关节空间下，关节（q1，q2）从（0，1）运动到（0，2），再到（2，4）
wpts = [0 0 2;1 2 4];
%初始时间是0s，终点时间是2s
tpts = [0:2];
%仿真所用的时长为2s，仿真时间间隔为0.01s
tvec = 0:0.01:2;
%利用三阶多项式进行路径规划，得到的结果是规划出来的路径q，速度qd，加速度qdd，已经三阶多项式的全部系数pp
[q, qd, qdd, pp] = cubicpolytraj(wpts, tpts, tvec);
%看看规划出来的路径q1 与q2
plot(tvec, q)
hold all
%看三个waypoints，用小叉表示，不是字母x
plot(tpts, wpts, 'x')
xlabel('t')
ylabel('Positions')
legend('X-positions','Y-positions')
hold off
%看q的速度
plot(tvec, qd)
%q的加速度
plot(tvec, qdd)
%用2D图看关节空间的整个变化情况
figure
plot(q(1,:),q(2,:),'-b',wpts(1,:),wpts(2,:),'or')
xlabel('X')
ylabel('Y')

仿真结果
q的变化以及各个路径点

q的速度

q的加速度

x y 二维图

2.五阶多项式路径规划函数

代码跟上面完全一样，只需要改一下路径规划语句对应的函数名

%假设是个二维机器人，关节空间下，关节（q1，q2）从（0，1）运动到（0，2），再到（2，4）
wpts = [0 0 2;1 2 4];
%初始时间是0s，终点时间是2s
tpts = [0:2];
%仿真所用的时长为2s，仿真时间间隔为0.01s
tvec = 0:0.01:2;
%利用三阶多项式进行路径规划，得到的结果是规划出来的路径q，速度qd，加速度qdd，已经三阶多项式的全部系数pp
[q, qd, qdd, pp] = quinticpolytraj(wpts, tpts, tvec);
%看看规划出来的路径q1 与q2
plot(tvec, q)
hold all
%看三个waypoints，用小叉表示，不是字母x
plot(tpts, wpts, 'x')
xlabel('t')
ylabel('Positions')
legend('X-positions','Y-positions')
hold off
%看q的速度
plot(tvec, qd)
%q的加速度
plot(tvec, qdd)
%用2D图看关节空间的整个变化情况
figure
plot(q(1,:),q(2,:),'-b',wpts(1,:),wpts(2,:),'or')
xlabel('X')
ylabel('Y')

仿真结果
q1，q2的变化以及各个路径点

q的速度

q的加速度

q1，q2的二维图像