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

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

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

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

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

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

二次多项式拟合例程

cpp 复制代码
#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)

cpp 复制代码
#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;
}
相关推荐
xyliiiiiL11 分钟前
ZGC初步了解
java·jvm·算法
爱的叹息40 分钟前
RedisTemplate 的 6 个可配置序列化器属性对比
算法·哈希算法
独好紫罗兰1 小时前
洛谷题单2-P5713 【深基3.例5】洛谷团队系统-python-流程图重构
开发语言·python·算法
每次的天空2 小时前
Android学习总结之算法篇四(字符串)
android·学习·算法
请来次降维打击!!!2 小时前
优选算法系列(5.位运算)
java·前端·c++·算法
qystca2 小时前
蓝桥云客 刷题统计
算法·模拟
别NULL3 小时前
机试题——统计最少媒体包发送源个数
c++·算法·媒体
weisian1513 小时前
Java常用工具算法-3--加密算法2--非对称加密算法(RSA常用,ECC,DSA)
java·开发语言·算法
程序员黄同学4 小时前
贪心算法,其优缺点是什么?
算法·贪心算法
仙人掌_lz5 小时前
机器学习ML极简指南
人工智能·python·算法·机器学习·面试·强化学习