五次多项式曲线插值

燚泽

已于 2024-02-23 16:33:56 修改

阅读量1k

点赞数 5

文章标签：算法自动驾驶 c++

于 2024-02-05 16:15:44 首次发布

本文链接：https://blog.csdn.net/u014318178/article/details/136043567

版权

1 算法概述

曲线插值是指基于特定的曲线类型（多项式曲线、贝塞尔曲线、B样条曲线等），根据自车期望达到的状态（比如自车到达某点的速度和加速度要为特定的数值），将此期望值作为边界条件带入曲线方程进行求解，获得曲线的系数，从而确定出满足要求的曲线。

2 算法说明

2.1 给定的约束为位置、速度、加速度

针对五次多项式曲线来说，最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度、加速度。

$\left\{ \begin{aligned} x(t) = a_{0} + a_{1}t + a_{2}t^{2} + a_{3}t^{3} + a_{4}t^{4} + a_{5}t^{5} \\ y(t) = b_{0} + b_{1}t + b_{2}t^{2} + b_{3}t^{3} + b_{4}t^{4} + b_{5}t^{5} \end{aligned} \right.$

设 $t_{0}$ 为初始时间，在此刻的位置、速度、加速度为已知量，五次多项式在x和y方向分别有以下方程：

位置：

$\left\{ \begin{aligned} x(t_{0}) = a_{0} + a_{1}t_{0}+ a_{2}t_{0}^{2} + a_{3}t_{0}^{3} + a_{4}t_{0}^{4} + a_{5}t_{0}^{5} \\ y(t_{0}) = b_{0} + b_{1}t_{0} + b_{2}t_{0}^{2} + b_{3}t_{0}^{3} + b_{4}t_{0}^{4} + b_{5}t_{0}^{5} \end{aligned} \right.$

速度：

$\left\{ \begin{aligned} x'(t_{0}) = a_{1}+ 2t_{0}a_{2} + 3t_{0}^2a_{3} +4t_{0}^{3} a_{4}+ 5t_{0}^{4}a_{5} \\ y'(t_{0}) = b_{1}+ 2t_{0}b_{2} + 3t_{0}^2b_{3} +4t_{0}^{3} b_{4}+ 5t_{0}^{4}b_{5} \end{aligned} \right.$

加速度：

$\left\{ \begin{aligned} x''(t_{0}) =2a_{2} + 6t_{0}a_{3} +12t_{0}^{2} a_{4}+ 20t_{0}^{3}a_{5} \\ y''(t_{0}) = 2b_{2} + 6t_{0}b_{3} +12t_{0}^{2} b_{4}+ 20t_{0}^{3}b_{5} \end{aligned} \right.$

设 $t_{1}$ 为终点时间，在此刻的位置、速度、加速度也为已知量，同样五次多项式在x和y方向分别有以下方程：

位置：

$\left\{ \begin{aligned} x(t_{0}) = a_{0} + a_{1}t_{1}+ a_{2}t_{1}^{2} + a_{3}t_{1}^{3} + a_{4}t_{1}^{4} + a_{5}t_{1}^{5} \\ y(t_{0}) = b_{0} + b_{1}t_{1} + b_{2}t_{1}^{2} + b_{3}t_{1}^{3} + b_{4}t_{1}^{4} + b_{5}t_{1}^{5} \end{aligned} \right.$

速度：

$\left\{ \begin{aligned} x'(t_{1}) = a_{1}+ 2t_{1}a_{2} + 3t_{1}^2a_{3} +4t_{1}^{3} a_{4}+ 5t_{1}^{4}a_{5} \\ y'(t_{1}) = b_{1}+ 2t_{1}b_{2} + 3t_{1}^2b_{3} +4t_{1}^{3} b_{4}+ 5t_{1}^{4}b_{5} \end{aligned} \right.$

加速度：

$\left\{ \begin{aligned} x''(t_{1}) =2a_{2} + 6t_{1}a_{3} +12t_{1}^{2} a_{4}+ 20t_{1}^{3}a_{5} \\ y''(t_{1}) = 2b_{2} + 6t_{1}b_{3} +12t_{1}^{2} b_{4}+ 20t_{1}^{3}b_{5} \end{aligned} \right.$

根据起点和终点的边界条件，可以分别列出x和y方向的线性方程组，用矩阵表达如下：

$X=\begin{bmatrix} x_0\\ x'_0\\ x''_0\\ x_1\\ x'_1\\ x''_1 \end{bmatrix} = \begin{bmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4 & t_0^5\\ 0 & 1 & 2t_0 & 3t_0^2 & 4t_0^3 &5t_0^4 \\ 0 & 0 & 2 & 6t_0 & 12t_0^2 & 20t_0^3\\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4 & t_1^5\\ 0 & 1 & 2t_1 & 3t_1^2 & 4t_1^3 & 5t_1^4\\ 0 & 0 & 2 & 6t_1 & 12t_1^2 & 20t_1^3 \end{bmatrix} \begin{bmatrix} a_0\\ a_1\\ a_2\\ a_3\\ a_4\\ a_5 \end{bmatrix} = T \times A$

$Y=\begin{bmatrix} y_0\\ y'_0\\ y''_0\\ y_1\\ y'_1\\ y''_1 \end{bmatrix} = \begin{bmatrix} 1 & t_0 & t_0^2 & t_0^3 & t_0^4 & t_0^5\\ 0 & 1 & 2t_0 & 3t_0^2 & 4t_0^3 &5t_0^4 \\ 0 & 0 & 2 & 6t_0 & 12t_0^2 & 20t_0^3\\ 1 & t_1 & t_1^2 & t_1^3 & t_1^4 & t_1^5\\ 0 & 1 & 2t_1 & 3t_1^2 & 4t_1^3 & 5t_1^4\\ 0 & 0 & 2 & 6t_1 & 12t_1^2 & 20t_1^3 \end{bmatrix} \begin{bmatrix} b_0\\ b_1\\ b_2\\ b_3\\ b_4\\ b_5 \end{bmatrix} = T \times B$

在上述的等式中，X矩阵和Y矩阵的数值都是已知的。x0，x0’，x0’’ 分别表示初始位置的横向坐标，速度，加速度；y0，y0’，y0’’ 分别表示初始位置的纵向坐标，速度，加速度。其次，时间t0和t1也是已知的，分别表示初始位置和终点位置的时刻。所以，X，Y，T矩阵都已知，可以求出x方向的系数矩阵A和y方向的系数矩阵B。

注意：有的表达式中采用的是时间间隔Δt代入T矩阵（Δt= t1 -t0），利用A矩阵和T矩阵可以求出初始位置和终点位置之间的点的x方向的位置、速度和加速度，利用B矩阵和T矩阵可以求出初始位置和终点位置之间的点的y方向的位置、速度和加速度。

五次多项式曲线的自变量为时间t，当x方向的系数矩阵A和y方向的系数矩阵B计算出来后，则曲线方程也随之唯一确定，曲线上每一点的位置、速度等也确定了，可以得到五次多项式生成的轨迹。曲线在x和y方向的导数分别为在x和y方向的速度，说明五次多项式规划出的轨迹是路径和速度耦合生成的。在此类情况下，五次多项式曲线上x方向和y方向都是关于t的五次多项式，不是y关于x的五次多项式。

2.1 给定的约束为位置、航向角、曲率

设五次多项式的表达式为：

$\left\{ \begin{aligned} x(t) = a_{0} + a_{1}t + a_{2}t^{2} + a_{3}t^{3} + a_{4}t^{4} + a_{5}t^{5} = f(t)) \\ y(t) = b_{0} + b_{1}t + b_{2}t^{2} + b_{3}t^{3} + b_{4}t^{4} + b_{5}t^{5} = g(t) \end{aligned} \right.$

约束条件为：起点和终点的位置、航向角、曲率，如下图所示：

此时，x和y方向的约束分别由以下方程表达：

$x(t) = f(t)$

$y(t)=g(t)$

$x(t)' = v*cos(theta)$

$y(t)' = v*sin(theta)$

$x(t)''=-v^2*k(t)*sin(theta)$

$y(t)''=v^2*k(t)*cos(theta)$

根据起点和终点的边界条件，同样可以分别列出x和y方向的线性方程组，用矩阵表达如下：

在上述的等式中，X矩阵和Y矩阵的数值根据新的约束条件(位置、航向角、曲率)可直接得出。x0，x0’，x0’’ 分别表示初始位置的横向坐标，速度，加速度；y0，y0’，y0’’ 分别表示初始位置的纵向坐标，速度，加速度。其次，时间t0和t1也是已知的，分别表示初始位置和终点位置的时刻。所以，X，Y，T矩阵都已知，也可以求出x方向的系数矩阵A和y方向的系数矩阵B。

关于时间间隔t，如果t0和t1的时刻无法直接给出，可以假设自车恒速为v,预估A点到B点的时间:

$t=|AB|/v$

多项式曲线的自变量为时间 t，故一旦求解出系数矩阵 A , B ，即可确定曲线方程曲线唯一确定后，则曲线上每一点的位置、速度等便确定了，轨迹即可求出。

每一点的theta计算： $theta = atan(y(t),x(t))$

每一点的曲率kappa计算： $(x'y''-x''y')/(x'^2+y'^2)^{3/2}$

每一点的曲率变化率dkappa计算（对上式kappa求导）： $Dkappa= [(x'y'''-x'''y')(x'^2+y'^2)^{1.5}-3(x'^2+y'^2)^{0.5}(x'x''+y'y'')(x'y''-x''y')]/(x'^2+y'^2)^{1.5}$

2.3 五次多项式的y(x)直接表达式

在2.1节和2.2节的五次多项式是分别在x和y方向求解x和y关于时间t的表达式，那么是否可以采用y关于x的表达式，以下进行讨论。

设五次多项式的表达式为：

$y = p_0 + p_1x +p_2x^2 + p_3x^3 + p_4x^4 + p_5x^5$

$y' = p_1 + 2p_2x +3p_3x^2 + 4p_4x^3 + 5p_5x^4$

$y'' = 2p_2 + 6p_3x +12p_4x^2 + 20p_5x^3$

则可以推出：

$y' = tan(theta)$

$k = y'' / (1 + y'^2)^{1.5}$

式中，theta表示航向角、k表示曲率。

设起点和终点的约束条件(因与时间t无关，此时是自变量为x)包括：

1.起点init_point坐标 $(x_0,y_0)$

2.终点end_point坐标 $(x_1,y_1)$

3.起点init_point的航向角 $(x_0, \theta_0 )$

4.终点end_point的航向角 $(x_1,\theta _1)$

5.起点init_point的曲率 $(x_0,k_0(1+y_0'^{2})^{1.5})=(x_0,k_0(1+tan^{2}(theta_0))^{1.5})$

6.终点end_point的曲率 $(x_1,k_1(1+y_1'^{2})^{1.5})=(x_1,k_1(1+tan^{2}(theta_1))^{1.5})$

根据上述6个约束，可以得到6个方程，从而求解出五次多项式的各项系数。

$B=\begin{bmatrix} y_0\\ y_1\\ y_0'\\ y_1'\\ y_0''\\ y_1'' \end{bmatrix} = \begin{bmatrix} y_0\\ y_1\\ tan(theta_0)\\ tan(theta_1)\\ k_0(1+tan^2(theta_0))^{1.5}\\ k_1(1+tan^2(theta_1))^{1.5} \end{bmatrix} = \begin{bmatrix} 1 & x_0 & x_0^2 & x_0^3 & x_0^4 & x_0^5\\ 1 & x_1 & x_1^2 & x_1^3 & x_1^4 &x_1^5 \\ 0 & 1 & 2x_0 & 3x_0^2 & 4x_0^3 & 5x_0^4\\ 0 & 1 & 2x_1 & 3x_1^2 & 4x_1^3 & 5x_1^4\\ 0 & 0 & 2 & 6x_0 & 12x_0^2 & 20x_0^3\\ 0 & 0 & 2 & 6x_1 & 12x_1^2 & 20x_1^3 \end{bmatrix} \begin{bmatrix} p_0\\ p_1\\ p_2\\ p_3\\ p_4\\ p_5 \end{bmatrix} = X \times P$

P向量的元素即是五次多项式的各项系数，求出系数也就确定了五次多项式。

由于计算机无法表达连续的曲线，实际应用中是通过离散点来近似连续曲线，根据以上的五次多项式，进行离散化采样就可以得到曲线上的一系列点。

离散化方式：从起点开始，每次沿着曲线间隔固定的长度 $\Delta l$ 进行取点采样。如果以固定的 $\Delta x$ 来采样，由于不是严格沿着曲线等间距采样，缺点是可能会造成离散点分布不均匀的情况，优点是计算简便，节省计算量。而沿着曲线固定长度 $\Delta s$ 采样离散化，缺点是需要计算相对复杂，优点是采样点是近似沿着曲线等间距的。

具体采样过程说明：

步骤1 根据 $(x_i,y_i)$ 点的已知量，可以求出 $\Delta x$ ，从而可以知道 $x_{i+1}$ 的值。

在点 $(x_i,y_i)$ 处有：

$tan(theta_i)=\frac{\Delta y}{\Delta x}$

$sin(theta_i)=\frac{\Delta y}{\Delta s}$

推知： $\Delta x=\Delta l*cos(theta_i) = \frac{\Delta l}{\sqrt{1+tan^{2}(theta_i)}}$ ，其中 $\Delta l$ 是根据需求设定的数值（已知量），所以在任一点 $(x_i,y_i)$ 处，可以由 $theta_i$ 计算出 $\Delta x$ ，从而确定 $x_{i+1}$ 的数值。

步骤2 根据五次多项式的计算表达式，根据得到的 $x_{i+1}$ 可以计算出 $y_{i+1},theta_{i+1},k_{i+1}$ 。

依次循环步骤1和步骤2，得到间隔的一系列点，直到选取的点 $x_{i+1}$ 与终点 $x_{end}$ 的距离首次在一个很小的阈值内，最后取终点作为最后一个采样点。

3 代码实现

3.1 约束条件x,y; vx,vy; ax,ay

假设x,y; vx,vy; ax,ay均已知，以一个换道轨迹为例，如下图所示：

在这种约束条件下的实现代码为：

#include <iostream>
#include<Eigen/Dense>
#include<cmath>
#include <tuple>
#include<algorithm>
#include<vector>
#include <thread>
/*
需要安装matplotlib 
链接：https://matplotlib-cpp.readthedocs.io/en/latest/compiling.html#compiling
*/
#include "../../matplotlibcpp.h"

namespace plt = matplotlibcpp;

using namespace std;
using namespace Eigen;
struct PathPoint {
    double x;
    double y;
};

int main () {
    PathPoint init_point, end_point;
    init_point.x = 0.0;
    init_point.y = -1.75;

    end_point.x = 20.0;
    end_point.y = 1.75;
    std::vector<double> x, y;
    
    
    Eigen::VectorXd X(6), Y(6);


    double x0 = init_point.x;
    double dx0 = 5.0;
    double ddx0 = 0.0;
    double x1 = end_point.x;
    double dx1 = 5.0;
    double ddx1 = 0.0;

    double y0 = init_point.y;
    double dy0 = 0.0;
    double ddy0 = 0.0;
    double y1 = end_point.y;
    double dy1 = 0.0;
    double ddy1 = 0.0;

    X << x0, dx0, ddx0, x1, dx1, ddx1;
    Y << y0, dy0, ddy0, y1, dy1, ddy1;

    double t0 = 0;
    double t1 = 3;

    Eigen::MatrixXd T(6, 6);


    T << 1, t0, pow(t0, 2), pow(t0, 3), pow(t0, 4), pow(t0, 5), 
        0, 1, 2 * t0, 3 * pow(t0, 2), 4 * pow(t0, 3), 5 * pow(t0, 4), 
        0, 0, 2, 6 * t0,12 * pow(t0, 2), 20 * pow(t0, 3), 
        1, t1, pow(t1, 2), pow(t1, 3), pow(t1, 4), pow(t1, 5), 
        0, 1, 2 * t1, 3 * pow(t1, 2), 4 * pow(t1, 3),5 * pow(t1, 4), 
        0, 0, 2, 6 * t1, 12 * pow(t1, 2), 20 * pow(t1, 3);

    //计算A和B两个系数矩阵
    Eigen::MatrixXd A = T.inverse() * X;
    Eigen::MatrixXd B = T.inverse() * Y;
    
    //保存横纵向坐标和横纵向速度用于画图
    vector<double>x_,y_,v_lon,v_lat;

    vector<double>time;
    int count = 0;
    for(double t=t0;t<t1+0.05;t+=0.05) {
        count++;
        time.push_back(t);

    }

    MatrixXd temp1(1,6),temp2(1,6);
    for(int i=0;i<count;i++){

        temp1<<1,time[i],pow(time[i], 2),pow(time[i], 3),pow(time[i], 4),pow(time[i], 5);
        x_.push_back((temp1*A)(0,0));
        y_.push_back((temp1*B)(0,0));

        temp2<<0,1,2*time[i],3*pow(time[i],2),4*pow(time[i],3),5*pow(time[i],4);
        v_lon.push_back((temp2*A)(0,0));
        v_lat.push_back((temp2*B)(0,0));
    }
    //画图
    plt::figure(1);
    plt::plot(x_, y_,"r");

    // 横向速度
    plt::figure(2);
    plt::plot(time,v_lon);
    // 纵向速度
    plt::figure(3);
    plt::plot(time,v_lat);

    plt::show();
  
  
  
  return 0;
}

生成的轨迹曲线如下所示：

3.2 约束条件(x,y,theta,kappa)

#include <iostream>
#include<Eigen/Dense>
#include<cmath>
#include <tuple>
#include<algorithm>
#include<vector>
#include <thread>
#include "../../matplotlibcpp.h"

namespace plt = matplotlibcpp;

using namespace std;
using namespace Eigen;
struct PathPoint {
    double x;
    double y;
    double theta;
    double kappa;
};


std::vector<double> a;
std::vector<double> b;
double g_t0 = 0.0;
double g_t1 = 0.0;
double v = 5.0;

/**
 * @description: 根据给定的五次多项式和x坐标，求出对应的y值。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return double y
 */
double polynomial(double t, std::vector<double>& coefficients) {
double result = 0.0;
for (int i = 0; i < coefficients.size(); i++) {
    result += coefficients[i] * std::pow(t, i);
}
return result;
}

/**
 * @description: 计算五次多项式的一阶导数。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return y'(一阶导)
 */
double polynomial_1stderivative(double t, std::vector<double>& coefficients) {
    double result = 0.0;
    for (int i = 1; i < coefficients.size(); i++) {
        result += i * coefficients[i] * std::pow(t, i - 1);
    }
    return result;
}

/**
 * @description: 计算五次多项式的二阶导数。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return y''(二阶导)
 */
double polynomial_2ndderivative(double t, std::vector<double>& coefficients) {
    double result = 0.0;
    for (int i = 2; i < coefficients.size(); i++) {
        result += i *(i - 1)* coefficients[i] * std::pow(t, i - 2);
    }
    return result;
}

double CalculatedCurvature(double dx, double ddx, double dy, double ddy) {
    double result = 0.0;
    result = (dx * ddy - ddx * dy) / std::pow((std::pow(dx, 2) + std::pow(dy, 2)), 1.5);
    return result;
}

void GetPoints(std::vector<PathPoint>& points, double delta_s) {
  // TODO: 根据已经确定的五次多项式系数，从起始点init_point_开始，等时间间隔取出Path点，直到终点end_point_

  //离散化时间得到一系列点集

  double t0 = g_t0;
  double t1 = g_t1;
  double delta_t = delta_s / v;

  std::cout << " delta_s------" << delta_s <<std::endl;
  std::cout << " v------" << v <<std::endl;
  std::cout << " delta_t------" << delta_t <<std::endl;
// double delta_t = 0.05;


  PathPoint p_temp;
  std::vector<double> time;
  int count  = 0;
  for (double t = t0; t < t1; t += delta_t) {
    count++;
    time.push_back(t);
  }

  for (int i = 0; i < count; i++) {
    p_temp.x = polynomial(time[i], a);
    p_temp.y = polynomial(time[i], b);
    p_temp.theta = atan2(polynomial_1stderivative(time[i], b), polynomial_1stderivative(time[i], a));
    p_temp.kappa = CalculatedCurvature(polynomial_1stderivative(time[i], a), 
                     polynomial_2ndderivative(time[i], a), 
                     polynomial_1stderivative(time[i], b), 
                     polynomial_2ndderivative(time[i], b));
    points.emplace_back(p_temp);
  }

  p_temp.x = polynomial(t1, a);
  p_temp.y = polynomial(t1, b);
  p_temp.theta = atan2(polynomial_1stderivative(t1, b), polynomial_1stderivative(t1, a));
  p_temp.kappa = CalculatedCurvature(polynomial_1stderivative(t1, a), 
                    polynomial_2ndderivative(t1, a), 
                    polynomial_1stderivative(t1, b), 
                    polynomial_2ndderivative(t1, b));
  points.emplace_back(p_temp);

}

void Create(const PathPoint& init_point, const PathPoint& end_point) {


  a.clear();
  b.clear();

  double v_ = std::max(v, 0.1);  //车速

  //初始状态与终点期望状态
  double t0 = 0;
  double t1 = sqrt(std::pow(end_point.x - init_point.x, 2) + std::pow(end_point.y - init_point.y, 2)) / v_;

  //把起末两点的横纵向方程统一用矩阵表达
  Eigen::VectorXd X(6), Y(6);
  double x0 = init_point.x;
  double dx0 = v_ * cos(init_point.theta);
  double ddx0 = -1.0 * v_ * v_ * init_point.kappa * sin(init_point.theta);
  double x1 = end_point.x;
  double dx1 = v_ * cos(end_point.theta);
  double ddx1 = -1.0 * v_ * v_ * end_point.kappa * sin(end_point.theta);

  double y0 = init_point.y;
  double dy0 = v_ * sin(init_point.theta);
  double ddy0 = 1.0 * v_ * v_ * init_point.kappa * cos(init_point.theta);
  double y1 = end_point.y;
  double dy1 = v_ * sin(end_point.theta);
  double ddy1 = 1.0 * v_ * v_ * end_point.kappa * cos(end_point.theta);

  X << x0, dx0, ddx0, x1, dx1, ddx1;
  Y << y0, dy0, ddy0, y1, dy1, ddy1;

  Eigen::MatrixXd T(6, 6);
  // T<<pow(t0, 5),pow(t0, 4),pow(t0, 3),pow(t0, 2),t0,1,
  //     5*pow(t0,4),4*pow(t0,3),3*pow(t0,2),2*t0,1,0,
  //     20*pow(t0,3),12*pow(t0,2),6*t0,2,0,0,
  //     pow(t1, 5),pow(t1, 4),pow(t1, 3),pow(t1, 2),t1,1,
  //     5*pow(t1,4),4*pow(t1,3),3*pow(t1,2),2*t1,1,0,
  //     20*pow(t1,3),12*pow(t1,2),6*t1,2,0,0;

  T << 1, t0, pow(t0, 2), pow(t0, 3), pow(t0, 4), pow(t0, 5), 
       0, 1, 2 * t0, 3 * pow(t0, 2), 4 * pow(t0, 3), 5 * pow(t0, 4), 
       0, 0, 2, 6 * t0,12 * pow(t0, 2), 20 * pow(t0, 3), 
       1, t1, pow(t1, 2), pow(t1, 3), pow(t1, 4), pow(t1, 5), 
       0, 1, 2 * t1, 3 * pow(t1, 2), 4 * pow(t1, 3),5 * pow(t1, 4), 
       0, 0, 2, 6 * t1, 12 * pow(t1, 2), 20 * pow(t1, 3);

  //计算A和B两个系数矩阵
  Eigen::MatrixXd A = T.inverse() * X;
  Eigen::MatrixXd B = T.inverse() * Y;

  a = std::vector<double>{A(0), A(1), A(2), A(3), A(4), A(5)};
  b = std::vector<double>{B(0), B(1), B(2), B(3), B(4), B(5)};
  g_t0 = t0;
  g_t1 = t1;
}


int main(){
    PathPoint init_point, end_point;
    init_point.x = 0.0;
    init_point.y = -1.75;
    init_point.theta = 1.57; // pi/2
    init_point.kappa = 0.001;

    end_point.x = 20;
    end_point.y = 1.75;
    end_point.theta = 0.0; // pi/2
    end_point.kappa = 0.001;


    std::vector<PathPoint> points;
    vector<double>x_,y_;

    Create(init_point, end_point);
    std::cout << " Create函数执行完毕" << std::endl;
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    if (true) {
        GetPoints(points, 1.0);
        std::cout << "-------------" << std::endl;
        for (int i = 0; i < points.size(); ++i) {
        x_.push_back(points[i].x);
        y_.push_back(points[i].y);
    }


        //画图
        plt::figure(1);
        plt::plot(x_, y_,"r");

        plt::show(); 
    }

}

3.3 y(x)表达式下的约束条件(x,y,theta,kappa)

#include <iostream>
#include<Eigen/Dense>
#include<cmath>
#include <tuple>
#include<algorithm>
#include<vector>
#include <thread>
#include "../../matplotlibcpp.h"

namespace plt = matplotlibcpp;

using namespace std;
using namespace Eigen;
struct PathPoint {
    double x;
    double y;
    double theta;
    double kappa;
};



std::vector<double> p;
PathPoint init_point_, end_point_;


/**
 * @description: 根据给定的五次多项式和x坐标，求出对应的y值。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return double y
 */
double polynomial(double t, std::vector<double>& coefficients) {
    double result = 0.0;
    for (int i = 0; i < coefficients.size(); i++) {
    result += coefficients[i] * std::pow(t, i);
    }
    return result;
}

/**
 * @description: 计算五次多项式的一阶导数。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return y'(一阶导)
 */
double polynomial_1stderivative(double t, std::vector<double>& coefficients) {
    double result = 0.0;
    for (int i = 1; i < coefficients.size(); i++) {
        result += i * coefficients[i] * std::pow(t, i - 1);
    }
    return result;
}

/**
 * @description: 计算五次多项式的二阶导数。
 * @param double x
 * @param std::vector<double>& coefficients
 * @return y''(二阶导)
 */
double polynomial_2ndderivative(double t, std::vector<double>& coefficients) {
    double result = 0.0;
    for (int i = 2; i < coefficients.size(); i++) {
        result += i *(i - 1)* coefficients[i] * std::pow(t, i - 2);
    }
    return result;
}



void GetPoints(std::vector<PathPoint>& points, double delta_s) {
  // TODO: 根据已经确定的五次多项式系数，从起始点init_point_开始，等距离取出Path点，直到终点end_point_
    PathPoint p_temp;
    double x = init_point_.x;  //init_point.x
    double y = end_point_.y; //init_point.y;
    
  while (x <= end_point_.x) {
    // 计算当前点的坐标
    y = polynomial(x, p);


      
      //给point添加属性
    p_temp.x = x;
    p_temp.y = y;
    p_temp.theta = std::atan(polynomial_1stderivative(x, p));
    p_temp.kappa = polynomial_2ndderivative(x, p) / std::pow((1 + std::pow(std::tan(init_point_.theta),2)), 1.5);

    
    // 将当前采样点塞入points
    points.emplace_back(p_temp);
    
    // 计算下一个点的 x 坐标
    double dx = delta_s / std::sqrt(1 + std::pow(polynomial_1stderivative(x, p), 2));
    x += dx;
    
    // 判断是否已经超过终点，若超过则直接取终点
    if (x > end_point_.x) {
        x = end_point_.x;
        break;
    }
  }
  
}

void Create(const PathPoint& init_point, const PathPoint& end_point) {
  init_point_ = init_point;
  end_point_ = end_point;

  // 计算系数
  Matrix<double, 6, 6> A;
  VectorXd B(6);
  
  A << 1, init_point.x, std::pow(init_point.x, 2), std::pow(init_point.x, 3), std::pow(init_point.x, 4), std::pow(init_point.x, 5),
  1, end_point.x, std::pow(end_point.x, 2), std::pow(end_point.x, 3), std::pow(end_point.x, 4), std::pow(end_point.x, 5),
  0, 1, 2*init_point.x, 3*std::pow(init_point.x, 2), 4*std::pow(init_point.x, 3), 5*std::pow(init_point.x, 4),
  0, 1, 2*end_point.x, 3*std::pow(end_point.x, 2), 4*std::pow(end_point.x, 3), 5*std::pow(end_point.x, 4),
  0, 0, 2, 6*init_point.x, 12*std::pow(init_point.x, 2), 20*std::pow(init_point.x, 3),
  0, 0, 2, 6*end_point.x, 12*std::pow(end_point.x, 2), 20*std::pow(end_point.x, 3);

  B << init_point.y, end_point.y, std::tan(init_point.theta), std::tan(end_point.theta), init_point.kappa * std::pow((1 + std::pow(std::tan(init_point.theta),2)), 1.5) , end_point.kappa * std::pow((1 + std::pow(std::tan(end_point.theta),2)), 1.5);

  // 解线性方程组
  VectorXd P = A.colPivHouseholderQr().solve(B);

  // 输出计算得到的多项式系数
  std::cout << "五次多项式的系数：" << std::endl;
  for (int i = 0; i < 6; i++) {
  std::cout << "P[" << i << "] = " << P(i) << std::endl;
  p.emplace_back(P(i));
  }


}

int main(){

    PathPoint init_point, end_point;
    init_point.x = 0.0;
    init_point.y = -1.75;
    init_point.theta = 0.0; // pi/2
    init_point.kappa = 0.001;

    end_point.x = 20;
    end_point.y = 1.75;
    end_point.theta = 0.0; 
    end_point.kappa = 0.001;

    std::vector<PathPoint> points;
    vector<double>x_,y_;

    Create(init_point, end_point);
    std::cout << " Create函数执行完毕" << std::endl;
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    if (true) {
        GetPoints(points, 1.0);
        std::cout << "-------------" << std::endl;
        for (int i = 0; i < points.size(); ++i) {
        x_.push_back(points[i].x);
        y_.push_back(points[i].y);
    }


        //画图
        plt::figure(1);
        plt::plot(x_, y_,"r");

        plt::show(); 
    }

}

曲线效果如下：

参考文档：

Algorithm: 本仓库用于搜集一些常用的轨迹规划算法 - Gitee.com
第十九篇，解析法求解五阶多项式_解一个五阶线性方程,需要几个初始条件-CSDN博客
 五次多项式C++版 - 知乎
 多项式轨迹--五次多项式 - 知乎
 【决策规划算法】曲线插值算法：五次多项式（C++） - 知乎
 自动驾驶轨迹预测算法系列（2）——五次多项式曲线插值算法 - 知乎
 机器人轨迹规划——五次多项式插值轨迹_五次多项式轨迹规划-CSDN博客
 【路径规划】局部路径规划算法——曲线插值法（含python实现 | c++实现）-CSDN博客
 自动驾驶决策规划-控制方向学习资料总结（附相关资料的链接）_自动驾驶决策规划技术理论与实践pdf-CSDN博客

燚泽

关注

5
点赞
踩
15

收藏

觉得还不错? 一键收藏
0
评论
五次多项式曲线插值

曲线插值是指基于特定的曲线类型（多项式曲线、贝塞尔曲线、B样条曲线等），根据自车期望达到的状态（比如自车到达某点的速度和加速度要为特定的数值），将此期望值作为边界条件带入曲线方程进行求解，获得曲线的系数，从而确定出满足要求的曲线。
复制链接

扫一扫