C++给定关键点做五次多项式轨迹规划

五次多项式轨迹规划是一种常用的机器人运动规划方法，可以通过给定起点、终点、起始速度、目标速度和加速度等关键点，生成一条平滑的轨迹。下面是C++实现五次多项式轨迹规划的示例代码：

#include <iostream>
#include <cmath>
using namespace std;

class QuinticPolynomial {
public:
    double a0, a1, a2, a3, a4, a5, t0, tf;

    // 构造函数
    QuinticPolynomial(double x0, double v0, double a0, double xf, double vf, double af, double T) {
        this->a0 = x0;
        this->a1 = v0;
        this->a2 = a0 / 2.0;
        double tmp = 1.0 / pow(T, 3);
        this->a3 = (20.0 * xf - 20.0 * x0 - (8.0 * vf + 12.0 * v0) * T - (3.0 * a0 - af) * pow(T, 2)) * tmp;
        this->a4 = (30.0 * x0 - 30.0 * xf + (14.0 * vf + 16.0 * v0) * T + (3.0 * a0 - 2.0 * af) * pow(T, 2)) * tmp;
        this->a5 = (12.0 * xf - 12.0 * x0 - (6.0 * vf + 6.0 * v0) * T - (a0 - af) * pow(T, 2)) * tmp;
        this->t0 = 0.0;
        this->tf = T;
    }

    // 计算位置
    double calcPoint(double t) {
        if (t <= t0) {
            return a0;
        } else if (t >= tf) {
            return a0 + a1 * (tf - t0) + a2 * pow(tf - t0, 2) + a3 * pow(tf - t0, 3) + a4 * pow(tf - t0, 4) + a5 * pow(tf - t0, 5);
        } else {
            return a0 + a1 * (t - t0) + a2 * pow(t - t0, 2) + a3 * pow(t - t0, 3) + a4 * pow(t - t0, 4) + a5 * pow(t - t0, 5);
        }
    }

    // 计算速度
    double calcVelocity(double t) {
        if (t <= t0) {
            return 0.0;
        } else if (t >= tf) {
            return a1 + 2.0 * a2 * (tf - t0) + 3.0 * a3 * pow(tf - t0, 2) + 4.0 * a4 * pow(tf - t0, 3) + 5.0 * a5 * pow(tf - t0, 4);
        } else {
            return a1 + 2.0 * a2 * (t - t0) + 3.0 * a3 * pow(t - t0, 2) + 4.0 * a4 * pow(t - t0, 3) + 5.0 * a5 * pow(t - t0, 4);
        }
    }

    // 计算加速度
    double calcAcceleration(double t) {
        if (t <= t0) {
            return 0.0;
        } else if (t >= tf) {
            return 2.0 * a2 + 6.0 * a3 * (tf - t0) + 12.0 * a4 * pow(tf - t0, 2) + 20.0 * a5 * pow(tf - t0, 3);
        } else {
            return 2.0 * a2 + 6.0 * a3 * (t - t0) + 12.0 * a4 * pow(t - t0, 2) + 20.0 * a5 * pow(t - t0, 3);
        }
    }
};

int main() {
    double x0 = 0.0, v0 = 0.0, a0 = 0.0;
    double xf = 10.0, vf = 0.0, af = 0.0;
    double T = 2.0;
    QuinticPolynomial qp(x0, v0, a0, xf, vf, af, T);

    for (double t = 0.0; t <= T; t += 0.1) {
        double x = qp.calcPoint(t);
        double v = qp.calcVelocity(t);
        double a = qp.calcAcceleration(t);
        cout << "t = " << t << ", x = " << x << ", v = " << v << ", a = " << a << endl;
    }

    return 0;
}

在上面的代码中，我们定义了一个 QuinticPolynomial 类，它包含了五次多项式轨迹的系数和时间范围。其中，构造函数根据给定的关键点计算出五次多项式的系数，calcPoint()、calcVelocity() 和 calcAcceleration() 分别用于计算在给定时间点上的位置、速度和加速度。在 main() 函数中，我们定义了起点、终点、起始速度、目标速度和加速度等关键点，通过 QuinticPolynomial 类计算出每个时间点上的运动状态。