Apollo RTK定位简介

Apollo RTK定位原理简介

RTK定位原理

RTK ( Real - time kinematic，实时动态)载波相位差分技术，工作原理如下：

• 卫星把观测数据给基站，也给车端的移动站；
• 基站根据多个卫星的钟差计算出误差项，然后把误差项传递给车端；
• 车端用这个误差项消除观测误差，得到精准的位置。

Apollo RTK定位

该模块是基于GPS和IMU融合后的信息再进行RTK校准，可以得到一个厘米级误差的定位信息，定位信息包含航向角、速度、加速度、经纬度等等。
输入：
GPS-全球定位系统获取的位置相关数据。

header {
  timestamp_sec: 1173545122.22
}
localization {
  position {  //位置
    x: 357.51331791372041
    y: 96.165912376788725
    z: -31.983237908221781
  }
  orientation {  //方向
    qx: 0.024015498296453403
    qy: 0.0021656820647661572
    qz: -0.99072964388722151
    qw: -0.13369120534226134
  }
  linear_velocity {  //线速度
    x: -1.5626866011382312
    y: -5.9852188341040344
    z: 0.024798037277423912
  }
  angular_velocity {  //角速度
    x: 0
    y: 0
    z: 0
  }
}

IMU-惯性测量单元获取的速度相关数据。

header {
  timestamp_sec: 1173545122.2
}
imu {
  linear_acceleration { //线性加速度
    x: -0.41125867975442232
    y: 0.052264811302594541
    z: 0.44301818085493777
  }
  angular_velocity {  //角速度
    x: 0.0079919345972956756
    y: -0.037232397859742021
    z: -0.0075406644688951127
  }
}

输出：
车辆的位置信息（Planning模块使用）
车辆的姿态，速度信息（Control模块使用）

header {
  timestamp_sec: 1173545122.22
  module_name: "localization"
  sequence_num: 128
}
pose {
  position {  //位置
    x: 357.51331791372041
    y: 96.165912376788725
    z: -31.983237908221781
  }
  orientation {  //方向
    qx: 0.024015498296453403
    qy: 0.0021656820647661572
    qz: -0.99072964388722151
    qw: -0.13369120534226134
  }
  linear_velocity { //线速度
    x: -1.5626866011382312
    y: -5.9852188341040344
    z: 0.024798037277423912
  }
  linear_acceleration {  //线性加速度
    x: -0.68775567663756187
    y: -0.079383290718229638
    z: -0.43889982872693695
  }
  angular_velocity {  //角速度
    x: 0.0035469168217162248
    y: -0.039127693784838637
    z: -0.0079623083093763921
  }
  heading: -1.8388082455104939  //航向
}

RTK定位源码解析

目录结构

核心代码解析

rtk_localization_component.cc
RTK定位模块在"Init"中初始化，每当接收到"localization::Gps"消息就触发执行"Proc"函数。

  bool Init() override;
  bool Proc(const std::shared_ptr<localization::Gps> &gps_msg) override;

rtk_localization.cc
RTK定位模块的核心实现，例如寻找匹配的IMU、IMU线性插值、GPS和IMU数据融合等等。

  void InitConfig(const rtk_config::Config &config);
  void GpsCallback(const std::shared_ptr<localization::Gps> &gps_msg);
  void GpsStatusCallback(
      const std::shared_ptr<drivers::gnss::InsStat> &status_msg);
  void ImuCallback(const std::shared_ptr<localization::CorrectedImu> &imu_msg);

  bool IsServiceStarted();
  void GetLocalization(LocalizationEstimate *localization);
  void GetLocalizationStatus(LocalizationStatus *localization_status);

模块初始化

RTK定位模块初始化

