Apollo項目導航模式下的座標轉換研究

嚴正聲明：本文系作者davidhopper原創，未經許可，不得轉載。

Apollo項目導航模式下，規劃模塊輸出的軌跡點使用FLU車身座標系（見我的另一篇博客《Apollo項目座標系研究》），在進行當前幀規劃前，需要將前一幀未行駛完軌跡點的車身座標轉換爲當前幀的車身座標，並在其中找到最爲匹配的點，作爲當前幀的規劃起點；若在指定的誤差範圍內找不到匹配點，則以當前車輛位置作爲新的規劃起點。該過程涉及到兩套FLU車身座標系的變換。本文首先圖解介紹座標變換的公式，然後給出Apollo項目的具體變換代碼。

一、座標變換公式

1.1 問題描述

如下圖所示，$XOY$是ENU全局座標系，$X_{old}O_{old}Y_{old}$與$X_{new}O_{new}Y_{new}$是FLU車身座標系。已知座標原點$O_{old}$ 在座標系$XOY$中的座標爲$(x_{01}, y_{01}, \theta_{1})$，$O_{ new}$ 在座標系$XOY$中的座標爲$(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);
                });
}

分析代碼可知，其中的座標變換代碼與第一部分推導的公式吻合。