Apoll——笛卡尔坐标系与Frenet坐标系转换

最新推荐文章于 2024-08-01 11:27:29 发布

Jack Ju

最新推荐文章于 2024-08-01 11:27:29 发布

阅读量764

点赞数 1

文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_40464599/article/details/132181948

版权

1.cartesian_to_frenet

在这里插入图片描述

  ComputeGivenFrenetState(matched_point, planning_init_point, &init_s, &init_d); // convert init_point to frenet frame
void CartesianFrenetConverter::cartesian_to_frenet(
    const float rs, const float rx, const float ry, const float rtheta,
    const float rkappa, const float rdkappa, const float x, const float y,
    const float v, const float a, const float theta, const float kappa,
    std::array<float, 3>* const ptr_s_condition,
    std::array<float, 3>* const ptr_d_condition) {
  const float dx = x - rx;
  const float dy = y - ry;

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

  const float cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
  ptr_d_condition->at(0) =
      std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

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

  const float one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);
  ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;

  const float kappa_r_d_prime =
      rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);

  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);

  ptr_s_condition->at(0) = rs;
  ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;

  const float delta_theta_prime =
      one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
  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;
}


//const float rs, const float rx, const float ry, const float rtheta,const float rkappa, const float rdkappa,表示匹配点
//const float x, const float y,const float v, const float a, const float theta, const float kappa,表示规划初始点