bool RTKLocalizationComponent::Init() {
  tf2_broadcaster_.reset(new apollo::transform::TransformBroadcaster(node_));
  if (!InitConfig()) {
    AERROR << "Init Config falseed.";
    return false;
  }

  if (!InitIO()) {
    AERROR << "Init Interval falseed.";
    return false;
  }
  // get imu to localizaiton transform
  if (!GetLocalizationToImuTF()) {
    AERROR << "Get IMU to Localization tranform failed.";
    return false;
  }
  return true;
}

初始化IO

bool RTKLocalizationComponent::InitIO() {
   // 1.读取IMU信息，每次接收到localization::CorrectedImu消息，则回调执行“RTKLocalization::ImuCallback”
  corrected_imu_listener_ = node_->CreateReader<localization::CorrectedImu>(
      imu_topic_, std::bind(&RTKLocalization::ImuCallback, localization_.get(),
                            std::placeholders::_1));
  ACHECK(corrected_imu_listener_);

// 2.读取GPS状态信息，每次接收到GPS状态消息，则回调执行
  gps_status_listener_ = node_->CreateReader<drivers::gnss::InsStat>(
      gps_status_topic_, std::bind(&RTKLocalization::GpsStatusCallback,
                                   localization_.get(), std::placeholders::_1));
  ACHECK(gps_status_listener_);
// 3.发布位置信息和位置状态信息
  localization_talker_ =
      node_->CreateWriter<LocalizationEstimate>(localization_topic_);
  ACHECK(localization_talker_);
  localization_status_talker_ =
      node_->CreateWriter<LocalizationStatus>(localization_status_topic_);
  ACHECK(localization_status_talker_);
  return true;
}

Proc

bool RTKLocalizationComponent::Proc(
    const std::shared_ptr<localization::Gps>& gps_msg) {
      // 1. 通过RTKLocalization处理GPS消息回调
  localization_->GpsCallback(gps_msg);

  if (localization_->IsServiceStarted()) {
    LocalizationEstimate localization;
    // 2. 获取定位消息
    localization_->GetLocalization(&localization);
    LocalizationStatus localization_status;
    // 3. 获取定位状态
    localization_->GetLocalizationStatus(&localization_status);
    // set localization pose at rear axle center
    CompensateImuLocalizationExtrinsic(&localization);
    // publish localization messages
    // 4. 发布位置信息
    PublishPoseBroadcastTopic(localization);
    // 5. 发布位置转换信息
    PublishPoseBroadcastTF(localization);
    // 6. 发布位置状态信息
    PublishLocalizationStatus(localization_status);
    ADEBUG << "[OnTimer]: Localization message publish success!";
  }
  return true;
}

GpsCallback
GPS回调函数中调用了 PrepareLocalizationMsg。

void RTKLocalization::PrepareLocalizationMsg(
    const localization::Gps &gps_msg, LocalizationEstimate *localization,
    LocalizationStatus *localization_status) {
  // find the matching gps and imu message
  double gps_time_stamp = gps_msg.header().timestamp_sec();
  CorrectedImu imu_msg;
  // 1.寻找最匹配的IMU信息  
  FindMatchingIMU(gps_time_stamp, &imu_msg);
   // 2.根据GPS和IMU信息，给位置信息赋值
  ComposeLocalizationMsg(gps_msg, imu_msg, localization);
  drivers::gnss::InsStat gps_status;
  // 3.查找最近的GPS状态信息
  FindNearestGpsStatus(gps_time_stamp, &gps_status);
  // 4.根据GPS状态信息，给位置状态信息赋值
  FillLocalizationStatusMsg(gps_status, localization_status);
}

线性插值

FindMatchingIMU:寻找最匹配的IMU信息
在队列中找到最匹配的IMU消息，其中区分了队列的第一个，最后一个，以及如果在中间位置则进行插值。插值的时候根据距离最近的原则进行反比例插值。

