三次样条插值(附完整代码)

  给定$n+1$个点$(t_i,q_i),i=0,1,...,n$，求解$n$段三次多项式$q_k(t)$，使得拼接而成的三次样条曲线$s(t)$经过所有给定点，而且二阶连续。三次样条曲线$s(t)$定义如下：
{ s ( t ) = { q k ( t ) , t ∈ [ t k , t k + 1 ] , k = 0 , 1 , . . . , n − 1 } q k ( t ) = a k 0 + a k 1 ( t − t k ) + a k 2 ( t − t k ) 2 + a k 3 ( t − t k ) 3 (1) \begin{cases} s(t) = \{q_k(t),t\in[t_k,t_{k+1}],k=0,1,...,n-1\} \\ q_k(t)=a_{k0}+a_{k1}(t-t_k)+a_{k2}(t-t_k)^2+a_{k3}(t-t_k)^3 \\ \tag 1 \end{cases} { s(t)={ qk​(t),t∈[tk​,tk+1​],k=0,1,...,n−1}qk​(t)=ak0​+ak1​(t−tk​)+ak2​(t−tk​)2+ak3​(t−tk​)3​(1)

  每一段三次多项式$q_k(t)$需要确定$4$个系数，因而未知数共$4n$个。三次样条曲线基本约束条件：(1)位置约束，每一段三次多项式都必须经过两个点，有约束条件$2n$个;（2)速度约束，相邻两段三次多项式在连接处的速度相等，有约束条件$n-1$个;（3)加速度约束，相邻两段三次多项式在连接处的加速度相等，有约束条件$n-1$个。故还需两个给定约束($4n-2n-(n-1)-(n-1)=2$)，便可唯一确定三次样条曲线。这里讨论三种不同的给定约束：
  (1) 给定起始速度$v_0$与结束速度$v_n$
  (2) 起始位置$q_0$与结束位置$q_n$相等(数据需满足的特性，非约束条件)，起始速度$v_0$与结束速度$v_n$相等，起始加速度$a_0$与结束加速度$a_n$相等（周期三次样条）
  (3) 给定起始速度$v_0$、结束速度$v_n$、起始加速度$a_0$、结束加速度$a_n$

一、给定起始速度$v_0$与结束速度$v_n$

1、三次样条的系数通过速度计算

  三次样条曲线所有约束：
