Apollo 项⽬导航模式下的坐标转换研究
严正声明：本⽂系作者davidhopper原创，未经许可，不得转载。
Apollo项⽬导航模式下，规划模块输出的轨迹点使⽤FLU车⾝坐标系（见我的另⼀篇博客《》），在进⾏当前帧规划前，需要将前⼀帧未⾏驶完轨迹点的车⾝坐标转换为当前帧的车⾝坐标，并在其中找到最为匹配的点，作为当前帧的规划起点；若在指定的误差范围内找不到匹配点，则以当前车辆位置作为新的规划起点。该过程涉及到两套FLU车⾝坐标系的变换。本⽂⾸先图解介绍坐标变换的公式，然后给出Apollo 项⽬的具体变换代码。
⼀、坐标变换公式
1.1 问题描述
如下图所⽰，是ENU全局坐标系，与是FLU车⾝坐标系。已知坐标原点 在坐标系中的坐标为， 在坐标系中的坐标为。点在前⼀帧车⾝坐标系中的坐标为，求解点在当前帧车⾝坐标系中的坐标
。
1.2 公式推导
如下图所⽰，当前帧坐标原点在前⼀帧车⾝坐标系中的坐标论语译注
可通过下述表达式计算：
XOY X O Y old old old X O Y new new new O old XOY (x ,y ,θ)01011O new XOY (x ,y ,θ)02022P X O Y old old old (x ,y ,θ)old old old P X O Y new new new (x ,y ,θ)new new new O new X O Y old old old (x ,y ,θ)d d d x =d O E +old EF =O E +old DC =(x −02x )cos θ+011(y −02y )sin θ(1)
011y =d O C −new FC =O C −new ED =(y −02y )cos θ−011(x −02x )sin θ(2)
011θ=d θ−2θ(3)
1
如下图所⽰，点在当前帧车⾝坐标系中的坐标可通过下述表达式计算：
P X O Y new new new (x ,y ,θ)new new new x =new O J +new JK =O J +new IH =(x −old x )cos θ+d d (y −old y )sin θ(4)d d y =new PH −KH =PH −JI =(y −old y )cos θ−d d (x −old x )sin θ(5)
d d
θ=new θ−old θ(6)d
⼆、坐标变换代码
坐标变换代码见modules/planning/中的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());
爱情公寓5评价
}
// ...
}
其中的NaviPlanning::ComputeVehicleConfigFromLocalization函数代码为：
NaviPlanning::VehicleConfig NaviPlanning::ComputeVehicleConfigFromLocalization(
const localization::LocalizationEstimate& localization)const{
NaviPlanning::VehicleConfig vehicle_config;
if(!localization.po().has_position()){
return vehicle_config;
}
vehicle_config.x_ = localization.po().position().x();
vehicle_config.y_ = localization.po().position().y();
const auto& orientation = localization.po().orientation();
if(localization.po().has_heading()){
vehicle_config.theta_ = localization.po().heading();
}el{
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/中，代码如下：
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()->t_x(x_new);
p.mutable_path_point()->t_y(y_new);
p.mutable_path_point()->t_theta(theta_new);
});
}
分析代码可知，其中的坐标变换代码与第⼀部分推导的公式吻合。
