Apollo项目导航模式下的坐标转换研究

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

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

一、坐标变换公式

1.1 问题描述

如下图所示，XOYXOYXOY是ENU全局坐标系，XoldOoldYoldX_{old}O_{old}Y_{old}XoldOoldYold与XnewOnewYnewX_{new}O_{new}Y_{new}XnewOnewYnew是FLU车身坐标系。已知坐标原点OoldO_{old}Oold 在坐标系XOYXOYXOY中的坐标为(x01,y01,θ1)(x_{01}, y_{01}, \theta_{1})(x01,y01,θ1)，OnewO_{ new}Onew 在坐标系XOYXOYXOY中的坐标为(x02,y02,θ2)(x_{02}, y_{02}, \theta_{2})(x02,y02,θ2)。PPP点在前一帧车身坐标系XoldOoldYoldX_{old}O_{old}Y_{old}XoldOoldYold中的坐标为(xold,yold,θold)(x_{old}, y_{old}, \theta_{old})(xold,yold,θold)，求解PPP点在当前帧车身坐标系XnewOnewYnewX_{ new}O_{new}Y_{new}XnewOnewYnew中的坐标(xnew,ynew,θnew)(x_{new}, y_{new}, \theta_{new})(xnew,ynew,θnew)。

1.2 公式推导

如下图所示，当前帧坐标原点OnewO_{ new}Onew在前一帧车身坐标系XoldOoldYoldX_{old}O_{old}Y_{old}XoldOoldYold中的坐标(xd,yd,θd)(x_d, y_d, \theta_d)(xd,yd,θd)可通过下述表达式计算：
xd=OoldE+EF=OoldE+DC=(x02−x01)cosθ1+(y02−y01)sinθ1(1)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) xd=OoldE+EF=OoldE+DC=(x02−x01)cosθ1+(y02−y01)sinθ1(1)
yd=OnewC−FC=OnewC−ED=(y02−y01)cosθ1−(x02−x01)sinθ1(2)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) yd=OnewC−FC=OnewC−ED=(y02−y01)cosθ1−(x02−x01)sinθ1(2)
θd=θ2−θ1(3)\theta_d=\theta_2-\theta_1\qquad(3) θd=θ2−θ1(3)

如下图所示，PPP点在当前帧车身坐标系XnewOnewYnewX_{ new}O_{new}Y_{new}XnewOnewYnew中的坐标(xnew,ynew,θnew)(x_{new}, y_{new}, \theta_{new})(xnew,ynew,θnew)可通过下述表达式计算：
xnew=OnewJ+JK=OnewJ+IH=(xold−xd)cosθd+(yold−yd)sinθd(4)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) xnew=OnewJ+JK=OnewJ+IH=(xold−xd)cosθd+(yold−yd)sinθd(4)
ynew=PH−KH=PH−JI=(yold−yd)cosθd−(xold−xd)sinθd(5)y_{new}=PH-KH=PH-JI=(y_{old}-y_{d})cos \theta_{d}-(x_{old}-x_{d})sin\theta_{d}\qquad(5) ynew=PH−KH=PH−JI=(yold−yd)cosθd−(xold−xd)sinθd(5)
θnew=θold−θd(6)\theta_{new}=\theta_{old}-\theta_{d}\qquad(6) θnew=θold−θd(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^-1double cos_theta = std::cos(theta_diff);double sin_theta = -std::sin(theta_diff);// -R^-1 * tauto 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);});
}

分析代码可知，其中的坐标变换代码与第一部分推导的公式吻合。