C++三次样条轨迹规划代码

以下是一个简单的三次样条轨迹规划代码示例，其中使用了numpy和matplotlib库：

import numpy as np
import matplotlib.pyplot as plt

# 定义插值函数
def cubic_spline(x, y):
    n = len(x)
    a = y.copy()
    b = np.zeros(n)
    d = np.zeros(n)
    h = np.diff(x)
    alpha = np.zeros(n - 1)
    for i in range(1, n - 1):
        alpha[i] = 3 * (a[i + 1] - a[i]) / h[i] - 3 * (a[i] - a[i - 1]) / h[i - 1]
    c = np.zeros(n - 1)
    l = np.ones(n)
    mu = np.zeros(n - 1)
    z = np.zeros(n)
    for i in range(1, n - 1):
        l[i] = 2 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1]
        mu[i] = h[i] / l[i]
        z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i]
    l[n - 1] = 1
    z[n - 1] = 0
    for j in range(n - 2, -1, -1):
        c[j] = z[j] - mu[j] * c[j + 1]
        b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2 * c[j]) / 3
        d[j] = (c[j + 1] - c[j]) / (3 * h[j])
    return a, b, c, d

# 定义数据点
x = np.array([0, 1, 2, 3])
y = np.array([1, 2, 3, 1])

# 计算插值函数系数
a, b, c, d = cubic_spline(x, y)

# 定义插值函数
def spline(x, a, b, c, d, x_i):
    j = np.searchsorted(x, x_i) - 1
    dx = x_i - x[j]
    return a[j] + b[j] * dx + c[j] * dx ** 2 + d[j] * dx ** 3

# 绘制插值函数图像
x_i = np.linspace(0, 3, 100)
y_i = spline(x, a, b, c, d, x_i)
plt.plot(x, y, 'o', label='data')
plt.plot(x_i, y_i, label='spline')
plt.legend(loc='best')
plt.show()

该代码使用三次样条函数计算插值函数系数，并使用插值函数在数据点之间绘制平滑曲线。可以将其应用于任何需要平滑轨迹规划的应用程序中。