文章目录
给定
n
+
1
n+1
n+1个点
(
t
i
,
q
i
)
,
i
=
0
,
1
,
.
.
.
,
n
(t_i,q_i),i=0,1,...,n
(ti,qi),i=0,1,...,n,求解
n
n
n段三次多项式
q
k
(
t
)
q_k(t)
qk(t),使得拼接而成的三次样条曲线
s
(
t
)
s(t)
s(t)经过所有给定点,而且二阶连续。三次样条曲线
s
(
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
)
q_k(t)
qk(t)需要确定
4
4
4个系数,因而未知数共
4
n
4n
4n个。三次样条曲线基本约束条件:(1)位置约束,每一段三次多项式都必须经过两个点,有约束条件
2
n
2n
2n个;(2)速度约束,相邻两段三次多项式在连接处的速度相等,有约束条件
n
−
1
n-1
n−1个;(3)加速度约束,相邻两段三次多项式在连接处的加速度相等,有约束条件
n
−
1
n-1
n−1个。故还需两个给定约束(
4
n
−
2
n
−
(
n
−
1
)
−
(
n
−
1
)
=
2
4n-2n-(n-1)-(n-1)=2
4n−2n−(n−1)−(n−1)=2),便可唯一确定三次样条曲线。这里讨论三种不同的给定约束:
(1) 给定起始速度
v
0
v_0
v0与结束速度
v
n
v_n
vn
(2) 起始位置
q
0
q_0
q0与结束位置
q
n
q_n
qn相等(数据需满足的特性,非约束条件),起始速度
v
0
v_0
v0与结束速度
v
n
v_n
vn相等,起始加速度
a
0
a_0
a0与结束加速度
a
n
a_n
an相等(周期三次样条)
(3) 给定起始速度
v
0
v_0
v0、结束速度
v
n
v_n
vn、起始加速度
a
0
a_0
a0、结束加速度
a
n
a_n
an
一、给定起始速度 v 0 v_0 v0与结束速度 v n v_n vn
1、三次样条的系数通过速度计算
三次样条曲线所有约束:
{
q
k
(
t
k
)
=
q
k
,
q
k
(
t
k
+
1
)
=
q
k
+
1
,
k
=
0
,
.
.
.
,
n
−
1
q
˙
k
(
t
k
+
1
)
=
q
˙
k
+
1
(
t
k
+
1
)
=
v
k
+
1
,
k
=
0
,
.
.
.
,
n
−
2
q
¨
k
(
t
k
+
1
)
=
q
¨
k
+
1
(
t
k
+
1
)
,
k
=
0
,
.
.
.
,
n
−
2
q
˙
0
(
t
0
)
=
v
0
,
q
˙
n
−
1
(
t
n
)
=
v
n
(2)
\begin{cases} 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 \\ \tag 2 \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧qk(tk)=qk,qk(tk+1)=qk+1,k=0,...,n−1q˙k(tk+1)=q˙k+1(tk+1)=vk+1,k=0,...,n−2q¨k(tk+1)=q¨k+1(tk+1),k=0,...,n−2q˙0(t0)=v0,q˙n−1(tn)=vn(2)
若所有中间点速度
v
k
(
k
=
1
,
.
.
.
,
n
−
1
)
v_k(k=1,...,n-1)
vk(k=1,...,n−1)都已知,根据位置与速度约束:
{
q
k
(
t
k
)
=
a
k
0
=
q
k
q
˙
k
(
t
k
)
=
a
k
1
=
v
k
q
k
(
t
k
+
1
)
=
a
k
0
+
a
k
1
T
k
+
a
k
2
T
k
2
+
a
k
3
T
k
3
=
q
k
+
1
q
˙
k
(
t
k
+
1
)
=
a
k
1
+
2
a
k
2
T
k
+
3
a
k
3
T
k
2
=
v
k
+
1
(3)
\begin{cases} 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}\\ \tag 3 \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧qk(tk)=ak0=qkq˙k(tk)=ak1=vkqk(tk+1)=ak0+ak1Tk+ak2Tk2+ak3Tk3=qk+1q˙k(tk+1)=ak1+2ak2Tk+3ak3Tk2=vk+1(3)
其中,
T
k
=
t
k
+
1
−
t
k
T_k=t_{k+1}-t_k
Tk=tk+1−tk。
由式(3)可解得:
{
a
k
,
0
=
q
k
a
k
,
1
=
v
k
a
k
,
2
=
[
3
(
q
k
+
1
−
q
k
)
/
T
k
−
2
v
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
(4)
\begin{cases} 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\\ \tag 4 \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧ak,0=qkak,1=vkak,2=[3(qk+1−qk)/Tk−2vk−vk+1]/Tkak,3=[2(qk−qk+1)/Tk+vk+vk+1]/Tk2(4)
考虑加速度约束:
q
¨
k
(
t
k
+
1
)
=
2
a
k
,
2
+
6
a
k
,
3
T
k
=
2
a
k
+
1
,
2
=
q
¨
k
+
1
(
t
k
+
1
)
(5)
\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}) \tag 5
q¨k(tk+1)=2ak,2+6ak,3Tk=2ak+1,2=q¨k+1(tk+1)(5)
将
a
k
,
2
,
a
k
,
3
,
a
k
+
1
,
2
a_{k,2},a_{k,3},a_{k+1,2}
ak,2,ak,3,ak+1,2代入式(5)化简得:
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
)
(6)
\begin{aligned} &T_{k+1}v_k+2(T_{k+1}+T_k)v_{k+1}+ T_kv_{k+2} \\ &=3[T_k^2(q_{k+2}-q_{k+1}) + T_{k+1}^2(q_{k+1}-q_{k})]/(T_kT_{k+1}) \tag 6 \end{aligned}
Tk+1vk+2(Tk+1+Tk)vk+1+Tkvk+2=3[Tk2(qk+2−qk+1)+Tk+12(qk+1−qk)]/(TkTk+1)(6)
其中,
k
=
0
,
1
,
.
.
.
,
n
−
2
k=0,1,...,n-2
k=0,1,...,n−2。
式(6)写成矩阵形式(
v
0
,
v
n
v_0,v_n
v0,vn已知):
[
2
(
T
0
+
T
1
)
T
0
0
⋯
0
T
2
2
(
T
1
+
T
2
)
T
1
0
⋮
0
⋱
0
⋮
0
T
n
−
2
2
(
T
n
−
3
+
T
n
−
2
)
T
n
−
3
0
⋯
0
T
n
−
1
2
(
T
n
−
2
+
T
n
−
1
)
]
[
v
1
v
2
⋮
v
n
−
2
v
n
−
1
]
=
[
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
]
(7)
\left[ \begin{matrix} 2(T_0+T_1) & T_0 & 0 & \cdots & 0\\ T_2 & 2(T_1+T_2) & T_1 & 0 & \vdots \\ 0 & & \ddots & & 0 \\ \vdots &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{matrix} \right] \left[ \begin{matrix} v_1\\ v_2 \\ \vdots\\ v_{n-2}\\ v_{n-1} \\ \end{matrix} \right] \\ = \left[ \begin{matrix} 3[T_0^2(q_2-q_1) + T_1^2(q_1-q_0)]/(T_0T_1) - T_1v_0\\ 3[T_1^2(q_3-q_2) + T_2^2(q_2-q_1)]/(T_1T_2) \\ \vdots\\ 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{matrix} \right] \tag{7}
⎣⎢⎢⎢⎢⎢⎢⎡2(T0+T1)T20⋮0T02(T1+T2)0⋯0T1⋱Tn−20⋯02(Tn−3+Tn−2)Tn−10⋮0Tn−32(Tn−2+Tn−1)⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎡v1v2⋮vn−2vn−1⎦⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡3[T02(q2−q1)+T12(q1−q0)]/(T0T1)−T1v03[T12(q3−q2)+T22(q2−q1)]/(T1T2)⋮3[Tn−32(qn−1−qn−2)+Tn−22(qn−2−qn−3)]/(Tn−3Tn−2)3[Tn−22(qn−qn−1)+Tn−12(qn−1−qn−2)]/(Tn−2Tn−1)−Tn−2vn⎦⎥⎥⎥⎥⎥⎤(7)
式(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
k
k段三次多项式的加速度:
q
¨
k
(
t
)
=
[
ω
k
+
1
(
t
−
t
k
)
+
ω
k
(
t
k
+
1
−
t
)
]
/
T
k
(8)
\ddot{q}_k(t)=[\omega_{k+1} (t-t_k)+\omega_{k} (t_{k+1}-t) ]/T_k \tag{8}
q¨k(t)=[ωk+1(t−tk)+ωk(tk+1−t)]/Tk(8)
其中,
ω
k
\omega_{k}
ωk为
t
k
t_k
tk处对应的加速度,
k
=
0
,
1
,
.
.
.
,
n
k=0,1,...,n
k=0,1,...,n。
对上式积分,得到速度:
q
˙
k
(
t
)
=
ω
k
+
1
(
t
−
t
k
)
2
/
(
2
T
k
)
+
ω
k
(
t
k
+
1
−
t
)
2
/
(
2
T
k
)
+
(
q
k
+
1
−
q
k
)
/
T
k
−
T
k
(
ω
k
+
1
−
ω
k
)
/
6
(9)
\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\tag{9}
q˙k(t)=ωk+1(t−tk)2/(2Tk)+ωk(tk+1−t)2/(2Tk)+(qk+1−qk)/Tk−Tk(ωk+1−ωk)/6(9)
t
k
t_k
tk处的速度与加速度约束:
{
q
˙
k
−
1
(
t
k
)
=
q
˙
k
(
t
k
)
q
¨
k
−
1
(
t
k
)
=
q
¨
k
(
t
k
)
=
ω
k
(10)
\begin{cases} \dot{q}_{k-1}(t_k)=\dot{q}_{k}(t_k)\\ \ddot{q}_{k-1}(t_k)=\ddot{q}_{k}(t_k)=\omega_{k}\\ \tag {10} \end{cases}
{q˙k−1(tk)=q˙k(tk)q¨k−1(tk)=q¨k(tk)=ωk(10)
由式(8)(9)(10)得:
(
T
k
−
1
/
T
k
)
ω
k
−
1
+
[
2
(
T
k
+
T
k
−
1
)
/
T
k
]
ω
k
+
ω
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
(11)
(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 \tag{11}
(Tk−1/Tk)ωk−1+[2(Tk+Tk−1)/Tk]ωk+ωk+1=(6/Tk)[(qk+1−qk)/Tk−(qk−qk−1)/Tk−1],k=1,2,...,n−1(11)
由
s
˙
(
t
0
)
=
v
0
\dot{s}(t_0)=v_0
s˙(t0)=v0得:
T
0
2
ω
0
/
3
+
T
0
2
ω
1
/
6
=
q
1
−
q
0
−
T
0
v
0
(12)
T_0^2\omega_0/3 + T_0^2\omega_1/6=q_1-q_0-T_0v_0 \tag{12}
T02ω0/3+T02ω1/6=q1−q0−T0v0(12)
由
s
˙
(
t
n
)
=
v
n
\dot{s}(t_n)=v_n
s˙(tn)=vn得:
T
n
−
1
2
ω
n
/
3
+
T
n
−
1
2
ω
n
−
1
/
6
=
q
n
−
1
−
q
n
+
T
n
−
1
v
n
(13)
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 \tag{13}
Tn−12ωn/3+Tn−12ωn−1/6=qn−1−qn+Tn−1vn(13)
将式(9)(12)(13)写成矩阵形式:
A
ω
=
c
(14)
A\omega=c \tag{14}
Aω=c(14)
其中:
A
=
[
2
T
0
T
0
0
⋯
0
T
0
2
(
T
0
+
T
1
)
T
1
0
⋮
0
⋱
0
⋮
0
T
n
−
2
2
(
T
n
−
2
+
T
n
−
1
)
T
n
−
1
0
⋯
0
T
n
−
1
2
T
n
−
1
)
]
(15)
A=\left[ \begin{matrix} 2T_0 & T_0 & 0 & \cdots & 0\\ T_0 & 2(T_0+T_1) & T_1 & 0 & \vdots \\ 0 & & \ddots & & 0 \\ \vdots &0 & T_{n-2} & 2(T_{n-2}+T_{n-1}) & T_{n-1} \\ 0 & \cdots & 0 & T_{n-1} & 2T_{n-1}) \end{matrix} \right] \tag {15}
A=⎣⎢⎢⎢⎢⎢⎢⎡2T0T00⋮0T02(T0+T1)0⋯0T1⋱Tn−20⋯02(Tn−2+Tn−1)Tn−10⋮0Tn−12Tn−1)⎦⎥⎥⎥⎥⎥⎥⎤(15)
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
]
]
(16)
c=\left[ \begin{matrix} 6[(q_1-q_0)/T_0 - v_0]\\ 6[(q_2-q_1)/T_1 -(q_1-q_0)/T_0]\\ \vdots \\ 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{matrix} \right] \tag {16}
c=⎣⎢⎢⎢⎢⎢⎡6[(q1−q0)/T0−v0]6[(q2−q1)/T1−(q1−q0)/T0]⋮6[(qn−qn−1)/Tn−1−(qn−1−qn−2)/Tn−2]6[vn−(qn−qn−1)/Tn−1]⎦⎥⎥⎥⎥⎥⎤(16)
求得
ω
\omega
ω,即可计算样条的系数:
{
a
k
0
=
q
k
a
k
1
=
(
q
k
+
1
−
q
k
)
/
T
k
−
T
k
(
ω
k
+
1
+
2
ω
k
)
/
6
a
k
2
=
ω
k
/
2
a
k
3
=
(
ω
k
+
1
−
ω
k
)
/
(
6
T
k
)
(17)
\begin{cases} 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)\\ \tag {17} \end{cases}
⎩⎪⎪⎪⎨⎪⎪⎪⎧ak0=qkak1=(qk+1−qk)/Tk−Tk(ωk+1+2ωk)/6ak2=ωk/2ak3=(ωk+1−ωk)/(6Tk)(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_0 q0与结束位置 q n q_n qn相等(数据需满足的特性,非约束条件),起始速度 v 0 v_0 v0与结束速度 v n v_n vn相等,起始加速度 a 0 a_0 a0与结束加速度 a n a_n an相等(周期三次样条)
对于周期三次样条,起始速度
v
0
v_0
v0,结束速度
v
n
v_n
vn,起始加速度
a
0
a_0
a0,结束加速度
a
n
a_n
an不可以任意指定,需满足约束:
v
0
=
q
˙
0
(
t
0
)
=
q
˙
n
−
1
(
t
n
)
=
v
n
(18)
v_0=\dot{q}_0(t_0)=\dot{q}_{n-1}(t_n)=v_n \tag{18}
v0=q˙0(t0)=q˙n−1(tn)=vn(18)
q
¨
0
(
t
0
)
=
2
a
0
,
2
=
2
a
n
−
1
,
2
+
6
a
n
−
1
,
3
T
n
−
1
=
q
¨
n
−
1
(
t
n
)
(19)
\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) \tag{19}
q¨0(t0)=2a0,2=2an−1,2+6an−1,3Tn−1=q¨n−1(tn)(19)
结合式(4)(7)可得矩阵方程:
[
2
(
T
n
−
1
+
T
0
)
T
n
−
1
0
⋯
0
T
0
T
1
2
(
T
0
+
T
1
)
T
0
0
⋮
0
0
⋱
⋮
⋮
0
T
n
−
2
2
(
T
n
−
3
+
T
n
−
2
)
T
n
−
3
T
n
−
2
0
⋯
0
T
n
−
1
2
(
T
n
−
2
+
T
n
−
1
)
]
[
v
0
v
1
v
2
⋮
v
n
−
2
v
n
−
1
]
=
[
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
)
]
(20)
\left[ \begin{matrix} 2(T_{n-1}+T_0) & T_{n-1} & 0 & \cdots & 0 & T_0\\ T_1 & 2(T_0+T_1) & T_0 & 0 & \vdots & 0 \\ 0 & & \ddots & & & \vdots \\ \vdots &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{matrix} \right] \left[ \begin{matrix} v_0\\ v_1 \\ v_2 \\ \vdots\\ v_{n-2}\\ v_{n-1} \\ \end{matrix} \right] \\ = \left[ \begin{matrix} 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_0T_1) \\ 3[T_1^2(q_3-q_2) + T_2^2(q_2-q_1)]/(T_1T_2) \\ \vdots\\ 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{matrix} \right] \tag{20}
⎣⎢⎢⎢⎢⎢⎢⎢⎡2(Tn−1+T0)T10⋮Tn−2Tn−12(T0+T1)000T0⋱⋯⋯0Tn−200⋮2(Tn−3+Tn−2)Tn−1T00⋮Tn−32(Tn−2+Tn−1)⎦⎥⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎡v0v1v2⋮vn−2vn−1⎦⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎡3[Tn−12(q1−q0)+T02(qn−1−qn−2)]/(Tn−1T0)3[T02(q2−q1)+T12(q1−q0)]/(T0T1)3[T12(q3−q2)+T22(q2−q1)]/(T1T2)⋮3[Tn−32(qn−1−qn−2)+Tn−22(qn−2−qn−3)]/(Tn−3Tn−2)3[Tn−22(qn−qn−1)+Tn−12(qn−1−qn−2)]/(Tn−2Tn−1)⎦⎥⎥⎥⎥⎥⎥⎥⎤(20)
式(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_0 v0、结束速度 v n v_n vn、起始加速度 a 0 a_0 a0、结束加速度 a n a_n an
对于三次样条,一般无法同时指定起始速度与加速度,结束速度与加速度。然而可以通过在第一段以及最后一段样条插入两个额外点以实现。假设样条插值有
n
−
1
n-1
n−1个点序列:
{
t
=
[
t
0
,
t
2
,
t
3
,
.
.
.
,
t
n
−
3
,
t
n
−
2
,
t
n
]
T
q
=
[
q
0
,
q
2
,
q
3
,
.
.
.
,
q
n
−
3
,
q
n
−
2
,
q
n
]
T
(21)
\begin{cases} \pmb{t}=[t_0,t_2,t_3,...,t_{n-3},t_{n-2},t_{n}]^T\\ \pmb{q}=[q_0,q_2,q_3,...,q_{n-3},q_{n-2},q_{n}]^T\\ \tag {21} \end{cases}
{ttt=[t0,t2,t3,...,tn−3,tn−2,tn]Tqqq=[q0,q2,q3,...,qn−3,qn−2,qn]T(21)
在第一段以及最后一段样条插入两个额外点:
(
t
‾
1
,
q
‾
1
)
,
(
t
‾
n
−
1
,
q
‾
n
−
1
)
(\overline{t}_1,\overline{q}_1),(\overline{t}_{n-1},\overline{q}_{n-1})
(t1,q1),(tn−1,qn−1),不妨取其为相邻两个点的中点。新的点序列:
{
t
=
[
t
0
,
t
‾
1
,
t
2
,
t
3
,
.
.
.
,
t
n
−
3
,
t
n
−
2
,
t
‾
n
−
1
,
t
n
]
T
q
=
[
q
0
,
q
‾
1
,
q
2
,
q
3
,
.
.
.
,
q
n
−
3
,
q
n
−
2
,
q
‾
n
−
1
,
q
n
]
T
(22)
\begin{cases} \pmb{t}=[t_0,\overline{t}_1,t_2,t_3,...,t_{n-3},t_{n-2},\overline{t}_{n-1},t_{n}]^T\\ \pmb{q}=[q_0,\overline{q}_1,q_2,q_3,...,q_{n-3},q_{n-2},\overline{q}_{n-1},q_{n}]^T\\ \tag {22} \end{cases}
{ttt=[t0,t1,t2,t3,...,tn−3,tn−2,tn−1,tn]Tqqq=[q0,q1,q2,q3,...,qn−3,qn−2,qn−1,qn]T(22)
根据式(12)(13)可得:
{
q
1
=
q
0
+
T
0
v
0
+
T
0
2
a
0
/
3
+
T
0
2
ω
1
/
6
q
n
−
1
=
q
n
−
T
n
−
1
v
n
+
T
n
−
1
2
a
n
/
3
+
T
n
−
1
2
ω
n
−
1
/
6
(23)
\begin{cases} q_1=q_0+T_0v_0+T_0^2a_0/3+T_0^2\omega_1/6\\ q_{n-1}=q_n-T_{n-1}v_n+T_{n-1}^2a_n/3+T_{n-1}^2\omega_{n-1}/6\\ \tag {23} \end{cases}
{q1=q0+T0v0+T02a0/3+T02ω1/6qn−1=qn−Tn−1vn+Tn−12an/3+Tn−12ωn−1/6(23)
根据式(15)(16)(23)可得矩阵方程:
A
ω
=
c
(24)
A\omega=c\tag{24}
Aω=c(24)
其中,
A
=
[
2
T
1
+
T
0
(
3
+
T
0
/
T
1
)
T
1
0
⋯
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
⋮
⋱
0
⋮
T
n
−
3
2
(
T
n
−
3
+
T
n
−
2
)
T
n
−
2
−
T
n
−
1
2
/
T
n
−
2
0
⋯
0
T
n
−
2
2
T
n
−
2
+
T
n
−
1
(
3
+
T
n
−
1
/
T
n
−
2
)
]
(25)
A=\left[ \begin{matrix} 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 & & & \vdots \\ 0 & T_2 & 2(T_2+T_3) & T_3 & & \\ \vdots & & \ddots & & &0 \\ \vdots & & & 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{matrix} \right] \tag{25}
A=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡2T1+T0(3+T0/T1)T1−T02/T10⋮⋮0T12(T1+T2)T20T22(T2+T3)⋱⋯T3Tn−30⋯2(Tn−3+Tn−2)Tn−20⋮0Tn−2−Tn−12/Tn−22Tn−2+Tn−1(3+Tn−1/Tn−2)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(25)
c
=
[
6
(
(
q
2
−
q
0
)
/
T
1
−
v
0
(
1
+
T
0
/
T
1
)
−
a
0
(
1
/
2
+
T
0
/
(
3
T
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
/
(
3
T
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
/
(
3
T
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
/
(
3
T
n
−
2
)
)
T
n
−
1
)
]
(26)
c= \left[ \begin{matrix} 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_0T_0/T_1+a_0T_0^2/(3T_1))\\ 6((q_4-q_3)/T_3-(q_3-q_2)/T_2)\\ \vdots\\ 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_nT_{n-1}/T_{n-2}+a_nT_{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{matrix} \right] \tag{26}
c=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡6((q2−q0)/T1−v0(1+T0/T1)−a0(1/2+T0/(3T1))T0)6((q3−q2)/T2−(q2−q0)/T1+v0T0/T1+a0T02/(3T1))6((q4−q3)/T3−(q3−q2)/T2)⋮6((qn−2−qn−3)/Tn−3−(qn−3−qn−4)/Tn−4)6((qn−qn−2)/Tn−2−(qn−2−qn−3)/Tn−3−vnTn−1/Tn−2+anTn−12/(3Tn−2))6((qn−2−qn)/Tn−2+vn(1+Tn−1/Tn−2)−an(1/2+Tn−1/(3Tn−2))Tn−1)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(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