当前位置：首页 > news >正文

五次样条速度规划方法介绍

news 2025/9/29 8:58:59

一.方法背景

在机器人、数控机床、自动驾驶等高精度运动控制系统中，轨迹不仅要经过指定路径点，还需保证 速度、加速度、加加速度（jerk）的平滑性。三次样条虽然能保证加速度连续，但无法控制起点和终点的速度与加速度。

五次样条（Quintic Spline） 每段采用五次多项式，共 6 个自由度，可同时满足以下 6 个边界条件：

起点位置
终点位置
起点速度
终点速度
起点加速度
终点加速度

从而实现 C² 连续（位置、速度、加速度连续），并显著降低 jerk（加加速度），提升运动平稳性。

二.数学模型

对于第 𝑖 段（在区间 [𝑡ᵢ, 𝑡ᵢ₊₁] 上），定义五次多项式轨迹：

位置函数：
𝑆ᵢ(𝑡) = 𝑎ᵢ + 𝑏ᵢ(𝑡−𝑡ᵢ) + 𝑐ᵢ(𝑡−𝑡ᵢ)² + 𝑑ᵢ(𝑡−𝑡ᵢ)³ + 𝑒ᵢ(𝑡−𝑡ᵢ)⁴ + 𝑓ᵢ(𝑡−𝑡ᵢ)⁵

速度函数：
𝑆′ᵢ(𝑡) = 𝑏ᵢ + 2𝑐ᵢ(𝑡−𝑡ᵢ) + 3𝑑ᵢ(𝑡−𝑡ᵢ)² + 4𝑒ᵢ(𝑡−𝑡ᵢ)³ + 5𝑓ᵢ(𝑡−𝑡ᵢ)⁴

加速度函数：
𝑆″ᵢ(𝑡) = 2𝑐ᵢ + 6𝑑ᵢ(𝑡−𝑡ᵢ) + 12𝑒ᵢ(𝑡−𝑡ᵢ)² + 20𝑓ᵢ(𝑡−𝑡ᵢ)³

加加速度（jerk）函数：
𝑆‴ᵢ(𝑡) = 6𝑑ᵢ + 24𝑒ᵢ(𝑡−𝑡ᵢ) + 60𝑓ᵢ(𝑡−𝑡ᵢ)²

三.边界条件（6 个）

设第 𝑖 段的时间跨度为 ℎᵢ = 𝑡ᵢ₊₁ − 𝑡ᵢ，边界条件如下：

条件	数学表达
起点位置	𝑆ᵢ(𝑡ᵢ) = 𝑝ᵢ
终点位置	𝑆ᵢ(𝑡ᵢ₊₁) = 𝑝ᵢ₊₁
起点速度	𝑆′ᵢ(𝑡ᵢ) = 𝑣ᵢ
终点速度	𝑆′ᵢ(𝑡ᵢ₊₁) = 𝑣ᵢ₊₁
起点加速度	𝑆″ᵢ(𝑡ᵢ) = 𝑎ᵢ
终点加速度	𝑆″ᵢ(𝑡ᵢ₊₁) = 𝑎ᵢ₊₁

四.系数求解公式（闭式解）

通过求解上述 6 个方程，得到系数的解析表达式（以 ℎᵢ = 𝑡ᵢ₊₁ − 𝑡ᵢ）：

𝑎ᵢ = 𝑝ᵢ
𝑏ᵢ = 𝑣ᵢ
𝑐ᵢ = 𝑎ᵢ / 2

𝑑ᵢ = (20(𝑝ᵢ₊₁ − 𝑝ᵢ) − (8𝑣ᵢ₊₁ + 12𝑣ᵢ)ℎᵢ − (3𝑎ᵢ₊₁ − 7𝑎ᵢ)ℎᵢ²) / (2ℎᵢ³)
𝑒ᵢ = (30(𝑝ᵢ − 𝑝ᵢ₊₁) + (14𝑣ᵢ₊₁ + 16𝑣ᵢ)ℎᵢ + (3𝑎ᵢ₊₁ − 8𝑎ᵢ)ℎᵢ²) / (2ℎᵢ⁴)
𝑓ᵢ = (12(𝑝ᵢ₊₁ − 𝑝ᵢ) − (6𝑣ᵢ₊₁ + 6𝑣ᵢ)ℎᵢ − (𝑎ᵢ₊₁ − 𝑎ᵢ)ℎᵢ²) / (2ℎᵢ⁵)