$$\{\begin{array}{l}{q}_k(t_k)=q_k,q_k(t_{k+1})=q_{k+1},k=0,...,n-1\\ {\dot{q}}_k(t_{k+1})={\dot{q}}_{k+1}(t_{k+1})=v_{k+1},k=0,...,n-2\\ {\ddot{q}}_k(t_{k+1})={\ddot{q}}_{k+1}(t_{k+1}),k=0,...,n-2\\ {\dot{q}}_0(t_0)=v_0,{\dot{q}}_{n-1}(t_n)=v_n\end{array}\text{\hspace{1em}(2)}$$
  若所有中间点速度$v_k(k=1,...,n-1)$都已知，根据位置与速度约束：
$$\{\begin{array}{l}{q}_k(t_k)=a_{k0}=q_k\\ {\dot{q}}_k(t_k)=a_{k1}=v_k\\ q_k(t_{k+1})=a_{k0}+a_{k1}{T}_k+a_{k2}{T}_k^2+a_{k3}{T}_k^3=q_{k+1}\\ {\dot{q}}_k(t_{k+1})=a_{k1}+2a_{k2}{T}_k+3a_{k3}{T}_k^2=v_{k+1}\end{array}\text{\hspace{1em}(3)}$$
  其中，$T_k=t_{k+1}-t_k$。
  由式(3)可解得：
$$\{\begin{array}{l}{a}_{k,0}=q_k\\ a_{k,1}=v_k\\ a_{k,2}=[3(q_{k+1}-q_k)/T_k-2v_k-v_{k+1}]/T_k\\ a_{k,3}=[2(q_k-q_{k+1})/T_k+v_k+v_{k+1}]/T_k^2\end{array}\text{\hspace{1em}(4)}$$
  考虑加速度约束：
$${\ddot{q}}_k(t_{k+1})=2a_{k,2}+6a_{k,3}{T}_k=2a_{k+1,2}={\ddot{q}}_{k+1}(t_{k+1})\text{\hspace{1em}(5)}$$
  将$a_{k,2},a_{k,3},a_{k+1,2}$代入式(5)化简得：
$$\begin{array}{rl}& T_{k+1}{v}_k+2(T_{k+1}+T_k)v_{k+1}+T_k{v}_{k+2}\\ & =3[T_k^2(q_{k+2}-q_{k+1})+T_{k+1}^2(q_{k+1}-q_k)]/(T_k{T}_{k+1})\end{array}\text{\hspace{1em}(6)}$$
  其中，$k=0,1,...,n-2$。
  式(6)写成矩阵形式($v_0,v_n$已知)：
$$\begin{gathered} \left[\begin{array}{ccccc}2(T_0+T_1)& T_0& 0& \cdots & 0\\ T_2& 2(T_1+T_2)& T_1& 0& ⋮\\ 0& & \ddots & & 0\\ ⋮& 0& T_{n-2}& 2(T_{n-3}+T_{n-2})& T_{n-3}\\ 0& \cdots & 0& T_{n-1}& 2(T_{n-2}+T_{n-1})\end{array}\right]\left[\begin{array}{c}{v}_1\\ v_2\\ ⋮\\ v_{n-2}\\ v_{n-1}\end{array}\right] \\ =\left[\begin{array}{c}3[T_0^2(q_2-q_1)+T_1^2(q_1-q_0)]/(T_0{T}_1)-T_1{v}_0\\ 3[T_1^2(q_3-q_2)+T_2^2(q_2-q_1)]/(T_1{T}_2)\\ ⋮\\ 3[T_{n-3}^2(q_{n-1}-q_{n-2})+T_{n-2}^2(q_{n-2}-q_{n-3})]/(T_{n-3}{T}_{n-2})\\ 3[T_{n-2}^2(q_n-q_{n-1})+T_{n-1}^2(q_{n-1}-q_{n-2})]/(T_{n-2}{T}_{n-1})-T_{n-2}{v}_n\end{array}\right]\text{\hspace{1em}(7)} \end{gathered}$$
  式(7)为对角占优的三对角矩阵方程，可以利用Thomas algorithm或称“追赶法”快速且稳定地求解，而无需求解矩阵的逆。参考博文:三对角方程与循环三对角方程数值解(附完整代码)

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
deltaT = 0.001; %插补周期

n = length(t); % n >= 4
T = diff(t);
a = zeros(n - 2, 1);
b = zeros(n - 2, 1);
c = zeros(n - 2, 1);
d = zeros(n - 2, 1);

for i = 2 : n - 2
    a(i) = T(i + 1);
end
for i = 1 : n - 2
    b(i) = 2.0 * (T(i) + T(i + 1));
end
for i = 1 : n - 3
    c(i) = T(i);
end

d(1) = 3.0 * (T(1)^2 * (q(3) - q(2)) + T(2)^2 * (q(2) - q(1))) / (T(1) * T(2)) - T(2) * v0;
for i = 2 : n - 3
   d(i) = 3.0 * (T(i)^2 * (q(i + 2) - q(i + 1)) + T(i + 1)^2 * (q(i + 1) - q(i))) / (T(i) * T(i + 1)); 
end
d(n - 2) = 3.0 * (T(n - 2)^2 * (q(n) - q(n - 1)) + T(n - 1)^2 * (q(n - 1) - q(n - 2))) / (T(n - 2) * T(n - 1)) - T(n - 2) * vn;

[v, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n - 2);
if sta == 0
    error('三对角矩阵方程求解出错!');
end
v = [v0; v; vn];

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; 2.0 * (3.0 * (q(2) - q(1)) / T(1) - 2.0 * v(1) - v(2)) / T(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    b0 = q(i);
    b1 = v(i);
    b2 = (3.0 * (q(i + 1) - q(i)) / T(i) - 2.0 * v(i) - v(i + 1)) / T(i);
    b3 = (2.0 * (q(i) - q(i + 1)) / T(i) + v(i) + v(i + 1)) / T(i)^2;

    time = [time; tt];
    pos = [pos; b0 + (tt - t(i)) .* (b1 + (tt - t(i)) .* (b2 + b3 .* (tt - t(i))))];
    vel = [vel; b1 + (tt - t(i)) .* (2.0 * b2 + 3.0 * b3 .* (tt - t(i)))];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    b2 = (3.0 * (q(n) - q(n - 1)) / T(n - 1) - 2.0 * v(n - 1) - v(n)) / T(n - 1);
    b3 = (2.0 * (q(n - 1) - q(n)) / T(n - 1) + v(n - 1) + v(n)) / T(n - 1)^2;
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* T(n - 1)];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t, q, 'ro');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
hold on
plot([t(1), t(n)], [v0, vn], 'ro');
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
xlabel('t')
ylabel('acc')

2、三次样条的系数通过加速度计算

  第$k$段三次多项式的加速度：
$${\ddot{q}}_k(t)=[{\omega }_{k+1}(t-t_k)+{\omega }_k(t_{k+1}-t)]/T_k\text{\hspace{1em}(8)}$$
  其中，${\omega }_k$为$t_k$处对应的加速度，$k=0,1,...,n$。
  对上式积分，得到速度：
$$\begin{gathered} {\dot{q}}_k(t)={\omega }_{k+1}(t-t_k{)}^2/(2T_k)+{\omega }_k(t_{k+1}-t{)}^2/(2T_k) \\ +(q_{k+1}-q_k)/T_k-T_k({\omega }_{k+1}-{\omega }_k)/6\text{\hspace{1em}(9)} \end{gathered}$$
  $t_k$处的速度与加速度约束：
$$\{\begin{array}{l}{\dot{q}}_{k-1}(t_k)={\dot{q}}_k(t_k)\\ {\ddot{q}}_{k-1}(t_k)={\ddot{q}}_k(t_k)={\omega }_k\end{array}\text{\hspace{1em}(10)}$$
  由式(8)(9)(10)得：
$$\begin{gathered} (T_{k-1}/T_k){\omega }_{k-1}+[2(T_k+T_{k-1})/T_k]{\omega }_k+{\omega }_{k+1} \\ =(6/T_k)[(q_{k+1}-q_k)/T_k-(q_k-q_{k-1})/T_{k-1}],k=1,2,...,n-1\text{\hspace{1em}(11)} \end{gathered}$$
  由$\dot{s}(t_0)=v_0$得：
$$T_0^2{\omega }_0/3+T_0^2{\omega }_1/6=q_1-q_0-T_0{v}_0\text{\hspace{1em}(12)}$$
  由$\dot{s}(t_n)=v_n$得：
$$T_{n-1}^2{\omega }_n/3+T_{n-1}^2{\omega }_{n-1}/6=q_{n-1}-q_n+T_{n-1}{v}_n\text{\hspace{1em}(13)}$$
  将式(9)(12)(13)写成矩阵形式：
$$A\omega =c\text{\hspace{1em}(14)}$$
  其中：
$$A=\left[\begin{array}{ccccc}2T_0& T_0& 0& \cdots & 0\\ T_0& 2(T_0+T_1)& T_1& 0& ⋮\\ 0& & \ddots & & 0\\ ⋮& 0& T_{n-2}& 2(T_{n-2}+T_{n-1})& T_{n-1}\\ 0& \cdots & 0& T_{n-1}& 2T_{n-1})\end{array}\right]\text{\hspace{1em}(15)}$$

$$c=\left[\begin{array}{c}6[(q_1-q_0)/T_0-v_0]\\ 6[(q_2-q_1)/T_1-(q_1-q_0)/T_0]\\ ⋮\\ 6[(q_n-q_{n-1})/T_{n-1}-(q_{n-1}-q_{n-2})/T_{n-2}]\\ 6[v_n-(q_n-q_{n-1})/T_{n-1}]\end{array}\right]\text{\hspace{1em}(16)}$$
  求得$\omega$，即可计算样条的系数：
$$\{\begin{array}{l}{a}_{k0}=q_k\\ a_{k1}=(q_{k+1}-q_k)/T_k-T_k({\omega }_{k+1}+2{\omega }_k)/6\\ a_{k2}={\omega }_k/2\\ a_{k3}=({\omega }_{k+1}-{\omega }_k)/(6T_k)\end{array}\text{\hspace{1em}(17)}$$

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
deltaT = 0.001; %插补周期

n = length(t); % n >= 4
T = diff(t);
a = zeros(n, 1);
b = zeros(n, 1);
c = zeros(n, 1);
d = zeros(n, 1);

for i = 1 : n - 1
    a(i + 1) = T(i);
end

b(1) = 2.0 * T(1);
for i = 2 : n - 1
    b(i) = 2.0 * (T(i - 1) + T(i));
end
b(n) = 2.0 * T(n - 1);

for i = 1 : n - 1
    c(i) = T(i);
end

d(1) = 6.0 * ((q(2) - q(1)) / T(1) - v0);
for i = 2 : n - 1
   d(i) = 6.0 * ((q(i + 1) - q(i)) / T(i) - (q(i) - q(i - 1)) / T(i - 1)); 
end
d(n) = 6.0 * (vn - (q(n) - q(n - 1)) / T(n - 1));

[w, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n);
if sta == 0
    error('三对角矩阵方程求解出错!');
end

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; w(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    a0 = q(i);
    a1 = (q(i + 1) - q(i)) / T(i) - T(i) * (w(i + 1) + 2.0 * w(i)) / 6.0;
    a2 = 0.5 * w(i);
    a3 = (w(i + 1) - w(i)) / (6.0 * T(i));

    time = [time; tt];
    pos = [pos; a0 + (tt - t(i)) .* (a1 + (tt - t(i)) .* (a2 + a3 .* (tt - t(i))))];
    vel = [vel; a1 + (tt - t(i)) .* (2.0 * a2 + 3.0 * a3 .* (tt - t(i)))];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; w(n)];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t, q, 'ro');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
xlabel('t')
ylabel('acc')

二、起始位置$q_0$与结束位置$q_n$相等(数据需满足的特性，非约束条件)，起始速度$v_0$与结束速度$v_n$相等，起始加速度$a_0$与结束加速度$a_n$相等（周期三次样条）

  对于周期三次样条，起始速度$v_0$，结束速度$v_n$，起始加速度$a_0$，结束加速度$a_n$不可以任意指定，需满足约束：
$$v_0={\dot{q}}_0(t_0)={\dot{q}}_{n-1}(t_n)=v_n\text{\hspace{1em}(18)}$$
$${\ddot{q}}_0(t_0)=2a_{0,2}=2a_{n-1,2}+6a_{n-1,3}{T}_{n-1}={\ddot{q}}_{n-1}(t_n)\text{\hspace{1em}(19)}$$

  结合式(4)(7)可得矩阵方程：
$$\begin{gathered} \left[\begin{array}{cccccc}2(T_{n-1}+T_0)& T_{n-1}& 0& \cdots & 0& T_0\\ T_1& 2(T_0+T_1)& T_0& 0& ⋮& 0\\ 0& & \ddots & & & ⋮\\ ⋮& 0& & T_{n-2}& 2(T_{n-3}+T_{n-2})& T_{n-3}\\ T_{n-2}& 0& \cdots & 0& T_{n-1}& 2(T_{n-2}+T_{n-1})\end{array}\right]\left[\begin{array}{c}{v}_0\\ v_1\\ v_2\\ ⋮\\ v_{n-2}\\ v_{n-1}\end{array}\right] \\ =\left[\begin{array}{c}3[T_{n-1}^2(q_1-q_0)+T_0^2(q_{n-1}-q_{n-2})]/(T_{n-1}{T}_0)\\ 3[T_0^2(q_2-q_1)+T_1^2(q_1-q_0)]/(T_0{T}_1)\\ 3[T_1^2(q_3-q_2)+T_2^2(q_2-q_1)]/(T_1{T}_2)\\ ⋮\\ 3[T_{n-3}^2(q_{n-1}-q_{n-2})+T_{n-2}^2(q_{n-2}-q_{n-3})]/(T_{n-3}{T}_{n-2})\\ 3[T_{n-2}^2(q_n-q_{n-1})+T_{n-1}^2(q_{n-1}-q_{n-2})]/(T_{n-2}{T}_{n-1})\end{array}\right]\text{\hspace{1em}(20)} \end{gathered}$$
  式(20)为循环三对角矩阵方程，可以利用Sherman-Morrison 公式快速且稳定地求解，而无需求解矩阵的逆。参考博文:三对角方程与循环三对角方程数值解(附完整代码)

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 3]'; %首尾数据一致
deltaT = 0.001; %插补周期
forwardPeriodCount = 2;
backwardPeriodCount = 1;

n = length(t);
T = diff(t);
a = zeros(n - 1, 1);
b = zeros(n - 1, 1);
c = zeros(n - 1, 1);
d = zeros(n - 1, 1);

a(1 : n - 1) = T(1 : n - 1);

b(1) = 2.0 * (T(n - 1) + T(1));
for i = 2 : n - 1
    b(i) = 2.0 * (T(i - 1) + T(i));
end

c(1) = T(n - 1);
for i = 2 : n - 2
    c(i) = T(i - 1);
end
c(n - 1) = T(n - 2);

d(1) = 3.0 * (T(n - 1)^2 * (q(2) - q(1)) + T(1)^2 * (q(n) - q(n - 1))) / (T(n - 1) * T(1));
for i = 2 : n - 1
    d(i) = 3.0 * (T(i - 1)^2 * (q(i + 1) - q(i)) + T(i)^2 * (q(i) - q(i - 1))) / (T(i - 1) * T(i));
end

[v, sta] = solve_cyclic_tridiagonal_matrix_equation(a, b, c, d, n - 1);
if sta == 0
    error('循环三对角矩阵方程求解出错!');
end
v = [v; v(1)];

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v(1)];
acc = [acc; 2.0 * (3.0 * (q(2) - q(1)) / T(1) - 2.0 * v(1) - v(2)) / T(1)];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    a0 = q(i);
    a1 = v(i);
    a2 = (3.0 * (q(i + 1) - q(i)) / T(i) - 2.0 * v(i) - v(i + 1)) / T(i);
    a3 = (2.0 * (q(i) - q(i + 1)) / T(i) + v(i) + v(i + 1)) / T(i)^2;
    
    time = [time; tt];
    pos = [pos; a0 + (tt - t(i)) .* (a1 + (tt - t(i)) .* (a2 + a3 .* (tt - t(i))))];
    vel = [vel; a1 + (tt - t(i)) .* (2.0 * a2 + 3.0 * a3 .* (tt - t(i)))];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    a2 = (3.0 * (q(n) - q(n - 1)) / T(n - 1) - 2.0 * v(n - 1) - v(n)) / T(n - 1);
    a3 = (2.0 * (q(n - 1) - q(n)) / T(n - 1) + v(n - 1) + v(n)) / T(n - 1)^2;
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; v(n)];
    acc = [acc; 2.0 * a2 + 6.0 * a3 .* T(n - 1)];
