【Apollo】【localization】调试与分析

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

message Imu {

// 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;
}

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

message Gps {

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

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

MSF参数配置文件
velodyne64_novatel_extrinsics_example.yaml
velodyne64_height.yaml
ant_imu_leverarm.yaml

1.2 算法的输出信息

topic：/apollo/localization/pose

message LocalizationEstimate {
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;
}

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
}
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

对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);

Vector3d orig(imu.angular_velocity().x(),
imu.angular_velocity().y(),
imu.angular_velocity().z());
Vector3d vec = common::math::QuaternionRotate(localization->pose().orientation(), orig);

MSF

目录：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

liblocalization_msf.so -> liblocalization_msf.so.1
liblocalization_msf.so.1 -> liblocalization_msf.so.1.0.2
liblocalization_msf.so.1.0.2

2. Localization两种运行模式

localization

方式一：dreamview上选择modules control->modules->localization

/apollo/bazel-bin/modules/localization/localization --flagfile=/apollo/modules/localization/conf/localization.conf

方式一：dreamview上选择modules control->modules->navi 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

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

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_config.pb.txt内容：LocalizationType=MSF
navi_localization_config.pb.txt 内容：LocalizationType=RTK