Apollo详解之定位模块——RTK定位

前言

Apollo的定位模块提供了三种定位方法，分别是RTK、NDT、MSF。
所谓RTK定位，可参考百度百科

RTK（Real Time Kinematic）实时动态测量技术，是以载波相位观测为根据的实时差分GPS（RTDGPS）技术，由基准站接收机、数据链、 流动站接收机三部分组成在基准站上安置1台接收机为参考站， 对卫星进行连续观测，并将其观测数据和测站信息，通过无线电传输设备，实时地发送给流动站，流动站GPS接收机在接收GPS卫星信号的同时，通过无线接收设备，接收基准站传输的数据，然后根据相对定位的原理，实时解算出流动站的三维坐标及其精度。

简单来说，就是依靠地面上拥有绝对准确位置的基站测量GPS定位的误差，将该误差发送给附近的接收机进行校准，提高GPS定位精度，往往可以达到厘米级的定位精度。

RTK定位组件简述

Apollo中RTK定位组件和上述RTK定位的定义并没有关系，其功能只是将GPS的数据和对应时间戳的IMU数据进行组合，最终输出高精地图下的位置。没有涉及到RTK（Real Time Kinematic）实时动态测量技术。
组合导航会输出GPS和IMU的消息，前者输出ENU坐标系下的点坐标、位姿、线速度，后者输出车辆坐标系的线加速度、角速度、欧拉角、航向，然而两者输出的频率相差很大，GPS数据更新频率要远低于IMU数据的更新频率，在Lgsvl仿真中，可以参考

可以看到，仿真中IMU消息的发布频率比GPS高将近十倍。

RTKLocalizationComponent

该组件会订阅GPS消息Gps和IMU消息CorrectedImu，根据Gps的时间戳找到最适合的CorrectedImu，将其中的数据组合，向相关话题输出LocalizationEstimate。此外，该组件会将获得的定位数据发布为localization和world坐标系之间的动态坐标变换。

配置文件

配置文件的定义可参考modules/localization/proto/rtk_config.proto

message Config {
  optional string localization_topic = 1; // 输出的定位话题
  optional string localization_status_topic = 11; // 输出的定位状态话题
  optional string imu_topic = 2; // 订阅的IMU消息话题
  optional string gps_topic = 3; // 订阅的GPS消息话题
  optional string gps_status_topic = 12; // 订阅的GPS定位状态话题
  optional string broadcast_tf_frame_id = 4; // 广播的父坐标系
  optional string broadcast_tf_child_frame_id = 5; // 广播的子坐标系
  optional int32 imu_list_max_size = 6; // IMU消息容器的大小
  optional double gps_imu_time_diff_threshold = 7; // GPS和IMU消息时间戳差值的阈值
  optional double map_offset_x = 8; // world到mpa坐标系的偏差值
  optional double map_offset_y = 9;
  optional double map_offset_z = 10;
}

RTKLocalizationComponent::Proc

该函数会订阅GPS定位消息，进行上述的处理。

RTKLocalization

RTKLocalization负责功能的具体实现，其拥有两个双向链表容器。

imu_list_

订阅得到的CorrectedImu消息会不断保存在该容器当中，新元素加入到尾部，容器中元素的个数大于容量时，头部的原始会被删除，因此该容器中，从头部到尾部，时间戳依次增大，即头部的数据最老，尾部的数据最新。
由于GPS消息的更新频率要远低于IMU的更新频率，因此要通过该容器保存一定时间的IMU消息，以供GPS找到与其时间戳最匹配的IMU消息。

gps_status_list_

作用与imu_list_类似，保存的是IMU的状态

RTKLocalization::FindMatchingIMU

该函数根据GPS的时间戳，在容器中找到与之最为匹配的IMU消息。可以分为三种情况：

• gps时间戳要比容器中的CorrectedImu都要新，则匹配结果为容器中最新的CorrectedImu
• gps时间比容器中的CorrectedImu都要老，则匹配结果为容器中最老的CorrectedImu
• gps时间在容器中时间戳最老和最新的两者之间，则匹配结果为容器中gps时间两端插值后的CorrectedImu

RTKLocalization::InterpolateIMU

若gps时间在容器中时间戳最老和最新的两者之间，则会对GPS时间戳两端最接近的两个CorrectedImu进行插值，插值的过程即根据GPS时间戳对两者取平均，时间戳靠近哪一方，平均时的占比就越大。
进行插值的量分别是角速度、线加速度、欧拉角。

RTKLocalization::ComposeLocalizationMsg

该函数将GPS和IMU的消息进行组合，得到LocalizationEstimate消息。

话题相关

该模块订阅和发布的话题可以在配置文件中进行定义，默认配置可参考modules/localization/conf/rtk_localization.pb.txt。

localization_topic: "/apollo/localization/pose"
localization_status_topic: "/apollo/localization/msf_status"
imu_topic: "/apollo/sensor/gnss/corrected_imu"
gps_topic: "/apollo/sensor/gnss/odometry"
gps_status_topic: "/apollo/sensor/gnss/ins_stat"
broadcast_tf_frame_id: "world"
broadcast_tf_child_frame_id: "localization"
imu_list_max_size: 20
gps_imu_time_diff_threshold: 0.02
map_offset_x: 0.0
map_offset_y: 0.0
map_offset_z: 0.0

注意：该模块输入话题/apollo/sensor/gnss/odometry中的Gps消息是在world坐标系下的位置坐标，而输出话题/apollo/localization/pose的LocalizationEstimate消息是指高精地图的map坐标系下的位置坐标，两坐标系之间的转换关系可通过配置文件中map_offset_x、map_offset_y、map_offset_z进行指定。
另外在建立高精地图时，也要打开该模块，通过订阅/apollo/localization/pose话题来生成高精地图。此时配置文件中map_offset_x、map_offset_y、map_offset_z定义的是世界坐标系和当前正在建立的高精地图的map坐标系之间的变换关系。
由于 /apollo/sensor/gnss/odometry话题输出的world坐标系下的坐标是根据经纬度获得的，数值较大，因此可以在建图时通过map_offset_x、map_offset_y、map_offset_z来指定map坐标的原点，不过注意，建图后定位时map_offset_x、map_offset_y、map_offset_z参数要与建图时保持一致。