bool RTKLocalization::FindMatchingIMU(const double gps_timestamp_sec,
                                      CorrectedImu *imu_msg) {
  if (imu_msg == nullptr) {
    AERROR << "imu_msg should NOT be nullptr.";
    return false;
  }

  std::unique_lock<std::mutex> lock(imu_list_mutex_);
  auto imu_list = imu_list_;
  lock.unlock();

  if (imu_list.empty()) {
    AERROR << "Cannot find Matching IMU. "
           << "IMU message Queue is empty! GPS timestamp[" << gps_timestamp_sec
           << "]";
    return false;
  }

  // scan imu buffer, find first imu message that is newer than the given
  // timestamp
  auto imu_it = imu_list.begin();
  for (; imu_it != imu_list.end(); ++imu_it) {
    if ((*imu_it).header().timestamp_sec() - gps_timestamp_sec >
        std::numeric_limits<double>::min()) {
      break;
    }
  }

  if (imu_it != imu_list.end()) {  // found one
    if (imu_it == imu_list.begin()) {
      AERROR << "IMU queue too short or request too old. "
             << "Oldest timestamp[" << imu_list.front().header().timestamp_sec()
             << "], Newest timestamp["
             << imu_list.back().header().timestamp_sec() << "], GPS timestamp["
             << gps_timestamp_sec << "]";
      *imu_msg = imu_list.front();  // the oldest imu
    } else {
      // here is the normal case
      auto imu_it_1 = imu_it;
      imu_it_1--;
      if (!(*imu_it).has_header() || !(*imu_it_1).has_header()) {
        AERROR << "imu1 and imu_it_1 must both have header.";
        return false;
      }
      if (!InterpolateIMU(*imu_it_1, *imu_it, gps_timestamp_sec, imu_msg)) {
        AERROR << "failed to interpolate IMU";
        return false;
      }
    }
  } else {
    // give the newest imu, without extrapolation
    *imu_msg = imu_list.back();
    if (imu_msg == nullptr) {
      AERROR << "Fail to get latest observed imu_msg.";
      return false;
    }

    if (!imu_msg->has_header()) {
      AERROR << "imu_msg must have header.";
      return false;
    }

    if (std::fabs(imu_msg->header().timestamp_sec() - gps_timestamp_sec) >
        gps_imu_time_diff_threshold_) {
      // 20ms threshold to report error
      AERROR << "Cannot find Matching IMU. IMU messages too old. "
             << "Newest timestamp[" << imu_list.back().header().timestamp_sec()
             << "], GPS timestamp[" << gps_timestamp_sec << "]";
    }
  }

  return true;
}

InterpolateIMU：IMU线性插值
根据上述函数得到两个IMU消息分别对角速度、线性加速度、欧拉角进行插值。原则是根据比例，反比例进行插值。

bool RTKLocalization::InterpolateIMU(const CorrectedImu &imu1,
                                     const CorrectedImu &imu2,
                                     const double timestamp_sec,
                                     CorrectedImu *imu_msg) {
  if (!(imu1.header().has_timestamp_sec() &&
        imu2.header().has_timestamp_sec())) {
    AERROR << "imu1 and imu2 has no header or no timestamp_sec in header";
    return false;
  }
  if (timestamp_sec < imu1.header().timestamp_sec()) {
    AERROR << "[InterpolateIMU1]: the given time stamp["
           << FORMAT_TIMESTAMP(timestamp_sec)
           << "] is older than the 1st message["
           << FORMAT_TIMESTAMP(imu1.header().timestamp_sec()) << "]";
    *imu_msg = imu1;
  } else if (timestamp_sec > imu2.header().timestamp_sec()) {
    AERROR << "[InterpolateIMU2]: the given time stamp[" << timestamp_sec
           << "] is newer than the 2nd message["
           << imu2.header().timestamp_sec() << "]";
    *imu_msg = imu2;
  } else {
    *imu_msg = imu1;
    imu_msg->mutable_header()->set_timestamp_sec(timestamp_sec);

    double time_diff =
        imu2.header().timestamp_sec() - imu1.header().timestamp_sec();
    if (fabs(time_diff) >= 0.001) {
      double frac1 =
          (timestamp_sec - imu1.header().timestamp_sec()) / time_diff;

      if (imu1.imu().has_angular_velocity() &&
          imu2.imu().has_angular_velocity()) {
        auto val = InterpolateXYZ(imu1.imu().angular_velocity(),
                                  imu2.imu().angular_velocity(), frac1);
        imu_msg->mutable_imu()->mutable_angular_velocity()->CopyFrom(val);
      }

      if (imu1.imu().has_linear_acceleration() &&
          imu2.imu().has_linear_acceleration()) {
        auto val = InterpolateXYZ(imu1.imu().linear_acceleration(),
                                  imu2.imu().linear_acceleration(), frac1);
        imu_msg->mutable_imu()->mutable_linear_acceleration()->CopyFrom(val);
      }

      if (imu1.imu().has_euler_angles() && imu2.imu().has_euler_angles()) {
        auto val = InterpolateXYZ(imu1.imu().euler_angles(),
                                  imu2.imu().euler_angles(), frac1);
        imu_msg->mutable_imu()->mutable_euler_angles()->CopyFrom(val);
      }
    }
  }
  return true;
}

