代码--矩形形式的b样条曲线

哎嗨人生公众号

已于 2024-07-10 14:51:43 修改

阅读量278

点赞数 9

文章标签：自动驾驶算法

于 2024-07-10 14:46:42 首次发布

本文链接：https://blog.csdn.net/qq_17405041/article/details/140323624

版权

在轨迹规划中，b样条曲线是非常常用的一种曲线。话不多说，先上代码，看效果。
这里我准备了两个工程，一个是c++版本，一个是python网页版，分别为
在这里插入图片描述

一、python网页版

项目包含一个前端页面用于用户交互和可视化，以及一个后端服务用于计算B样条曲线。前端页面将运行在 http://localhost:8000

在这里插入图片描述

项目工程如下图：

在这里插入图片描述

二、c++版本

在这里插入图片描述

项目如下：
在这里插入图片描述

矩阵形式的b样条，比递推形式的b样条简洁的多，实现起来非常容易，代码量也很少，如下为局部代码：

#include "bspline_curve.h"
#include "math/math_common.h"
namespace ahrs {
BsplineCurve::BsplineCurve(const double& interval)
    : interval_(interval), ctp_size_(0) {}

BsplineCurve::~BsplineCurve() {}

std::vector<Point> BsplineCurve::GenerateCurve() {
  std::vector<Point> trajectory;
  for (size_t i = 0; i < ctp_size_ - 3; ++i) {
    for (double j = 0; j < 1.0; j += interval_) {
      Point p = GetPos(i, j);
      trajectory.push_back(p);
    }
  }

  return trajectory;
}

void BsplineCurve::SetControlPoints(const std::vector<Vec2d>& control_points) {
  ctp_ = control_points;
  ctp_size_ = ctp_.size();
}

Point BsplineCurve::GetPos(const size_t& k, const double& ti) {
  // 定义 B 样条基函数矩阵
  MatrixXd m(4, 4);
  m << -1, 3, -3, 1, 3, -6, 3, 0, -3, 0, 3, 0, 1, 4, 1, 0;

  // 定义时间矢量 t 和其一阶、二阶、三阶导数
  MatrixXd t(1, 4);
  MatrixXd dt(1, 4);
  MatrixXd ddt(1, 4);
  MatrixXd dddt(1, 4);