【Apollo源码分析】系列的第五部分【localization】

来源：互联网发布：阿里云 docker服务器编辑：程序博客网时间：2024/06/09 00:48

【Apollo源码分析】系列的第五部分【localization】

localization 是车辆定位模块，是自动驾驶的核心模块之一。
目前Apollo的定位模块大概有1000行代码。

localization的输入:

GPS全球信号
IMU惯性测量单元信号

localization的输出:

由localization/proto/localization.proto文件描述的LocalizationEstimate定位信息

目前Apollo 1.0采用的是 RTK-GPS定位法。

RTK-GPS定位法：

RTK（Real - time kinematic）载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。这是一种新的常用的GPS测量方法，以前的静态、快速静态、动态测量都需要事后进行解算才能获得厘米级的精度，而RTK是能够在野外实时得到厘米级定位精度的测量方法，它采用了载波相位动态实时差分方法，是GPS应用的重大里程碑，它的出现为工程放样、地形测图，各种控制测量带来了新曙光，极大地提高了外业作业效率。
高精度的GPS测量必须采用载波相位观测值，RTK定位技术就是基于载波相位观测值的实时动态定位技术，它能够实时地提供测站点在指定坐标系中的三维定位结果，并达到厘米级精度。在RTK作业模式下，基准站通过数据链将其观测值和测站坐标信息一起传送给流动站。流动站不仅通过数据链接收来自基准站的数据，还要采集GPS观测数据，并在系统内组成差分观测值进行实时处理，同时给出厘米级定位结果，历时不足一秒钟。流动站可处于静止状态，也可处于运动状态；可在固定点上先进行初始化后再进入动态作业，也可在动态条件下直接开机，并在动态环境下完成整周模糊度的搜索求解。在整周未知数解固定后，即可进行每个历元的实时处理，只要能保持四颗以上卫星相位观测值的跟踪和必要的几何图形，则流动站可随时给出厘米级定位结果。

GPS-RTK 至少有一个基准站接收机，一个流动站接收机。

基准站的接收机有两个任务：

1）接收卫星信号
2）把自己的观测量通过发送给流动站

流动站有三个任务：

1）接收卫星信号
2）接收基准站所发送的基准站观测量
3）对卫星信号与基准站观测量进行对比处理，从而对自己的观测量进行修正

关于RTK-GPS可以参阅：
[1]https://en.wikipedia.org/wiki/Real_Time_Kinematic
[2]https://baike.baidu.com/item/RTK
[3]https://www.zhihu.com/question/24471733/answer/28676022
[4]http://www.sbsm.gov.cn/article/zszygx/chzs/chkp/ddcl/200912/20091200059052.shtml

localization/main.cc

照例先看看 localization文件夹下面最重要的文件main.cc 写了什么：

#include "modules/common/apollo_app.h"
#include "modules/localization/localization.h"
APOLLO_MAIN(::apollo::localization::Localization)

结合common/apollo_app.h文件，上述代码一看便知是用于注册localization模块，并运行本模块。而本文件没有重写ApolloApp类就不能实例化，那么只能是localization.h文件重写了ApolloApp类。

localization/localization.h

class Localization : public apollo::common::ApolloApp
果然，Localization类继承了ApolloApp类，并重写了父类的纯虚函数接口。

重写的接口有:

std::string Name() const override;
模块名称

apollo::common::Status Init() override;
初始化步骤

apollo::common::Status Start() override;
模块执行内容

void Stop() override;
模块清理内容

以上全都是public访问权限。

本模块独有的 private函数有:

void RegisterLocalizationMethods();
工厂函数。注册RTK定位实例和CAMERA定位实例。

再看看localization.cc.cc文件源码：

