机器人中的轨迹规划（Trajectory Planning ）
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 = (0:t-1)'; else t = t(:); end if nargin < 4 qd0 = 0; end if nargin < 5 qdf = 0; end tf = max(t); % solve for the polynomial coefficients using least squares X = [ 0 0 0 0 0 1 tf^5 tf^4 tf^3 tf^2 tf 1 0 0 0 0 1 0 5*tf^4 4*tf^3 3*tf^2 2*tf 1 0 0 0 0 2 0 0 20*tf^3 12*tf^2 6*tf 2 0 0 ]; coeffs = (X \ [q0 qf qd0 qdf 0 0]')'; % coefficients of derivatives coeffs_d = coeffs(1:5) .* (5:-1:1); coeffs_dd = coeffs_d(1:4) .* (4:-1:1); % 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(0, 1, 50);
其位置、速度、加速度曲线如下图所示：
虽然这三条曲线都是连续且光滑的，但却存在一个很实际的问题。从速图曲线中可以看出在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 = (0:t-1)'; else t = t(:); end tf = max(t(:)); if nargin < 4 % 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) > 2*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), 1); sd = s; sdd = s; for i = 1:length(t) tt = t(i); if tt <= tb % 匀加速段 % initial blend s(i) = q0 + a/2*tt^2; sd(i) = a*tt; sdd(i) = a; elseif tt <= (tf-tb) % 匀速段 % linear motion s(i) = (q1+q0-V*tf)/2 + V*tt; sd(i) = V; sdd(i) = 0 else % 匀减速段 % final blend s(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2; 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=1: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
