基元轨迹：多项式轨迹——三次多项式轨迹

多项式轨迹

\quad 一般情况下，通过指定初始时刻$t_0$和终点时刻$t_1$ 的位置、速度和加速度等条件来定义一段运动。上面的表达可用一个函数来描述：
$$q=q(t),t\in [t_0,t_1]$$
此函数能满足给定条件：
$$q(t)=a_0+a_{1}t+a_2{t}^2+...+a_n{t}^n$$
其中，根据初始时刻和终点时刻的约束条件可确定$n+1$个$a_i$系数。多项式的次数$n$取决于需满足条件的数量和目标运动的期望“平滑度”。由于边界条件的数量通常是偶数，所以确定的系数个数也是偶数，即多项式函数的次数$n$是奇数，如3、5、7等。
\quad 通常情况下，除指定轨迹商的初始和终点时刻约束条件外，还可以指定其在某个时刻$t_f\in [t_0,t_1]$的条件，如速度、加速度和加加速度等。

三次多项式

\quad 当同时指定$t_0$和$t_1$时刻的位置和速度时，即已知$q_0、q_1、v_0、v_1$时，轨迹需要满足四个约束条件，即可以采用三次多项式来表示轨迹：
$$q(t)=a_0+a_1(t-t_0)+a_2(t-t_0{)}^2+a_3(t-t_0{)}^3{t}_0\le t\le t_1$$
将上述约束条件代入，即有：
$$\{\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\end{array}$$
可解得系数的值：
$$\{\begin{array}{rl}{a}_0& =q_0\\ a_1& =v_0\\ a_2& =\frac{3h-(2v_0+v_1)T}{T^2}\\ a_3& =\frac{-2h+(v_0+v_1)T}{T^3}\end{array}$$
其中：$h=q_1-q_0,T=t_1-t_0$。
\quad 基于上述结论可获得通过$n$个点序列并具有连续速度的轨迹。整段轨迹可分为$n-1$段，每一段在$t_k$和$t_{k+1}$时刻通过位置点$q_k$和$q_{k+1}$，并且速度分别为$v_k$和$v_{k+1}$。上述式子用于定义每一段中$4(n-1)$个参数$a_{0k}、a_{1k}$、$a_{2k}、a_{3k}$。

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

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


cubic_trajectory(t, q, v);



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

    n = length(t) - 1; % 轨迹段数
    a = zeros(n, 4); % 每一段的四个系数
    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) = (3*h(k) - (2*v(k) + v(k+1))*T(k)) / T(k)^2;
        a(k, 4) = (-2*h(k) + (v(k) + v(k+1))*T(k)) / T(k)^3;
    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;
                v_plot(i) = a(k, 2) + 2*a(k, 3)*tau + 3*a(k, 4)*tau^2;
                a_plot(i) = 2*a(k, 3) + 6*a(k, 4)*tau;
                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;
    v_points(1) = a(1, 2) + 2*a(1, 3)*t(1) + 3*a(1, 4)*t(1)^2;
    a_points(1) = 2*a(1, 3) + 6*a(1, 4)*t(1);
    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;
        v_points(k) = a(k-1, 2) + 2*a(k-1, 3)*tau + 3*a(k-1, 4)*tau^2;
        a_points(k) = 2*a(k-1, 3) + 6*a(k-1, 4)*tau;
    end
    q_points(end) = q(end);
    v_points(end) = v(end);
    a_points(end) = 2*a(n, 3) + 6*a(n, 4)*(t(end)-t(end-1));

    figure;
    subplot(3,1,1);
    plot(t_plot, q_plot, 'LineWidth', 1.5);
    hold on;
    plot(t, q_points, 'o', 'MarkerEdgeColor', 'r', 'MarkerFaceColor', 'none');
    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, 'o', 'MarkerEdgeColor', 'r', 'MarkerFaceColor', 'none');
    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, 'o', 'MarkerEdgeColor', 'r', 'MarkerFaceColor', 'none');
    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$， 即

t = [0, 8];
q = [0, 10];
v = [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 \end{gathered}$$
即输入为：

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