LQR路径跟踪要求路径中带角度，即坐标（x,y,yaw），而一般我们的规划出来的路径不带角度。这里通过总结相关方法，并提供一个案例。
将点路径拟合成一条完整的线路径算法
将点路径拟合成一条完整的线路径是一个常见的问题，在计算机图形学、机器人导航、地图制作等领域都有广泛的应用。以下是一些常见的用于路径拟合的算法：
- 多项式拟合：使用多项式函数来逼近给定的数据点。常见的方法包括最小二乘法拟合线性、二次或更高次的多项式。多项式拟合简单直观，但对于复杂的路径可能不够灵活。
- 样条插值：样条插值通过在相邻数据点之间拟合局部小段来逼近数据。常见的样条插值包括三次样条插值，它保证在每个数据点处平滑连接，并具有良好的数学性质。
- 贝塞尔曲线：贝塞尔曲线是一种通过控制点来定义路径的方法。通过调整控制点的位置和权重，可以创建平滑的曲线。贝塞尔曲线在计算机图形学中被广泛应用。
- B样条曲线：B样条曲线是一种基于控制点和节点向量定义的曲线。它具有局部控制和局部性质，能够创建复杂的曲线并且有良好的数学性质。
- 最小二乘法拟合直线或曲线：最小二乘法可以用于拟合直线或曲线到数据点，通过最小化拟合曲线与数据点之间的误差来找到最佳拟合。可以使用直线、圆弧等简单几何形状来拟合路径。
- 分段线性拟合：将路径分成若干小段，分别用直线来拟合每一小段，从而逼近整体路径。这种方法简单直观，适用于一些较为简单的路径。
- 最优控制理论：最优控制理论可以用来解决路径规划问题，将路径拟合问题转化为优化问题，并通过最小化代价函数来找到最优路径。这种方法在自动驾驶、机器人导航等领域有广泛应用。
以上算法各有特点，选择合适的方法取决于具体的应用场景、路径形状和性能要求。在实际应用中，通常会结合多种算法来完成路径拟合任务。
二次多项式拟合例程
#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);// 构建系数矩阵 Afor (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() {// 设定时刻 t0double 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;
}
