开发者说丨Apollo项目导航模式下的坐标转换研究
Apollo项目导航模式下，规划模块输出的轨迹点使用FLU车身坐标系（见博客《Apollo项目坐标系研究》），在进行当前帧规划前，需要将前一帧未行驶完轨迹点的车身坐标转换为当前帧的车身坐标，并在其中找到最为匹配的点，作为当前帧的规划起点；若在指定的误差范围内找不到匹配点，则以当前车辆位置作为新的规划起点。该过程涉及到两套FLU车身坐标系的变换。本文首先图解介绍坐标变换的公式，然后给出Apollo项目的具体变换代码。
本文由社区荣誉布道师——贺志国撰写，对Apollo项目导航模式下的坐标转换研究进行了详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。
以下，ENJOY
如下图所示，XOY是ENU全局坐标系，
如下图所示，当前帧坐标原点
如下图所示，P点在当前帧车身坐标系
坐标变换代码见modules/planning/navi_planning.cc中的NaviPlanning::RunOnce函数，具体代码如下：
1void NaviPlanning::RunOnce(const LocalView& local_view,
2 ADCTrajectory* const trajectory_pb) {
3 // ...
4 auto vehicle_config =
5 ComputeVehicleConfigFromLocalization(*local_view_.localization_estimate);
6
7 if (last_vehicle_config_.is_valid_ && vehicle_config.is_valid_) {
8 auto x_diff_map = vehicle_config.x_ - last_vehicle_config_.x_;
9 auto y_diff_map = vehicle_config.y_ - last_vehicle_config_.y_;
10
11 auto cos_map_veh = std::cos(last_vehicle_config_.theta_);
12 auto sin_map_veh = std::sin(last_vehicle_config_.theta_);
13
14 auto x_diff_veh = cos_map_veh * x_diff_map + sin_map_veh * y_diff_map;
15 auto y_diff_veh = -sin_map_veh * x_diff_map + cos_map_veh * y_diff_map;
16
17 auto theta_diff = vehicle_config.theta_ - last_vehicle_config_.theta_;
18
19 TrajectoryStitcher::TransformLastPublishedTrajectory(
20 x_diff_veh, y_diff_veh, theta_diff, last_publishable_trajectory_.get());
21 }
22 // ...
23}<左右滑动以查看完整代码>
其中的
NaviPlanning::ComputeVehicleConfigFromLocalization函数代码为：
1NaviPlanning::VehicleConfig NaviPlanning::ComputeVehicleConfigFromLocalization(
2 const localization::LocalizationEstimate& localization) const {
3 NaviPlanning::VehicleConfig vehicle_config;
4
5 if (!localization.pose().has_position()) {
6 return vehicle_config;
7 }
8
9 vehicle_config.x_ = localization.pose().position().x();
10 vehicle_config.y_ = localization.pose().position().y();
11
12 const auto& orientation = localization.pose().orientation();
13
14 if (localization.pose().has_heading()) {
15 vehicle_config.theta_ = localization.pose().heading();
16 } else {
17 vehicle_config.theta_ = common::math::QuaternionToHeading(
18 orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz());
19 }
20
21 vehicle_config.is_valid_ = true;
22 return vehicle_config;
23}
<左右滑动以查看完整代码>
TrajectoryStitcher::TransformLastPublishedTrajectory函数位于文件modules/planning/common/trajectory_stitcher.cc中，代码如下：
1void TrajectoryStitcher::TransformLastPublishedTrajectory(
2 const double x_diff, const double y_diff, const double theta_diff,
3 PublishableTrajectory* prev_trajectory) {
4 if (!prev_trajectory) {
5 return;
6 }
7
8 // R^-1
9 double cos_theta = std::cos(theta_diff);
10 double sin_theta = -std::sin(theta_diff);
11
12 // -R^-1 * t
13 auto tx = -(cos_theta * x_diff - sin_theta * y_diff);
14 auto ty = -(sin_theta * x_diff + cos_theta * y_diff);
15
16 std::for_each(prev_trajectory->begin(), prev_trajectory->end(),
17 [&cos_theta, &sin_theta, &tx, &ty,
18 &theta_diff](common::TrajectoryPoint& p) {
19 auto x = p.path_point().x();
20 auto y = p.path_point().y();
21 auto theta = p.path_point().theta();
22
23 auto x_new = cos_theta * x - sin_theta * y + tx;
24 auto y_new = sin_theta * x + cos_theta * y + ty;
25 auto theta_new =
26 common::math::NormalizeAngle(theta - theta_diff);
27
28 p.mutable_path_point()->set_x(x_new);
29 p.mutable_path_point()->set_y(y_new);
30 p.mutable_path_point()->set_theta(theta_new);
31 });
32}<左右滑动以查看完整代码>
分析代码可知，其中的坐标变换代码与第一部分推导的公式吻合。
*《Apollo项目坐标系研究》
【https://blog.csdn.net/davidhopper/article/details/79162385】
更多话题讨论、技术交流
可以添加『Apollo小哥哥』为好友
进开发者交流群
* 以上内容为开发者原创，不代表百度官方言论。
内容来自开发者CSDN：
https://blog.csdn.net/davidhopper/article/details/100104377，欢迎大家收藏点赞。已获开发者授权，原文地址请戳阅读原文。