std::string Localization::Name() const {
  return FLAGS_localization_module_name;//模块名称
}
void Localization::RegisterLocalizationMethods() {
  localization_factory_.Register(//工厂函数。注册RTK定位方法的实例。
      LocalizationConfig::RTK,
      []() -> LocalizationBase* { return new RTKLocalization(); });
  localization_factory_.Register(
      LocalizationConfig::CAMERA,//工厂函数。注册CAMERA定位方法实例。
      []() -> LocalizationBase* { return new CameraLocalization(); });
}
Status Localization::Init() { //初始化操作，包括获取配置文件信息+注册定位方法实例
  RegisterLocalizationMethods();
  if (!apollo::common::util::GetProtoFromFile(FLAGS_localization_config_file,
                                              &config_)) {
    AERROR << "failed to load localization config file "
           << FLAGS_localization_config_file;
    return Status(ErrorCode::LOCALIZATION_ERROR,
                  "failed to load localization config file: " +
                      FLAGS_localization_config_file);
  }
  return Status::OK();
}
Status Localization::Start() { //开始本模块的工作内容
  localization_ =
      localization_factory_.CreateObject(config_.localization_type());//注册定位方法工厂信息
  if (!localization_) {
    return Status(ErrorCode::LOCALIZATION_ERROR,
                  "localization is not initialized with config : " +
                      config_.DebugString());
  }
  localization_->Start(); //开始定位
  return Status::OK();
}
void Localization::Stop() {}

localization/localization.h先分析到这里。先分析其他然后再回过头来总结整理。

localization/common/localization_gflags.h

.h文件和.cc文件声明和定义了一系列localization模块会用到的string/int32/bool/double变量。概览如下：

localization_gflags.h：

#include "gflags/gflags.h"
DECLARE_string(localization_module_name); //声明变量
DECLARE_double(localization_publish_freq);//声明变量
DECLARE_string(localization_config_file); //声明变量
以下略...

localization_gflags.cc:


DEFINE_string(localization_module_name, "localization",
              "localization module name"); 
              //定义变量
DEFINE_double(localization_publish_freq, 100,
              "localization publishing frequency.");
              //定义变量
DEFINE_string(rtk_adapter_config_file,
              "modules/localization/conf/rtk_adapter.conf",
              "rtk adapter configuration");
              //定义变量
以下略...

整个模块需要用到的预定义变量还是很多的。上面只是概览。

下面分析具体的定位算法，包括RTK-GPS定位算法，基于图像的定位算法，IMU惯性测量单元定位算法。

首先分析定位算法基类class LocalizationBase 。

modules/localization/localization_base.h

LocalizationBase是定位算法的基类。其他的任何定位算法都继承自这个类。
包括 RTKLocalization和CameraLocalization都继承自这个类。

该类提供定位算法的公有抽象接口。
2个public成员函数，纯虚函数，子类必须重写这2个接口才能实例化。

class LocalizationBase {
 public:
  virtual ~LocalizationBase() = default;//虚基类应该有虚析构函数
  /**
   * @brief module start function
   * @return start status
   本模块执行任务，返回状态码
   */
  virtual apollo::common::Status Start() = 0;
  /**
   * @brief module stop function
   * @return stop status
   清理工作
   */
  virtual apollo::common::Status Stop() = 0;
};

基类比较简单。下面分析具体的定位算法。

camera.proto文件定义了camera相机image。


message Camera {
  optional apollo.common.Header header = 1;
  // The image from camera
  optional bytes image = 2;
  // The image width
  optional uint32 width = 3;
  // The image height
  optional uint32 height = 4;
}

camera_parameter.proto定义了相机内参和外参

//内参。
message CameraIntrinsicParameter {
  // focus x pixels
  optional double fx = 1; // pixels
  // focus y pixels
  optional double fy = 2; // pixels
  // center x pixels
  optional double cx = 3; // pixels
  // center y pixels
  optional double cy = 4; // pixels
}
//外参
message CameraExtrinsicParameter {
  // camera rotation parameters [-pi - pi]
  optional double roll = 1;
  optional double pitch = 2;
  optional double yaw = 3;
  // coordinates in the vehicle coordinates [meters]
  optional double tx = 4; // meters
  optional double ty = 5; // meters
  optional double tz = 6; // meters
}
message CameraParameter {
  optional CameraIntrinsicParameter intrisic_parameter = 1;
  optional CameraExtrinsicParameter extrisic_parameter = 2;
}

localization/camera/camera_localization.h

class RTKLocalization : public LocalizationBase

CameraLocalization类继承自LocalizationBase，重写了Start()和Stop() 2个函数。
该类主要是利用摄像机image进行定位算法。
补充】基于相机的定位算法主要是基于视觉传感器摄像机(简称VO)和惯性测量单元的 visual inertial odometry（简称VIO）由于现在Apollo也没有具体的算法，暂时不表。更多相关知识可以搜索vslam。

