差速机器人模型LQR 控制仿真——路径模拟

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,