4、Python实现：使用NumPy求解线性方程组，代码实现单段五次多项式

好，前面我们把五次多项式的数学公式和边界条件都理清了。现在，咱们来点实在的——用Python把它跑起来。

我个人习惯用NumPy来处理这类线性代数问题。为什么？因为NumPy的线性代数模块（numpy.linalg）封装得特别好，求解六元一次方程组，一行代码就搞定。你想想看，要是手写高斯消元，那得多折腾。

4.1 核心思路：把问题转化成矩阵方程

还记得上一节我们推导的六个方程吗？

对于单段五次多项式 q(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵，我们已知起点和终点的位置、速度、加速度。把这些条件代入，就得到一个关于系数 a₀~a₅ 的线性方程组。

写成矩阵形式就是：

M · A = B

其中：

• M 是6×6的系数矩阵（由时间t的各次幂构成）
• A 是待求的系数向量 [a₀, a₁, a₂, a₃, a₄, a₅]ᵀ
• B 是边界条件向量 [q₀, v₀, acc₀, q₁, v₁, acc₁]ᵀ

说白了，就是解 A = M⁻¹ · B。NumPy里直接用 np.linalg.solve(M, B) 就完事了。

4.2 代码实现：单段五次多项式求解器

下面这段代码，是我在实际项目中反复用过的模板。你直接拿去改改参数就能用。

import numpy as np

def quintic_trajectory(t0, tf, q0, qf, v0, vf, acc0, accf):
    """
    单段五次多项式轨迹规划
    参数：
        t0, tf : 起始和终止时间
        q0, qf : 起始和终止位置
        v0, vf : 起始和终止速度
        acc0, accf : 起始和终止加速度
    返回：
        系数数组 [a0, a1, a2, a3, a4, a5]
    """
    # 构造系数矩阵 M (6x6)
    M = np.array([
        [1, t0, t0**2, t0**3, t0**4, t0**5],   # 位置方程
        [0, 1, 2*t0, 3*t0**2, 4*t0**3, 5*t0**4], # 速度方程
        [0, 0, 2, 6*t0, 12*t0**2, 20*t0**3],    # 加速度方程
        [1, tf, tf**2, tf**3, tf**4, tf**5],    # 终点位置
        [0, 1, 2*tf, 3*tf**2, 4*tf**3, 5*tf**4], # 终点速度
        [0, 0, 2, 6*tf, 12*tf**2, 20*tf**3]     # 终点加速度
    ])
    
    # 边界条件向量 B
    B = np.array([q0, v0, acc0, qf, vf, accf])
    
    # 求解线性方程组
    coeffs = np.linalg.solve(M, B)
    
    return coeffs

# 示例：从0到1，起止速度加速度均为0
t0, tf = 0.0, 2.0
q0, qf = 0.0, 1.0
v0, vf = 0.0, 0.0
acc0, accf = 0.0, 0.0

coeffs = quintic_trajectory(t0, tf, q0, qf, v0, vf, acc0, accf)
print("五次多项式系数:", coeffs)

嗯，这里要注意：系数矩阵M的行顺序必须和边界条件B一一对应。我曾经在项目里搞反过顺序，结果轨迹直接飞到了外太空……排查了半天才发现是矩阵行序错了。

4.3 验证轨迹：计算任意时刻的位置

有了系数，我们就能算任意时刻的位置了。写个辅助函数：

def calc_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

# 验证起点和终点
print(f"起点位置: {calc_quintic(coeffs, t0):.6f}")  # 应为0.0
print(f"终点位置: {calc_quintic(coeffs, tf):.6f}")  # 应为1.0

输出：

起点位置: 0.000000
终点位置: 1.000000

完美吻合。这说明我们的求解器没问题。

4.4 避坑指南：我踩过的三个坑

做这个实现的时候，我遇到过几个典型问题，分享给你：

• 矩阵奇异：如果t0和tf相等，矩阵M会奇异（两行相同）。所以一定要保证时间区间长度大于0。
• 数值稳定性：当时间尺度很大（比如tf=1000秒）时，t的高次幂会非常大，导致矩阵条件数恶化。我建议把时间归一化到[0,1]区间再求解。
• 边界条件冲突：如果给定的位置、速度、加速度在物理上不可能同时满足（比如要求瞬间跳变），求解器虽然能算出系数，但轨迹会剧烈震荡。我曾经在机械臂上遇到过，结果电机直接过载报警。

4.5 知识体系图：单段五次多项式实现流程

下面这张图，帮你把整个实现逻辑串起来：

4.6 完整代码：带可视化验证

最后，给你一个可以直接跑的完整版本。它会把轨迹画出来，让你直观看到效果：

import numpy as np
import matplotlib.pyplot as plt

def quintic_trajectory(t0, tf, q0, qf, v0, vf, acc0, accf):
    M = np.array([
        [1, t0, t0**2, t0**3, t0**4, t0**5],
        [0, 1, 2*t0, 3*t0**2, 4*t0**3, 5*t0**4],
        [0, 0, 2, 6*t0, 12*t0**2, 20*t0**3],
        [1, tf, tf**2, tf**3, tf**4, tf**5],
        [0, 1, 2*tf, 3*tf**2, 4*tf**3, 5*tf**4],
        [0, 0, 2, 6*tf, 12*tf**2, 20*tf**3]
    ])
    B = np.array([q0, v0, acc0, qf, vf, accf])
    return np.linalg.solve(M, B)

def calc_quintic(coeffs, t):
    a0, a1, a2, a3, a4, a5 = coeffs
    return a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5

# 参数设置
t0, tf = 0.0, 2.0
q0, qf = 0.0, 1.0
v0, vf = 0.0, 0.0
acc0, accf = 0.0, 0.0

# 求解
coeffs = quintic_trajectory(t0, tf, q0, qf, v0, vf, acc0, accf)

# 生成轨迹点
t_vals = np.linspace(t0, tf, 100)
q_vals = [calc_quintic(coeffs, t) for t in t_vals]

# 可视化
plt.figure(figsize=(8, 4))
plt.plot(t_vals, q_vals, 'b-', linewidth=2, label='五次多项式轨迹')
plt.scatter([t0, tf], [q0, qf], color='red', s=80, zorder=5, label='起止点')
plt.xlabel('时间 (s)')
plt.ylabel('位置')
plt.title('单段五次多项式轨迹')
plt.grid(True, alpha=0.3)
plt.legend()
plt.show()

好了，单段五次多项式的Python实现就讲到这里。代码不长，但背后涉及的矩阵构造、边界条件匹配、数值稳定性这些细节，都是实际工程中必须注意的。你拿这段代码去跑几个不同的参数组合，感受一下轨迹形状的变化，会理解得更深。

---

公众号:蓝海资料掘金营，微信deep3321