Lidar L-shape corner点跟踪算法

paper:

L-Shape Model Switching-Based Precise Motion Tracking of Moving Vehicles Using Laser Scanners

摘要

• 提出了一种基于L-shape的lidar目标跟踪算法，跟踪L-shape的角点而不是传统算法中的center点。
• 传统算法中center点的位置受到目标shape的影响，当目标的shape信息改变时，其位置会发生变化，甚至出现跳变。而
• 所提算法跟踪corner点，corner点的位置不会受到目标shape变化的影响，因此更加稳定

引言

数据关联有关算法

• NN
• PDA
• JPDA
• IPDA
• JIPDA

应对杂波环境

• MHP – multiple hypothesis tracking
  MHT provides multiple hypotheses for tracking the measured association and combines all probabilities in the hypothesis.
• PF
  PF is a Bayesian filter based on samples named “particles” and can represent uncertainty through the probabilistic distribution of itself.
• RFS – random finite set
  RFS uses the Bayesian framework, taking into account the probability of association
  with the number of tracks and observations. The algorithms show powerful estimation performance in multi-target tracking problems in cluttered environments but they require massive computation power to achieve this powerful performance.

激光雷达点云目标shape

• use vague line ends
  suppress longitudinal motion error from parked vehicles successfully，the reduced error is only
  limited to the longitudinal motion of the track
  只适用于直线跟踪运动场景
• I-shape, IS-shape, L-shape, C-shape, and E-shape
  但是，shape的误分类会导致参数的估计错误，甚至是参数的跳变
  
• geometry model-based vehicle tracking（基于几何模型的车辆跟踪算法）
  估计几何模型的length、width以及相应的anchor点的相对位置
  利用点云的每个点的信息来做粒子滤波的每个粒子的似然估计
  计算量大
• L-shape fitting method
  
  
  ####L-shape算法
• 与U-shape模型的比较
  (1) The L-shape feature can represent a shape-invariant track point with a simple structure as well as the box model
  L-shape适用于形状不变的，结构类似于box模型的跟踪场景
  (2) The U-shape is close to the actual shape of a vehicle with curved bumpers but the expression and fitting process for the U-shape is more complex than L-shape feature extraction
  U-shape虽然更适用于车辆的实际形状，描述弯曲保险杠结构更加准确，但是相应的，其表达式和适配过程相比于L-shape模型的特征提取，更加复杂。
• L-shape的跟踪特征
  （1）anchor point
  （2）two lines
  同时存在anchor point 在box模型中的相对位置模糊问题

角点跟踪算法（所提算法）

算法特点

• 跟踪角点位置，而不是跟踪中心点位置

• 角点的运动状态有平移和旋转

• 角点的运动模型采用匀加速模型

• 提供角点change方案

• 提供size 更新方案

• L-shape feature extraction
  ——an L-shape tracker, and L-shape to box model conversion
  （1）L-shape feature extraction 从激光点云中提取L-shape模型。采用聚类以及直线特征提取算法。
  （2）从L-shape测量值中，检测跟踪以及shape model 的状态信息
  （3）卡尔曼滤波以及最近邻关联算法。同时提供对于角点变化的状态信息补偿算法

• L-shape的旋转问题
  adaptive break-point detector
  iterative end-point fit method 迭代终点拟合方法

算法模块

特征提取

• 1. Clustering聚类
  在极坐标系下，角度排序的连续点，使用距离波门，进行聚类
• 2. Line Extraction（还没有看懂，需要进一步阅读，或者阅读其余论文）
  line extraction method that uses an iterative end-point fit
  1）选取两个末端点，点云中的两个末端点即为最大角度点和最小角度点
  2）连接两个末端点，其连接线为baseline
  3）计算其余所有的点，相对于baseline的欧式距离，并选取距离最大的点为anchor point
  4）根据两个末端点以及anchor point 得到L-shape 的两条线
• 3. L-Shape Extraction
  1）根据两条提取line的长线，计算box模型的旋转角度
  2）根据两条line，按照最小面积，进行box模型拟合[35][36]
  3）输出测量信息[x_correr, y_correr]用于更新目标运动状态信息，输出[L1, L2, theta]用于更新shape信息（跟踪center点，同理）
  

模型及预测

• 4. Track Model
  1）Dynamic Model 运动状态信息模型
  
  2）Shape Model 形状模型
  
  状态向量转移方程只预测角度变化信息
• 5. Prediction
  进行Dynamic Model以及Shape Model 的状态向量以及状态向量协方差的预测
  #####模型切换及数据关联
  corner点是距离lidar最近的点，但是，该点会随着目标和自车相对角度的变化而发生跳变
  
• 6. Model Transition Detection模型转换检测
  ——比较view angle 以及 L-shape orientation
  ——如果L-shape orientation 处于 view angle 的阈值范围内，则会检测到模型转换
  ——而当L-shape orientation 非常接近 view angle 时，模型会频繁发生转换
  ——针对非常接近view angle，设定hysteresis angle（滞留角度），此时模型不发生变化，而测量结果发生变化
  
  a）顺时针测量值转换
  
  
  b）逆时针测量值转换
  
• 7. Corner Point Switching角点切换
  当检测到模型需要转换后，对角点进行切换，也存在顺时针转换和逆时针转换两种情况。
  
  
  
• 8. Uncertainty Propagation协方差传递
  协方差包括，two lines 的协方差，corner 协方差
  line协方差，直接进行简单的协方差传递赋值
  
  L-shape的line length 以及 orientation 的检测值受到协方差的影响
  根据上述的转换方程，书写角点状态向量的状态转移方程
  
  计算jacobian矩阵，从而得到角点状态向量的协方差
  
• 9. Nearest Neighbor Association最近邻关联
  采用马氏距离，来选定最近点
  
  DM：马氏距离
  VD：新息
  SD：新息协方差
  
  #####Updata更新
• 9. corner点的状态向量更新
  
• 10. shape状态向量更新
  
  由于lidar的检测值，几乎不可能得到实际大小更大的shape测量值，因此：
  当最新的测量值大于预测值，权重应该更大
  当最新的测量值小于预测值，权重应该较小
  
  
  #####L-SHAPE TO BOX MODEL CONVERSION
• 11. corner状态向量转换
  
  
  其中r表示corner点到lidar的欧式距离
  
• 11. 状态向量协方差转换
  同样也是采用计算jacobian矩阵的方式，计算协方差