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 ：跟踪器完全丢失目标

File truncated at 100 lines see the full file