end

period = time(end) - time(1);

figure(1)
subplot(3, 1, 1)
plot(time, pos, 'k');
hold on
plot(t, q, 'ko');
for i = 1 : forwardPeriodCount
    plot(time + i * period, pos, '--');
    plot(t + i * period, q, 'bo');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, pos, '--');
    plot(t - i * period, q, 'bo');
end
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel, 'k');
hold on
for i = 1 : forwardPeriodCount
    plot(time + i * period, vel, '--');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, vel, '--');
end
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc, 'k');
hold on
for i = 1 : forwardPeriodCount
    plot(time + i * period, acc, '--');
end
for i = 1 : backwardPeriodCount
    plot(time - i * period, acc, '--');
end
xlabel('t')
ylabel('acc')

三、给定起始速度$v_0$、结束速度$v_n$、起始加速度$a_0$、结束加速度$a_n$

  对于三次样条，一般无法同时指定起始速度与加速度，结束速度与加速度。然而可以通过在第一段以及最后一段样条插入两个额外点以实现。假设样条插值有$n-1$个点序列：
$$\{\begin{array}{l}t\text{t}t=[t_0,t_2,t_3,...,t_{n-3},t_{n-2},t_n{]}^T\\ q\text{q}q=[q_0,q_2,q_3,...,q_{n-3},q_{n-2},q_n{]}^T\end{array}\text{\hspace{1em}(21)}$$
  在第一段以及最后一段样条插入两个额外点:$({\overline{t}}_1,{\overline{q}}_1),({\overline{t}}_{n-1},{\overline{q}}_{n-1})$，不妨取其为相邻两个点的中点。新的点序列：
$$\{\begin{array}{l}t\text{t}t=[t_0,{\overline{t}}_1,t_2,t_3,...,t_{n-3},t_{n-2},{\overline{t}}_{n-1},t_n{]}^T\\ q\text{q}q=[q_0,{\overline{q}}_1,q_2,q_3,...,q_{n-3},q_{n-2},{\overline{q}}_{n-1},q_n{]}^T\end{array}\text{\hspace{1em}(22)}$$
  根据式(12)(13)可得：
$$\{\begin{array}{l}{q}_1=q_0+T_0{v}_0+T_0^2{a}_0/3+T_0^2{\omega }_1/6\\ q_{n-1}=q_n-T_{n-1}{v}_n+T_{n-1}^2{a}_n/3+T_{n-1}^2{\omega }_{n-1}/6\end{array}\text{\hspace{1em}(23)}$$
  根据式(15)(16)(23)可得矩阵方程：
$$A\omega =c\text{\hspace{1em}(24)}$$
  其中，
$$A=\left[\begin{array}{cccccc}2T_1+T_0(3+T_0/T_1)& T_1& 0& & \cdots & 0\\ T_1-T_0^2/T_1& 2(T_1+T_2)& T_2& & & ⋮\\ 0& T_2& 2(T_2+T_3)& T_3& & \\ ⋮& & \ddots & & & 0\\ ⋮& & & T_{n-3}& 2(T_{n-3}+T_{n-2})& T_{n-2}-T_{n-1}^2/T_{n-2}\\ 0& & \cdots & 0& T_{n-2}& 2T_{n-2}+T_{n-1}(3+T_{n-1}/T_{n-2})\end{array}\right]\text{\hspace{1em}(25)}$$
$$c=\left[\begin{array}{c}6((q_2-q_0)/T_1-v_0(1+T_0/T_1)-a_0(1/2+T_0/(3T_1))T_0)\\ 6((q_3-q_2)/T_2-(q_2-q_0)/T_1+v_0{T}_0/T_1+a_0{T}_0^2/(3T_1))\\ 6((q_4-q_3)/T_3-(q_3-q_2)/T_2)\\ ⋮\\ 6((q_{n-2}-q_{n-3})/T_{n-3}-(q_{n-3}-q_{n-4})/T_{n-4})\\ 6((q_n-q_{n-2})/T_{n-2}-(q_{n-2}-q_{n-3})/T_{n-3}-v_n{T}_{n-1}/T_{n-2}+a_n{T}_{n-1}^2/(3T_{n-2}))\\ 6((q_{n-2}-q_n)/T_{n-2}+v_n(1+T_{n-1}/T_{n-2})-a_n(1/2+T_{n-1}/(3T_{n-2}))T_{n-1})\end{array}\right]\text{\hspace{1em}(26)}$$

clc;
clear;
close all;

t = [0, 5, 7, 8, 10, 15, 18]'; %递增时间序列
q = [3, -2, -5, 0, 6, 12, 8]';
v0 = 2; %起始速度
vn = -3; %结束速度
a0 = 0; %起始加速度
an = 0; %结束加速度
deltaT = 0.001; %插补周期

%前两个时间之间、后两个时间之间插入时间
n = length(t); % n >= 4
t2 = 0.5 * (t(1) + t(2));
t_n_1 = 0.5 * (t(n) + t(n - 1));
t = [t(1); t2; t(2 : n - 1); t_n_1; t(n)];
q = [q(1); 0.0; q(2 : n - 1); 0.0; q(n)];

n = n + 2;
T = diff(t);
a = zeros(n - 2, 1);
b = zeros(n - 2, 1);
c = zeros(n - 2, 1);
d = zeros(n - 2, 1);

a(2) = T(2) - T(1)^2 / T(2);
for i = 3 : n - 2
    a(i) = T(i);
end

b(1) = 2.0 * T(2) + T(1) * (3.0 + T(1) / T(2));
for i = 2 : n - 3
    b(i) = 2.0 * (T(i) + T(i + 1));
end
b(n - 2) = 2.0 * T(n - 2) + T(n - 1) * (3.0 + T(n - 1) / T(n - 2));

for i = 1 : n - 4
    c(i) = T(i + 1);
end
c(n - 3) = T(n - 2) - T(n - 1)^2 / T(n - 2);

d(1) = 6.0 * ((q(3) - q(1)) / T(2) - v0 * (1.0 + T(1) / T(2)) - a0 * (0.5 + T(1) / (3.0 * T(2))) * T(1));
d(2) = 6.0 * ((q(4) - q(3)) / T(3) - (q(3) - q(1)) / T(2) + v0 * T(1) / T(2) + a0 * T(1)^2 / (3.0 * T(2)));
for i = 3 : n - 4
    d(i) = 6.0 * ((q(i + 2) - q(i + 1)) / T(i + 1) - (q(i + 1) - q(i)) / T(i));
end
d(n - 3) = 6.0 * ((q(n) - q(n - 2)) / T(n - 2) - (q(n - 2) - q(n - 3)) / T(n - 3) - vn * T(n - 1) / T(n - 2) + an * T(n - 1)^2 / (3.0 * T(n - 2)));
d(n - 2) = 6.0 * ((q(n - 2) - q(n)) / T(n - 2) + vn * (1.0 + T(n - 1) / T(n - 2)) - an * (0.5 + T(n - 1) / (3.0 * T(n - 2))) * T(n - 1));

[w, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n - 2);
if sta == 0
    error('三对角矩阵方程求解出错!');
end

w = [a0; w; an];

q(2) = q(1) + T(1) * v0 + T(1)^2 * a0 / 3.0 + T(1)^2 * w(2) / 6.0;
q(n - 1) = q(n) - T(n - 1) * vn + T(n - 1)^2 * an / 3.0 + T(n - 1)^2 * w(n - 1) / 6.0;

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; q(1)];
vel = [vel; v0];
acc = [acc; a0];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    b0 = q(i);
    b1 = (q(i + 1) - q(i)) / T(i) - T(i) * (w(i + 1) + 2.0 * w(i)) / 6.0;
    b2 = 0.5 * w(i);
    b3 = (w(i + 1) - w(i)) / (6.0 * T(i));
    
    time = [time; tt];
    pos = [pos; b0 + (tt - t(i)) .* (b1 + (tt - t(i)) .* (b2 + b3 .* (tt - t(i))))];
    vel = [vel; b1 + (tt - t(i)) .* (2.0 * b2 + 3.0 * b3 .* (tt - t(i)))];
    acc = [acc; 2.0 * b2 + 6.0 * b3 .* (tt - t(i))];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; q(n)];
    vel = [vel; vn];
    acc = [acc; an];
end

figure(1)
subplot(3, 1, 1)
plot(time, pos);
hold on
plot(t([1, 3 : n - 2, n]), q([1, 3 : n - 2, n]), 'ro');
plot(t([2, n - 1]), q([2, n - 1]), 'bs');
xlabel('t')
ylabel('pos')

subplot(3, 1, 2)
plot(time, vel);
hold on
plot(t([1, n]), [v0, vn], 'ro');
xlabel('t')
ylabel('vel')

subplot(3, 1, 3)
plot(time, acc);
hold on
plot(t([1, n]), [a0, an], 'ro');
xlabel('t')
ylabel('acc')

四、参考文献

Trajectory Planning for Automatic Machines and Robots中章节：4.4 Cubic Splines