Cuda加速横纵轨迹合并_cuda 进行轨迹规划-CSDN博客

本文链接：https://blog.csdn.net/yfc1996/article/details/141302699

1、需求来源

轨迹规划的三板斧：采样、搜索和优化。其中采样的方法存在一个弊端，随着横纵轨迹采样数量增多，轨迹长度的增加，轨迹点数非常容易来到 $10^5$ ~ $10^6$ 这个量级。在主流的车载运算平台上，仅靠CPU，面对这样的数据量，很难在较短时间内完成横纵向轨迹的合并。因此，在不提升CPU性能和缩减采样数量的前提下，如何降低轨迹合并的耗时成为工程师需要考虑的问题。

2、Cuda内核设计

1.目标：
对横纵向轨迹进行合并，并从frenet坐标系规划的轨迹转换到map坐标系下
2.计算思路：
纵向曲线进行等时间间隔采样可以得到std::vectorsample_t，再根据sample_t获取纵向曲线中的s,ds,dds以及横向曲线中的l,dl,ddl。上面这个6个变量就组成了FrenetState，从FrenetState到map_point的转换需要reference_line中一个关键的函数GetPoint(rs)，对reference_line进行等间距采样获得一个检索表可以加速GetPoint(rs)的计算。
3.输入设计：
横、纵向路径集合而不是点集合（FrenetState），这样的才能保证内存拷贝进显存和显存拷贝进内存不会因为轨迹采样的数量而爆炸。

struct alignas(16) LatCurve{
    float l;
    float dl;
    float ddl;
    int n;
};
 
struct alignas(16) LonCurve{
    float s;
    float ds;
    float dds;
    int n;
};
 
struct alignas(16) ReferenceLine{
    float x;
    float y;
    float s_m;
    float theta_rad;
    float kappa_pm;
    float dkappa_pmm;
    float heading;    //sample from reference_line.heading   
};
 
float dt; //dt for lon_boundle
 
sizet LatBoundles;
 
sizet LonBoundles;
 
float ds; //ds for reference_line
 
sizet reference_line_point_nums;

4.输出设计
输出为std::vector，根据横、纵向路径组合会有一个编号std::pair<size_t, size_t>，另外由于是等间距采样，横纵组合点数也是固定的，这样就方便记录一组轨迹的起index和终index，方便最后从cuda中轨迹的提取。

struct alignas(16) BasicTrajectoryPoint {
  float x = 0.0;
  float y = 0.0;
  float heading = 0.0;
  float ref_s = 0.0;  // corresponding s on reference line
  float v = 0.0;
  float a = 0.0;
  float course = 0.0;
  float kappa = 0.0;  // curvature, i.e. station derivative of course angle
};

5.计算步骤
Step1: 初始化函数中，申请一块足够大的显存（可以按照10000条轨迹，每条轨迹100个点申请）
Step2: 在核函数中根据等时间间隔获取所有纵向曲线的，s,ds,dds，并生成出s对应的所有横向曲线的l,dl和ddl，以组成FrenetState
Step3: 在每个线程中进行FrenetStateToCartesian计算
Step4: 输出计算结果，cuda外进行轨迹点的组合