四次多项式求解系数

  

使用五阶多项式来表示随时间变化的位置：
$s(t)=c_{1} t^{4}+c_{2} t^{3}+c_{3} t^{2}+c_{4} t+c_{5}$

可以推导出:

速度(一阶导数):
$v(t)=\frac{d s}{d t}=4 c_{1} t^{3}+3 c_{2} t^{2}+2 c_{3} t+ c_{4}$

加速度(二阶导数)
$a(t)=\frac{d v}{d t}=12c_{1} t^{2}+6 c_{2} t+2 c_{3}$
约束条件:
1.初始条件：

在初始时刻$t_0$和终止时刻$t_1$分别设置约束：

$t_0=0$

初始位置：$s(t_0)=c_5=s_0$
初始速度：$v(t_0)=c_4=v_0$
初始加速度：$a(t_0)=2c_3=a_0$

 

2.终止条件约束：

终止位置：$s(t_1)=c_1t_1^4+c_2t_1^3+c_3t_1^2+c_4t_1+c_5=s_1$
终止速度：$v(t_1)=4c_1t_1^3+3c_2t_1^2+2c_3t_1+c_4=v_1$
终止加速度：$a(t_1)=12c_1t_1^2+6c_2t_1+2c_3=a_1$
 

方程组求解：

6个方程，可以解出6个系数$c_1$到$c_6$：
$c_5=s_0$（直接得出)
$c_4=v_0$   (直接得出)
$2c_3=a_0\Rightarrow c_3=\frac{a_0}{2} $
 

对于剩余的三个系数$c_1, c_2$,需要解以下矩阵方程：
$\begin{bmatrix}
  t_1^4& t_1^3\\
  4t_1^3&  3t_1^2\\
\end{bmatrix}\begin{bmatrix}
 c_1\\
 c_2\\
\end{bmatrix}=\begin{bmatrix}
 s_1-c_3t_1^2-c_4t_1-c_5\\
 v_1-2c_3t_1-c_4\\
\end{bmatrix}$


使用 LAPACK 库的 LU 分解法，解方程
python代码示例：

import numpy as np

def solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1):
    """
    求解四次多项式 s(t) = c1*t^4 + c2*t^3 + c3*t^2 + c4*t + c5 的系数
    输入参数：
        t1: 终止时间
        s0, v0, a0: 初始位置、速度、加速度
        s1, v1, a1: 终止位置、速度、加速度
    返回：
        系数列表 [c1, c2, c3, c4, c5]
    """
    # 直接已知的系数
    c5 = s0
    c4 = v0
    c3 = a0 / 2

    # 构建矩阵 A 和向量 b
    A = np.array([
        [t1**4, t1**3],
        [4*t1**3, 3*t1**2]
    ])

    # 终止时刻的加速度方程：12*c1*t1^2 + 6*c2*t1 + 2*c3 = a1
    # 速度方程：4*c1*t1^3 + 3*c2*t1^2 + 2*c3*t1 + c4 = v1
    # 位移方程：c1*t1^4 + c2*t1^3 + c3*t1^2 + c4*t1 + c5 = s1
    b = np.array([
        s1 - c3*t1**2 - c4*t1 - c5,
        v1 - 2*c3*t1 - c4
    ])

    # 解线性方程组 Ax = b
    c1, c2 = np.linalg.solve(A, b)

    return [c1, c2, c3, c4, c5]

# 示例参数（与五次多项式示例一致）
t1 = 1.0
s0, v0, a0 = 0.0, 0.0, 0.0
s1, v1, a1 = 1.0, 0.0, 0.0

# 求解系数
coefficients = solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1)
print("系数 c1 到 c5:", coefficients)

# 验证终止条件
t = t1
c1, c2, c3, c4, c5 = coefficients

s = c1*t**4 + c2*t**3 + c3*t**2 + c4*t + c5
v = 4*c1*t**3 + 3*c2*t**2 + 2*c3*t + c4
a = 12*c1*t**2 + 6*c2*t + 2*c3

print(f"验证 t={t}: s={s}, v={v}, a={a}")