运动控制------电子凸轮曲线设计（多项式）
- 凸轮运动规律的参数名称和定义
- 多项式运动规律
- 一次多项式运动规律
- 抛物线二次轨迹（恒定类加速度轨迹）
- 具有非对称恒定类加速度的二次多项式轨迹
- 三次多项式轨迹
-
- 正向求解实现：
- [逆向求解(三次方程如何根据 y 反解 x)：](#逆向求解(三次方程如何根据 y 反解 x)：)
-
- [方法一：卡尔丹公式（Cardano's Formula，纯代数精确解）](#方法一：卡尔丹公式（Cardano's Formula，纯代数精确解）)
- [方法二：盛金公式（Shengjin's Formulas）](#方法二：盛金公式（Shengjin's Formulas）)
- 方法三：数值解法（最推荐，工程与编程首选）
- 牛顿迭代法（Newton-Raphson）
- 处理多根问题
- 特别提醒（唯一性判断）
- 仿真
- 五次多项式轨迹
凸轮运动规律的参数名称和定义
凸轮运动规律
图中给出了一组尖底直动从动件平面凸轮机构在运转过程中的4个过程。对于尖底从动件凸轮机构，以回转中心O为圆心，以O点至凸轮轮廓曲线的最小距离为半径画圆，称为基圆。基圆的半径用 R b R_b Rb表示。
运动过程：
-
推程期：当凸轮按ω方向回转时，凸轮推动从动件上升，直至B点转到B'位置时，从动件到达最高位置，如图1-1b所示。凸轮机构这一阶段的工作过程称为推程期，图a为推程起始位置，图b为推程终止位置。从动件的最大运动距离称为冲程，用h表示。对于摆动从动件，冲程为从动件的最大摆动幅度，用角度参数 ψ m ψ_m ψm表示。与推程期对应的凸轮转角称为推程角，用φ表示。
-
远休止期：当从动件尖底的运动轨迹线偏离凸轮回转中心时（偏距E≠0)，凸轮的推程段轮廓AB所包含的中心角∠AOB与凸轮的推程角不相等。凸轮继续回转，接触点由B点转移至C点，如图1-1c所示。BC段上各点向径不变，从动件在最远位置上停留，该过程称为远休止期，所对应的凸轮转角称为远休止角，用 φ s φ_s φs表示。
-
回程期：从接触点C开始至D点，凸轮轮廓向径逐渐减小，从动件在外力作用下逐渐返回到初始位置，如图1-1d所示。该段时期称为回程期，对应的凸轮转角称为回程角，用φ'表示。
-
近休止期：凸轮由图1-1d所示位置转至图1-1a所示位置，从动件在起始位置停留，称为近休止期。对应的凸轮运动角称为近休止角，用φs'，表示。
在运转过程中，从动件的位移与凸轮转角间的函数关系可用图1-1e所示的位移线图表示。推程期和回程期中任意瞬时的位移值按所选用的运动规律方程式求得。令推程起始位置所对应的凸轮转角φ=0，从动件位移s=0。图1-1e中横坐标为凸轮转角，纵坐标为从动件位移。当凸轮匀速回转时，横坐标也可表示凸轮的转动时间t。
凸轮运动参数
| 概念 | 数学定义 | 与真实运动的关系 | 物理意义与工程应用 |
|---|---|---|---|
| 类速度 ( s ′ s' s′) | 从动件位移 s s s 对凸轮转角 φ \varphi φ 的一阶导数： s ′ = d s d φ s' = \frac{ds}{d\varphi} s′=dφds | 真实速度 v = s ′ ⋅ ω v = s' \cdot \omega v=s′⋅ω （ ω \omega ω 为凸轮角速度） | 反映从动件的速度变化趋势。类速度越大，在相同转速下从动件运动越快。 |
| 类加速度 ( s ′ ′ s'' s′′) | 从动件位移 s s s 对凸轮转角 φ \varphi φ 的二阶导数： s ′ ′ = d 2 s d φ 2 s'' = \frac{d^2s}{d\varphi^2} s′′=dφ2d2s | 真实加速度 a = s ′ ′ ⋅ ω 2 a = s'' \cdot \omega^2 a=s′′⋅ω2 | 反映从动件的加速度变化趋势 。类加速度与从动件的惯性力 直接相关，其最大值 A m A_m Am 是衡量冲击和振动的重要指标。 |
| 类跃度 ( s ′ ′ ′ s''' s′′′) | 从动件位移 s s s 对凸轮转角 φ \varphi φ 的三阶导数： s ′ ′ ′ = d 3 s d φ 3 s''' = \frac{d^3s}{d\varphi^3} s′′′=dφ3d3s | 真实跃度 j = s ′ ′ ′ ⋅ ω 3 j = s''' \cdot \omega^3 j=s′′′⋅ω3 | 反映加速度的变化率，是从动件运动平滑性的关键指标。类跃度越小，机构的振动和噪声通常越低。 |
后续描述计算y=s， x = φ x=\varphi x=φ
多项式运动规律
y = y ( x ) = k 0 + k 1 x + k 2 x 2 + . . . + k n x n y= y(x)=k_0+k_1x+k_2x^2+...+k_nx^n y=y(x)=k0+k1x+k2x2+...+knxn
根据初始边界条件和结束边界条件可确定n+1个 k i k_i ki的系数。多项式的次数n取决于需满足条件的数量和目标运动的期望"平滑度"。由于边界条件的数量通常是偶数，所以多项式的次数n是奇数，如3、5、7等。
当然并不是次数越高越好，次数越高的问题点如下：
- 龙格现象。用次数极高的多项式（如次数 > 10）去拟合有限的数据点时，模型会精确穿过每一个训练点（训练误差为0）。但在数据点之间或两端，曲线会发生剧烈的、不符合物理意义的高频震荡。这就是著名的龙格现象（Runge's phenomenon）------等高次多项式在非均匀节点上，边界处会疯狂摆动。
这个脚本会绘制经典龙格函数 f(x) = 1 / (1 + 25x^2) 的图像，并在同一坐标系下，分别用不同次数的插值多项式去逼近它。
matlab
%% 龙格现象 (Runge Phenomenon) 直观演示
% 该脚本演示了使用高次多项式对龙格函数进行插值时，在区间端点附近
% 产生的剧烈振荡现象。
% 参考: https://www.mathworks.com/matlabcentral/fileexchange/ (示例风格)
% 以及 百度百科"龙格现象"词条中的MATLAB代码[reference:4]
clear; close all; clc;
%% 1. 定义目标函数 (龙格函数)
% 这是一个在 x=0 处有尖锐峰值的经典函数[reference:5][reference:6]
f = @(x) 1 ./ (1 + 25 * x.^2);
% 生成用于绘制光滑目标函数曲线的密集点
x_fine = linspace(-1, 1, 500);
y_fine = f(x_fine);
%% 2. 设置要进行插值的多项式次数 (即插值点个数-1)
% 为了清晰展示效果，我们选择次数逐渐增大的几个多项式
degrees = [3, 5, 7, 9, 11, 13, 15];
% 对应的插值点个数为 degrees + 1
%% 3. 创建图形窗口并绘制
figure('Name', '龙格现象演示', 'Position', [100, 100, 1200, 800]);
% 首先绘制目标函数 (作为参考基准)
plot(x_fine, y_fine, 'r-', 'LineWidth', 2.5);
hold on;
grid on;
% 用于存储图例标签
legend_labels = {'目标函数 f(x) = 1/(1+25x^2)'};
% 循环，对不同次数的多项式进行插值并绘图
for i = 1:length(degrees)
n = degrees(i); % 当前多项式次数
num_points = n + 1; % 需要的插值点个数
% 在 [-1, 1] 区间上生成 n+1 个等距插值节点
x_nodes = linspace(-1, 1, num_points);
y_nodes = f(x_nodes);
% 使用 polyfit 进行多项式拟合，得到 n 次多项式的系数[reference:10]
p = polyfit(x_nodes, y_nodes, n);
% 在密集点 x_fine 上计算插值多项式的值[reference:11]
y_interp = polyval(p, x_fine);
% 绘制插值结果
plot(x_fine, y_interp, '-', 'LineWidth', 1.5);
% 在图上标记出插值节点
plot(x_nodes, y_nodes, 'o', 'MarkerSize', 6, 'MarkerFaceColor', 'auto');
% 更新图例标签
legend_labels{end+1} = sprintf('%d 次插值多项式', n);
end
% 添加辅助信息
xlabel('x');
ylabel('y');
title('龙格现象 (Runge Phenomenon): 高次多项式插值的振荡');
legend(legend_labels, 'Location', 'best');
axis([-1, 1, -0.5, 1.2]); % 固定坐标轴范围以清晰对比[reference:13]
hold off;
- 病态的范德蒙德矩阵，系数 β对数据 y 的微小扰动极度敏感。训练数据只要有一丝噪声，拟合出的系数就会天差地别，完全失去可解释性。
一次多项式运动规律
线性轨迹------等速运动规律、一次项运动规律
正向求解过程
最简单的轨迹是已知起点位置 x s ， y s x_s，y_s xs，ys 和终点位置 x s ， y e x_s，y_e xs，ye，其运动轨迹定义为
y ( x ) = k 0 + k 1 ( x − x s ) y(x) = k_0 + k_1(x - x_s) y(x)=k0+k1(x−xs)
一旦指定了初始角度 x s x_s xs 和终点角度 x e x_e xe 对应的位置 y s y_s ys 和 y e y_e ye，就可求解得到上述公式的系数 k 0 k_0 k0 和 k 1 k_1 k1。
{ y ( x s ) = y 0 = k 0 y ( x e ) = y 1 = k 0 + k 1 ( x e − x s ) ⟹ 1 0 1 X k 0 k 1 = y s y e \begin{cases} y(x_s) = y_0 = k_0 \\ y(x_e) = y_1 = k_0 + k_1(x_e - x_s) \end{cases} \implies \begin{bmatrix} 1 & 0 \\ 1 & X \end{bmatrix} \begin{bmatrix} k_0 \\ k_1 \end{bmatrix} = \begin{bmatrix} y_s \\ y_e \end{bmatrix} {y(xs)=y0=k0y(xe)=y1=k0+k1(xe−xs)⟹110Xk0k1=ysye
其中， X = x 1 − x 0 X= x_1 - x_0 X=x1−x0，表示时间间隔。因此，可得：
{ k 0 = y 0 k 1 = q 1 − q 0 t 1 − t 0 = Δ y X \begin{cases} k_0 = y_0 \\ k_1 = \frac{q_1 - q_0}{t_1 - t_0} = \frac{\Delta y }{X} \end{cases} {k0=y0k1=t1−t0q1−q0=XΔy
其中， Δ y = y e − y s \Delta y = y_e - y_s Δy=ye−ys，表示位移量。类速度在主轴位置区间 x s , x e x_s, x_e xs,xe 上是恒定的，其值为
y ˙ ( x ) = Δ y X ( = k 1 ) \dot{y}(x) = \frac{\Delta y }{X} \quad (= k_1) y˙(x)=XΔy(=k1)
显然，上述轨迹内部的加速度为零，故在起点和终点处会存在冲击。
下面是MtaLab代码实现：
matlab
% 根据X求YVA（从轴位置、从轴速度、从轴加速度），一次曲线，类速度恒定
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Xe - 终点X
% Ye - 终点Y
% X - 设定X
% 输出参数:
% y - X时的y值
% v - X时的v值
% a - X时的a值
% 作者: MrControll
% 日期: 20260703
function [y,v,a,Result] = GetYVA_Poly1(Xs,Ys,Xe,Ye,x)
% --- 以下是函数的主体代码 ---
y = 0; v =0; a = 0;
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
%% 超过边界值处理
if x >= Xe
y = Ye;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
elseif x <= Xs
y = Ys;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
end
%% 未超过边界值处理
if Xe == Xs
y = Ye;
v = 0;
a = 0;
Result = true;
return;
else
K =(Ye-Ys)/(Xe-Xs);
y = K*(x-Xs)+Ys;
v = K;
a = 0;
Result = true;
return;
end
end
function Result = SEL(X,Y1,Y2)
if X == true
Result = Y1;
else
Result = Y2;
end
end逆向求解过程
有的场景会涉及到反向求解，根据输入的Y值，反算主轴位置值。
y ( x ) = k 0 + k 1 ( x − x s ) y(x) = k_0 + k_1(x - x_s) y(x)=k0+k1(x−xs)
反向求解式为：
x ( y ) = 1 k 1 ( y − k 0 ) + x s x(y) =\frac{1}{k_1}(y - k_0) + x_s x(y)=k11(y−k0)+xs
Matlab代码如下：
matlab
% 根据YVA求X，一次曲线，类速度恒定
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Xe - 终点X
% Ye - 终点Y
% y - 设定Y
% 输出参数:
% x - y时的x值
% 作者: MrControll
% 日期: 20260703
function [Count,x,Result] = GetX_Poly1(Xs,Ys,Xe,Ye,y)
% --- 以下是函数的主体代码 ---
Count = 0;
x = [];
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
if y == Ye && Ye == Ys
Count = 2;
x(1) = Xe;
x(2) = Xe;
Result = true;
return;
end
% 超范围跳出
if y<min(Ys ,Ye) || y>max(Ys ,Ye)
Result = false;
return;
end
if Xe==Xs
Result = false;
return;
else
if (Ye == Ys)
Result = false;
return;
else
x(1) =(y-Ys)/(Ye-Ys)*(Xe-Xs)+Xs;
Count =1;
Result = true;
return;
end
end
end
function Result = SEL(X,Y1,Y2)
if X == true
Result = Y1;
else
Result = Y2;
end
end仿真演示
matlab
% 定义直线参数（起点、终点）
Xs = 0; Ys = 0;
Xe = 10; Ye = 5;
% 生成一系列 x 值（包含边界内外）
x_vals = linspace(-2, 12, 500);
% 预分配输出数组
y_vals = zeros(size(x_vals));
v_vals = zeros(size(x_vals));
a_vals = zeros(size(x_vals));
Result_vals = false(size(x_vals));
% 循环调用函数
for i = 1:length(x_vals)
[y_vals(i), v_vals(i), a_vals(i), Result_vals(i)] = ...
GetYVA_Poly1(Xs, Ys, Xe, Ye, x_vals(i));
end
% 检查所有调用是否成功（应全部为 true）
if all(Result_vals)
disp('所有点计算成功。');
else
warning('存在计算失败的点，请检查输入。');
end
% 绘图
figure('Position', [100, 100, 800, 600]);
subplot(3,1,1);
plot(x_vals, y_vals, 'b-', 'LineWidth', 1.5);
hold on;
plot([Xs, Xe], [Ys, Ye], 'ro', 'MarkerSize', 8, 'LineWidth', 2); % 标记端点
xlabel('x'); ylabel('y'); title('位置 y 随 x 的变化');
grid on; legend('y(x)', '端点', 'Location', 'best');
subplot(3,1,2);
plot(x_vals, v_vals, 'r-', 'LineWidth', 1.5);
xlabel('x'); ylabel('v'); title('速度 v 随 x 的变化');
grid on;
subplot(3,1,3);
plot(x_vals, a_vals, 'g-', 'LineWidth', 1.5);
xlabel('x'); ylabel('a'); title('加速度 a 随 x 的变化（恒为 0）');
grid on;
抛物线二次轨迹（恒定类加速度轨迹）
抛物线轨迹也被称为重力轨迹 或具有恒定加速度的轨迹 ，其特征在于在加速和减速阶段具有恒定的加速度值且符号相反。进一步分析可知，它由两个二次多项式组合而成，其中一段是从 x s x_s xs到 x f x_f xf，另一段是从 x f x_f xf 到 x e x_e xe。
对称情况
现在我们考虑轨迹相对于中间点对称的情况，定义：
x f = x s + x e 2 和 y f ( x f ) = y f = y s + y e 2 x_f = \frac{x_s + x_e}{2} \quad \text{和} \quad y_f(x_f) = y_f = \frac{y_s + y_e}{2} xf=2xs+xe和yf(xf)=yf=2ys+ye
此时满足：
X a = ( x f − x s ) = Δ x 2 , ( y f − y s ) = Δ y 2 X_a = (x_f - x_s) = \frac{\Delta x}{2}, \quad (y_f - y_s) = \frac{\Delta y}{2} Xa=(xf−xs)=2Δx,(yf−ys)=2Δy
在第一阶段，即"加速"阶段，轨迹定义为：
y a ( x ) = k 0 + k 1 ( x − x s ) + k 2 ( x − x s ) 2 , x ∈ x s , x f y_a(x) = k_0 + k_1 (x- x_s) + k_2 (x - x_s)^2, \quad x \in x_s, x_f ya(x)=k0+k1(x−xs)+k2(x−xs)2,x∈xs,xf
上述中的参数 k 0 , k 1 , k 2 k_0, k_1, k_2 k0,k1,k2可根据轨迹的约束条件（途经点 q s , q f q_s, q_f qs,qf和初始速度 v s v_s vs来确定。
约束方程组：
{ y a ( x s ) = y s = k 0 y a ( x f ) = y f = k 0 + k 1 ( x f − x s ) + k 2 ( x f − x s ) 2 y ˙ a ( x s ) = v s = k 1 \begin{cases} y_a(x_s) = y_s = k_0 \\6pt y_a(x_f) = y_f = k_0 + k_1 (x_f - x_s) + k_2 (x_f - x_s)^2 \\6pt \dot{y}_a(x_s) = v_s = k_1 \end{cases} ⎩ ⎨ ⎧ya(xs)=ys=k0ya(xf)=yf=k0+k1(xf−xs)+k2(xf−xs)2y˙a(xs)=vs=k1
根据式：
k 0 = y s , k 1 = v s , k 2 = 2 Δ x 2 ( Δ y − v s Δ x ) k_0 = y_s, \quad k_1 = v_s, \quad k_2 = \frac{2}{\Delta x^2}(\Delta y - v_s\Delta x) k0=ys,k1=vs,k2=Δx22(Δy−vsΔx)
因此，对于 x ∈ x s , x f x\in x_s, x_f x∈xs,xf的轨迹可定义为
{ y a ( x ) = y s + v s ( x − x s ) + 2 Δ x 2 ( Δ y − v s Δ x ) ( x − x s ) 2 y ˙ a ( x ) = v s + 4 Δ x 2 ( Δ y − v s Δ x ) ( x − x s ) y ¨ a ( x ) = 4 Δ x 2 ( Δ y − v s Δ x ) ( 常量 ) \begin{cases} y_a(x) = y_s + v_s(x - x_s) + \dfrac{2}{\Delta x^2}(\Delta y - v_s\Delta x)(x - x_s)^2 \\8pt \dot{y}_a(x) = v_s + \dfrac{4}{\Delta x^2}(\Delta y - v_s\Delta x)(x - x_s) \\8pt \ddot{y}_a(x) = \dfrac{4}{\Delta x^2}(\Delta y - v_s\Delta x) \quad (\text{常量}) \end{cases} ⎩ ⎨ ⎧ya(x)=ys+vs(x−xs)+Δx22(Δy−vsΔx)(x−xs)2y˙a(x)=vs+Δx24(Δy−vsΔx)(x−xs)y¨a(x)=Δx24(Δy−vsΔx)(常量)
在拐点处的速度为
v max = y ˙ a ( t f ) = 2 Δ y Δ x − v s v_{\max} = \dot{y}_a(t_f) = 2\frac{\Delta y}{\Delta x} - v_s vmax=y˙a(tf)=2ΔxΔy−vs
加速度除了拐点处均为 0，加速度在拐点处改变符号并且其值为无穷大。
在第二阶段，即在拐点和终点之间，轨迹可定义为
y b ( x ) = k 3 + k 4 ( x − x f ) + k 5 ( x − x f ) 2 , x ∈ x f , x e y_b(x) = k_3 + k_4(x- x_f) + k_5(x -x_f)^2, \quad x \in x_f, x_e yb(x)=k3+k4(x−xf)+k5(x−xf)2,x∈xf,xe
根据指定的速度 v e v_e ve，则在 x = x e x = x_e x=xe时，可通过下面的等式来计算得到参数 k 3 , k 4 , k 5 k_3, k_4, k_5 k3,k4,k5：
{ y b ( x f ) = y f = k 3 y b ( x e ) = y e = k 3 + k 4 ( x e − x f ) + k 5 ( x e − x f ) 2 y ˙ b ( x e ) = v e = k 4 + 2 k 5 ( x e − x f ) \begin{cases} y_b(x_f) = y_f = k_3 \\6pt y_b(x_e) = y_e = k_3 + k_4(x_e - x_f) + k_5(x_e - x_f)^2 \\6pt \dot{y}_b(x_e) = v_e = k_4 + 2k_5(x_e - x_f) \end{cases} ⎩ ⎨ ⎧yb(xf)=yf=k3yb(xe)=ye=k3+k4(xe−xf)+k5(xe−xf)2y˙b(xe)=ve=k4+2k5(xe−xf)
由上述可得：
k 3 = y f = y s + y e 2 , k 4 = 2 Δ y Δ x − v e , k 5 = 2 Δ x 2 ( v e Δ x − Δ y ) k_3 = y_f = \frac{y_s + y_e}{2}, \quad k_4 = 2\frac{\Delta y}{\Delta x} - v_e, \quad k_5 = \frac{2}{\Delta x^2}(v_e\Delta x - \Delta y) k3=yf=2ys+ye,k4=2ΔxΔy−ve,k5=Δx22(veΔx−Δy)
对应 x ∈ x f , x e x\in x_f, x_e x∈xf,xe的轨迹表达式为
{ y b ( x ) = y f + ( 2 Δ y Δ x − v e ) ( x − x f ) + 2 Δ x 2 ( v e Δ x − Δ y ) ( x − x f ) 2 y ˙ b ( x ) = 2 Δ y Δ x − v e + 4 Δ x 2 ( v e Δ x − Δ y ) ( x − x f ) y ¨ b ( x ) = 4 Δ x 2 ( v e Δ x − Δ y ) \begin{cases} y_b(x) = y_f + \left(2\frac{\Delta y}{\Delta x} - v_e\right)(x -x_f) + \frac{2}{\Delta x^2}(v_e\Delta x - \Delta y)(x- x_f)^2 \\8pt \dot{y}_b(x) = 2\frac{\Delta y}{\Delta x} - v_e + \frac{4}{\Delta x^2}(v_e\Delta x - \Delta y)(x- x_f)\\8pt \ddot{y}_b(x) = \frac{4}{\Delta x^2}(v_e\Delta x - \Delta y) \end{cases} ⎩ ⎨ ⎧yb(x)=yf+(2ΔxΔy−ve)(x−xf)+Δx22(veΔx−Δy)(x−xf)2y˙b(x)=2ΔxΔy−ve+Δx24(veΔx−Δy)(x−xf)y¨b(x)=Δx24(veΔx−Δy)
注意到，若 v s ≠ v e v_s \neq v_e vs=ve，则上述轨迹的速度在 x = x f x = x_f x=xf时刻是不连续的。
正向求解工程
Matlab代码如下：
matlab
% 根据X求YVA，二次曲线,类加速度恒定，加速度对称
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Vs - 起点V
% Xe - 终点X
% Ye - 终点Y
% Ve - 终点V
% X - 设定X
% 输出参数:
% y - X时的y值
% v - X时的v值
% a - X时的a值
% 作者: MrControll
% 日期: 20260704
function [y,v,a,Result] = GetYVA_Poly2S(Xs,Ys,Vs,Xe,Ye,Ve,x)
% --- 以下是函数的主体代码 ---
y = 0; v =0; a = 0;
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
%% 输入值不在范围内直接输出边界值
if (x >= Xe)
y = Ye;
v = Ve;
a = 0;
Result = true;
return;
elseif (x <= Xs)
y = Ys;
v = Vs;
a = 0;
Result = true;
return;
end
%% 超过边界值处理
if x >= Xe
y = Ye;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
elseif x <= Xs
y = Ys;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
end
%% 未超过边界值处理
if Xe == Xs
y = Ye;
v = Ve;
a = 0;
Result = true;
return;
else
T = Xe - Xs;
h = Ye - Ys;
% 中点
Xf = (Xe + Xs)/2;
Yf = (Ye + Ys)/2;
% 多项式系数
k0 = Ys;
k1 = Vs;
k2 = 2/(T*T)*(h - Vs*T);
k3 = Yf;
k4 = 2*h/T - Ve;
k5 = 2/(T*T)*(Ve*T-h);
if x>=Xs && x<=Xf
y = k0 + ( k1*(x-Xs) ) + ( k2*(x-Xs)^2 ) ;
v = k1+ 2*(k2*(x-Xs)) ;
a = 2*k2 ;
Result = true;
return;
elseif x>=Xf && x<=Xe
y = k3 + ( k4*(x-Xf) ) + ( k5*(x-Xf)^2 ) ;
v = k4 + 2*(k5*(x-Xf)) ;
a = 2*k5 ;
Result = true;
return;
end
end
end反向求解工程
将二次函数简写为如下显示
y = a x 2 + b x + c y = ax^2+ bx+c y=ax2+bx+c
要根据y，求x，即计算一元二次方程的根
0 = a x 2 + b x + c − y 0 = ax^2+ bx+c-y 0=ax2+bx+c−y
{ x = − b ± b 2 − 4 a ( c − y ) 2 a , a < > 0 x = y − c b , a = 0 , b < > 0 \begin{cases} x = \frac{-b± \sqrt b^2-4a(c-y) }{2a} ,a<>0\\8pt x = \frac{y-c }{b} ,a=0,b<>0 \end{cases} ⎩ ⎨ ⎧x=2a−b±b 2−4a(c−y),a<>0x=by−c,a=0,b<>0
Matlab代码如下：
matlab
% 根据YVA求X，二次曲线，类加速度恒定，加速度对称
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Xe - 终点X
% Ye - 终点Y
% y - 设定Y
% 输出参数:
% x - y时的x值
% 作者: MrControll
% 日期: 20260703
function [Count,x,Result] = GetX_Poly2S(Xs,Ys,Vs,Xe,Ye,Ve,y)
% --- 以下是函数的主体代码 ---
Count = 0;
x = [];
xTem = [];
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
%% 求解过程
if y==Ye && Ye==Ys && Vs==0 && Ve==0
x(0) =Xs;
x(1) =Xe;
Count =2;
Result = TRUE;
return;
end
for i =1:4:1
x(i) = 0 ;
end
if Xe==Xs
return;
end
X1 = (Xe-Xs)/2;
X1 = clip(tol,X1,Xe-Xs-tol);
Y1 = (2.0*(Ye-Ys) + X1*(Ve-Vs) - Ve*(Xe-Xs))/(Xe-Xs);
% 第一段求解
Num = 0;
A =(Y1-Vs)/(2.0*X1);
B = Vs;
C = Ys-y;
delta = B*B-4*A*C;
if A~=0
if delta>=0
if A>0
xTem(1) =(-B-sqrt(delta))/(2*A);
xTem(2) =(-B+sqrt(delta))/(2*A);
else
xTem(1) =(-B+sqrt(delta))/(2*A);
xTem(2) =(-B-sqrt(delta))/(2*A);
end
Num = 2;
elseif delta==0
xTem(1) = (-B+sqrt(delta))/(2*A);
Num =1;
else
Num =0;
end
else
if B~=0
xTem(1) = - C/B;
Num = 1;
else
Num = 0;
end
end
% 区间判断
uiCount = 1;
for i=1:Num:1
if xTem(i)+Xs>=Xs && xTem(i)<=X1
x(uiCount) = xTem(i)+fxs;
uiCount = uiCount+1;
end
end
% 第2段求解
A =(Y1-Ve)/(2.0*(X1-(Xe-Xs)));
B = Ve - 2*A*(Xe-Xs);
C = Ye-Ve*(Xe-Xs) + A*(Xe-Xs)^2 -y;
delta = B^2-4*A*C;
if A~=0
if delta>=0
if A>0
xTem(1) =(-B-sqrt(delta))/(2*A);
xTem(2) =(-B+sqrt(delta))/(2*A);
else
xTem(1) =(-B+sqrt(delta))/(2*A);
xTem(2) =(-B-sqrt(delta))/(2*A);
end
Num = 2;
elseif delta==0
xTem(1) =(-B+sqrt(delta))/(2*A);
Num =1;
else
Num =0;
end
else
if B~=0
xTem(1) =-C/B;
Num =1;
else
Num =0;
end
end
%区间判断
for i=1:Num:1
if xTem(i)>= X1 && xTem(i)+Xs<=Xe
x(uiCount) = xTem(i)+ Xs;
uiCount =uiCount+1;
end
end
import Other.*
bubbleSortUnique(x);
Result = true;
Count = uiCount-1;
return;
end仿真演示
matlab
import CamFun.Polynominal.*
% 测试 GetYVA_Poly2S 函数并绘制曲线、速度、加速度图
clear; clc; close all;
% 设置起点、终点及速度
Xs = 2; Ys = 0; Vs = 0; % 起点 (0,0), 速度1
Xe = 10; Ye = 4; Ve = 0; % 终点 (10,5), 速度0
[Count,xt,Resultx] = GetX_Poly2S(Xs, Ys, Vs, Xe, Ye, Ve, 2);
% 生成一系列查询点
x = linspace(Xs, Xe, 500);
n = length(x);
y = zeros(1, n);
v = zeros(1, n);
a = zeros(1, n);
Result = true;
% 逐个查询
for i = 1:n
[y(i), v(i), a(i), flag] = GetYVA_Poly2S(Xs, Ys, Vs, Xe, Ye, Ve, x(i));
if ~flag
error('查询失败，请检查输入参数');
end
end
% 绘图
figure('Position', [100, 100, 800, 600]);
% 子图1：位置曲线
subplot(3,1,1);
plot(x, y, 'b-', 'LineWidth', 2);
hold on;
plot([Xs, Xe], [Ys, Ye], 'ro', 'MarkerSize', 8, 'LineWidth', 2);
grid on;
xlabel('x'); ylabel('y');
title('位置曲线');
legend('y(x)', '端点', 'Location', 'best');
% 子图2：速度曲线
subplot(3,1,2);
plot(x, v, 'r-', 'LineWidth', 2);
hold on;
plot([Xs, Xe], [Vs, Ve], 'ko', 'MarkerSize', 8, 'LineWidth', 2);
grid on;
xlabel('x'); ylabel('v = dy/dx');
title('速度曲线');
legend('v(x)', '端点速度', 'Location', 'best');
% 子图3：加速度曲线
subplot(3,1,3);
plot(x, a, 'g-', 'LineWidth', 2);
grid on;
xlabel('x'); ylabel('a = d^2y/dx^2');
title('加速度曲线（分段常数）');
legend('a(x)', 'Location', 'best');
% 调整整体布局
sgtitle('二次曲线插值示例（两段加速度恒定）');
具有非对称恒定类加速度的二次多项式轨迹
考虑在某个主轴位置 x s < x f < x e x_s < x_f < x_e xs<xf<xe 对应的某一点的轨迹信息，此时不一定满足 x = ( t e + t s ) / 2 x= (t_e + t_s) / 2 x=(te+ts)/2，该轨迹可由两个多项式来描述：
y a ( x ) = k 0 + k 1 ( x − x s ) + k 2 ( x − x s ) 2 x s ≤ x < x f y_a(x) = k_0 + k_1 (x - x_s) + k_2 (x - x_s)^2 \quad x_s \leq x< x_f ya(x)=k0+k1(x−xs)+k2(x−xs)2xs≤x<xf
y b ( x ) = k 3 + k 4 ( x − x f ) + k 5 ( x − x f ) 2 x f ≤ x < x e y_b(x) = k_3 + k_4 (x - x_f) + k_5 (x - x_f)^2 \quad x_f \leq x < x_e yb(x)=k3+k4(x−xf)+k5(x−xf)2xf≤x<xe
其中，通过指定 x s x_s xs, x e x_e xe主轴位置对应的从轴位置和速度及 x f x_f xf 时刻的两个连续性条件（位置和速度）可获得参数 k 0 k_0 k0, k 1 k_1 k1, k 2 k_2 k2, k 3 k_3 k3, k 4 k_4 k4 和 k 5 k_5 k5 的值：
{ y a ( t 0 ) = k 0 = y 0 y b ( t 1 ) = k 3 + k 4 ( x e − x f ) + k 5 ( x e − x f ) 2 = y 1 y ˙ a ( t 0 ) = k 1 = v s y ˙ b ( t 1 ) = k 4 + 2 k 5 ( x e − x f ) = v 1 q a ( t f ) = k 0 + k 1 ( x f − x s ) + k 2 ( x f − x s ) 2 = k 3 ( = y b ( t f ) ) y ˙ a ( t f ) = k 1 + 2 k 2 ( x f − x s ) = k 4 ( = q ˙ b ( x f ) ) \begin{cases} y_a(t_0) = k_0 = y_0 \\ y_b(t_1) = k_3 + k_4 (x_e - x_f) + k_5 (x_e - x_f)^2 = y_1 \\ \dot{y}_a(t_0) = k_1 = v_s \\ \dot{y}_b(t_1) = k_4 + 2k_5 (x_e -x_f) = v_1 \\ q_a(t_f) = k_0 + k_1 (x_f - x_s) + k_2 (x_f - x_s)^2 =k_3 (= y_b(t_f)) \\ \dot{y}_a(t_f) = k_1 + 2k_2 (x_f - x_s) = k_4 (= \dot{q}_b(x_f)) \end{cases} ⎩ ⎨ ⎧ya(t0)=k0=y0yb(t1)=k3+k4(xe−xf)+k5(xe−xf)2=y1y˙a(t0)=k1=vsy˙b(t1)=k4+2k5(xe−xf)=v1qa(tf)=k0+k1(xf−xs)+k2(xf−xs)2=k3(=yb(tf))y˙a(tf)=k1+2k2(xf−xs)=k4(=q˙b(xf))
通过定义 X a = ( x f − x 0 ) X_a = (x_f - x_0) Xa=(xf−x0) 和 X d = ( x e − x f ) X_d = (x_e - x_f) Xd=(xe−xf)，可获得如下的参数值：
{ k 0 = y s k 1 = v s k 2 = 2 Δ y − v s ( Δ x + X a ) − v e X d 2 Δ x X a k 3 = 2 y e X a + X d ( 2 y s + T a ( v s − v e ) ) 2 Δ x k 4 = 2 Δ y − v s T a − v e T d Δ x k 5 = − 2 Δ y − v s T a − v e ( Δ x + X d ) 2 Δ x X d . \begin{cases} k_0 = y_s \\ k_1 = v_s \\ k_2 = \frac{2 \Delta y - v_s(\Delta x + X_a) - v_eX_d}{2\Delta xX_a} \\ k_3 = \frac{2y_eX_a + X_d(2y_s + T_a(v_s - v_e))}{2\Delta x} \\ k_4 = \frac{2\Delta y - v_sT_a - v_eT_d}{\Delta x} \\ k_5 = -\frac{2\Delta y- v_sT_a - v_e(\Delta x + X_d)}{2\Delta xX_d}. \end{cases} ⎩ ⎨ ⎧k0=ysk1=vsk2=2ΔxXa2Δy−vs(Δx+Xa)−veXdk3=2Δx2yeXa+Xd(2ys+Ta(vs−ve))k4=Δx2Δy−vsTa−veTdk5=−2ΔxXd2Δy−vsTa−ve(Δx+Xd).
正向求解如下：
matlab
% GetYVA_Poly2AS 非对称恒定加速度轨迹规划
% 输入：
% q0 : 起点位置
% v0 : 起点速度
% q1 : 终点位置
% v1 : 终点速度
% t0 : 起始时刻
% tf : 切换时刻（加速度变化时刻）
% t1 : 终止时刻
% t : 待求时刻（标量或向量），须满足 t0 <= t <= t1
% 输出：
% q : 位置
% qd : 速度
% qdd : 加速度
%
% 说明：轨迹由两段二次多项式构成，加速度在 tf 处阶跃变化，
% 但位置和速度连续。适用于非对称加减速时间的情形。
function [q, qd, qdd] = GetYVA_Poly2AS(q0, v0, q1, v1, t0, tf, t1, t)
% 参数检查
if nargin < 8
error('需要8个输入参数：q0, v0, q1, v1, t0, tf, t1, t');
end
if any(t < t0) || any(t > t1)
error('时间 t 必须位于 [t0, t1] 区间内');
end
if tf <= t0 || tf >= t1
error('切换时刻 tf 必须满足 t0 < tf < t1');
end
% 计算时间间隔
T = t1 - t0; % 总时间
Ta = tf - t0; % 上升段时长
Td = t1 - tf; % 下降段时长
% 位移差
h = q1 - q0;
% 计算多项式系数 (参考公式 2.1.3)
a0 = q0;
a1 = v0;
a2 = (2*h - v0*(T + Ta) - v1*Td) / (2*T*Ta);
a3 = (2*q1*Ta + Td*(2*q0 + Ta*(v0 - v1))) / (2*T);
a4 = (2*h - v0*Ta - v1*Td) / T;
a5 = -(2*h - v0*Ta - v1*(T + Td)) / (2*T*Td);
% 初始化输出
q = zeros(size(t));
qd = zeros(size(t));
qdd = zeros(size(t));
% 分段计算
% 上升段 [t0, tf]
idx_a = (t <= tf);
if any(idx_a)
tau = t(idx_a) - t0; % 相对于 t0 的时间
q(idx_a) = a0 + a1*tau + a2*tau.^2;
qd(idx_a) = a1 + 2*a2*tau;
qdd(idx_a) = 2*a2;
end
% 下降段 [tf, t1]
idx_b = (t > tf);
if any(idx_b)
tau = t(idx_b) - tf; % 相对于 tf 的时间
q(idx_b) = a3 + a4*tau + a5*tau.^2;
qd(idx_b) = a4 + 2*a5*tau;
qdd(idx_b) = 2*a5;
end
end仿真代码如下：
matlab
% 演示 GetYVA_Poly2S 函数并绘制非对称恒定类加速度轨迹
clear; clc; close all;
import CamFun.Polynominal.*
% 定义轨迹参数
q0 = 0; v0 = 0.5; % 起点
q1 = 10; v1 = 0.2; % 终点
t0 = 0; tf = 2; t1 = 3.0; % 时间节点
% 生成时间向量
t = linspace(t0, t1, 100);
% 计算轨迹
[q, qd, qdd] = GetYVA_Poly2AS(q0, v0, q1, v1, t0, tf, t1, t);
% 绘图
figure;
subplot(3,1,1); plot(t, q); ylabel('位置 q'); grid on;
subplot(3,1,2); plot(t, qd); ylabel('速度 qd'); grid on;
subplot(3,1,3); plot(t, qdd); ylabel('加速度 qdd'); grid on;
xlabel('时间 t');三次多项式轨迹
y ( x ) = k 0 + k 1 ( x − x s ) + k 2 ( x − x s ) 2 + k 3 ( x − x s ) 3 x s ≤ x ≤ x e y(x) = k_0 + k_1(x - x_s) + k_2(x - x_s)^2 + k_3(x-x_s)^3 \quad x_s \leq x \leq x_e y(x)=k0+k1(x−xs)+k2(x−xs)2+k3(x−xs)3xs≤x≤xe
当同时指定 x s x_s xs和 x e x_e xe 主轴时的从轴位置和速度时，即已知 y s , y e , v s , v e y_s, y_e, v_s, v_e ys,ye,vs,ve，轨迹可通过这 4 个约束条件用于求解三次多项式：
根据上述的约束条件，可以得到参数 k 0 , k 1 , k 2 , k 3 k_0, k_1,k_2,k_3 k0,k1,k2,k3的值：
{ k 0 = y s k 1 = v s k 2 = 3 Δ y − ( 2 v s + v e ) 3 Δ x Δ x 2 k 3 = − 2 Δ y + ( v s + v e ) Δ x Δ x 3 \begin{cases} k_0 = y_s \\ k_1 = v_s \\ k_2 = \dfrac{3 \Delta y - (2v_s + v_e)3 \Delta x}{\Delta x^2} \\ k_3 = \dfrac{-2 \Delta y + (v_s + v_e)\Delta x}{\Delta x^3} \end{cases} ⎩ ⎨ ⎧k0=ysk1=vsk2=Δx23Δy−(2vs+ve)3Δxk3=Δx3−2Δy+(vs+ve)Δx
其中， Δ y = y e − y s \Delta y= y_e-y_s Δy=ye−ys ， Δ x = x e − x s \Delta x= x_e-x_s Δx=xe−xs
正向求解实现：
matlab
% 根据X求YVA，三次曲线,类加加速度恒定
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Vs - 起点V
% Xe - 终点X
% Ye - 终点Y
% Ve - 终点V
% X - 设定X
% 输出参数:
% y - X时的y值
% v - X时的v值
% a - X时的a值
% 作者: MrControll
% 日期: 20260705
function [y,v,a,Result] = GetYVA_Poly3(Xs,Ys,Vs,Xe,Ye,Ve,x)
% --- 以下是函数的主体代码 ---
y = 0; v =0; a = 0;
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
%% 输入值不在范围内直接输出边界值
if (x >= Xe)
y = Ye;
v = Ve;
a = 0;
Result = true;
return;
elseif (x <= Xs)
y = Ys;
v = Vs;
a = 0;
Result = true;
return;
end
%% 超过边界值处理
if x >= Xe
y = Ye;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
elseif x <= Xs
y = Ys;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
Result = true;
return;
end
%% 未超过边界值处理
if Xe == Xs
y = Ye;
v = Ve;
a = 0;
Result = true;
return;
else
[k0,k1,k2,k3]=GetThreeCurveFactor(Xs,Ys,Vs,Xe,Ye,Ve);
[kk0,kk1,kk2]=DerivCubic(k0,k1,k2,k3);
[kkk0,kkk1]=DerivQuadratic(kk0,kk1,kk2);
y = k0 + k1*(x-Xs)+ k2.*(x-Xs).*(x-Xs) + k3.*(x-Xs).*(x-Xs).*(x-Xs) ;
v = kk0 + kk1.*(x-Xs)+ kk2.*(x-Xs).*(x-Xs) ;
a = kkk0 + kkk1*(x-Xs) ;
end
end
%% 三此项系数求解
function [k0,k1,k2,k3]=GetThreeCurveFactor(Xs,Ys,Vs,Xe,Ye,Ve)
% 数据精度
tol = 1e-10;
if abs(Xs-Xe-tol) <=0
return;
end
T = Xe -Xs;
h = Ye -Ys;
k0 = Ys;
k1 = Vs;
k2 = (3*h - (2*Vs+Ve)*T)/(T^2);
k3 = (-2*h + (Vs+Ve)*T)/(T^3);
end
%% 三次函数求导
function [kk0,kk1,kk2]=DerivCubic(k0,k1,k2,k3) %#ok<INUSD,DEFNU>
kk0 = k1;
kk1 = 2*k2;
kk2 = 3*k3;
end
%% 二次函数求导
function [kkk0,kkk1]=DerivQuadratic(kk0,kk1,kk2) %#ok<INUSD,DEFNU>
kkk0 = kk1;
kkk1 = 2*kk2;
end逆向求解(三次方程如何根据 y 反解 x)：
根据 y 反解 x，本质上就是求解关于 x 的三次方程：
a x 3 + b x 2 + c x + d − y = 0 ax^3 + bx^2 + cx + d - y = 0 ax3+bx2+cx+d−y=0
（令 F ( x ) = a x 3 + b x 2 + c x + ( d − y ) = 0 F(x)=ax^3+bx^2+cx+(d-y)=0 F(x)=ax3+bx2+cx+(d−y)=0）
针对不同的使用场景（理论推导、手算、编程），主要有以下 3 种解法：
方法一：卡尔丹公式（Cardano's Formula，纯代数精确解）
这是最通用的代数解法，适用于需要写出精确根号表达式的场景。为了简化计算，通常先通过换元消去二次项。
标准步骤
-
归一化 ：两边同除以 (a)，得到 x 3 + A x 2 + B x + C = 0 x^3 + Ax^2 + Bx + C = 0 x3+Ax2+Bx+C=0 其中 (A=b/a,; B=c/a,; C=(d-y)/a)。
-
消去二次项 ：令 x = t − A 3 x = t - \frac{A}{3} x=t−3A，代入原式得到缺项三次方程 ：
t 3 + p t + q = 0 t^3 + pt + q = 0 t3+pt+q=0
其中：
p = B − A 2 3 , q = 2 A 3 27 − A B 3 + C p = B - \frac{A^2}{3}, \quad q = \frac{2A^3}{27} - \frac{AB}{3} + C p=B−3A2,q=272A3−3AB+C
-
计算判别式 ：
Δ = ( q 2 ) 2 + ( p 3 ) 3 \Delta = \left(\frac{q}{2}\right)^2 + \left(\frac{p}{3}\right)^3 Δ=(2q)2+(3p)3
-
代入求根公式（卡尔丹公式）：
-
若 Δ > 0 \Delta > 0 Δ>0：1个实根 + 2个共轭复根
t = − q 2 + Δ 3 + − q 2 − Δ 3 t = \sqrt3{-\frac{q}{2} + \sqrt{\Delta}} + \sqrt3{-\frac{q}{2} - \sqrt{\Delta}} t=3−2q+Δ +3−2q−Δ
-
若 Δ = 0 \Delta = 0 Δ=0：全部为实根 （有重根）
t 1 = 2 − q 2 3 , t 2 = t 3 = − − q 2 3 t_1 = 2\sqrt3{-\frac{q}{2}}, \quad t_2 = t_3 = -\sqrt3{-\frac{q}{2}} t1=23−2q ,t2=t3=−3−2q
-
若 Δ < 0 \Delta < 0 Δ<0：3个不相等实根 （此时用三角形式更简洁，避免虚数开方）：
t k = 2 − p 3 ⋅ cos ( 1 3 arccos ( 3 q 2 p − 3 p ) − 2 π k 3 ) , k = 0 , 1 , 2 t_k = 2\sqrt{-\frac{p}{3}} \cdot \cos\left( \frac{1}{3} \arccos\left( \frac{3q}{2p}\sqrt{-\frac{3}{p}} \right) - \frac{2\pi k}{3} \right), \quad k=0,1,2 tk=2−3p ⋅cos(31arccos(2p3q−p3 )−32πk),k=0,1,2
-
-
回代 ： x = t − A 3 x = t - \frac{A}{3} x=t−3A。
适用场景：数学证明、作业推导、需要精确根号表达式。
方法二：盛金公式（Shengjin's Formulas）
由中国数学家范盛金提出，本质是卡尔丹公式的换元变形，将判别式重新定义为 ( Δ = B 2 − 4 A C (\Delta = B^2 - 4AC (Δ=B2−4AC 形式。
方法三：数值解法（最推荐，工程与编程首选）
在实际编程或工程计算中，不推荐用复杂的根号公式 （涉及浮点数精度丢失、复数处理麻烦）。最稳定、最快速的方法是牛顿迭代法。
牛顿迭代法（Newton-Raphson）
- 构建函数
f ( x ) = a x 3 + b x 2 + c x + ( d − y ) f(x) = ax^3 + bx^2 + cx + (d - y) f(x)=ax3+bx2+cx+(d−y) - 求导：
f ′ ( x ) = 3 a x 2 + 2 b x + c f'(x) = 3ax^2 + 2bx + c f′(x)=3ax2+2bx+c - 任取初始值 x 0 x_0 x0（例如 0，或根据物理意义取值），循环迭代：
x n + 1 = x n − f ( x n ) f ′ ( x n ) x_{n+1} = x_n - \frac{f(x_n)}{f'(x_n)} xn+1=xn−f′(xn)f(xn) - 当 ∣ x n + 1 − x n ∣ < ϵ |x_{n+1} - x_n| < \epsilon ∣xn+1−xn∣<ϵ（如 10 − 8 10^{-8} 10−8）时停止。
处理多根问题
三次方程必定有至少一个实根 。如果你只需要任意一个实根 ，牛顿法从任意实数出发基本都能收敛到某个实根。
如果你需要所有实根 ，可以先用牛顿法求出一个根 (x_1)，然后用多项式除法（综合除法）降为二次方程 (Ax^2+Bx+C=0)，再用求根公式解出剩下两个。
适用场景：C/Python/Excel VBA 编程、单片机、实验数据处理。
- Matlab ：
roots([a, b, c, d-y])直接返回所有根。
特别提醒（唯一性判断）
如果原三次函数 y = a x 3 + b x 2 + c x + d y=ax^3+bx^2+cx+d y=ax3+bx2+cx+d在整个实数域上严格单调 （即其导数 3 a x 2 + 2 b x + c ≥ 0 3ax^2+2bx+c \ge 0 3ax2+2bx+c≥0 或 ≤ 0 \le 0 ≤0 恒成立），那么 y 与 x 一一对应，反函数唯一 。此时只需取卡尔丹公式中唯一的那个实根 即可（判别式 Δ > 0 \Delta > 0 Δ>0 时的根）。
如果函数有极值（导数有两个零点），则对于某些 y 值，方程会有 3 个实根，反解出的 x 不唯一，需要根据定义域或实际物理意义（如时间、长度必须为正）来筛选。
仿真
matlab
% 演示 GetYVA_Poly3 函数并绘制非对称恒定类加速度轨迹
clear; clc; close all;
import CamFun.Polynominal.*
% 定义轨迹参数
q0 = 0; v0 = 0.5; % 起点
q1 = 10; v1 = 0.2; % 终点
t0 = 0; tf = 2; t1 = 3.0; % 时间节点
% 生成时间向量
t = linspace(t0, t1, 100);
% 计算轨迹
[q, qd, qdd] = GetYVA_Poly3(t0, q0, v0, t1, q1,v1, t);
% 绘图
figure;
subplot(3,1,1); plot(t, q); ylabel('位置 q'); grid on;
subplot(3,1,2); plot(t, qd); ylabel('速度 qd'); grid on;
subplot(3,1,3); plot(t, qdd); ylabel('加速度 qdd'); grid on;
xlabel('时间 t');
五次多项式轨迹
当将一系列途经点 y 0 , ⋯ , y n y_0, \cdots, y_n y0,⋯,yn拟合为三次多项式轨迹时，其对应的位置和速度是连续的，但是通常情况下加速度是不连续的。尽管上述轨迹通常看上去足够"平滑"，但是加速度不连续会在某些应用中对运动链和惯性负载产生非期望的影响。尤其是在较小的时间内指定了高的加速度值（速度值）或者在执行系统中机械弹性较大。
为了获得加速度连续的轨迹，除必须给定位置和速度的约束条件外，还必须指定加速度的初始值和最终值。因此，对应地存在六个边界约束条件（位置、速度和加速度），其对应的五次多项式如下所示：
y ( x ) = k 0 + k 1 ( x − x s ) + k 2 ( x − x s ) 2 + k 3 ( x − x s ) 3 + k 4 ( x − x s ) 4 + k 5 ( x − x s ) 5 y(x) = k_0 + k_1(x- x_s) + k_2(x- x_s)^2 + k_3(x - x_s)^3 + k_4(x- x_s)^4 + k_5(x- x_s)^5 y(x)=k0+k1(x−xs)+k2(x−xs)2+k3(x−xs)3+k4(x−xs)4+k5(x−xs)5
满足如下约束条件：
y ( x s ) = q s , y ˙ ( x s ) = v s , y ¨ ( x s ) = a s , y ( x e ) = q e , y ˙ ( x e ) = v e , y ¨ ( x e ) = a e \begin{aligned} y(x_s) &= q_s, \dot{y}(x_s) &= v_s,\ddot{y}(x_s) &= a_s, \\ y(x_e) &= q_e, \dot{y}(x_e) &= v_e, \ddot{y}(x_e) &= a_e \end{aligned} y(xs)y(xe)=qs,y˙(xs)=qe,y˙(xe)=vs,y¨(xs)=ve,y¨(xe)=as,=ae
因此，定义 (T = t_1 - t_0)，其对应的多项式系数为
k 0 = y s , k 1 = v s , k 2 = 1 2 a 0 , k 3 = 1 2 T 3 20 h − ( 8 v e + 12 v s ) T − ( 3 a s − a e ) T 2 k 4 = 1 2 T 4 − 30 h + ( 14 v e + 16 v s ) T + ( 3 a s − 2 a e ) T 2 , k 5 = 1 2 T 5 12 h − 6 ( v e + v s ) T + ( a e − a s ) T 2 \begin{aligned} k_0 &= y_s, \\ k_1 &= v_s, \\ k_2 &= \frac{1}{2}a_0, \\ k_3 &= \frac{1}{2T^3}20h - (8v_e + 12v_s)T - (3a_s - a_e)T\^2 \\ k_4 &= \frac{1}{2T^4}-30h + (14v_e + 16v_s)T + (3a_s - 2a_e)T\^2, \\ k_5 &= \frac{1}{2T^5}12h - 6(v_e + v_s)T + (a_e - a_s)T\^2 \end{aligned} k0k1k2k3k4k5=ys,=vs,=21a0,=2T3120h−(8ve+12vs)T−(3as−ae)T2=2T41−30h+(14ve+16vs)T+(3as−2ae)T2,=2T5112h−6(ve+vs)T+(ae−as)T2
其中： h = y e − y s h = y_e-y_s h=ye−ys， T = x e − x s T = x_e-x_s T=xe−xs
正向求解：
matlab
% 根据X求YVA，五次曲线
% 输入参数:
% Xs - 起点X
% Ys - 起点Y
% Vs - 起点V
% Xe - 终点X
% Ye - 终点Y
% Ve - 终点V
% X - 设定X
% 输出参数:
% y - X时的y值
% v - X时的v值
% a - X时的a值
% 作者: MrControll
% 日期: 20260705
function [y,v,a,j,Result] = GetYVA_Poly5(Xs,Ys,Vs,As,Xe,Ye,Ve,Ae,x)
% --- 以下是函数的主体代码 ---
y = 0; v =0; a = 0;
% 数据精度
tol = 1e-10;
%% 输入数据校验
% X是否升序
% X相等Y不相等
if ((Xe- Xs -tol ) < 0) || ((abs(Xe-Xs )-tol) <=0 && abs (abs(Ye-Ys )-tol) > 0)
Result = false;
return;
end
%% 输入值不在范围内直接输出边界值
if (x >= Xe)
y = Ye;
v = Ve;
a = 0;
j = 0;
Result = true;
return;
elseif (x <= Xs)
y = Ys;
v = Vs;
a = 0;
j = 0;
Result = true;
return;
end
%% 超过边界值处理
if x >= Xe
y = Ye;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
j = 0;
Result = true;
return;
elseif x <= Xs
y = Ys;
v = SEL(Xe ~=Xs,0,(Ye-Ys)/(Xe-Xs));
a = 0;
j = 0;
Result = true;
return;
end
%% 未超过边界值处理
if Xe == Xs
y = Ye;
v = Ve;
a = 0;
j = 0;
Result = true;
return;
else
[k0,k1,k2,k3,k4,k5]= GetFiveCurveFactor(Xs,Ys,Vs,As,Xe,Ye,Ve,Ae);
[a0,a1,a2,a3,a4] = DerivFive(k0,k1,k2,k3,k4,k5)
[b0,b1,b2,b3] = DerivFour(a0,a1,a2,a3,a4)
[kk0,kk1,kk2] = DerivCubic(b0,b1,b2,b3);
% [kkk0,kkk1] = DerivQuadratic(kk0,kk1,kk2);
y = k0 + k1*(x-Xs)+ k2.*(x-Xs).*(x-Xs) + ...
k3.*(x-Xs).*(x-Xs).*(x-Xs) + ...
k4.*(x-Xs).*(x-Xs).*(x-Xs).*(x-Xs) + ...
k5.*(x-Xs).*(x-Xs).*(x-Xs).*(x-Xs).*(x-Xs);
v = a0 + a1.*(x-Xs)+ a2.*(x-Xs).*(x-Xs) + ...
a3.*(x-Xs).*(x-Xs).*(x-Xs) + ...
a4.*(x-Xs).*(x-Xs).*(x-Xs).*(x-Xs) ;
a = b0 + b1.*(x-Xs)+ b2.*(x-Xs).*(x-Xs) + ...
b3.*(x-Xs).*(x-Xs).*(x-Xs) ;
j = kk0 + kk1.*(x-Xs)+ kk2.*(x-Xs).*(x-Xs) ;
Result = true;
return;
end
end
%% 三此项系数求解
function [k0,k1,k2,k3,k4,k5]=GetFiveCurveFactor(Xs,Ys,Vs,As,Xe,Ye,Ve,Ae)
% 数据精度
tol = 1e-10;
if abs(Xs-Xe-tol) <=0
return;
end
T = Xe -Xs;
h = Ye -Ys;
k0 = Ys;
k1 = Vs;
k2 = 0.5*k0;
k3 = (20*h-(8*Ve+12*Vs)*T-(3*As-Ae)*T^2)/(2*T^3);
k4 = (-30*h+(14*Ve+16*Vs)*T+(3*As-2*Ae)*T^2)/(2*T^4);
k5 = (12*h-6*(Ve+Vs)*T+(Ae-As)*T^2)/(2*T^5);
end
%% 五次函数求导
function [a0,a1,a2,a3,a4]=DerivFive(k0,k1,k2,k3,k4,k5) %#ok<INUSD,DEFNU>
a0 = k1;
a1 = 2*k2;
a2 = 3*k3;
a3 = 4*k4;
a4 = 5*k5;
end
%% 四次函数求导
function [b0,b1,b2,b3]=DerivFour(a0,a1,a2,a3,a4) %#ok<INUSD,DEFNU>
b0 = a1;
b1 = 2*a2;
b2 = 3*a3;
b3 = 4*a4;
end
%% 三次函数求导
function [kk0,kk1,kk2]=DerivCubic(k0,k1,k2,k3) %#ok<INUSD,DEFNU>
kk0 = k1;
kk1 = 2*k2;
kk2 = 3*k3;
end
%% 二次函数求导
function [kkk0,kkk1]=DerivQuadratic(kk0,kk1,kk2) %#ok<INUSD,DEFNU>
kkk0 = kk1;
kkk1 = 2*kk2;
end反向求解
给定五次多项式
y ( x ) = k 0 + k 1 ( x − x s ) + k 2 ( x − x s ) 2 + k 3 ( x − x s ) 3 + a 4 ( x − x s ) 4 + a 5 ( x − x s ) 5 y(x)=k_0+k_1(x-x_s)+k_2(x-x_s)^2+k_3(x-x_s)^3+a_4(x-x_s)^4+a_5(x-x_s)^5 y(x)=k0+k1(x−xs)+k2(x−xs)2+k3(x−xs)3+a4(x−xs)4+a5(x−xs)5
若已知 y y y，要求对应的 x x x，本质上是解一个关于 x x x 的五次方程。通常没有简单的"反函数"解析式，只能用数值方法求解。
1. 变量代换
令 t = x − x s t=x-x_s t=x−xs
则原式变为
y = k 0 + k 1 t + k 2 t 2 + k 3 t 3 + k 4 t 4 + k 5 t 5 y=k_0+k_1t+k_2t^2+k_3t^3+k_4t^4+k_5t^5 y=k0+k1t+k2t2+k3t3+k4t4+k5t5
移项得
F ( t ) = k 5 t 5 + k 4 t 4 + k 3 t 3 + k 2 t 2 + k 1 t + ( k 0 − y ) = 0 F(t)=k_5t^5+k_4t^4+k_3t^3+k_2t^2+k_1t+(k_0-y)=0 F(t)=k5t5+k4t4+k3t3+k2t2+k1t+(k0−y)=0
求出 t t t后，再代回
x = x s + t x=x_s+t x=xs+t
2. 数值求解方法
方法一：牛顿迭代法
牛顿迭代公式为
t n + 1 = t n − F ( t n ) F ′ ( t n ) t_{n+1}=t_n-\frac{F(t_n)}{F'(t_n)} tn+1=tn−F′(tn)F(tn)
其中
F ′ ( t ) = 5 k 5 t 4 + 4 k 4 t 3 + 3 k 3 t 2 + 2 k 2 t + k 1 F'(t)=5k_5t^4+4k_4t^3+3k_3t^2+2k_2t+k_1 F′(t)=5k5t4+4k4t3+3k3t2+2k2t+k1
选择一个合适的初值 t 0 t_0 t0进行迭代，直到 F ( t n ) F(t_n) F(tn)足够接近 0。
但牛顿法可能不收敛，建议先粗略判断根的位置，例如采样。
方法二：二分法
如果知道根所在区间 t L , t R t_L,t_R tL,tR，且 F ( t L ) F(t_L) F(tL) 与 F ( t R ) F(t_R) F(tR) 异号，则可用二分法不断缩小区间，得到根。
方法三：用多项式求根函数
- MATLAB：
roots([a5, a4, k3, k2, k1, k0-y])
这样会得到 5 个复数根，保留实部虚部接近 0 的根即可。
3. 注意事项
- 五次方程可能有 1 个、3 个或 5 个实根，因此给定一个 (y) 值，可能对应多个 (x)。实际应用中要根据物理约束或区间要求选择合适的根。
- 如果多项式在关心的区间内单调，则反解唯一，此时二分法或牛顿法很可靠。
- 一般情况下，五次方程没有根式解（阿贝尔-鲁菲尼定理），所以不要指望得到像二次方程那样的显式公式。
- 如果 (a_5=0)，则退化为四次或更低次方程，此时可以尝试解析求解。