（一）三次多項式軌跡規劃

一、什麼是多項式？

二、軌跡規劃中的三次多項式

選擇滿足要求的運動學性質的物理量如，如：位移、速度和加速度其實就完成了一個軌跡的規劃。以單軸關節角$x(t)$爲例，容易得：
$x(t)=c_0+c_1t+c_2t^2+c_3t^3$

$v(t)=c_1t+2c_2t+3c_3t^2$

$a(t)=2c_2+6c_3t$

三次多項式對四個運動量進行了規劃（約束），顯然x(t) v(t) a(t)均是連續函數：

• 起始絕對量
• 起始速度
• 結束絕對量
• 結束速度

每一個約束都對應一個方程，方程如下：

$x(t_s)=c_0+c_1t_s+c_2t_s^2+c_3t_s^3=x_s$

$x(t_e)=c_0+c_1t_e+c_2t_e^2+c_3t_e^3=x_e$

$v(t_s)=c_1t_s+2c_2t_s+3c_3t_s^2=vs$

$v(t_e)=c_1t_e+2c_2t_e+3c_3t_e^2=ve$

寫成矩陣形式：
$\begin{pmatrix} 1 & ts & ts^2&ts^3 \\ 1 & te & te^2&te^3 \\ 0 & 1 & 2ts&3ts^2\\ 0&1&2te&3te^2 \end{pmatrix} \begin{pmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \end{pmatrix}= \begin{pmatrix} x_s \\ x_e \\ v_s \\ v_e \end{pmatrix}$
到係數矩陣
$Trans*C=P$
$C=Trans^{-1}P$

獲得係數之後，軌跡就唯一確定了。即每一時刻都可以對應一個“位移量”。

Matlab實現如下,連續進行了三段曲線連續軌跡規劃：0-1，1-2，2-3

% 三次多項式插補
close all
clear;
clc;

% 0-1
t0=0; x0=30; v0=75;
t1=5; x1=60; v1=10;
[x01,v01,a01]=plan(t0,x0,v0,t1,x1,v1);

% 1-2
t2=13; x2=80; v2=10;
[x12,v12,a12]=plan(0,x1,v1,t2-t1,x2,v2);

% 2-3
t3=20; x3=10; v3=0;
[x23,v23,a23]=plan(0,x2,v2,t3-t2,x3,v3);


x=[x01 x12 x23];
v=[v01 v12 v23];
a=[a01 a12 a23];

figure(1);
subplot(3,1,1);
plot(x,'r','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('position')

subplot(3,1,2);
plot(v,'g','LineWidth',1.2)
axis([ t0*100 t3*100 -inf inf]);
ylabel('velocity')

subplot(3,1,3);
plot(a,'b','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('acceleration')
xlabel('time')

function [x,v,a]=plan(ts,start_x,start_v,te,end_x,end_v)
    para=[start_x,end_x,start_v,end_v]';
    Tran=[1,ts,ts^2,ts^3;1,te,te^2,te^3;0,1,2*ts,3*ts^2;0,1,2*te,3*te^2];
    C=(inv(Tran))*para;
    c0=C(1);
    c1=C(2);
    c2=C(3);
    c3=C(4);
    x=[];v=[];a=[];
    for i=ts:0.01:te
        t=i;
        x=[x c0+c1*t+c2*t^2+c3*t^3];
        v=[v c1+2*c2*t+3*c3*t^2];
        a=[a 2*c2+6*c3*t];
    end
end

結果如下：

雖然我們每一段的位移、速度和加速度都是連續的，但是顯然對於加速度而言，出現了間斷點，在這個例子裏，瞬間加速度超過了$20mm/s^2$，是否能夠提供這樣的加速度取決於硬件（電機）