【Apollo】【localization】调试与分析

本文主要介绍localization模块
版本信息： ubuntu16.04，apollo2.5，docker

1.Localization使用的算法

RTK算法(Real Time Kinematic)：合并GPS和IMU信息。
MSF算法(multi-sensor fusion)：融合多个传感器信息，包括 GPS, IMU, 和LiDAR

1.1 算法的输入信息

RTK算法：输入IMU，GPS信息
MSF算法：输入IMU，GPS，LiDAR，Localization map信息和参数配置文件

IMU

IMU(Inertial Measurement Unit)：惯性测量单元，主要提供物体姿态信息，如三轴的角速度和加速度。

topic：/apollo/sensor/gnss/imu
输入格式：modules/drivers/gnss/proto/imu.proto
message Imu {
  optional apollo.common.Header header = 1;

  // The time of IMU measurement, seconds since the GPS epoch (Jan 6, 1980).
  optional double measurement_time = 2;  // In seconds.

  // When measurement_span is non-zero, the gyroscope and accelerometer
  // measurements are averaged for the period from
  // (measurement_time - measurement_span) to measurement_time. Usually,
  //      measurement_span = 1 / sampling_frequency.
  //
  // When measurement_span is 0, angular_velocity and linear_acceleration are
  // instantaneous at measurement_time.
  optional float measurement_span = 3 [default = 0.0];  // In seconds.

  // Forward/left/up in meters per square second.
  optional apollo.common.Point3D linear_acceleration = 4;

  // Around forward/left/up axes in radians per second.
  optional apollo.common.Point3D angular_velocity = 5;
}
举例：
header {
  timestamp_sec: 1523917547.85
}
measurement_time: 1207952765.83
measurement_span: 0.00499999988824
linear_acceleration { //线加速度
  x: -0.164657831192
  y: -0.844970345497
  z: 11.2289190292
}
angular_velocity { //角速度
  x: 0.0260528154901
  y: -0.00899050934794
  z: -0.00442620494063
}

GPS

GPS(Global Positioning System)：全球定位系统，主要提供物体的经度，纬度和高度等信息

topic：/apollo/sensor/gnss/odometry
输入格式：modules/localization/proto/gps.proto
message Gps {
  optional apollo.common.Header header = 1;

  // Localization message: from GPS or localization
  optional apollo.localization.Pose localization = 2;
}

举例：
header {
  timestamp_sec: 1523917543.96
}
localization {
  position { //距离
    x: 561747.727666 //经度
    y: 4147026.24329 //纬度
    z: 88.9852279071 //海拔高度
  }
  orientation { //朝向
    qx: 0.0478558557909
    qy: 0.00121971574564
    qz: -0.301740953505
    qw: -0.952187337838
  }
  linear_velocity { //线速度
    x: -13.2144204223
    y: 19.1608083434
    z: -0.0888519308495
  }
}

LiDAR

LiDAR(Light Detection And Ranging Sensor)：激光探测与测量。其所测得的数据为数字表面模型(Digital Surface Model, DSM)的离散点表示，数据中含有空间三维信息和激光强度信息。

topic：/apollo/sensor/velodyne64/compensator/PointCloud2
输入格式：sensor_msgs/PointCloud2 ros标准点云2格式
从topic可以看出，这是3D雷达的点云数据，需要的设备是3D雷达，品牌：velodyne
这里就不举例了，数据量太大

MSF参数配置文件

velodyne64_novatel_extrinsics_example.yaml
velodyne64_height.yaml
ant_imu_leverarm.yaml
所在目录：modules/localization/msf/params/

1.2 算法的输出信息

topic：/apollo/localization/pose
输出格式：modules/localization/proto/localization.proto
message LocalizationEstimate {
  optional apollo.common.Header header = 1;
  optional apollo.localization.Pose pose = 2; //有Gps和IMU信息
  optional Uncertainty uncertainty = 3;

  // The time of pose measurement, seconds since the GPS epoch (Jan 6, 1980).
  optional double measurement_time = 4;  // In seconds.

  // Future trajectory acturally driven by the drivers
  repeated apollo.common.TrajectoryPoint trajectory_point = 5;
}