这些公式是标准五次样条插值的闭式解，可直接用于编程实现。

五.MATLAB 可执行代码

% === 输入：时间 t 和位置 p ===
t = [0, 1, 2.5, 4, 5]; % 时间节点
p = [0, 2, 3, 1, 0]; % 位置值

% === 可选：指定每段的边界速度与加速度 ===
% 若未指定，设为 0 或使用平滑过渡
v_start = [0, 0, 0, 0]; % 每段起始速度
v_end = [0, 0, 0, 0]; % 每段结束速度
a_start = [0, 0, 0, 0]; % 每段起始加速度
a_end = [0, 0, 0, 0]; % 每段结束加速度

% === 预处理 ===
n = length(t);
h = diff(t); % h(i) = t(i+1) - t(i)

% === 初始化系数向量 ===
a_coef = zeros(n-1, 1);
b_coef = zeros(n-1, 1);
c_coef = zeros(n-1, 1);
d_coef = zeros(n-1, 1);
e_coef = zeros(n-1, 1);
f_coef = zeros(n-1, 1);

% === 计算每段的五次样条系数 ===
for i = 1:n-1
dt = h(i); % 时间跨度 Δt
pi = p(i);
pf = p(i+1);
vi = v_start(i);
vf = v_end(i);
ai = a_start(i);
af = a_end(i);

% 使用 Unicode 风格的五次样条系数公式
a_coef(i) = pi;
b_coef(i) = vi;
c_coef(i) = ai / 2;

d_coef(i) = (20*(pf - pi) - (8*vf + 12*vi)*dt - (3*af - 7*ai)*dt^2) / (2*dt^3);
e_coef(i) = (30*(pi - pf) + (14*vf + 16*vi)*dt + (3*af - 8*ai)*dt^2) / (2*dt^4);
f_coef(i) = (12*(pf - pi) - (6*vf + 6*vi)*dt - (af - ai)*dt^2) / (2*dt^5);
end

% === 生成高分辨率轨迹（用于绘图）===
t_plot = linspace(t(1), t(end), 1000);
s_plot = zeros(size(t_plot)); % 位置
v_plot = zeros(size(t_plot)); % 速度
a_plot = zeros(size(t_plot)); % 加速度

for k = 1:length(t_plot)
tk = t_plot(k);

% 查找所在区间
i = find(tk >= t(1:end-1) & tk <= t(2:end), 1, 'last');
if isempty(i)
i = max(1, find(t <= tk, 1, 'last') - 1);
end
if i < 1 || i > n-1, i = 1; end

dt_local = tk - t(i); % τ = t - t_i

% 提取系数
a_i = a_coef(i); b_i = b_coef(i); c_i = c_coef(i);
d_i = d_coef(i); e_i = e_coef(i); f_i = f_coef(i);

% 计算轨迹
s_plot(k) = a_i + b_i*dt_local + c_i*dt_local^2 + ...
d_i*dt_local^3 + e_i*dt_local^4 + f_i*dt_local^5;

v_plot(k) = b_i + 2*c_i*dt_local + 3*d_i*dt_local^2 + ...
4*e_i*dt_local^3 + 5*f_i*dt_local^4;

a_plot(k) = 2*c_i + 6*d_i*dt_local + 12*e_i*dt_local^2 + ...
20*f_i*dt_local^3;
end

六.方法优势

特性	说明
C² 连续	位置、速度、加速度全程连续
低 jerk	加加速度有限，减少机械振动
端点可控	可指定每段起点/终点的速度与加速度
解析解	系数有闭式表达，计算高效
模块化	每段独立求解，易于扩展