3、单段轨迹求解：已知起点和终点的位置、速度、加速度，求解系数。
好，咱们直接进入正题。
上一章我们聊了五次多项式的数学形式，说白了就是那个 s(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵。六个系数，六个未知数，对吧？
那怎么把这六个系数求出来？
答案就在边界条件里。
你想想看，一条轨迹从起点到终点，我们不光要知道它从哪出发、到哪结束，还得知道它出发时的速度和加速度，以及到达时的速度和加速度。这就是六个条件——正好对应六个系数。
3.1 边界条件的数学表达
假设我们规划的时间区间是 [0, T]。起点时刻 t=0，终点时刻 t=T。
那么六个边界条件可以写成：
| 时刻 | 位置 | 速度 | 加速度 |
|---|---|---|---|
| 起点 (t=0) | s₀ | v₀ | a₀ |
| 终点 (t=T) | s₁ | v₁ | a₁ |
嗯，这里要注意：s₀、v₀、a₀ 是已知的起点状态，s₁、v₁、a₁ 是已知的终点状态。这些值通常由上层规划器给出，或者由你手动设定。
把 t=0 代入五次多项式及其一阶、二阶导数，我们立刻得到：
s(0) = a0 = s₀
s'(0) = a1 = v₀
s''(0) = 2*a2 = a₀ → a2 = a₀ / 2看到了吗？前三个系数直接由起点条件确定，根本不用算。我个人习惯先把这三个系数写下来，剩下的三个才是重头戏。
3.2 求解剩余三个系数
把 t=T 代入，得到终点条件：
s(T) = a0 + a1*T + a2*T² + a3*T³ + a4*T⁴ + a5*T⁵ = s₁
s'(T) = a1 + 2*a2*T + 3*a3*T² + 4*a4*T³ + 5*a5*T⁴ = v₁
s''(T) = 2*a2 + 6*a3*T + 12*a4*T² + 20*a5*T³ = a₁把已知的 a0、a1、a2 代入，整理后得到一个关于 a3、a4、a5 的线性方程组。
写成矩阵形式更清晰：
[ T³ T⁴ T⁵ ] [ a3 ] [ s₁ - (a0 + a1*T + a2*T²) ]
[ 3T² 4T³ 5T⁴ ] * [ a4 ] = [ v₁ - (a1 + 2*a2*T) ]
[ 6T 12T² 20T³ ] [ a5 ] [ a₁ - 2*a2 ]这个3x3矩阵是可逆的（只要T>0），直接求逆或者用克莱姆法则都能解。不过在实际工程中，我建议直接用数值方法——比如NumPy的 linalg.solve，又快又稳。
核心结论：五次多项式轨迹的六个系数，前三个由起点条件直接给出，后三个通过求解一个3x3线性方程组得到。整个过程是解析的，没有迭代，计算量极小。
3.3 Python代码实现
光说不练假把式。咱们直接上代码。
import numpy as np
def quintic_trajectory_coeffs(s0, v0, a0, s1, v1, a1, T):
"""
计算五次多项式轨迹的系数
参数:
s0, v0, a0: 起点位置、速度、加速度
s1, v1, a1: 终点位置、速度、加速度
T: 总时间
返回:
coeffs: [a0, a1, a2, a3, a4, a5]
"""
# 前三个系数直接赋值
a0_val = s0
a1_val = v0
a2_val = a0 / 2.0
# 构建线性方程组
A = np.array([
[T**3, T**4, T**5],
[3*T**2, 4*T**3, 5*T**4],
[6*T, 12*T**2, 20*T**3]
])
b = np.array([
s1 - (a0_val + a1_val*T + a2_val*T**2),
v1 - (a1_val + 2*a2_val*T),
a1 - 2*a2_val
])
# 求解后三个系数
a3_val, a4_val, a5_val = np.linalg.solve(A, b)
return [a0_val, a1_val, a2_val, a3_val, a4_val, a5_val]
# 举个实际例子
# 从位置0出发，静止，加速度0
# 到位置10，静止，加速度0
# 用时2秒
coeffs = quintic_trajectory_coeffs(0, 0, 0, 10, 0, 0, 2)
print("系数:", coeffs)
# 输出: [0, 0, 0, 10.0, -15.0, 6.0]这段代码我用了很多次，基本没出过问题。不过有一次我在项目中遇到一个坑——T传成了0，结果矩阵奇异，直接报错。所以后来我加了个判断：
注意：总时间T必须大于0。如果T=0，矩阵不可逆，物理上也没意义——瞬间移动？不存在的。
3.4 一个完整的可视化示例
咱们用上面求出的系数，生成一条完整的轨迹看看。
import matplotlib.pyplot as plt
def evaluate_quintic(coeffs, t):
"""计算五次多项式在时刻t的值"""
a0, a1, a2, a3, a4, a5 = coeffs
return a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5
def evaluate_velocity(coeffs, t):
"""计算速度"""
a1, a2, a3, a4, a5 = coeffs[1], coeffs[2], coeffs[3], coeffs[4], coeffs[5]
return a1 + 2*a2*t + 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
def evaluate_acceleration(coeffs, t):
"""计算加速度"""
a2, a3, a4, a5 = coeffs[2], coeffs[3], coeffs[4], coeffs[5]
return 2*a2 + 6*a3*t + 12*a4*t**2 + 20*a5*t**3
# 生成时间序列
T = 2.0
t_vals = np.linspace(0, T, 100)
# 计算轨迹
pos = [evaluate_quintic(coeffs, t) for t in t_vals]
vel = [evaluate_velocity(coeffs, t) for t in t_vals]
acc = [evaluate_acceleration(coeffs, t) for t in t_vals]
# 绘制
plt.figure(figsize=(10, 8))
plt.subplot(3, 1, 1)
plt.plot(t_vals, pos, 'b-', linewidth=2)
plt.ylabel('位置')
plt.grid(True)
plt.subplot(3, 1, 2)
plt.plot(t_vals, vel, 'g-', linewidth=2)
plt.ylabel('速度')
plt.grid(True)
plt.subplot(3, 1, 3)
plt.plot(t_vals, acc, 'r-', linewidth=2)
plt.ylabel('加速度')
plt.xlabel('时间 (s)')
plt.grid(True)
plt.tight_layout()
plt.show()运行这段代码，你会看到位置曲线从0平滑上升到10，速度从0开始、中间达到峰值、最后归零，加速度也是连续变化的。这就是五次多项式的魅力——全程平滑，没有突变。
小技巧：如果你只需要位置轨迹，可以只保存系数。但如果你还需要速度和加速度反馈，建议把三个评估函数都封装好，随调随用。我曾经在调试时频繁切换查看位置和速度，后来干脆写了个类，一劳永逸。
3.5 知识体系总览
为了让你对整个求解过程有个直观印象，我画了一张流程图：
这张图把整个求解过程串起来了。你跟着箭头走一遍：输入条件 → 前三个系数直接拿 → 后三个系数解方程 → 合并 → 输出轨迹。逻辑很清晰，对吧？
3.6 避坑指南
最后，分享几个我踩过的坑：
- 时间单位要统一。 我曾经把T设成毫秒，但位置单位是米，结果算出来的速度大得离谱。后来统一用秒，问题解决。
- 数值稳定性。 当T很小（比如0.001秒）时，矩阵中的T³、T⁴、T⁵会非常小，可能导致数值误差。我建议T至少大于0.01秒，否则考虑用其他方法。
- 边界条件要合理。 如果起点和终点的速度、加速度相差太大，五次多项式可能会产生很大的中间值。我曾经遇到过加速度峰值超过电机极限的情况，后来加了约束检查才搞定。
一句话总结：五次多项式单段轨迹求解，就是六个方程解六个未知数。前三个直接拿，后三个解矩阵。代码实现不超过20行，但边界条件的合理性需要你根据实际场景判断。
好了，这一节就到这里。代码你拿去就能用，但记得根据你的实际参数调一调T和边界条件。嗯，动手试试吧。