機器人軌跡規劃：三次樣條曲線

寫在前面

在一些避障的應用場景下，一般都是先在任務空間中對多軸機械臂的末端進行路徑規劃，得到的是末端的運動路徑點數據。這條軌跡只包含位置關係，並沒有告訴機器人應該以怎樣的速度、加速度運動，這就需要進行帶時間參數的軌跡規劃處理，也就是對這條空間軌跡進行速度、加速度約束，並且計算運動到每個路點的時間，高級的算法有TOPP等，一般的呢就是貝塞爾、三次準/非/均勻B、五次及三次樣條等。下面從最簡單的三次樣條開始討論。

三次樣條曲線性質

當給出n+1個點時，可以使用n個p次多項式（通常較低）代替唯一的n次插值多項式，每個多項式定義一段軌跡。以這種方式定義的總函數s（t）稱爲p階的樣條曲線。p的值是根據所需的樣條連續度來選擇的。例如，爲了在兩個連續段之間發生過渡的時刻tk獲得速度和加速度的連續性，可以假定多項式的階數p＝3（三次多項式）。

定義三次樣條曲線的函數形式爲：

這段軌跡由n個三次多項式構成，並且每個多項式需要計算四個參數。由於n個多項式是定義一條通過n+1點的軌跡所必需的，因此需要確定的係數總數爲4n。爲了解決這個問題，必須考慮以下條件：

• 給定點插值的2n條件，因爲每一個三次函數必須在其極值處穿過點。

• n-1個條件，過渡點的速度要連續；

• n-1個條件，過渡點的加速度要連續；

這樣的話，就已經限制了2n+2(n-1)個條件，還剩下2個自由度還未限制。通過前面分析，還需要兩個限制條件才行，這裏討論的就是初始點和終點的速度以及加速度。下面是幾種可能的選擇，可以任意選擇：

• 如圖4.6所示，初始速度$\dot{s}\left(t_{0}\right)=v_{0}$，終止速度$\dot{s}\left(t_{n}\right)=v_{n}$；

• 自然條件下， 初始加速度和最終加速度$\ddot{s}\left(t_{0}\right), \ddot{s}\left(t_{n}\right)$均爲0；

• 當需要定義週期T=tn-t0這樣的樣條曲線時，條件$\dot{s}\left(t_{0}\right)=\dot{s}\left(t_{n}\right)$、$\ddot{s}\left(t_{0}\right)=\ddot{s}\left(t_{n}\right)$被使用；

• t1和tn-1時刻時jerk連續，表現爲：

通常情況，樣條曲線具有如下幾個特性：

• 對於由給定點(tk,qk),k=0,…n得到的p階樣條曲線s(t)，[n(p+1)]個參數可以確定

• 給定n+1個點，並且給定邊界條件，則p階插值樣條曲線s(t)能被唯一確定

• 用於構造樣條曲線的多項式的階數p不取決於數據點的數目

• 函數s(t)p-1階連續可導

• 自然樣條曲線是指初始加速度和最終加速度均爲0的樣條曲線

當指定初始速度v0和最終速度vn時的參數計算（也就是v0和vn已知）

在定義自動機械的軌跡時，速度剖面的連續性條件至關重要。因此，計算樣條曲線的典型選擇是指定初始和最終速度v0和vn。因此，給定點(tk,qk),k=0,…n以及速度的邊界條件（初始速度和最終速度）v0,vn，就有如下幾個條件成立：

可以最終確定樣條曲線的函數s(t)爲

係數ak,i可以由以下算法進行確定：

第一種情況，如果中間點（插補點）的速度我們已知，也就是vk,k=1,…,n-1，對於每段三次樣條曲線，有

其中，$T_{k}=t_{k+1}-t_{k}$。通過解上面的方程，可以得到

這就可以把每一段曲線的係數都求出來，從而得到樣條曲線。這是最簡單的情況！

子函數如下：