InterpolateXYZ
根据距离插值，反比例，即Frac1越小，则越靠近P1，Frac1越大，则越靠近P2。

template <class T>
T RTKLocalization::InterpolateXYZ(const T &p1, const T &p2,
                                  const double frac1) {
  T p;
  double frac2 = 1.0 - frac1;
  if (p1.has_x() && !std::isnan(p1.x()) && p2.has_x() && !std::isnan(p2.x())) {
    p.set_x(p1.x() * frac2 + p2.x() * frac1);
  }
  if (p1.has_y() && !std::isnan(p1.y()) && p2.has_y() && !std::isnan(p2.y())) {
    p.set_y(p1.y() * frac2 + p2.y() * frac1);
  }
  if (p1.has_z() && !std::isnan(p1.z()) && p2.has_z() && !std::isnan(p2.z())) {
    p.set_z(p1.z() * frac2 + p2.z() * frac1);
  }
  return p;
}

填充位置信息

ComposeLocalizationMsg
根据GPS和IMU消息对位置信息进行赋值。

void RTKLocalization::ComposeLocalizationMsg(
    const localization::Gps &gps_msg, const localization::CorrectedImu &imu_msg,
    LocalizationEstimate *localization) {
  localization->Clear();

  FillLocalizationMsgHeader(localization);

  localization->set_measurement_time(gps_msg.header().timestamp_sec());

  // combine gps and imu
  auto mutable_pose = localization->mutable_pose();
  if (gps_msg.has_localization()) {
    const auto &pose = gps_msg.localization();

    if (pose.has_position()) {
      // position
      // world frame -> map frame
      mutable_pose->mutable_position()->set_x(pose.position().x() -
                                              map_offset_[0]);
      mutable_pose->mutable_position()->set_y(pose.position().y() -
                                              map_offset_[1]);
      mutable_pose->mutable_position()->set_z(pose.position().z() -
                                              map_offset_[2]);
    }

    // orientation
    if (pose.has_orientation()) {
      mutable_pose->mutable_orientation()->CopyFrom(pose.orientation());
      double heading = common::math::QuaternionToHeading(
          pose.orientation().qw(), pose.orientation().qx(),
          pose.orientation().qy(), pose.orientation().qz());
      mutable_pose->set_heading(heading);
    }
    // linear velocity
    if (pose.has_linear_velocity()) {
      mutable_pose->mutable_linear_velocity()->CopyFrom(pose.linear_velocity());
    }
  }

  if (imu_msg.has_imu()) {
    const auto &imu = imu_msg.imu();
    // linear acceleration
    if (imu.has_linear_acceleration()) {
      if (localization->pose().has_orientation()) {
        // linear_acceleration:
        // convert from vehicle reference to map reference
        Vector3d orig(imu.linear_acceleration().x(),
                      imu.linear_acceleration().y(),
                      imu.linear_acceleration().z());
        Vector3d vec = common::math::QuaternionRotate(
            localization->pose().orientation(), orig);
        mutable_pose->mutable_linear_acceleration()->set_x(vec[0]);
        mutable_pose->mutable_linear_acceleration()->set_y(vec[1]);
        mutable_pose->mutable_linear_acceleration()->set_z(vec[2]);

        // linear_acceleration_vfr
        mutable_pose->mutable_linear_acceleration_vrf()->CopyFrom(
            imu.linear_acceleration());
      } else {
        AERROR << "[PrepareLocalizationMsg]: "
               << "fail to convert linear_acceleration";
      }
    }

    // angular velocity
    if (imu.has_angular_velocity()) {
      if (localization->pose().has_orientation()) {
        // angular_velocity:
        // convert from vehicle reference to map reference
        Vector3d orig(imu.angular_velocity().x(), imu.angular_velocity().y(),
                      imu.angular_velocity().z());
        Vector3d vec = common::math::QuaternionRotate(
            localization->pose().orientation(), orig);
        mutable_pose->mutable_angular_velocity()->set_x(vec[0]);
        mutable_pose->mutable_angular_velocity()->set_y(vec[1]);
        mutable_pose->mutable_angular_velocity()->set_z(vec[2]);

        // angular_velocity_vf
        mutable_pose->mutable_angular_velocity_vrf()->CopyFrom(
            imu.angular_velocity());
      } else {
        AERROR << "[PrepareLocalizationMsg]: fail to convert angular_velocity";
      }
    }

    // euler angle
    if (imu.has_euler_angles()) {
      mutable_pose->mutable_euler_angles()->CopyFrom(imu.euler_angles());
    }
  }
}

