armor_solver

fyt::ArmorSolverNode

装甲板识别节点

发布话题

• armor_solver/target (rm_interfaces/msg/Target) - 整车估计的状态
• armor_solver/measurement (rm_interfaces/msg/Measurement) - EKF的输入观测量
• armor_solver/cmd_gimbal (rm_interfaces/msg/GimbalCmd) - 云台控制指令

订阅话题

• armor_detector/armors (rm_interfaces/msg/Armors) - 识别到的装甲板信息

参数

• debug (bool, default: false) - 是否开启调试模式
• target_frame (string, default: “odom”) - 目标坐标系
• ekf.sigma2_q_xyz (double, default: 0.05) - 状态转移噪声方差 (x,y,z)
• ekf.sigma2_q_yaw (double, default: 1.0) - 状态转移噪声方差 (yaw)
• ekf.sigma2_q_r (double, default: 80.0) - 状态转移噪声方差 (r)
• r_xyz_factor (double, default: 1.0) - 位置观测噪声方差系数 (x,y,z)
• r_yaw_factor (double, default: 1.0) - 位置观测噪声方差系数 (yaw)
• tracker.max_match_distance (double, default: 0.5) - 两帧间目标可匹配的最大距离
• tracker.max_match_yaw_diff (double, default: 0.5) - 两帧间目标同一块装甲板可匹配的最大yaw角差（大于这个值则认为装甲板发生跳变）
• tracker.tracking_thres (int, default: 2) - DETECTING 状态进入 TRACKING 状态需要连续识别到的帧数
• tracker.lost_thres (double, default: 1.0) - TRACKING 状态进入 LOST 状态需要连续丢失的时间（s）
• solver.prediction_delay (double, default: 0.0) - 预测延迟时间（s），会影响选版
• solver.controller_delay (double, default: 0.0) - 控制延迟时间（s），不会影响选版
• solver.max_tracking_v_yaw (double, default: 60.0) - 转速大于这个值时，瞄准中心
• solver.side_angle (double, default: 15.0) - 跳转到下一装甲板的角度阈值
• solver.bullet_speed (double, default: 25.0) - 子弹速度
• solver.gravity (double, default: 9.8) - 重力加速度
• solver.compensator_type (string, default: “ideal”) - 补偿器类型
• solver.resistance (double, default: 0.001) - 空气阻力

ArmorSolverNode

装甲板处理节点

订阅识别节点发布的装甲板三维位置及机器人的坐标转换信息，将装甲板三维位置变换到指定惯性系（一般是以云台中心为原点，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}\]

Tracker

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

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

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

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

跟踪器共有四个状态：

File truncated at 100 lines see the full file