% 三次樣條：指定初始速度v0和終止速度vn，並且中間插補點的速度已知，這是最簡單的情況
% Input：
%   q：給定點的位置
%   t：給定點位置對應的時間
%   v：包括給定起始、中間及終止速度的速度向量
%   tt：插補週期
% Output：
%   yy dyy ddyy：樣條曲線函數值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_1(q, t, v, tt)
if length(q) ~= length(t)
    error('輸入的數據應成對')
end
n = length(q);
T = t(n) - t(1); % 運行總時長
nn = T / tt; % 總點數
yy = zeros(1, nn);
dyy = zeros(1, nn);
ddyy = zeros(1, nn);
j = 1;
for i = 1: n-1
    Tk = t(i+1) - t(i);
    a0 = q(i);
    a1 = v(i);
    a2 = (1/Tk) * ((3*(q(i+1)-q(i)))/Tk - 2*v(i) - v(i+1));
    a3 = (1/(Tk*Tk)) * ((2*(q(i)-q(i+1)))/Tk + v(i) + v(i+1));
    
    for tk = t(i): tt: t(i+1)
        if i > 1 && tk == t(i)
            continue
        end
        yy(j) = a0 + a1*(tk-t(i)) + a2*power(tk-t(i), 2) + a3*power(tk-t(i), 3);
        dyy(j) = a1 + 2*a2*(tk-t(i)) + 3*a3*power(tk-t(i), 2);
        ddyy(j) = 2*a2 + 6*a3*(tk-t(i));
        j = j + 1;
    end
end

end

第二種情況，如果中間點的速度我們未知，也就是vk,k=1,…,n-1均未知，然而這些值是必須被計算的。爲此，考慮了中間加速度的連續條件：

在這些條件下，通過考慮參數$a_{k, 2}, \quad a_{k, 3}, \quad a_{k+1,2}$的表達式乘以$\left(T_{k} T_{k+1}\right) / 2$，在簡單計算整理之後能夠得到

其中k=0,…,n-2。

上面的關係可以整理成矩陣的形式，$A^{\prime} v^{\prime}=c^{\prime}$，其中

其中常數項ck僅取決於中間位置和已知的樣條曲線段的持續時間Tk。由於速度v0和vn也是已知的，所以可以消除矩陣A‘的相應列並獲得

也就是

其中，$T=\left[T_{0}, T_{1}, \ldots, \mathrm{T}_{n-1}\right]^{T}, \quad q=\left[q_{0}, \mathrm{q}_{1}, \ldots, \mathrm{q}_{n-1}\right]^{T}$。A具有對角佔優結構，這就轉化成求解係數矩陣爲對角佔優的三對角線方程組的問題，普通就是利用追趕法求解>。因此，如果$T_{k}>0\left(\left|a_{k k}\right|>\sum_{j \neq k}\left|a_{k j}\right|\right)$，則A總是可逆的。一旦計算出A的逆，速度v1，…，vn-1（中間點的速度）可以從$v=A^{-1}c$計算出來，因此問題得到了解決：用（4.8）得到樣條係數。

例子

另外v0=2，v6=-3。

% 三次樣條：指定初始速度v0和終止速度vn，但是中間點速度未知
% Input：
%   q：給定點的位置
%   t：給定點位置對應的時間
%   v0：初始速度
%   vn：終止速度
%   tt：插補週期
% Output：
%   yy dyy ddyy：樣條曲線函數值、速度、加速度值
function [yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
if length(q) ~= length(t)
    error('輸入的數據應成對');
end
n = length(q);
c = zeros(n-2, 1);
% 矩陣A是個(n-2)*(n-2)的對角佔優矩陣
A = zeros(n-2);
for i = 1: n-2
    Tk_1 = t(i+2) - t(i+1);
    Tk = t(i+1) - t(i);
    if i == 1
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk;
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk_1*v0;
    elseif i == n-2
        A(i, i-1) = Tk_1;
        A(i, i) = 2*(Tk + Tk_1);
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i))) - Tk*vn;
    else
        A(i, i-1) = Tk_1;
        A(i, i) = 2*(Tk + Tk_1);
        A(i, i+1) = Tk;
        c(i, 1) = (3/(Tk*Tk_1))*(Tk^2*(q(i+2)-q(i+1))+Tk_1^2*(q(i+1)-q(i)));
        
    end