查找最近的GPS状态信息

获取最近的GPS状态信息，这里实现的算法是遍历查找离"gps_time_stamp"最近的状态。

bool RTKLocalization::FindNearestGpsStatus(const double gps_timestamp_sec,
                                           drivers::gnss::InsStat *status) {
  CHECK_NOTNULL(status);

  std::unique_lock<std::mutex> lock(gps_status_list_mutex_);
  auto gps_status_list = gps_status_list_;
  lock.unlock();

  double timestamp_diff_sec = 1e8;
  auto nearest_itr = gps_status_list.end();
  for (auto itr = gps_status_list.begin(); itr != gps_status_list.end();
       ++itr) {
    double diff = std::abs(itr->header().timestamp_sec() - gps_timestamp_sec);
    if (diff < timestamp_diff_sec) {
      timestamp_diff_sec = diff;
      nearest_itr = itr;
    }
  }

  if (nearest_itr == gps_status_list.end()) {
    return false;
  }

  if (timestamp_diff_sec > gps_status_time_diff_threshold_) {
    return false;
  }

  *status = *nearest_itr;
  return true;
}

位置状态赋值

FillLocalizationStatusMsg
void RTKLocalization::FillLocalizationStatusMsg(
    const drivers::gnss::InsStat &status,
    LocalizationStatus *localization_status) {
  apollo::common::Header *header = localization_status->mutable_header();
  double timestamp = apollo::cyber::Clock::NowInSeconds();
  header->set_timestamp_sec(timestamp);
  localization_status->set_measurement_time(status.header().timestamp_sec());

  if (!status.has_pos_type()) {
    localization_status->set_fusion_status(MeasureState::ERROR);
    localization_status->set_state_message(
        "Error: Current Localization Status Is Missing.");
    return;
  }

参考资料

1、RTK原理相关：https://developer.baidu.com/article/detail.html?id=290583
2、解算相关：https://www.zhihu.com/column/c_1114864226103037952
3、插值相关：https://zhuanlan.zhihu.com/p/108853312
4、源码解读1：https://apollo.baidu.com/community/article/15
5、源码解读2：https://www.findmb.top/posts/37997/