armor_processor

0.自瞄预测逻辑

预测节点接收检测节点发布的目标装甲板信息，根据目标的状态（目标是否丢失/目标是否处于小陀螺/目标是否发生切换）调整预测器状态（追踪/预测/丢失预测/丢失），将目标位姿信息输入预测器，输出目标预测位置后解算出pitch和yaw轴角度并发布消息。

• 预测器
  • 基于两个模型，分别为singer模型、整车模型（还有IMM模型，当前未使用），各模型详解见下一部分Update log。
  • 预测器分为4个状态，分别为Tracking（追踪）、Predicting（预测）、Losting（丢失预测）、Lost（完全丢失）。预测器4个状态的切换条件为：
    • 目标上帧存在，此帧丢失则进入Losting状态；
      • 此状态下，预测器仅对目标当前位置进行状态估计。
    • 目标上帧不存在且当前帧丢失或预测器处于Losting状态的帧数大于等于5，则切入Lost状态；
      • 此状态直接重置预测器。
    • 车辆发生切换或目标未完成初始化（首次识别到），则切入Tracking状态；
      • 此状态对预测器进行初始化，给状态量赋初值。
    • 目标已完成初始化则进入Predicting状态。
      • 此状态下对目标当前位置进行估计后，对目标在延迟时间后（结合发弹延迟、弹丸飞行时间、串口通信延迟、云台响应延迟等）做超前预测。

1.Update log

Date:2023-05-03

从车辆中心的运动状态看，只有各个方向的平动，因此机动模型应用于车辆中心则可规避对小陀螺状态下单个装甲板的预测出现超调的问题，从而提高预测效果的上限。
整车状态预测思路分为前后两个阶段：车辆中心的位置估计和车辆中心的位置预测。

• 对车辆中心建模

  车辆中心和装甲板的位置关系对应于圆心和圆上的点，而PnP解算出的装甲板的姿态则包含装甲板相对于车辆中心的角度信息。
  整车模型如下： \(\begin{cases} x=x_c+r*sin({\omega}t+{\theta}) \\ y=y_c-r*cos({\omega}t+{\theta}) \\ z=z_c \end{cases}\)

  模型中已知$x，y，z，\theta$，我们需要估计的状态量有车辆中心$(x_c,y_c,z_c)$和车辆半径$r$以及车辆旋转角速度$\omega$。

• 基于扩展卡尔曼滤波的车辆中心状态估计:
  • 滤波的状态量 \(X= \begin{bmatrix} x_c \\ y_c \\ z_c \\ r \\ \theta \\ \omega \\ \end{bmatrix}\)
  • 状态转移矩阵 \(F= \begin{bmatrix} 1 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 1 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 1 \ \ 0 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 0 \ \ 1 \ \ 0 \ \ 0 \\ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 1 \ \ dt \\ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 0 \ \ 1 \\ \end{bmatrix}\)

    dt为图像两帧之间的时间间隔。

  • 滤波的观测量 \(Z= \begin{bmatrix} x \\ y \\ z \\ \theta \\ \end{bmatrix}\)
  • 测量矩阵 \(H= \begin{bmatrix} 1 \ 0 \ 0 \ \ \ sin\theta \ \ \ 0 \ 0 \\ 1 \ 0 \ 0 \ -cos\theta \ 0 \ 0 \\ 0 \ 0 \ 1 \ \ \ \ \ \ 0 \ \ \ \ \ \ 0 \ 0 \\ 0 \ 0 \ 0 \ \ \ \ \ \ 0 \ \ \ \ \ \ 1 \ 0 \\ \end{bmatrix}\)
  • 测量矩阵的雅克比形式 \(J_H= \begin{bmatrix} 1 \ 0 \ 0 \ \ \ sin\theta \ \ \ rcos\theta \ 0 \\ 1 \ 0 \ 0 \ -cos\theta \ rsin\theta \ 0 \\ 0 \ 0 \ 1 \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ 0 \ \ \ \ \ \ \ 0 \\ 0 \ 0 \ 0 \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ 1 \ \ \ \ \ \ \ 0 \\ \end{bmatrix}\)

    首次检测到装甲板目标后会对滤波进行初始化，即对滤波状态量赋初值，此时的圆心直接通过模型计算给出；之后则不断通过滤波的预测+更新步骤迭代收敛中心坐标$(x_c,y_c,z_c)$和车辆半径$r$。
    装甲板发生切换处理：考虑到车辆中心、半径和角速度不变，因此我们只重置滤波状态量中的角度$\theta$，从而使滤波更快收敛。
    车辆发生切换：重置滤波器。

• 基于卡尔曼滤波的车辆中心状态预测:

  将上一阶段的输出量作为singer模型的输入量，对车辆中心进行预测。（singer模型见下面基于‘当前’统计模型（CS model）的卡尔曼滤波算法）

• 目标点预测 上一阶段的输出量即为目标中心的预测点$(x_c,y_c,z_c)$，结合时间延迟量计算当前装甲板的预测旋转角度，

File truncated at 100 lines see the full file