感知融合模块小结

摘要

本文对感知模块的fusion部分进行总结，通过代码和流程图相结合的方式，对融合中运用的算法进行介绍。

• 大致流程
  

数据初始化

结合上述流程图，对代码块进行分析：

• 传感器目标数据分析：传感器数据类型在sensor.h中声明，包含了目标的x,y,z坐标，及速度等。

struct SensorObjects {
 double timestamp;   //时间戳
 double sensor_id;   //传感器id
 double x;
 double y;
 double z;
 double v_x;
 double v_y;
 double v_z;
 int track_id = 0;
};

• track中包含的数据类型

class Track{
public:
  explicit Track(SensorObjects obj);//初始化track
  double fused_timestamp_; //融合时间更新
  int age_;
  double sensor_id;
  Eigen::Vector4d state;  //保存X,Y,v_x,v_y
  bool is_dead_; //判断该track是否存在
  int assign_age ; //匹配上的周期数
  int unassign_age; //未匹配上的周期数
  int flag ;//flag = 0 old  flag = 1  new 判断是否是新的目标
  ...
  int assign;//判断目标是否匹配上，匹配上=1，否则=0
  };

• 目标聚类

cluster函数依据目标之间的距离对每一个时刻的目标进行聚类，得到final_data存放聚类得到的sensor数据。
MatrixXd final_data = cluster(m_data,j);

融合

1. 匹配match
   在match.cc文件中，包含了以下算法：
   核心函数 Match(tracks,sensor_objects,&assignments,&unassigned_tracks,&unassigned_objects, &track2measurements_dist,&measurement2tracks_dist,flag_id[i]);
   主要是求assignments，unassigned_tracks，unassigned_objects这个三个参数，在该函数中通过不同传感器id进行trackers的匹配算法，flag_id中包含了所有传感器id的数据。
   void ComputeAssociationMat(........);//计算关联矩阵，track向前递推一个时间戳，计算与sensor的距离，这是HM之前最重要的步骤，直接影响后面融合的效果。这里的HM求得的最优匹配关系就是匹配和最小，而每一条匹配的边长是多少，就是这里的关联矩阵求得的值。如果以后想要对匹配结果优化，这里是一个很重要的地方，这个关联系数能否正确的反应匹配关系
   void MinimizeAssignment(......);//最小匹配值
   bool HmAssign(........);//匈牙利算法计算

2. 更新匹配后的结果
   由匹配得到的结果，分两部分进行目标更新：
   1）更新匹配上的trackUpdateAssignedTracks：对匹配上的每条track进行数据更新，这里通过传感器id进行区分，将不同传感器测得的数据都进行融合，主要算法见UpdateWithSensorObject。
   在UpdateWithSensorObject中，通过kalman滤波的方式进行融合。
   (*tracks)[local_track_index] = updataEFK1((*tracks)[local_track_index],obj.x,obj.y);
   并且对融合时间戳，匹配周期等数据进行更新。
   UpdateMeasurementsLifeWithMeasurement(&lidar_objects_, sensor_id, timestamp, s_max_lidar_invisible_period_);
   在该函数中包含了对传感器目标信息的记录std::map<double, SensorObjects> lidar_objects_;用于判断目标的存在周期是否超时，对map中的匹配目标进行及时的更新。（如果传感器id不同，则判断周期，超时则删除。）
   2）Unassigned_Objects对没有匹配上的sensor数据unassigned_sensor_objects进行更新，对track中没有匹配上的数据unassigned_fusion_tracks更新
   3）更新未匹配上的tracksUpdateUnassignedTracks
   在函数UpdateMeasurementsLifeWithoutMeasurement(&lidar_objects_,time,s_max_lidar_invisible_period_);中，首先判断传感器id是否相同，如果有该id值，但是没有匹配到该目标，则从map中删除该目标。
   is_dead_ = lidar_objects_.empty();
   判断is_dead_是否为空，作为是否删除track的依据。
   若不为空，递推一步，更新X,Y.

3. 创建新的跟踪目标CreatNewTracks
   对于没有匹配的sensor_objects,设置flag标签，新的目标设为1，如果连续三个周期都测到该目标flag=0，设置为old_object,新的sensor_object创建了新的tracker，但是不显示该路径，当assign_age>3时才显示。(避免杂点的影响)

4. 移除丢失目标 RemoveLostTracks
   判断is_dead_是否为0即可