Apollo6.0代码Lattice算法详解——Part2:寻找匹配点

• 0.前置知识
• 1.涉及主要函数
• 2.函数关系
• 3.函数代码详解
• •     3.1 lattice_planner.cc中代码部分
  •     3.2 函数-MatchToPath
  •     3.3 函数-FindProjectionPoint
  •     3.4 函数-InterpolateUsingLinearApproximation
  •     3.5 函数-slerp
  •     3.6 函数-NormalizeAngle
• 4.参考资料

0.前置知识

        要理解该部分代码需要了解一下protobuf中定义的TrajectoryPoint具体内容, 如下:

message TrajectoryPoint {
  // path point
  optional PathPoint path_point = 1;
  // linear velocity
  optional double v = 2;  // in [m/s]
  // linear acceleration
  optional double a = 3;
  // relative time from beginning of the trajectory
  optional double relative_time = 4;
  // longitudinal jerk
  optional double da = 5;
  // The angle between vehicle front wheel and vehicle longitudinal axis
  optional double steer = 6;

  // Gaussian probability information
  optional GaussianInfo gaussian_info = 7;
}

        代码位置: modules/common/proto/pnc_point.proto

1.涉及主要函数

        1) 函数名: MatchToPath(const std::vector< PathPoint >& reference_line, const double x, const double y)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 在参考线上计算匹配点
        2) 函数名: FindProjectionPoint(const PathPoint& p0,const PathPoint& p1, const double x,const double y)
            函数位置: modules/common/math/path_matcher.cc
            函数作用: 从自车当前点做投影到参考线上,找到参考线上精确的位置
        3) 函数名: InterpolateUsingLinearApproximation(const PathPoint &p0, const PathPoint &p1,const double s)
            函数位置: modules/common/math/linear_interpolation.cc
            函数作用: 使用线性插值的办法来计算投影得到的点在参考线上的坐标,heading等一系列值
        4) 函数名: slerp(const double a0, const double t0, const double a1, const double t1, const double t)
            函数位置: modules/common/math/linear_interpolation.cc
            函数作用: 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间
        5) 函数名: lerp(const T &x0, const double t0, const T &x1, const double t1, const double t)
            函数位置: modules/common/math/linear_interpolation.h
            函数作用: 线性差值
        6) 函数名: NormalizeAngle(const double angle)
            函数位置: modules/common/math/math_utils.cc
            函数作用: 对角度处理使得在[-pi,pi)之间

2.函数关系

3.函数代码详解

    3.1 lattice_planner.cc中代码部分

  // 在参考线上计算匹配点, 入参:参考线,起点x,起点y, 相当于说在参考线上找到一点,该点离我当前轨迹
  // 上车辆所在点的距离是最短的,即frenet公式推导中的 l 最短
  // 2. compute the matched point of the init planning point on the reference
  // line.
  PathPoint matched_point = PathMatcher::MatchToPath(
      *ptr_reference_line, planning_init_point.path_point().x(),
      planning_init_point.path_point().y());

    3.2 函数-MatchToPath

            代码部分:

// 在参考线上计算匹配点
PathPoint PathMatcher::MatchToPath(const std::vector<PathPoint>& reference_line,
                                   const double x, const double y) {
  CHECK_GT(reference_line.size(), 0);

  // 定义了一个内部的函数,用于计算距离
  auto func_distance_square = [](const PathPoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  // 首先计算了一下参考线的起点到traj上当前车辆位置的距离
  double distance_min = func_distance_square(reference_line.front(), x, y);
  std::size_t index_min = 0; // 不仅要知道大小,还要知道索引

  // 从index为1的点开始(因为index=0的点前面刚计算过),依次计算和车辆位置的距离
  for (std::size_t i = 1; i < reference_line.size(); ++i) {
    double distance_temp = func_distance_square(reference_line[i], x, y);
    // 找到参考线上距离自车当前位置最近的点的index
    if (distance_temp < distance_min) {
      distance_min = distance_temp;
      index_min = i;
    }
  } 
 
  // index_min == 0,意味着最近的点是referenceLine的起点,不等于0就 index_min - 1,
  std::size_t index_start = (index_min == 0) ? index_min : index_min - 1;
  // 判断index_min+1的值是否等于参考线的size,否的话 index_end=index_min+1
  std::size_t index_end =
      (index_min + 1 == reference_line.size()) ? index_min : index_min + 1;
  // 到这里找下来相当于在referenceLine上找了三个点,即参考线上距离车辆位置最近的点,以及这个点
  // 的前后各一个点,为什么要在前后各找一个点呢?因为他要做车辆当前的点 在referenceLine上的投影,
  // 这里会把index_start和index_end连起来,当作一条线(方向start指向end),供我们做投影,公式为:
  // 向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end) 

  if (index_start == index_end) {
    return reference_line[index_start];
  }

  // 做投影,向量A(index_start -> traj上的车辆当前位置) 点乘 向量B(index_start -> index_end)
  return FindProjectionPoint(reference_line[index_start],
                             reference_line[index_end], x, y);
}

    3.3 函数-FindProjectionPoint

            图解:

            代码部分:

PathPoint PathMatcher::FindProjectionPoint(const PathPoint& p0,
                                           const PathPoint& p1, const double x,
                                           const double y) {
  // 为什么投影用的 向量start->vehicle_position 在 向量start->end 上的投影而不用
  // 向量index_min->vehicle_position 在 向量start->end 上的投影呢?
  // 参考:https://www.bilibili.com/video/BV1rM4y1g71p?spm_id_from=333.999.0.0
  double v0x = x - p0.x();
  double v0y = y - p0.y();

  double v1x = p1.x() - p0.x();
  double v1y = p1.y() - p0.y();

  double v1_norm = std::sqrt(v1x * v1x + v1y * v1y);
  double dot = v0x * v1x + v0y * v1y;

  // v0 · v1 = ||v0||*||v1||*cos(theta)    theta为v0和v1的夹角
  // (v0 · v1) / ||v1|| = ||v0||*cos(theta)
  // ||v0||*cos(theta) 即为v0在v1上的投影
  double delta_s = dot / v1_norm;
  // 这个函数主要使用线性插值的办法来计算投影得到的点在参考线上的坐标,heading等一系列值,p0.s(),
  // 为从参考线的起点开始到这个点的距离,p0.s() + delta_s就是v0在参考线上的投影到参考线起点的距离
  return InterpolateUsingLinearApproximation(p0, p1, p0.s() + delta_s);
}

    3.4 函数-InterpolateUsingLinearApproximation

            代码部分:

PathPoint InterpolateUsingLinearApproximation(const PathPoint &p0,
                                              const PathPoint &p1,
                                              const double s) {
  double s0 = p0.s();
  double s1 = p1.s();

  PathPoint path_point;
  double weight = (s - s0) / (s1 - s0);  // 权重
  // 公式: x = p0.x + weight * (p1.x - p0.x),  下面他用的是合并同类项之后的,都一样
  double x = (1 - weight) * p0.x() + weight * p1.x();
  double y = (1 - weight) * p0.y() + weight * p1.y();
  // 核心还是用的线性插值,不过对角度做了处理,保证在[-pi,pi)之间
  double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s);
  double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa();
  double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa();
  double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa();
  path_point.set_x(x);
  path_point.set_y(y);
  path_point.set_theta(theta);
  path_point.set_kappa(kappa);
  path_point.set_dkappa(dkappa);
  path_point.set_ddkappa(ddkappa);
  path_point.set_s(s);
  return path_point;
}

    3.5 函数-slerp

            代码部分:

double slerp(const double a0, const double t0, const double a1, const double t1,
             const double t) {
  // start和end两个点距离太近
  if (std::abs(t1 - t0) <= kMathEpsilon) {
    ADEBUG << "input time difference is too small";
    return NormalizeAngle(a0);
  }
  // 对两个heading角处理使得在[-pi,pi)之间
  const double a0_n = NormalizeAngle(a0);
  const double a1_n = NormalizeAngle(a1);
  // 得到delta(heading)
  double d = a1_n - a0_n;
  if (d > M_PI) {
    d = d - 2 * M_PI;
  } else if (d < -M_PI) {
    d = d + 2 * M_PI;
  }

  const double r = (t - t0) / (t1 - t0);
  const double a = a0_n + d * r;
  return NormalizeAngle(a);
}

    3.6 函数-NormalizeAngle

            代码部分:

double NormalizeAngle(const double angle) {
  // 这一步取模的操作可以保证结果在(-2pi,2pi)范围内
  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
  // 这个保证结果在(0,2pi)范围内
  if (a < 0.0) {
    a += (2.0 * M_PI);
  }
  // 这个保证结果在(-pi,pi)范围内
  return a - M_PI;
}

4.参考资料

         1.参考视频：Lattice.
         2.参考文章 : Lattice Planner从学习到放弃（一）.额不…到实践.