举例：
header {
  timestamp_sec: 1523917538.58
  module_name: "localization"
  sequence_num: 123481
}
pose {
  position {
    x: 561818.485365
    y: 4146926.41422
    z: 89.4170070803
  }
  orientation {
    qx: 0.0461626200696
    qy: -0.000436453759158
    qz: -0.303360973546
    qw: -0.951756766062
  }
  linear_velocity {
    x: -12.3781029258
    y: 17.7279602348
    z: -0.198215327172
  }
  linear_acceleration {
    x: -0.21070725231
    y: 0.514807550817
    z: -1.19697054554
  }
  angular_velocity {
    x: 0.0233370735942
    y: -0.00198736751646
    z: 0.00046349163234
  }
  heading: 2.1891824954
  linear_acceleration_vrf {
    x: 0.159849510809
    y: 0.644404153701
    z: -1.14076915633
  }
  angular_velocity_vrf {
    x: 0.0178808466867
    y: -0.0151306894135
    z: -0.000347876008712
  }
  euler_angles {
    x: 0.0289540234456
    y: -0.0877188160321
    z: -5.66479913857
  }
}
measurement_time: 1523917538.56

1.3 算法处理过程

RTK
对IMU数据处理:将相对于地球East/North/Up坐标系上的IMU信息转化成相对于车辆本身x/y/z坐标系的IMU信息，然后将数据拷贝到输出数据中

对linear acceleration进行处理：
Vector3d orig(imu.linear_acceleration().x(),
              imu.linear_acceleration().y(),
              imu.linear_acceleration().z());
Vector3d vec = common::math::QuaternionRotate(localization->pose().orientation(), orig);

对angular velocity 进行处理：
Vector3d orig(imu.angular_velocity().x(), 
              imu.angular_velocity().y(),
              imu.angular_velocity().z());
Vector3d vec = common::math::QuaternionRotate(localization->pose().orientation(), orig);

对于GPS数据则直接拷贝到输出数据中

MSF
在apollo中没有开放msf算法的代码，而只提供liblocalization_msf.so.1.0.2文件(失望吧～窝当时也是找了好长时间)，最后发现涉及到实现的东东都在/usr/local/apollo/local_integ 这里面

目录：local_integ/include
gnss_solver.h
gnss_struct.h
lidar_locator.h
localization_integ.h
pose_forcast.h
sins.h
sins_data_transfer.h
sins_struct.h

目录：local_integ/lib
liblocalization_msf.so -> liblocalization_msf.so.1
liblocalization_msf.so.1 -> liblocalization_msf.so.1.0.2
liblocalization_msf.so.1.0.2

具体的逻辑可参考由几个大牛写的文章 [Robust and Precise Vehicle Localization based on Multi-sensor Fusion in Diverse City Scenes] (https://arxiv.org/abs/1711.05805)。这里就不详细介绍了。

2. Localization两种运行模式

localization

方式一：dreamview上选择modules control->modules->localization
实际启动方式： supervisorctl start localization &
方式二：手动启动
/apollo/bazel-bin/modules/localization/localization --flagfile=/apollo/modules/localization/conf/localization.conf

navigation localization

方式一：dreamview上选择modules control->modules->navi localization
实际启动方式： supervisorctl start navigation_localization &
方式二：手动启动
/apollo/bazel-bin/modules/localization/localization --flagfile=/apollo/modules/localization/conf/localization.conf --localization_config_file=/apollo/modules/localization/conf/navi_localization_config.pb.txt

实际上运行模式不一样的地方就是localization_config_file参数不一样。
再详细看运行参数：

flagfile参数：localization.conf 保存localization的基本配置参数
localization_config_file参数：有两种，格式来自localization_config.proto

1.navi_localization_config.pb.txt ： navigation localization 的算法选择
2.localization_config.pb.txt ： localization 的算法选择

localization_config.proto，里面只有一个属性
message LocalizationConfig {
  enum LocalizationType {
    RTK = 0;
    MSF = 1;
  };
  optional LocalizationType localization_type = 1 [default = RTK];
}

所以，localization与navigation localization不一样的地方就是LocalizationType

localization_config.pb.txt内容：LocalizationType=MSF
navi_localization_config.pb.txt 内容：LocalizationType=RTK

以上是默认情况下的配置，实际上可以修改

3 . 新增定位算法

可参考 /modules/localization/README.md