【CameraLocalization】类 继承自【LocalizationBase】，重写了Start()和Stop() 2个函数。
该类主要是利用摄像机进行定位算法。但是目前Apollo的1.0版本还没有用具体的相机定位算法。可能后续会补充。
目前只是添加了这个功能原型，搭建了消息收发的接口。但是目前没有实用。
 */
class CameraLocalization : public LocalizationBase {
 public:
  CameraLocalization();
  virtual ~CameraLocalization() = default;
  /**
   * @brief module start function
   * @return start status
   初始化配置，并执行本类的定位算法
   */
  apollo::common::Status Start() override;
  /**
   * @brief module stop function
   * @return stop status
  清理工作
   */
  apollo::common::Status Stop() override;
 private:
  void OnTimer(const ros::TimerEvent &event);//数据到达，进行一次定位算法
  bool PublishLocalization();//发布gps和camera二者的综合定位结果
  void RunWatchDog();//检查时间合理性，是否延迟过大。
  bool PrepareLocalizationMsg(LocalizationEstimate *localization);
  //该函数尚未实现。应该是利用camera产生定位结果
  //根据gps_msg和camera_msg，得到综合的定位结果
  bool CreateLocalizationMsg(const ::apollo::localization::Gps &gps_msg,
                             const ::apollo::localization::Camera &camera_msg,
                             LocalizationEstimate *localization);
 private:
  ros::Timer timer_;  //ros系统的时间
  //监视本模块工作状态，以便于发送给ros topic。
  apollo::common::monitor::Monitor monitor_; 
  CameraParameter camera_parameter_; //相机参数，包括内参和外参。啥是内参,啥是外参？google一下 ^_^。
  double last_received_timestamp_sec_ = 0.0;//最近接收data的时间
  double last_reported_timestamp_sec_ = 0.0;//最近发布定位的时间
  bool use_imu_ = false;                    //融合imu
  bool service_started_ = false;            //开始工作
};

localization/rtk/rtk_localization.h

Real_Time_Kinematic，即差分GPS技术：将一台GPS接收机安置在基准站上进行观测。根据基准站已知精密坐标，计算出基准站到卫星的距离改正数，并由基准站实时将这一数据发送出去。用户接收机在进行GPS观测的同时，也接收到基准站发出的改正数，并对其定位结果进行改正，从而提高定位精度。

目前Apollo的1.0主要依靠RTK-GPS定位算法。其他算法目前尚为实现。
关于RTK-GPS可以参阅https://en.wikipedia.org/wiki/Real_Time_Kinematic

优点:精度能达到厘米级。
缺点：要安装基站，成本高，在没有基站的户外就不适用了。

RTKLocalization类继承自LocalizationBase，重写了Start()和Stop() 2个函数。

