init_sl_state_ = reference_line.ToFrenetFrame(planning_start_point);

planning模块(9)-路径规划(二次规划) 上一篇介绍到了ToFrenetFrame函数,接下继续介绍笛卡尔坐标转换为frenet坐标的流程.

  common::SLPoint sl_point;
  XYToSL(traj_point.path_point().theta(),
         {traj_point.path_point().x(), traj_point.path_point().y()}, &sl_point);

bool ReferenceLine::XYToSL(const double heading,
                           const common::math::Vec2d& xy_point,
                           common::SLPoint* const sl_point,
                           double warm_start_s) const {
  double s = warm_start_s;
  double l = 0.0;
  if (warm_start_s < 0.0) {
    if (!map_path_.GetProjection(heading, xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path.";
      return false;
    }
  } else {
    if (!map_path_.GetProjectionWithWarmStartS(xy_point, &s, &l)) {
      AERROR << "Cannot get nearest point from path with warm_start_s: "
             << warm_start_s;
      return false;
    }
  }

  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

warm_start_s为默认参数-1

map_path_.GetProjection(heading, xy_point, &s, &l)

heading:规划起点的方向角

xy_point:规划起点笛卡尔坐标系的坐标数据

s/l:基于map_path_获取到规划起点投影点的横纵向距离

map_path_:可以理解为未平滑前的参考线(这里称为粗糙参考线)

map_path_.GetProjection函数

bool Path::GetProjection(const Vec2d& point, double* accumulate_s,
                         double* lateral, double* min_distance)

此流程use_path_approximation_为false

  *min_distance = std::numeric_limits<double>::infinity();
  int min_index = 0;
  for (int i = 0; i < num_segments_; ++i) {
    const double distance = segments_[i].DistanceSquareTo(point);
    if (distance < *min_distance) {
      min_index = i;
      *min_distance = distance;
    }
  }
  *min_distance = std::sqrt(*min_distance);

在粗糙参考线上距离最近的segment获取规划起点及最小横向距离

const auto prod = nearest_seg.ProductOntoUnit(point);

计算规划起点在segment(由两个路径点构成)的法向量方向上的投影

const auto proj = nearest_seg.ProjectOntoUnit(point);

计算规划起点在segment的切向量方向上的投影

if (min_index == 0) {
    *accumulate_s = std::min(proj, nearest_seg.length());
    if (proj < 0) {
      *lateral = prod;
    } else {
      *lateral = (prod > 0.0 ? 1 : -1) * *min_distance;
    }
  }

1.当segment是粗糙参考线的第一个segment时

纵向距离取segment切向量方向上的投影长度与segment长度取小值

如果proj < 0,说明规划起点在segment起点后,横向距离为垂直距离

否则,横向距离取min_distance,方向由法向量方向上的投影正负决定

2.当segment是粗糙参考线的倒数第二个segment时纵向距离取累积距离

如果proj <0 0,说明规划起点在segment起点后,取min_distance,方向由法向量方向上的投影正负决定否则,横向距离为垂直距离

3.落在中间segment

纵向距离取累积距离

横向距离取min_distance,方向由法向量方向上的投影正负决

ReferencePoint ref_point = GetReferencePoint(sl_point.s());

根据规划起点获取参考点数据,参考点数据依旧是根据粗糙参考线数据获取到的

  CartesianFrenetConverter::cartesian_to_frenet(
      sl_point.s(), ref_point.x(), ref_point.y(), ref_point.heading(),
      ref_point.kappa(), ref_point.dkappa(), traj_point.path_point().x(),
      traj_point.path_point().y(), traj_point.v(), traj_point.a(),
      traj_point.path_point().theta(), traj_point.path_point().kappa(),
      &s_condition, &l_condition);

sl_point.s():规划起点在粗糙参考线上纵向距离

ref_point.x():规划起点在粗糙参考线上获取到的参考点的笛卡尔横坐标

ref_point.y():规划起点在粗糙参考线上获取到的参考点的笛卡尔纵坐标

ref_point.heading():规划起点在粗糙参考线上获取到的参考点的方向角(笛卡尔坐标下)

ref_point.kappa():规划起点在粗糙参考线上获取到的参考点的曲率

ref_point.dkappa():规划起点在粗糙参考线上获取到的参考点的曲率变化率

traj_point.path_point().x():是自车位置的横坐标

traj_point.path_point().y():是自车位置的纵坐标

traj_point.v():是自车速度

traj_point.a():是自车加速度

traj_point.path_point().theta():是自车方向角

traj_point.path_point().kappa():自车自身的曲率,由角速度除以线速度得到

s_condition:中有三个元素分别是转换后的纵向距离,纵向速度,纵向加速度

l_condition:中有三个元素分别是转换后的横向距离,横向距离对s的导数,横向距离对s的二阶导数

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;

(dx, dy)是参考点指向自车位置的向量

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

(cos_theta_r, sin_theta_r)是在笛卡尔坐标系下,参考点切线方向上的单位向量

const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;

cross_rd_nd是参考点指向自车位置的向量a与参考点切线方向上的单位向量b的叉积

叉积还可以表示为如下式子,因为b是单位向量模为1,所以cross_rd_nd就是点(x, y)到参考点切线方向的有向的垂线距离,如果为正,表示点(x, y)在参考点切线方向的左侧,否则在参考点切线方向的右侧

这里主要就是用cross_rd_nd来判断自车在参考线的哪一侧

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

ptr_d_condition->at(0)存的就是自车到参考点的欧几里德距离,方向由cross_rd_nd来决定,也就是纵向距离.

推导过程如上,到这cartesian_to_frenet函数就介绍完了.