七次多项式求解：从线性方程组到数值稳定性

大家好，我是你们的老朋友。今天我们来聊聊七次多项式求解这个硬核话题。

说实话，七次多项式在机器人规划里用得不算特别多。但一旦用上，往往都是高精度、高平滑度的场景——比如医疗手术机器人、精密装配，或者某些需要连续加加速度（Jerk）控制的场合。

我个人习惯把七次多项式叫做「规划界的瑞士军刀」。为什么？因为它能同时约束位置、速度、加速度、加加速度这四个量。你想想看，六次多项式只能约束三个，七次刚好多一个维度，这个维度就是加加速度的连续性。

线性方程组的构建

七次多项式长这样：

s(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵ + a₆t⁶ + a₇t⁷

它有8个未知系数：a₀ 到 a₇。所以我们需要8个方程。

这8个方程怎么来？很简单——起点和终点的四个约束：

• 位置约束：s(0) = p₀, s(T) = p₁
• 速度约束：v(0) = v₀, v(T) = v₁
• 加速度约束：a(0) = a₀, a(T) = a₁
• 加加速度约束：j(0) = j₀, j(T) = j₁

嗯，这里要注意：加加速度（Jerk）是位置的三阶导数。很多初学者会漏掉这个，结果算出来的轨迹在端点处加加速度突变，执行器会抖得厉害。

我在项目中遇到过类似问题。有一次做协作机器人的轨迹规划，六次多项式跑起来挺顺，但末端执行器在抓取瞬间总有个微小的抖动。后来换成七次多项式，把加加速度也约束住，问题就解决了。

矩阵求解方法

把上面的约束写成矩阵形式：

M · a = b

其中 M 是 8×8 的系数矩阵，a 是待求系数向量，b 是边界条件向量。

具体展开是这样的：

方程编号	约束条件	对应矩阵行
1	s(0) = p₀	[1, 0, 0, 0, 0, 0, 0, 0]
2	s(T) = p₁	[1, T, T², T³, T⁴, T⁵, T⁶, T⁷]
3	v(0) = v₀	[0, 1, 0, 0, 0, 0, 0, 0]
4	v(T) = v₁	[0, 1, 2T, 3T², 4T³, 5T⁴, 6T⁵, 7T⁶]
5	a(0) = a₀	[0, 0, 2, 0, 0, 0, 0, 0]
6	a(T) = a₁	[0, 0, 2, 6T, 12T², 20T³, 30T⁴, 42T⁵]
7	j(0) = j₀	[0, 0, 0, 6, 0, 0, 0, 0]
8	j(T) = j₁	[0, 0, 0, 6, 24T, 60T², 120T³, 210T⁴]

求解这个线性方程组，最直接的方法是高斯消元。但说实话，在实际工程中我很少直接用高斯消元——因为当 T 比较大时，矩阵会变得病态。

系数计算流程

整个计算流程其实就三步：

1. 组装矩阵：根据边界条件填充 M 矩阵和 b 向量
2. 求解线性方程组：用 LU 分解或 Cholesky 分解（如果矩阵对称正定）
3. 提取系数：得到 a₀ 到 a₇ 八个系数

代码实现大概是这样的：

import numpy as np

def solve_septic(p0, p1, v0, v1, a0, a1, j0, j1, T):
    # 归一化时间
    t = 1.0  # 归一化后的终点时间
    
    # 构建矩阵 M
    M = np.array([
        [1, 0, 0, 0, 0, 0, 0, 0],
        [1, t, t**2, t**3, t**4, t**5, t**6, t**7],
        [0, 1, 0, 0, 0, 0, 0, 0],
        [0, 1, 2*t, 3*t**2, 4*t**3, 5*t**4, 6*t**5, 7*t**6],
        [0, 0, 2, 0, 0, 0, 0, 0],
        [0, 0, 2, 6*t, 12*t**2, 20*t**3, 30*t**4, 42*t**5],
        [0, 0, 0, 6, 0, 0, 0, 0],
        [0, 0, 0, 6, 24*t, 60*t**2, 120*t**3, 210*t**4]
    ])
    
    # 构建边界向量 b
    b = np.array([p0, p1, v0, v1, a0, a1, j0, j1])
    
    # 求解
    coeffs = np.linalg.solve(M, b)
    
    # 反归一化系数（如果需要）
    # 这里省略反归一化步骤，实际使用时需要根据 T 调整
    
    return coeffs

数值稳定性分析

说到数值稳定性，这可是七次多项式的「阿喀琉斯之踵」。

为什么会这样？因为七次多项式的系数矩阵中，元素的数量级差异太大了。你看矩阵最后一行，有 t⁴ 这样的项。如果 T=100，那 t⁴ 就是 10⁸ 的量级。而第一行全是 1。这种数量级差异会导致矩阵条件数爆炸。

我曾经在一个项目中，直接用原始时间求解七次多项式，结果算出来的系数完全不对——位置轨迹在中间段出现了诡异的振荡。排查了半天，发现就是数值问题。

后来我总结了几条经验：

• 时间归一化：把时间缩放到 [0, 1]，这是最有效的办法
• 使用双精度浮点：单精度绝对不够，至少 double
• 避免大时间跨度：如果 T 很大，分段规划比单段七次多项式更靠谱
• 检查条件数：求解前先算一下矩阵的条件数，如果超过 10⁶，就要小心了

最后，我给大家画了一张流程图，把整个知识体系串起来：

这张图把整个流程串起来了。从输入边界条件开始，到构建线性方程组，再到选择求解方法，最后进行数值稳定性分析。每一步都有坑，每一步也都有对应的解决方案。

好了，关于七次多项式求解的核心内容就讲到这里。记住：理论要扎实，但工程实践更重要。下次遇到七次多项式的问题，先想想时间归一化做了没有，条件数检查了没有——这两步做好了，能省掉你80%的调试时间。