armor_tracker

• ArmorTrackerNode
  • Tracker
    • KalmanFilter

ArmorTrackerNode

装甲板处理节点

订阅识别节点发布的装甲板三维位置及机器人的坐标转换信息，将装甲板三维位置变换到指定惯性系（一般是以云台中心为原点，IMU 上电时的 Yaw 朝向为 X 轴的惯性系）下，然后将装甲板目标送入跟踪器中，输出跟踪机器人在指定惯性系下的状态

订阅：

• 已识别到的装甲板 /detector/armors
• 机器人的坐标转换信息 /tf /tf_static

发布：

• 最终锁定的目标 /tracker/target

参数：

• 跟踪器参数 tracker
  • 两帧间目标可匹配的最大距离 max_match_distance
  • DETECTING 状态进入 TRACKING 状态的阈值 tracking_threshold
  • TRACKING 状态进入 LOST 状态的阈值 lost_threshold

ExtendedKalmanFilter

\(x_c = x_a + r * cos (\theta)\) \(y_c = y_a + r * sin (\theta)\)

\[x = [x_c, y_c,z, yaw, v_{xc}, v_{yc},v_z, v_{yaw}, r]^T\]

参考 OpenCV 中的卡尔曼滤波器使用 Eigen 进行了实现

卡尔曼滤波器

考虑到自瞄任务中对于目标只有观测没有控制，所以输入－控制模型 $B$ 和控制器向量 $u$ 可忽略。

预测及更新的公式如下：

预测：

\[x_{k|k-1} = F * x_{k-1|k-1}\] \[P_{k|k-1} = F * P_{k-1|k-1}* F^T + Q\]

更新:

\[K = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1}\] \[x_{k|k} = x_{k|k-1} + K * (z_k - H * x_{k|k-1})\] \[P_{k|k} = (I - K * H) * P_{k|k-1}\]

扩展卡尔曼滤波过程噪声 (Q) 矩阵中平移与旋转参数负相关关系，暂时选用自然常数为底的指数函数作为基础，未考究合理性，待完善替换:

\[d_{move} = \sqrt {v_x^2+v_y^2}\] \[d_{spin} = v_{yaw}\] \[s2qxyz = e^{-d_{spin}} \cdot (s2qxyz_{max}-s2qxyz_{min})+s2qxyz_{min}\] \[s2qyaw = e^{-d_{move}} \cdot (s2qyaw_{max}-s2qyaw_{min})+s2qyaw_{min}\]

以上关系在目标不同状态下对应参数：

• max ■■■■■
• min ◻

目标平移状态	目标旋转状态	s2qxyz	s2qyaw
无	无	■■■■■	■■■■■
无	低速	■■■	■■■■■
无	高速	■	■■■■■
低速	无	■■■■■	■■■
低速	低速	■■■	■■■
低速	高速	■	■■■
高速	无	■■■■■	■
高速	低速	■■■	■
高速	高速	■	■

在 Q 矩阵中 s2qxyz 与 s2qyaw 均为对应物理量的期望加速度，由于在 RoboMaster 比赛场景下存在功率限制，因此二者存在负相关关系，即平移速度越大时旋转速度越小，旋转速度越大时平移速度越小。但此关系的逆命题不一定成立，这是使用匀速运动作为扩展卡尔曼滤波的运动模型难以避免的缺陷。考虑 RoboMaster 比赛工况下目标车的加速度在旋转平移时不为定值，可尝试更换为匀加速运动模型，即认为目标加加速度为定值。

Tracker

参考 SORT(Simple online and realtime tracking) 中对于目标匹配的方法，使用卡尔曼滤波器对单目标在三维空间中进行跟踪

在此跟踪器中，卡尔曼滤波器观测量为目标在指定惯性系中的位置（xyz），状态量为目标位置及速度（xyz+vx vy vz）

在对目标的运动模型建模为在指定惯性系中的匀速线性运动，即状态转移为 $x_{pre} = x_{post} + v_{post} * dt$

目标关联的判断依据为三维位置的 L2 欧式距离

跟踪器共有四个状态：

• DETECTING ：短暂识别到目标，需要更多帧识别信息才能进入跟踪状态
• TRACKING ：跟踪器正常跟踪目标中
• TEMP_LOST ：跟踪器短暂丢失目标，通过卡尔曼滤波器预测目标
• LOST ：跟踪器完全丢失目标

工作流程：

• init：

  跟踪器默认选择离相机光心最近的目标作为跟踪对象，选择目标后初始化卡尔曼滤波器，初始状态设为当前目标位置，速度设为 0

• update:

  首先由卡尔曼滤波器得到目标在当前帧的预测位置，然后遍历当前帧中的目标位置与预测位置进行匹配，若当前帧不存在目标或所有目标位置与预测位置的偏差都过大则认为目标丢失，重置卡尔曼滤波器。

  最后选取位置相差最小的目标作为最佳匹配项，更新卡尔曼滤波器，将更新后的状态作为跟踪器的结果输出