end
% 經過上述步驟得到對角佔優矩陣A和c
% vk = A \ c; % 這一步matlab計算很慢，應換成追趕法求vk
for i = 1: n-2
    a(i) = A(i, i); % 對角線
    if i == n-3
        b(i) = A(i, i+1); % 上邊
        d(i) = A(i+1, i); % 下邊
        continue;
    elseif i < n-2
        b(i) = A(i, i+1); % 上邊
        d(i) = A(i+1, i); % 下邊
    end
end
[~, ~, vk] = crout(a, b, d, c); % 追趕法

% 得到中間插補點的速度vk，然後調用cubicSpline_1即可
v_ = [v0, vk', vn];
[yy dyy ddyy] = cubicSpline_1(q, t, v_, tt);

end


%追趕法求解三對角線性方程組，Ax=b，A用一維數組a，c，d存儲。
function [L,U,x]=crout(a,c,d,b)%數組a存儲三角矩陣A的主對角線元素，c、d存儲主對角線上邊下邊帶寬爲1的元素
    n=length(a);
    n1=length(c);
    n2=length(d);
    %錯誤檢查
    if n1~=n2%存儲矩陣的數組維數錯誤
        error('MATLAB:Crout:不是三對角矩陣，參數數組中元素個數錯誤.');
    elseif n~=n1+1
        error('MATLAB:Crout:不是三對角矩陣，參數數組中元素個數錯誤.');
    end
   
    %初始化
    L=zeros(n);%生成n*n的全零矩陣
    U=zeros(n);
    p=1:n;
    q=1:n-1;
    x=1:n;
    y=1:n;
   
    %追趕法程序主體
    p(1)=a(1);
    for i=1:n-1
        q(i)=c(i)/p(i);
        p(i+1)=a(i+1)-d(i)*q(i);%d的下標改爲1到n-1
    end
    %正解y
    y(1)=b(1)/p(1);%用x存儲y
    for i=2:n
        y(i)=(b(i)-d(i-1)*y(i-1))/p(i);
    end
    %倒解x
    x(n)=y(n);
    for i=(n-1):-1:1
        x(i)=y(i)-q(i)*x(i+1);
    end
    %L,U矩陣
    for i=1:n
        L(i,i)=p(i);
        U(i,i)=1;
    end
    for i=1:n-1
        L(i+1,i)=d(i);
        U(i,i+1)=q(i);
    end
end %end of function

測試：

%% 自寫cubicSpline_2函數測試
q = [3, -2, -5, 0, 6, 12, 8];
t = [0, 5, 7, 8, 10, 15, 18];
n = length(t);
v0 = 2; vn = -3; tt = 0.1;
[yy dyy ddyy] = cubicSpline_2(q, t, v0, vn, tt);
subplot(3, 1, 1)
plot(t, q, 'o');
ylabel('位置')
grid on
hold on
plot([t(1):tt:t(n)], yy);
subplot(3, 1, 2)
plot([t(1), t(n)], [v0, vn], 'o');
grid on
hold on
plot([t(1):tt:t(n)], dyy);
ylabel('速度')
subplot(3, 1, 3)
grid on
hold on
plot([t(1):tt:t(n)], ddyy);
ylabel('加速度')

週期三次樣條：沒有指定初始速度v0和最終速度vn（也就是v0和vn未知）

具有指定初始速度v0和最終速度vn的三次樣條（v0和vn已知）：基於加速度的計算

具有指定初始、最終速度以及加速度的三次樣條曲線

未完待續。。。