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);
// 构建系数矩阵 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;
}
