机器人中的轨迹规划（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 = (: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第一个参数为单自由度轨迹生成函数的句柄，q₀和q_f分别为起始和目标点的坐标（是一个多维向量）。

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) 

　　在指定的时间内x₁从0运动到1，x₂从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