Apollo项目导航模式下的坐标转换研究_本文首先图解介绍坐标变换的公式,然后给出apollo项目的具体变换代码。-优快云博客

本文链接：https://blog.youkuaiyun.com/davidhopper/article/details/100104377

严正声明：本文系作者davidhopper原创，未经许可，不得转载。

Apollo项目导航模式下，规划模块输出的轨迹点使用FLU车身坐标系（见我的另一篇博客《Apollo项目坐标系研究》），在进行当前帧规划前，需要将前一帧未行驶完轨迹点的车身坐标转换为当前帧的车身坐标，并在其中找到最为匹配的点，作为当前帧的规划起点；若在指定的误差范围内找不到匹配点，则以当前车辆位置作为新的规划起点。该过程涉及到两套FLU车身坐标系的变换。本文首先图解介绍坐标变换的公式，然后给出Apollo项目的具体变换代码。

一、坐标变换公式

1.1 问题描述

如下图所示， $X O Y$ 是ENU全局坐标系， $X_{old}O_{old}Y_{old}$ 与 $X_{new}O_{new}Y_{new}$ 是FLU车身坐标系。已知坐标原点 $O_{old}$ 在坐标系 $X O Y$ 中的坐标为 $(x_{01}, y_{01}, \theta_{1})$ ， $O_{ new}$ 在坐标系 $X O Y$ 中的坐标为 $(x_{02}, y_{02}, \theta_{2})$ 。 $P$ 点在前一帧车身坐标系 $X_{old}O_{old}Y_{old}$ 中的坐标为 $(x_{old}, y_{old}, \theta_{old})$ ，求解 $P$ 点在当前帧车身坐标系 $X_{ new}O_{new}Y_{new}$ 中的坐标 $(x_{new}, y_{new}, \theta_{new})$ 。

1.2 公式推导

如下图所示，当前帧坐标原点 $O_{ new}$ 在前一帧车身坐标系 $X_{old}O_{old}Y_{old}$ 中的坐标 $(x_d, y_d, \theta_d)$ 可通过下述表达式计算：
$x_d=O_{old}E+EF=O_{old}E+DC=(x_{02}-x_{01})cos \theta_{1}+(y_{02}-y_{01})sin\theta_{1}\qquad(1)$
$y_d=O_{ new}C-FC=O_{new}C-ED=(y_{02}-y_{01})cos \theta_{1}-(x_{02}-x_{01})sin\theta_{1}\qquad(2)$
$\theta_d=\theta_2-\theta_1\qquad(3)$

如下图所示， $P$ 点在当前帧车身坐标系 $X_{ new}O_{new}Y_{new}$ 中的坐标 $(x_{new}, y_{new}, \theta_{new})$ 可通过下述表达式计算：
$x_{new}=O_{new}J+JK=O_{new}J+IH=(x_{old}-x_{d})cos \theta_{d}+(y_{old}-y_{d})sin\theta_{d}\qquad(4)$
$y_{new}=PH-KH=PH-JI=(y_{old}-y_{d})cos \theta_{d}-(x_{old}-x_{d})sin\theta_{d}\qquad(5)$
$\theta_{new}=\theta_{old}-\theta_{d}\qquad(6)$

二、坐标变换代码

坐标变换代码见modules/planning/navi_planning.cc中的NaviPlanning::RunOnce函数，具体代码如下：

void NaviPlanning::RunOnce(const LocalView& local_view,
                           ADCTrajectory* const trajectory_pb) {
  // ...
  auto vehicle_config =
      ComputeVehicleConfigFromLocalization(*local_view_.localization_estimate);

  if (last_vehicle_config_.is_valid_ && vehicle_config.is_valid_) {
    auto x_diff_map = vehicle_config.x_ - last_vehicle_config_.x_;
    auto y_diff_map = vehicle_config.y_ - last_vehicle_config_.y_;

    auto cos_map_veh = std::cos(last_vehicle_config_.theta_);
    auto sin_map_veh = std::sin(last_vehicle_config_.theta_);

    auto x_diff_veh = cos_map_veh * x_diff_map + sin_map_veh * y_diff_map;
    auto y_diff_veh = -sin_map_veh * x_diff_map + cos_map_veh * y_diff_map;

    auto theta_diff = vehicle_config.theta_ - last_vehicle_config_.theta_;

    TrajectoryStitcher::TransformLastPublishedTrajectory(
        x_diff_veh, y_diff_veh, theta_diff, last_publishable_trajectory_.get());
  }
  // ...
}

其中的NaviPlanning::ComputeVehicleConfigFromLocalization函数代码为：

NaviPlanning::VehicleConfig NaviPlanning::ComputeVehicleConfigFromLocalization(
    const localization::LocalizationEstimate& localization) const {
  NaviPlanning::VehicleConfig vehicle_config;

  if (!localization.pose().has_position()) {
    return vehicle_config;
  }

  vehicle_config.x_ = localization.pose().position().x();
  vehicle_config.y_ = localization.pose().position().y();

  const auto& orientation = localization.pose().orientation();

  if (localization.pose().has_heading()) {
    vehicle_config.theta_ = localization.pose().heading();
  } else {
    vehicle_config.theta_ = common::math::QuaternionToHeading(
        orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz());
  }

  vehicle_config.is_valid_ = true;
  return vehicle_config;
}

TrajectoryStitcher::TransformLastPublishedTrajectory函数位于文件modules/planning/common/trajectory_stitcher.cc中，代码如下：

void TrajectoryStitcher::TransformLastPublishedTrajectory(
    const double x_diff, const double y_diff, const double theta_diff,
    PublishableTrajectory* prev_trajectory) {
  if (!prev_trajectory) {
    return;
  }

  // R^-1
  double cos_theta = std::cos(theta_diff);
  double sin_theta = -std::sin(theta_diff);

  // -R^-1 * t
  auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
  auto ty = -(sin_theta * x_diff + cos_theta * y_diff);

  std::for_each(prev_trajectory->begin(), prev_trajectory->end(),
                [&cos_theta, &sin_theta, &tx, &ty,
                 &theta_diff](common::TrajectoryPoint& p) {
                  auto x = p.path_point().x();
                  auto y = p.path_point().y();
                  auto theta = p.path_point().theta();

                  auto x_new = cos_theta * x - sin_theta * y + tx;
                  auto y_new = sin_theta * x + cos_theta * y + ty;
                  auto theta_new =
                      common::math::NormalizeAngle(theta - theta_diff);

                  p.mutable_path_point()->set_x(x_new);
                  p.mutable_path_point()->set_y(y_new);
                  p.mutable_path_point()->set_theta(theta_new);
                });
}