三次多项式Python程序

线性插值(Linear Interpolation)，顾名思义，就是使用线性的方法来进行插值。即将给定的数据点依次用线段连起来，点与点之间运动的速度是恒定值。假设我们用 q ( t ) q(t) q(t)来表示插值以后的曲线，则用数学的方式来表示线性插值就是：
q ( t ) = a 0 + a 1 ( t − t 0 ) , , t 0 ≤ t ≤ t 1 ( 2.1 ) q(t) = a_0 + a_1(t-t_0), quad ,t_0 leq t leq t_1 qquad(2.1) q(t)=a0​+a1​(t−t0​),,t0​≤t≤t1​(2.1)
其中， a 0 , a 1 a_0,a_1 a0​,a1​是待确定的常量参数。 t 0 t_0 t0​表示初始时刻， a 0 a_0 a0​表示初始时刻的位置， a 1 a_1 a1​表示斜率，也就是速度，这里为常量。因此，给定下一个时刻 t 1 t_1 t1​ 处的位置 q ( t 1 ) q(t_1) q(t1​)，我们就有：
{ q ( t 0 ) = q 0 = a 0 q ( t 1 ) = q 1 = a 0 + a 1 ( t 1 − t 0 ) ( 2.2 ) begin{cases} q(t_0) = q_0 = a_0 \ q(t_1) = q_1=a_0 +a_1(t_1-t_0) \ end{cases} qquad(2.2) {q(t0​)=q0​=a0​q(t1​)=q1​=a0​+a1​(t1​−t0​)​(2.2)
可以计算得到两个常量参数：
{ a 0 = q 0 a 1 = ( q 1 − q 0 ) / ( t 1 − t 0 ) t 0 ≠ t 1 ( 2.3 ) begin{cases} a_0 = q_0 \ a_1 = (q_1-q_0)/(t_1-t_0) \ end{cases} quad t_0 neq t_1 qquad(2.3) {a0​=q0​a1​=(q1​−q0​)/(t1​−t0​)​t0​​=t1​(2.3)

三次多项式插值方法（Cubic Polynomial Spline）是一种常用的插值方法，其位置和速度曲线是连续的，加速度是可变的，但加速度不一定连续。考虑2个数据点之间插值的情况，其数学表达式为：
{ q ( t ) = a 0 + a 1 ( t − t 0 ) + a 2 ( t − t 0 ) 2 + a 3 ( t − t 0 ) 3 q ˙ ( t ) = a 1 + 2 a 2 ( t − t 0 ) + 3 a 2 ( t − t 0 ) 2 q ¨ ( t ) = 2 a 2 + 6 a 3 ( t − t 0 ) , t 0 ≤ t ≤ t f ( 2.4 ) begin{cases} q(t) = a_0 + a_1(t-t_0) + a_2(t-t_0)^2 + a_3(t-t_0)^3 \ dot q(t) = a_1 + 2a_2(t-t_0) + 3a_2(t-t_0)^2 \ ddot q(t) = 2a_2 + 6a_3(t-t_0) end{cases} ,t_0 leq t leq t_f qquad(2.4) ⎩⎪⎨⎪⎧​q(t)=a0​+a1​(t−t0​)+a2​(t−t0​)2+a3​(t−t0​)3q˙​(t)=a1​+2a2​(t−t0​)+3a2​(t−t0​)2q¨​(t)=2a2​+6a3​(t−t0​)​,t0​≤t≤tf​(2.4)
其中， a 0 , a 1 , a 2 , a 3 a_0,a1,a_2,a_3 a0​,a1,a2​,a3​为待确定的参数。
考察给定2个数据点进行插值的情况，如果给定了在初始时刻 t 0 t_0 t0​ 和最终时刻 t f t_f tf​ 处的位置与速度信息 ( q 0 , v 0 ) , ( q f , v f , ) (q_0,v_0),(q_f,v_f,) (q0​,v0​),(qf​,vf​,)，设 h = q f − q 0 , T = t f − t 0 h = q_f - q_0,T=t_f-t_0 h=qf​−q0​,T=tf​−t0​ ，
初始位置和速度：
{ q ( t 0 ) = q 0 = a 0 q ˙ ( t 0 ) = v 0 = a 1 ( 2.5 ) begin{cases} q(t_0) = q_0 =a_0 \ dot q(t_0) = v_0 = a_1 \ end{cases} qquad(2.5) {q(t0​)=q0​=a0​q˙​(t0​)=v0​=a1​​(2.5)
末端位置和速度：
{ q ( t f ) = q f = a 0 + a 1 T + a 2 T 2 + a 3 T 3 q ˙ ( t f ) = v f = a 1 + 2 a 2 T + 3 a 3 T 2 ( 2.6 ) begin{cases} q(t_f) = q_f = a_0 + a_1T + a_2T^2 + a_3T^3 \ dot q(t_f) = v_f = a_1 + 2a_2T + 3a_3T^2 \ end{cases} qquad(2.6) {q(tf​)=qf​=a0​+a1​T+a2​T2+a3​T3q˙​(tf​)=vf​=a1​+2a2​T+3a3​T2​(2.6)
结合公式 ( 2.5 ) , ( 2.6 ) (2.5),(2.6) (2.5),(2.6) ,则这些参数可以使用以下公式计算：
{ a 0 = q 0 a 1 = v 0 a 2 = 3 h − ( v f + 2 v 0 ) T T 2 = 3 T 2 ( q f − q 0 ) − 1 T ( v f + 2 v 0 ) a 3 = − 2 h + ( v f + v 0 ) T T 3 = − 2 T 3 ( q f − q 0 ) + 1 T 2 ( v f + v 0 ) ( 2.7 ) begin{cases} a_0 = q_0 \ a_1 = v_0 \ a_2 = frac {3h-(v_f+2v_0)T}{T^2} = frac {3}{T^2}(q_f - q_0)- frac{1}{T}(v_f +2v_0) \ a_3 = frac {-2h+(v_f+v_0)T}{T^3} = frac {-2}{T^3}(q_f - q_0)+ frac{1}{T^2}(v_f+v_0) \ end{cases} qquad(2.7) ⎩⎪⎪⎪⎨⎪⎪⎪⎧​a0​=q0​a1​=v0​a2​=T23h−(vf​+2v0​)T​=T23​(qf​−q0​)−T1​(vf​+2v0​)a3​=T3−2h+(vf​+v0​)T​=T3−2​(qf​−q0​)+T21​(vf​+v0​)​(2.7)
对于给定 n n n 个一系列数据点进行插值的情况，只需要对所有相邻的两个数据点使用上述公式即可依次计算得到整条插值曲线，五次多项式会在后面补充。

#!/usr/bin/python3
"""
Copyright © 2022 cyb_cqu. All rights reserved.

@file polynomial_ cubic.py
@date: 23:25:30, June 15, 2022
"""

import numpy as np
import matplotlib.pyplot as plt

class StateCubicPolynomial:
    """
    定义三次多项式使用的类对象状态变量（类似c语言结构体）
    t：时间
    q：位置
    v:速度
    a:加速度
    """
    def __init__(self,t = 0.0,q = 0.0,v = 0.0):
        self.t = t
        self.q = q
        self.v = v
        self.a = 0.0

    def PrintState(self):
        """
        打印类对象状态量
        :return:
        """
        print(self.t, end="  ")
        print(self.q, end="  ")
        print(self.v)

class CoefficientsCubicPolynomial:
    """
      定义三次多项式系数：a_0、a_1,a_2,a_3
    """
    def __ini__(self):
        self.a_0 = 0
        self.a_1 = 0
        self.a_2 = 0
        self.a_3 = 0

class CubicPolynomial:
    """
    三次多项式系数及函数值求解
    其中三次多项式为：q_t = a_0 + a_1 * (t - t_0) + a_2 * (t - t_0)**2 + a_3 * (t - t_0)**3
    """
    def __init__(self, state_0 = StateCubicPolynomial(0.0,0.0,0.0),state_f = StateCubicPolynomial(0.0,0.0,0.0)):
        self.coefficients = CoefficientsCubicPolynomial()
        self.coefficients = self.CalcuCoefficients(state_0,state_f)

    def CalcuCoefficients(self, state_0 = StateCubicPolynomial(0.0,0.0,0.0),state_f = StateCubicPolynomial(0.0,0.0,0.0)):
        """
        三次多项式系数及函数值求解函数
        :param state_0: class StateCubicPolynomial 三次多项式求解的初始条件
        :param state_f: class StateCubicPolynomial 三次多项式求解的末端条件
        :return: class CoefficientsCubicPolynomial 类型的三次多项式系数
        """
        h = state_f.q - state_0.q
        T = state_f.t - state_0.t
        Coefficients = CoefficientsCubicPolynomial()
        Coefficients.a_0 = state_0.q
        Coefficients.a_1 = state_0.v
        Coefficients.a_2 = (3*h - (state_f.v + 2*state_0.v)*T)/(T*T)
        Coefficients.a_3 = (-2 * h + (state_f.v + state_0.v) * T) / (T **3)
        return Coefficients

    def PrintCoefficients(self):
        """
        打印三次多项式系数 a_0,a_1,a_2,a_3,
        :return:
        """
        print(self.coefficients.a_0, end="  ")
        print(self.coefficients.a_1, end="  ")
        print(self.coefficients.a_2, end="  ")
        print(self.coefficients.a_3)

    def CalcuCubicPolynomial(self,coefficients = CoefficientsCubicPolynomial(),t = 0.0,t_0 = 0.0):
        """
        三次多项式位置、速度和加速度状态量求解
        :param coefficients: class CoefficientsCubicPolynomial
        :param t: 末端时刻
        :param t_0: 初始时刻
        :return: 返回 t 时刻的 class StateCubicPolynomial 类型对象，包含位置、速度、加速度状态量
        """
        state = StateCubicPolynomial(0.0, 0.0, 0.0)
        state.t = t
        state.q = coefficients.a_0 + coefficients.a_1 * (t - t_0) + coefficients.a_2 * (t - t_0)**2 + coefficients.a_3 * (t - t_0)**3
        state.v = coefficients.a_1 + 2 * coefficients.a_2 * (t -t_0) + 3*coefficients.a_3 * (t - t_0)**2
        state.a = 2*coefficients.a_2 + 6 * coefficients.a_3 * (t -t_0)
        return state


if __name__ == "__main__":
    # 实例化并赋初值
    state_0 = StateCubicPolynomial()
    state_0.t = 0
    state_0.q = 0
    state_0.v = 0
    print(state_0.PrintState())
    plt.plot(state_0.t,state_0.q,"ro")

    # 实例化并赋末端值
    state_f = StateCubicPolynomial()
    state_f.t = 5
    state_f.q = 10
    state_f.v = 0
    print(state_f.PrintState())
    plt.plot(state_f.t, state_f.q, "ro")
    # plt.show()

    cubicPolynomial = CubicPolynomial(state_0,state_f)
    cubicPolynomial.PrintCoefficients()

    step = 0.1
    t_list = np.arange(state_0.t,state_f.t + step,step)
    print(t_list)
    q_list = []
    v_list = []
    a_list = []

    # 三次多现实位置、速度和加速度求解
    for t in t_list:
        q_t = cubicPolynomial.CalcuCubicPolynomial(cubicPolynomial.coefficients,t,state_0.t)
        q_list.append(q_t.q)
        v_list.append(q_t.v)
        a_list.append(q_t.a)

    # 设置标题
    plt.suptitle("QuinticPolynomial")

    # 绘制位置曲线
    plt.subplot(311)
    plt.plot(state_0.t, state_0.q, "ro")
    plt.plot(state_f.t, state_f.q, "ro")
    plt.plot(t_list, q_list,"r")
    plt.grid('on')
    plt.ylabel("Position")
    # plt.xlim(t_list[0] - 1, t_list[-1] + 1)

    # 绘制速度曲线
    plt.subplot(312)
    plt.plot(state_0.t, state_0.v, "ro")
    plt.plot(state_f.t, state_f.v, "ro")
    plt.plot(t_list, v_list,"g")
    plt.ylabel("Velocity")
    plt.grid('on')

    # 绘制加速度曲线
    plt.subplot(313)
    plt.plot(state_0.t, state_0.a, "ro")
    plt.plot(state_f.t, state_f.a, "ro")
    plt.plot(t_list, a_list,"b")
    plt.ylabel("Acceleration")
    plt.grid('on')

    plt.xlabel("time")
    plt.show()

创作不易，希望大家多多支持。
github源码地址

参考链接：
https://zhuanlan.zhihu.com/p/269230598
https://github.com/chauby/PolynomialInterpolation
https://www.likecs.com/show-204912023.html
https://blog.csdn.net/qq_43412584/article/details/109143103