LQR路径跟踪要求路径中带角度，即坐标（x,y,yaw），而一般我们的规划出来的路径不带角度。这里通过总结相关方法，并提供一个案例。

将点路径拟合成一条完整的线路径算法

将点路径拟合成一条完整的线路径是一个常见的问题，在计算机图形学、机器人导航、地图制作等领域都有广泛的应用。以下是一些常见的用于路径拟合的算法：

1. 多项式拟合：使用多项式函数来逼近给定的数据点。常见的方法包括最小二乘法拟合线性、二次或更高次的多项式。多项式拟合简单直观，但对于复杂的路径可能不够灵活。
2. 样条插值：样条插值通过在相邻数据点之间拟合局部小段来逼近数据。常见的样条插值包括三次样条插值，它保证在每个数据点处平滑连接，并具有良好的数学性质。
3. 贝塞尔曲线：贝塞尔曲线是一种通过控制点来定义路径的方法。通过调整控制点的位置和权重，可以创建平滑的曲线。贝塞尔曲线在计算机图形学中被广泛应用。
4. B样条曲线：B样条曲线是一种基于控制点和节点向量定义的曲线。它具有局部控制和局部性质，能够创建复杂的曲线并且有良好的数学性质。
5. 最小二乘法拟合直线或曲线：最小二乘法可以用于拟合直线或曲线到数据点，通过最小化拟合曲线与数据点之间的误差来找到最佳拟合。可以使用直线、圆弧等简单几何形状来拟合路径。
6. 分段线性拟合：将路径分成若干小段，分别用直线来拟合每一小段，从而逼近整体路径。这种方法简单直观，适用于一些较为简单的路径。
7. 最优控制理论：最优控制理论可以用来解决路径规划问题，将路径拟合问题转化为优化问题，并通过最小化代价函数来找到最优路径。这种方法在自动驾驶、机器人导航等领域有广泛应用。

以上算法各有特点，选择合适的方法取决于具体的应用场景、路径形状和性能要求。在实际应用中，通常会结合多种算法来完成路径拟合任务。

二次多项式拟合例程

#include <iostream>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>

using namespace cv;

// 多项式拟合函数
Eigen::VectorXd polynomialFit(const Eigen::VectorXd& x, const Eigen::VectorXd& y, int degree) {
    int n = x.size();
    Eigen::MatrixXd A(n, degree + 1);

    // 构建系数矩阵 A
    for (int i = 0; i < n; ++i) {
        for (int j = 0; j <= degree; ++j) {
            A(i, j) = pow(x(i), j);
        }
    }

    // 使用最小二乘法求解系数
    return A.householderQr().solve(y);
}

int main() {
    // 构造示例数据
    Eigen::VectorXd x(5);
    Eigen::VectorXd y(5);
    x << 0, 1, 2, 3, 4;
    y << 0, 1, 4, 9, 16;

    // 进行二次多项式拟合
    int degree = 2;
    Eigen::VectorXd coeffs = polynomialFit(x, y, degree);

    // 打印拟合的多项式系数
    std::cout << "拟合的多项式系数：" << coeffs.transpose() << std::endl;

    // 绘制拟合的曲线和原始数据
    Mat img(300, 300, CV_8UC3, Scalar(255, 255, 255));
    for (int i = 0; i < img.cols/5; ++i) {
        double y_fit = 0;
        for (int j = 0; j <= degree; ++j) {
            y_fit += coeffs(j) * pow(i, j);
        }
        Point pt(i*5, y_fit*5);
        circle(img, pt, 1, Scalar(0, 0, 255), FILLED); // 绘制拟合的曲线，使用红色
    }

    for (int i = 0; i < x.size(); ++i) {
        Point pt(x(i)*5, y(i)*5); // 缩放原始数据，以便在图像中显示
        circle(img, pt, 3, Scalar(0, 255, 0), FILLED); // 绘制原始数据，使用绿色
    }

    // 显示结果
    imshow("Polynomial Fit with Original Data", img);
    waitKey(0);

    return 0;
}

上面的方法如果是在仿真中太麻烦，可以用一个特殊函数代替路径如sin(t)函数直接生成轨迹，并可以通过求导的方法计算yaw值。

sin(t)

#include <iostream>
#include <cmath>

double curvature(double t) {
    // 计算 sin(t) 关于 t 的一阶导数
    double dx_dt = cos(t);
    
    // 计算 sin(t) 关于 t 的二阶导数
    double d2x_dt2 = -sin(t);
    
    // 计算曲率
    double curvature = std::abs(d2x_dt2) / pow(1 + pow(dx_dt, 2), 1.5);
    
    return curvature;
}

double slopeAngle(double t) {
    // 计算 sin(t) 关于 t 的一阶导数
    double dx_dt = cos(t);
    
    // 计算斜率的角度（以弧度为单位）
    double angle = atan2(dx_dt);
    
    return angle;
}

int main() {
    // 设定时刻 t0
    double t0 = 0.5; // 例如，这里设定 t0 为 0.5
    
    // 计算曲率
    double curv = curvature(t0);
    
    // 计算斜率角度
    double angle = slopeAngle(t0);
    
    // 输出结果
    std::cout << "sin(" << t0 << ") 函数在时刻 t=" << t0 << " 的曲率为: " << curv << std::endl;
    std::cout << "sin(" << t0 << ") 函数在时刻 t=" << t0 << " 的斜率角度为: " << angle << " 弧度" << std::endl;
    
    return 0;
}