【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 cameraoptional bytes image = 2;// The image widthoptional uint32 width = 3;// The image heightoptional uint32 height = 4;}
camera_parameter.proto定义了相机内参和外参
//内参。message CameraIntrinsicParameter {// focus x pixelsoptional double fx = 1; // pixels// focus y pixelsoptional double fy = 2; // pixels// center x pixelsoptional double cx = 3; // pixels// center y pixelsoptional 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; // metersoptional double ty = 5; // metersoptional 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; //融合imubool 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);///插值结果就是2EXPECT_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。
注释版源码:源码
Apollo的定位模块(localization)是其核心组件之一,主要依赖RTK-GPS技术实现厘米级精度定位。输入包括GPS和IMU信号,输出为LocalizationEstimate定位信息。RTK-GPS基于基准站和流动站的载波相位差分,通过数据链实时传递信息。文章还介绍了localization模块的主要文件,如main.cc、localization.h等,并探讨了RTKLocalization和CameraLocalization的实现。未来改进方向包括多传感器融合和降低成本。
4891

被折叠的 条评论
为什么被折叠?