class RTKLocalization : public LocalizationBase {
 public:
  RTKLocalization();
  virtual ~RTKLocalization() = default;
  /**
   * @brief module start function
   * @return start status
   */
  apollo::common::Status Start() override;
  /**
   * @brief module stop function
   * @return stop status
   */
  apollo::common::Status Stop() override;
 private:
  void OnTimer(const ros::TimerEvent &event);//数据到达，进行一次定位算法
  void PublishLocalization();//发布 定位结果
  void RunWatchDog();//检查时间合理性，是否延迟过大。
  void PrepareLocalizationMsg(LocalizationEstimate *localization);
  ///计算gps的定位结果
  void ComposeLocalizationMsg(const ::apollo::localization::Gps &gps,
                              const ::apollo::localization::Imu &imu,
                              LocalizationEstimate *localization);
  ///将gps和imu做数据融合。
  ///根据时间戳找到大于该时间戳的第一个imu数据
  bool FindMatchingIMU(const double gps_timestamp_sec, Imu *imu_msg);
  ///对imu数据进行插值计算，得到给定timestamp_sec的值
  void InterpolateIMU(const Imu &imu1, const Imu &imu2,
                      const double timestamp_sec, Imu *msgbuf);
  template <class T>
  T InterpolateXYZ(const T &p1, const T &p2, const double &frac1);
 private:
  ros::Timer timer_;//ros系统的时间
  //监视本模块工作状态，以便于发送给ros topic。
  apollo::common::monitor::Monitor monitor_;
  const std::vector<double> map_offset_;//在地图上的偏移量
  double last_received_timestamp_sec_ = 0.0;//最近接收data的时间
  double last_reported_timestamp_sec_ = 0.0;//最近发布定位的时间
  bool service_started_ = false; //开始工作

rtk_localization_test.cc

理解RTK-GPS的定位算法最好还是通过test测量代码理解。并且还有定位实例：

TEST_F(RTKLocalizationTest, InterpolateIMU) {
  // timestamp inbetween + time_diff is big enough(>0.001), interpolate
  {
    //加载imu1测量结果
    apollo::localization::Imu imu1;
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    //加载imu2测量结果
    apollo::localization::Imu imu2;
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    //期望结果
    apollo::localization::Imu expected_result;
    load_data("modules/localization/testdata/1_imu_result.pb.txt",
              &expected_result);
    //2和exp完全一样。
    apollo::localization::Imu imu;
    double timestamp = 1173545122.69;
    ///imu返回的是timestamp处的插值结果。
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    ///插值结果就是2
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
  }
  // timestamp inbetween + time_diff is too small(<0.001), no interpolate
  {
    ///1和exp在时间上不一致，其余一致。
    apollo::localization::Imu imu1;
    load_data("modules/localization/testdata/2_imu_1.pb.txt", &imu1);
    ///imu2与imu1时间戳很小，则直接使用imu1的数据。
    apollo::localization::Imu imu2;
    load_data("modules/localization/testdata/2_imu_2.pb.txt", &imu2);
    apollo::localization::Imu expected_result;
    load_data("modules/localization/testdata/2_imu_result.pb.txt",
              &expected_result);
    apollo::localization::Imu imu;
    double timestamp = 1173545122.2001;
    //返回在timestamp处的插值结果。
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    ///不插值，直接使用imu1的数据
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
  }
  // timestamp < imu1.timestamp
  {
    apollo::localization::Imu imu1;
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    apollo::localization::Imu imu2;
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    apollo::localization::Imu expected_result;
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &expected_result);
    apollo::localization::Imu imu;
    double timestamp = 1173545122;
    //timestamp小于imu1的时间戳，则直接使用imu1的数据，不插值
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
  }
  // timestamp > imu2.timestamp
  {
    apollo::localization::Imu imu1;
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    apollo::localization::Imu imu2;
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    apollo::localization::Imu expected_result;
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &expected_result);
    apollo::localization::Imu imu;
    double timestamp = 1173545122.70;
    //timestamp大于imu2的时间戳，则直接使用imu1的数据。
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
  }
}


TEST_F(RTKLocalizationTest, ComposeLocalizationMsg) {
  // FLAGS_enable_map_reference_unify: false
  {
    FLAGS_enable_map_reference_unify = false;
    apollo::localization::Gps gps;
    load_data("modules/localization/testdata/3_gps_1.pb.txt", &gps);
    apollo::localization::Imu imu;
    load_data("modules/localization/testdata/3_imu_1.pb.txt", &imu);
    apollo::localization::LocalizationEstimate expected_result;
    load_data("modules/localization/testdata/3_localization_result_1.pb.txt",
              &expected_result);
    apollo::localization::LocalizationEstimate localization;
    //使用GPS和imu对定位结果进行数据融合。
    rtk_localizatoin_->ComposeLocalizationMsg(gps, imu, &localization);
    EXPECT_EQ(1, localization.header().sequence_num());
    EXPECT_STREQ("localization", localization.header().module_name().c_str());
    EXPECT_STREQ(expected_result.pose().DebugString().c_str(),
                 localization.pose().DebugString().c_str());
  }

总结

localization模块主要定义了使用RTK-GPS和camera相结合的定位方法，并且融合了imu传感器。但是基于图像的定位方法尚未发布。目前Apollo主要采用RKT-GPS并辅助imu用于车辆定位。总的来说，目前还有很大的改进空间，但是Apollo的定位框架目前已搭建完毕，能run起来，精度+普适廉价性+可靠性+多传感器融合算法应该是未来改进的地方。每一个方向都值得深究。

本文首发于微信公众号slamcode。

注释版源码：源码

阅读全文

0 0