Apollo6.0代码Lattice算法详解——Part3:笛卡尔坐标转Frenet坐标

0.前置知识

        想要了解笛卡尔坐标系转Frenet坐标系中公式的推导过程,可以看一下之前写过的博客(不理解公式推导过程也不影响对代码的理解)<<Frenet坐标系的推导>>.

1.涉及主要函数

        1) 函数名: ComputeInitFrenetState(const PathPoint& matched_point,const TrajectoryPoint& cartesian_state, std::array<double, 3>* ptr_s, std::array<double, 3>* ptr_d)
            函数位置: modules/planning/planner/lattice/lattice_planner.cc
            函数作用: 跟据匹配点,计算Frenet坐标系的S-L值
        2) 函数名: cartesian_to_frenet(const double rs, const double rx, const double ry, const double rtheta, const double rkappa, const double rdkappa, const double x, const double y, const double v, const double a, const double theta, const double kappa, std::array<double, 3>* const ptr_s_condition, std::array<double, 3>* const ptr_d_condition)
            函数位置: modules/common/math/cartesian_frenet_conversion.cc
            函数作用: 笛卡尔转frenet

2.函数关系

3.部分函数代码详解

    3.1 lattice_planner.cc中代码部分

  // 跟据匹配点,计算Frenet坐标系的S-L值
  // 3. according to the matched point, compute the init state in Frenet frame.
  std::array<double, 3> init_s;
  std::array<double, 3> init_d;
  ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);

  ADEBUG << "ReferenceLine and Frenet Conversion Time = "
         << (Clock::NowInSeconds() - current_time) * 1000;
  current_time = Clock::NowInSeconds();

    3.2 函数-cartesian_to_frenet

            代码部分:

void CartesianFrenetConverter::cartesian_to_frenet(
    const double rs, const double rx, const double ry, const double rtheta,
    const double rkappa, const double rdkappa, const double x, const double y,
    const double v, const double a, const double theta, const double kappa,
    std::array<double, 3>* const ptr_s_condition,
    std::array<double, 3>* const ptr_d_condition) {
  const double dx = x - rx;
  const double dy = y - ry;

  const double cos_theta_r = std::cos(rtheta);
  const double sin_theta_r = std::sin(rtheta);

  // 叉乘
  const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
  // l的方向和大小
  ptr_d_condition->at(0) =
      std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

  const double delta_theta = theta - rtheta;
  const double tan_delta_theta = std::tan(delta_theta);
  const double cos_delta_theta = std::cos(delta_theta);

  // 1-kl
  const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);
  // l' = (1-kl)tan(delta_theta)
  ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;

  // (k'l+kl')
  const double kappa_r_d_prime =
      rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);

  // l" = -(k'l+kl')tan(delta_theta) + (1-kl)/cos(delta_theta)^2 * [(1-kl)kx/cos(delta_theta)-kr]
  ptr_d_condition->at(2) =
      -kappa_r_d_prime * tan_delta_theta +
      one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
          (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);

  // s
  ptr_s_condition->at(0) = rs;

  // ds/dt = vx*cos(delta_theta)/(1-kl)
  ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;

  // (1-kl)*kx/cos(delta_theta)-kr
  const double delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
  // d(ds/dt)/dt = [ ax*cos(delta_theta)-(ds/dt)^2[ l'*[(1-kl)*kx/cos(delta_theta)-kr] - (k'l+kl')]/(1-kl)
  ptr_s_condition->at(2) =
      (a * cos_delta_theta -
       ptr_s_condition->at(1) * ptr_s_condition->at(1) *
           (ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /
      one_minus_kappa_r_d;
}

4.参考资料

         1.参考视频：Lattice.