基元轨迹：多项式轨迹——五次多项式曲线

五次多项式曲线

\quad 由上一篇文章可知，当将一系列途经点$q_0,...,q_n$拟合为三次多项式轨迹时，其对应的位置和速度是连续的，但是通常情况下加速度是不连续的（加速度始末并不是0）。
\quad 尽管上述轨迹通常看上去足够“平滑”，但是加速度不连续会在某些应用中对运动链和惯性负载产生非期望的影响。尤其是站在较小的时间内指定了高的加速度值（速度值）或者在执行系统中机械弹性较大。
\quad 为了获得加速度连续的轨迹，除必须给定位置和速度的约束条件外，还必须指定加速度的初始值和最终值。因此，对应地存在六个边界约束条件（位置、速度和加速度），其对应的五次多项式如下所示：
$$q(t)=a_0+a_1(t-t_0)+a_2(t-t_0{)}^2+a_3(t-t_0{)}^3+a_4(t-t_0{)}^4+a_5(t-t_0{)}^5$$
满足以下约束条件：
$$\{\begin{array}{rlr}q(t_0)& =& q_0\\ q(t_1)& =& q_1\\ \dot{q}(t_0)& =& v_0\\ \dot{q}(t_1)& =& v_1\\ \ddot{q}(t_0)& =& A_0\\ \ddot{q}(t_1)& =& A_1\end{array}$$
可解得系数的值：
$$\{\begin{array}{rl}{a}_0& =q_0\\ a_1& =v_0\\ a_2& =\frac{1}{2}{A}_0\\ a_3& =\frac{1}{2T^3}[20h-(8v_1+12v_0)T-(3A_0-A_1)T^2]\\ a_4& =\frac{1}{2T^4}[-30h+(14v_1+16v_0)T+(3A_0-2A_1)T^2]\\ a_5& =\frac{1}{2T^5}[12h-6(v_1+v_0)T+(A_1-A_0)T^2]\end{array}$$
其中：$h=q_1-q_0,T=t_1-t_0$。

五次多项式轨迹代码实现（Matlab）

clc;
clear all;
t = [0, 8];
q = [0, 10];
v = [0, 0];
A = [0, 0];
quintic_trajectory(t, q, v, A);

function quintic_trajectory(t, q, v, A)
    % t: 时间点数组
    % q: 对应的位移点数组
    % v: 对应的速度点数组
    % A: 对应的加速度点数组

    n = length(t) - 1; % 轨迹段数
    a = zeros(n, 6); % 每一段的六个系数
    h = q(2:end) - q(1:end-1);
    T = t(2:end) - t(1:end-1);

    for k = 1:n
        a(k, 1) = q(k);
        a(k, 2) = v(k);
        a(k, 3) = 0.5 * A(k);
        a(k, 4) = (20 * h(k) - (8 * v(k + 1) + 12 * v(k)) * T(k) - (3 * A(k) - A(k + 1)) * T(k)^2) / (2 * T(k)^3);
        a(k, 5) = (-30 * h(k) + (14 * v(k + 1) + 16 * v(k)) * T(k) + (3 * A(k) - 2 * A(k + 1)) * T(k)^2) / (2 * T(k)^4);
        a(k, 6) = (12 * h(k) - 6 * (v(k + 1) + v(k)) * T(k) + (A(k + 1) - A(k)) * T(k)^2) / (2 * T(k)^5);
    end

    % 插值和绘图
    t_plot = linspace(t(1), t(end), 1000);
    q_plot = zeros(size(t_plot));
    v_plot = zeros(size(t_plot));
    A_plot = zeros(size(t_plot));

    for i = 1:length(t_plot)
        for k = 1:n
            if t_plot(i) >= t(k) && t_plot(i) <= t(k + 1)
                tau = t_plot(i) - t(k);
                q_plot(i) = a(k, 1) + a(k, 2) * tau + a(k, 3) * tau^2 + a(k, 4) * tau^3 + a(k, 5) * tau^4 + a(k, 6) * tau^5;
                v_plot(i) = a(k, 2) + 2 * a(k, 3) * tau + 3 * a(k, 4) * tau^2 + 4 * a(k, 5) * tau^3 + 5 * a(k, 6) * tau^4;
                A_plot(i) = 2 * a(k, 3) + 6 * a(k, 4) * tau + 12 * a(k, 5) * tau^2 + 20 * a(k, 6) * tau^3;
                break;
            end
        end
    end

    % 计算每个时间点的q, v, a
    q_points = zeros(size(t));
    v_points = zeros(size(t));
    A_points = zeros(size(t));
    q_points(1) = a(1, 1) + a(1, 2)*t(1) + a(1, 3)*t(1)^2 + a(1, 4)*t(1)^3 + a(1, 5) * t(1)^4 + a(1, 6) * t(1)^5;
    v_points(1) = a(1, 2) + 2*a(1, 3)*t(1) + 3*a(1, 4)*t(1)^2 + 4 * a(1, 5) * t(1)^3 + 5 * a(1, 6) * t(1)^4;
    A_points(1) = 2*a(1, 3) + 6*a(1, 4)*t(1) + 12 * a(1, 5) * t(1)^2 + 20 * a(1, 6) * t(1)^3;
    for k = 2:n
        tau = t(k) - t(k-1);
        q_points(k) = a(k-1, 1) + a(k-1, 2) * tau + a(k-1, 3) * tau^2 + a(k-1, 4) * tau^3 + a(k-1, 5) * tau^4 + a(k-1, 6) * tau^5;
        v_points(k) = a(k-1, 2) + 2 * a(k-1, 3) * tau + 3 * a(k-1, 4) * tau^2 + 4 * a(k-1, 5) * tau^3 + 5 * a(k-1, 6) * tau^4;
        A_points(k) = 2 * a(k-1, 3) + 6 * a(k-1, 4) * tau + 12 * a(k-1, 5) * tau^2 + 20 * a(k-1, 6) * tau^3;
    end
    q_points(end) = q(end);
    v_points(end) = v(end);
    A_points(end) = A(end);

    figure;
    subplot(3, 1, 1);
    plot(t_plot, q_plot, 'LineWidth', 1.5);
    hold on;
    plot(t, q_points, 'ro', 'MarkerFaceColor', 'r');
    xlabel('时间 (s)');
    ylabel('位置 (m)');
    title('位置-时间图');
    hold off;

    subplot(3, 1, 2);
    plot(t_plot, v_plot, 'LineWidth', 1.5);
    hold on;
    plot(t, v_points, 'ro', 'MarkerFaceColor', 'r');
    xlabel('时间 (s)');
    ylabel('速度 (m/s)');
    title('速度-时间图');
    hold off;

    subplot(3, 1, 3);
    plot(t_plot, A_plot, 'LineWidth', 1.5);
    hold on;
    plot(t, A_points, 'ro', 'MarkerFaceColor', 'r');
    xlabel('时间 (s)');
    ylabel('加速度 (m/s^2)');
    title('加速度-时间图');
    hold off;
end

结果

1. 当输入为：$q_0=0、q_1=10、t_0=0、t_1=8、v_0=v_1=0、A_1=A_2=0$， 即

t = [0, 8];
q = [0, 10];
v = [0, 0];
A = [0, 0];

2. 当输入为多点轨迹时间、位置、速度时：
$$\begin{gathered} t_0=0,t_1=2,t_2=4,t_3=8,t_4=10, \\ {q}_0=10,q_1=20,q_2=0,q_3=30,q_4=40, \\ {v}_0=0,v_1=-10,v_2=10,v_3=3,v_4=0, \\ {A}_0=0,A_1=0,A_2=0,A_3=0,A_4=0 \end{gathered}$$
即输入为：

t = [0, 2, 4, 8, 10];
q = [10, 20, 0, 30, 40];
v = [0, -10, 10, 3, 0];
A = [0, 0, 0, 0, 0];