autoware planning trajectory_smoother 模块解读

Posted 2021-10-20 怪皮蛇皮怪

autoware planning trajectory_smoother 模块解读

• 说明
• 总结
• • 代码解读
  • • 函数 TrajectorySmoother
    • 函数 Filter

说明

博主仅凭代码本身进行理解，并未对轨迹平滑，速度平滑，曲线优化等部分有任何理解（毕竟菜鸡，写这个也只是为了留个档）
如本文说明有任何问题欢迎评论提醒指点，但拒绝指指指指指指点点点点点点

总结

模块代码
该模块的功能是对已经提供出来的路径（含有速度信息的路径），进行速度上的平滑

如图

速度原本是黑色的折线，不连续，通过trajectory_smoother变成平滑的速度曲线（画的有亿点难看233）

代码解读

函数 TrajectorySmoother

该函数作用是通过指数函数生成一张权重表

TrajectorySmoother::TrajectorySmoother(TrajectorySmootherConfig config)
{
	// 参数s 的作用是作为除数，具体用在后面的x上面
  float32_t s = 2 * config.standard_deviation * config.standard_deviation;
  
  float32_t sum = 0.0;
	
  // Generate a gaussian filter kernel
  // 这里的kernel_size应该可以理解为n，即预先定义了每个点的速度由周围n个点影响，n越大整个速度曲线会越平滑
  #i从1到kernel_size就有点像是1/n,2/n,.....,(n-1)/n枚举每个点位置的权重
  for (std::size_t i = 0; i < config.kernel_size; ++i) {
  	// x是计算当前点到n/2的距离，即权重在点n/2的时候取最小（在这里是0）（但通过后面指数函数分析其实这里反而是最大的），向两边扩散时权重以平方的量级扩大
    int32_t x = static_cast<int32_t>(i - config.kernel_size / 2);
    
    // 这里就是通过指数函数的方式，利用上面制作出来的平方权重表，制造一个指数函数的权重表（什么鬼形容啊。。。）
    float32_t value = std::exp(static_cast<float32_t>(-(x * x)) / s);
    m_kernel.push_back(value);
    sum += value;
  }

  // Normalize the kernel
  //对权重表进行归一化
  for (std::size_t i = 0; i < config.kernel_size; ++i) {
    m_kernel[i] /= sum;
  }
}

总的来说这个函数最终会生成一张长度为kernel_size的权重表，其中所有权重之和为1

函数 Filter

该函数是通过预先计算好的权重表，对现有路径的速度信息进行平滑处理

void TrajectorySmoother::Filter(Trajectory & trajectory)
{
  if (trajectory.points.size() > 2) {
    // zero out velocity at a few points at the end of trajectory so that the post filter velocity
    // gradually ramp down to zero. The last point would have already been zeroed by the
    // estimator.
    
    //zero_run_length是速度为0的路径长度
    //即路径规划到终点的时候车应该停在终点。
    //但显然如果只设置终点的速度为0，而终点前的点速度是99999999的话
    // 车不可能停在终点，所以设置了一个zero_run_length参数
    //旨在通过一段zero_run_length长度的速度为0的路径点，让车停下来
    
    std::size_t zero_run_length = std::min(trajectory.points.size() / 2, m_kernel.size() / 2);
    for (std::size_t i = trajectory.points.size() - 1 - zero_run_length;
      i < trajectory.points.size() - 1; ++i)
    {
      trajectory.points[i].longitudinal_velocity_mps = 0;
    }

	/*
	这部分就是通过计算每一个点以及他前后kernel_size/2的点的加权和
	重新计算路径在当前点的速度



*/

    // avoid changing the start and end point of trajectory
    // use same padding for points beyond either end of the trajectory
    std::vector<float32_t> velocity_profile{};
    for (std::size_t i = 1; i < trajectory.points.size() - 1; ++i) {
      float32_t sum = 0;
      for (std::size_t j = 0; j < m_kernel.size(); ++j) {
        std::int32_t points_index = static_cast<int32_t>(i - (m_kernel.size() / 2) + j);
        if (points_index < 0) {
          points_index = 0;
        } else if (points_index >= static_cast<int32_t>(trajectory.points.size())) {
          points_index = static_cast<int32_t>(trajectory.points.size() - 1);
        }
        sum +=
          trajectory.points[static_cast<std::size_t>(points_index)].longitudinal_velocity_mps *
          m_kernel[j];
      }

      velocity_profile.push_back(sum);
    }
	
	//最后再对速度进行重新赋值回去，同时不改变初始点以及终点的速度
	
    // Apply the velocity profile
    for (std::size_t i = 1; i < trajectory.points.size() - 1; ++i) {
      trajectory.points[i].longitudinal_velocity_mps = velocity_profile[i - 1];
    }
  }
}