Figure. Several possible path shapes for a single joint
五次多项式曲线(quintic polynomial)
$$\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$$
考虑边界条件：
$$\begin{align*}
\theta_0&=a_0\\
\theta_f&=a_0+a_1t+a_2{t_f}^2+a_3{t_f}^3+a_4{t_f}^4+a_5{t_f}^5\\
\dot{\theta_0}&=a_1\\
\dot{\theta_f}&=a_1+2a_2t_f+3a_3{t_f}^2+4a_4{t_f}^3+5a_5{t_f}^4\\
\ddot{\theta_0}&=2a_2\\
\ddot{\theta_f}&=2a_2+6a_3{t_f}+12a_4{t_f}^2+20a_5{t_f}^3\\
\end{align*}$$
这6组约束构成了一个6个未知数的线性方程组，可以求出系数为：
$$\begin{align*}
a_0&=\theta_0\\
a_1&=\dot{\theta_0}\\
a_2&=\frac{\ddot{\theta_0}}{2}\\
a_3&=\frac{20\theta_f-20\theta_0-(8\dot{\theta_f}+12\dot{\theta_0})t_f-(3\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^3}\\
a_4&=\frac{30\theta_0-30\theta_f+(14\dot{\theta_f}+16\dot{\theta_0})t_f+(3\ddot{\theta_0}-2\ddot{\theta_f}){t_f}^2}{2{t_f}^4}\\
a_5&=\frac{12\theta_f-12\theta_0-(6\dot{\theta_f}+6\dot{\theta_0})t_f-(\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^5}
\end{align*}$$
在MATLAB机器人工具箱中函数tpoly可以用于计算并生成机器人单轴的五次多项式轨迹曲线。当$t \in [0,T]$时，五次多项式曲线以及其一阶导数、二阶导数都是连续光滑的多项式曲线：
$$\begin{align*}
S(t)&=At^5+Bt^4+Ct^3+Dt^2+Et+F\\
\dot{S}(t)&=5At^4+4Bt^3+3Ct^2+2Dt+E\\
\ddot{S}(t)&=20At^3+12Bt^2+6Ct+2D
\end{align*}$$
根据约束条件
可以写出矩阵方程如下：
利用MATLAB提供的左除（反除）操作符，可以方便的求解线性方程组：Ax=b → x=A\b（表示矩阵A的逆乘以b)
tpoly.m主要内容如下：
%TPOLY Generate scalar polynomial trajectory % [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial
% and final joint velocity for the trajectory and time vector T. function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf) if isscalar(t)
t = (:t-)';
else
t = t(:);
end
if nargin <
qd0 = ;
end
if nargin <
qdf = ;
end tf = max(t);
% solve for the polynomial coefficients using least squares
X = [ tf^ tf^ tf^ tf^ tf *tf^ *tf^ *tf^ *tf *tf^ *tf^ *tf
];
coeffs = (X \ [q0 qf qd0 qdf ]')'; % coefficients of derivatives
coeffs_d = coeffs(:) .* (:-:);
coeffs_dd = coeffs_d(:) .* (:-:); % evaluate the polynomials
p = polyval(coeffs, t);
pd = polyval(coeffs_d, t);
pdd = polyval(coeffs_dd, t);
在MATLAB中输入下面命令生成从位置0运动到1的五次多项式曲线（时间步数为50步）：
>> [s, sd, sdd] = tpoly(, , );
其位置、速度、加速度曲线如下图所示：
虽然这三条曲线都是连续且光滑的，但却存在一个很实际的问题。从速图曲线中可以看出在t=25时速度达到最大值，没有匀速段，其它时刻速度都小于最大值。平均速度除以最大速度的值为：mean(sd) / max(sd) = 0.5231，即平均速度只有最大速度的一半左右，速度利用率较低。对于大多数实际伺服系统，电机的最大速度一般是固定的，因此希望速度曲线在最大速度的时间尽可能长。
梯形速度曲线 / Linear segment with parabolic blend (LSPB) trajectory
高次多项式轨迹曲线的计算量比较大，我们也可以考虑用直线段来构造简单的轨迹曲线，但是在不同直线段的交接处会发生速度跳变的情况（位移曲线不光滑），如果用抛物线（parabolic blend）进行拼接就可以得到光滑的轨迹。如下图所示，单轴从$t_0$开始匀加速运动（位移曲线为抛物线）；$t_b$时刻达到最大速度，进行匀速直线运动（位移曲线为直线段）；从$t_f-t_b$时刻开始进行匀减速运动，$t_f$时刻减速为零并到达目标位置。曲线关于时间中点$t_h$对称，由于这种轨迹的速度曲线是梯形的，因此也称为梯形速度（trapezoidal velocity trajectory）曲线，在电机驱动器中被广泛使用。
Figure. Linear segment with parabolic blends
MATLAB机器人工具箱中函数lspb可以用于计算并生成梯形速度曲线，下面的命令生成从位置0运动到1的梯形速度轨迹曲线，时间步数为50步，最大速度为默认值：
>> [s, sd, sdd] = lspb(0, 1, 50);
另外也可以指定最大速度（In fact the velocity cannot be chosen arbitrarily, too high or toolow a value for the maximum velocity will result in an infeasible trajectory ）：
>> s = lspb(0, 1, 50, 0.025);
>> s = lspb(0, 1, 50, 0.035);
下图a是默认最大速度的曲线，图b是指定不同速度的对比。
lspb.m的主要内容如下：
%LSPB Linear segment with parabolic blend
%
% [S,SD,SDD] = LSPB(S0, SF, M) is a scalar trajectory (Mx1) that varies
% smoothly from S0 to SF in M steps using a constant velocity segment and
% parabolic blends (a trapezoidal velocity profile). Velocity and
% acceleration can be optionally returned as SD (Mx1) and SDD (Mx1)
% respectively.
%
% [S,SD,SDD] = LSPB(S0, SF, M, V) as above but specifies the velocity of
% the linear segment which is normally computed automatically. function [s,sd,sdd] = lspb(q0, q1, t, V) if isscalar(t)
t = (:t-)';
else
t = t(:);
end tf = max(t(:)); if nargin <
% if velocity not specified, compute it
V = (q1-q0)/tf * 1.5;
else
V = abs(V) * sign(q1-q0); % 判断实际速度符号
if abs(V) < abs(q1-q0)/tf
error('V too small');
elseif abs(V) > *abs(q1-q0)/tf
error('V too big');
end
end if q0 == q1 % 目标位置和起始位置相同
s = ones(size(t)) * q0;
sd = zeros(size(t));
sdd = zeros(size(t));
return
end tb = (q0 - q1 + V*tf)/V; % 计算匀加减速段时间
a = V/tb; s = zeros(length(t), );
sd = s;
sdd = s; for i = :length(t)
tt = t(i); if tt <= tb % 匀加速段
% initial blend
s(i) = q0 + a/*tt^;
sd(i) = a*tt;
sdd(i) = a;
elseif tt <= (tf-tb) % 匀速段
% linear motion
s(i) = (q1+q0-V*tf)/ + V*tt;
sd(i) = V;
sdd(i) =
else % 匀减速段
% final blend
s(i) = q1 - a/*tf^ + a*tf*tt - a/*tt^;
sd(i) = a*tf - a*tt;
sdd(i) = -a;
end
end
多*度轨迹规划
机器人工具箱中的函数mtraj可以在内部调用单*度轨迹生成函数，来生成多个轴的运动轨迹。mtraj第一个参数为单*度轨迹生成函数的句柄，q0和qf分别为起始和目标点的坐标（是一个多维向量）。
function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M)
if ~isa(tfunc, 'function_handle')
error('first argument must be a function handle');
end
M0 = M;
if ~isscalar(M)
M = length(M);
end
if numcols(q0) ~= numcols(qf)
error('must be same number of columns in q0 and qf')
end
s = zeros(M, numcols(q0));
sd = zeros(M, numcols(q0));
sdd = zeros(M, numcols(q0));
for i=:numcols(q0)
% for each axis
[s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M);
end
mtraj可以调用tpoly或lspb，在50步内生成从(0, 2)运动到(1, -1)的轨迹。返回值x是一个50×2的矩阵，每一列代表一个轴的数据，每一行代表一个时间点。
>> x = mtraj(@tpoly, [0 2], [1 -1], 50);
>> x = mtraj(@lspb, [0 2], [1 -1], 50);
>> plot(x)
在指定的时间内x1从0运动到1，x2从2运动到-1：
参考：
V-rep学习笔记：Reflexxes Motion Library 4
Introduction to Robotics - Mechanics and Control. Chapter 7 Trajectory generation
Robotics, vision and control fundamental algorithms in MATLAB Chapter 3 Time and Motion