Robotics

RoboMaster 自瞄系统技术报告

Posted on 2026-07-06RoboMasterAutoAimEKFPnPCalibration

RCIA 2025 赛季自瞄系统拆解：从手眼标定、坐标系转换到 EKF 目标状态估计与调参优先级。

项目地址：https://github.com/BenmaoNeko/RCIA_SP25_Vision

项目背景：RCIA25赛季完整形态和联盟赛所使用的自瞄代码，实现了：手眼标定，yolo检测，opencv角点提取，pnp算法得到其对应的6DoF检测，TF树坐标系转换，卡尔曼滤波，整车估计，装甲板预测估计，弹道解算，选点优化等功能

项目结构

RCIA_Vision_SP25/
├── src/                              # 主程序入口，包含 standard、standard_mpc、自瞄调试、打符调试等运行模式
├── tasks/                            # 核心任务算法模块，按功能拆分自瞄、打符和全向感知
│   ├── auto_aim/                     # 装甲板自瞄主链路：YOLO 检测、PnP 解算、Tracker、EKF、Aimer、Shooter
│   ├── auto_buff/                    # 能量机关打符链路：扇叶检测、R 标中心估计、Buff PnP、目标预测和瞄准
│   └── omniperception/               # 全向感知相关模块，用于多相机/辅助目标信息接入
├── io/                               # 硬件与外部通信层，封装相机、云台、串口、IMU、ROS2 桥接等接口
│   ├── gimbal/                       # 云台状态读取与控制命令发送，负责 yaw/pitch、弹速和模式信息交互
│   ├── huaray/                       # 华睿相机驱动和运行时库
│   ├── hikrobot/                     # 海康相机驱动封装
│   ├── mindvision/                   # 迈德威视相机驱动封装
│   ├── dm_imu/                       # 达妙 IMU 数据读取与姿态信息处理
│   └── serial/                       # 串口通信基础库和协议收发支持
├── tools/                            # 通用工具库，包含 EKF、弹道模型、数学工具、日志、绘图和录像等功能
├── configs/                          # 不同机器人/模式的 YAML 配置，包含相机参数、手眼标定、算法阈值和控制参数
├── calibration/                      # 相机标定、手眼标定和 robot-world hand-eye 标定相关程序
├── assets/                           # 标定图片、手眼数据、模型文件等实验和部署资源
├── scripts/                          # 运行、打包、实验分析等辅助脚本
├── tests/                            # 回归测试与调试验证脚本，用于验证弹道、PnP、配置和诊断工具
├── docs/                             # 设计方案、部署说明和阶段性计划文档
├── docker/                           # miniPC / Docker 部署相关配置
├── CMakeLists.txt                    # CMake 构建入口，组织各模块编译和链接
├── Dockerfile                        # 容器化构建环境定义
├── docker-compose.yml                # CPU/通用部署编排配置
├── docker-compose.gpu.yml            # GPU/OpenVINO 等加速环境部署配置
├── autostart.sh                      # 上电自启动脚本
└── watchdog.sh                       # 运行守护脚本，用于异常退出后的恢复

整体框架

整体来看，可以分成五个层次：

  1. 感知层：完成图像采集、ROI 裁剪、YOLO 推理、关键点整理和候选目标输出。
  2. 几何层：利用相机内参、畸变参数、手眼标定和 PnP，将二维像素观测还原为三维空间位姿。
  3. 状态估计层：通过 Tracker 状态机和 EKF，把单帧不稳定观测转化为连续、可预测的目标状态。
  4. 决策预测层：根据目标运动状态选择装甲板，处理普通目标、小陀螺目标、前哨站等不同情况，并计算发弹延迟后的目标位置。
  5. 控制执行层：进行弹道补偿、yaw/pitch 指令生成、开火判断和串口下发，使算法结果最终变成云台动作。

在解构整个项目之前。首先我们就要进行最最重要的一步，和整个系统的前提

手眼标定

整个自瞄代码是运作在一个耦合度相当高的坐标系转化中的，也就无论是那一层都是运行在一棵庞大的TF树上的，那手眼标定的重要性就是相当于整棵TF的树根在哪里一样。也就是说如果一开始手眼标定都没有做好的话，整个系统是完全没有办法运行的。因此可见手眼标定的重要性。

1.首先要知道的就是，相机看到的是二维像素，云台控制的是 yaw/pitch，而 PnP 解出来的三维坐标一开始也只是在“相机坐标系”下，自瞄真正要控制云台，就必须知道： 相机坐标系 和 云台坐标系 之间到底差了多少旋转、多少平移。

这些对应在代码里面的就是： R_camera2gimbal：相机坐标系到云台坐标系的旋转矩阵 t_camera2gimbal：相机坐标系到云台坐标系的平移向量 R_gimbal2imubody：云台坐标系和 IMU 机体系之间的旋转关系

这些就是通过手眼标定得到的。简单来说，就是通过手眼标定，将相机坐标系正确的转化成云台坐标系。 手眼标定的流程：流程大致是： 1.拍多组标定板图片，同时记录每张图片对应的 IMU 四元数。 2.对每张图检测棋盘格或圆点板角点。 3.用 solvePnP 求出标定板相对于相机的位姿。 4.用 IMU 姿态求出云台在世界坐标系中的姿态。 5.多组数据联立，求出固定不变的 camera -> gimbal 变换。 6.将结果写回 YAML 配置，供自瞄主程序使用

相应的代码：

  armor.xyz_in_gimbal = R_camera2gimbal_ * xyz_in_camera + t_camera2gimbal_;
  armor.xyz_in_world = R_gimbal2world_ * armor.xyz_in_gimbal;

所以手眼标定本质上提供的是 camera -> gimbal 这段外参。如果这段错了，PnP 在相机坐标系里算得再准，转到云台坐标系后也会系统性偏移，后面的 EKF、装甲板预测、弹道解算都会基于错误空间位置工作

就算其他的都不知道，那也要重点知道，手眼标定最终要输出什么 R_gimbal2imubody：云台坐标系到 IMU 机体系的旋转关系。 R_camera2gimbal：相机坐标系到云台坐标系的旋转关系。 t_camera2gimbal：相机原点到云台坐标系下的平移量。

具体输出的可以看这个手眼标定的：RCIA_Vision_SP25/run_手眼标定.sh。我们解决这个手眼标定不准的问题就是，通过这个手眼标定脚本的思路进行的 重点就是这个脚本会这个脚本会穷举 24 种右手坐标系轴向组合, 正确的 R_gimbal2imubody 应该让标定出来的相机安装 roll 接近 0 度。因为如果 IMU 轴向错了，后面再怎么标定，R_camera2gimbal 都会被迫吸收 IMU 坐标错误，结果会偏。 相机安装方式大概率不会绕roll进行太大角度的偏移,因此进行的延伸出来的一个判断标准.效果基本不会有问题. 然后值得一说的是, t_camera2gimbal,这个部分,其实手眼标定的本质就是一个AX=BX的一个过程,一般来说,我们在进行标定的时候是只是进行的了云台的旋转,没有进行云台的平移,因此求出来的平移也是相当不准确的,以及,即使后续我们进行了云台的平移进行,也会受pnp算法的精度影响,应此建议直接通过sw的工程图,量相机光轴到云台坐标系云台(pitch轴的旋转中心)之间的距离是最准的.

这里只对此项目的手眼标定进行简单的讲解,关于其他有关知识.如原点标定,棋盘标定之类的不在这里进行赘述.

前面提到,整个自瞄是建立在耦合度相当高的坐标系转化中的,因此我们就需要对坐标系有一定的了解.

坐标系

我们需要对坐标系转化有足够的了解。这个项目里涉及到的坐标系比较多

首先是相机坐标系,这个不赘述了,只要知道作为坐标原点是定在左上角的就行

云台坐标系:定在云台的pitch/yaw的中心点.同时也是最终输入的基准

imu坐标系:这个可以分为imu body坐标系和imu abs坐标系 imu body坐标系的意思是,imu本身的自己的轴向,硬件出场的时候刻在芯片上面的 imu abs的意思是,上电的时候,在imu body 的基础上做的绝对参考,简单来说就是零点,imu输出的四原数q就是在这个imu abs的基础上面输出的 world坐标系:这个必须要知道的事情是,整个卡尔曼层都是作用在world坐标系上面的,世界坐标系不准卡尔曼层是没有办法正常工作的

坐标系转化

有以下几个比较重要的转化关系: R_gimbal2imubody,R_imubody2imuabs，R_gimbal2world，R_camera2gimbal,R_gimbal2imubody_.transpose() * R_imubody2imuabs * R_gimbal2imubody_

先看手眼标定进行的得到的R_camera2gimbal，t_camera2gimbal,R_gimbal2imubody这几个转化关系

R_camera2gimbal:是相机坐标系转化到云台坐标. 也就是通过手眼标定的外参 代码:

// solvePnP 得到目标在相机系下的位置Eigen::Vector3d xyz in camera;
cv::cv2eigen(tvec，xyzin camera);
// 变换到云台系
armor.xyz in gimbal=Rcamera2gimbal*xyz in camera +t camera2gimbal ;

云台坐标系本身是人为定义的，原点通常取在云台 yaw/pitch 旋转中心附近，轴向由项目约定。但相机坐标系到云台坐标系的外参并不是单靠定义就能完全确定的，因为相机的光心位置、安装角度和机械加工误差都会影响真实变换。因此 R_camera2gimbal 和 t_camera2gimbal 可以先通过机械结构和手工测量得到近似值，再通过手眼标定或实测效果进行修正。当前项目中 R_camera2gimbal 使用的是根据轴向关系写出的理想旋转矩阵，而 t_camera2gimbal 使用手工测量值。 在本项目中，IMU 主要提供云台姿态变化的参考。手眼标定不是为了直接得到 camera -> imu，而是借助 IMU/云台姿态和标定板观测，反推出相机和云台机械坐标系之间的固定外参。

R_imubody2imuabs:机体系和绝对参考系的转化 imu在上电的时候会进行零点的标定,也就是会定下一个imu abs坐标系,imu在发送的四元数的都是在imu abs坐标系中进行得到的,也就是说，R_imubody2imuabs，通过一直读电控发送来的四元数，可以知道imu body在变没变，怎么变

R_gimbal2imubody:云台系向imu body系的转化 这里容易产生一个误会：既然我们做的是手眼标定，为什么配置里不是直接使用 R_camera2imubody，而是使用 R_gimbal2imubody？原因是本项目并没有把相机坐标系直接作为控制基准。PnP 得到目标在相机系下的位置后，代码会先通过 R_camera2gimbal 和 t_camera2gimbal 将目标转换到云台坐标系。因此云台坐标系是视觉结果进入控制系统前的核心中间坐标系。

IMU 的作用则是提供当前云台姿态的绝对参考。由于 IMU body 坐标系和云台坐标系的轴向并不一定一致，所以需要 R_gimbal2imubody 描述二者之间的旋转关系。这样代码才能利用 IMU 四元数计算出 R_gimbal2world，再把云台系下的目标位置转换到 world 坐标系中。

R_gimbal2world:云台坐标系到世界坐标系的旋转

R_gimbal2world 表示当前时刻云台坐标系到 world 坐标系的旋转关系 代码中先将 IMU 四元数 q 转换为旋转矩阵：

Eigen::Matrix3d R_imubody2imuabs = q.toRotationMatrix();

这里的 R_imubody2imuabs 表示 IMU body 坐标系相对于 IMU 上电后绝对参考系的姿态。由于 IMU body 坐标系和云台坐标系的轴向并不完全一致，所以还需要用 R_gimbal2imubody 将这个姿态关系转换到云台坐标系定义下：

R_gimbal2world_ =
    R_gimbal2imubody_.transpose()
    * R_imubody2imuabs
    * R_gimbal2imubody_;
当前云台→R_gimbal2imubody→当前imu机体系→R_imubody2imuabs→IMU abs（上电时的IMU body）→R_gimbal2imubody⁻¹→上电的时候的云台坐标系（世界坐标系）

差不多是这样：当前云台——>R_gimbal2imubody——>得到当前imu机体系——>R_imubody2imuabs——>得到IMU abs（这个等于上电时的IMU body）——>R_gimbal2imubody⁻¹——是不是就可以拿到上电的时候的云台坐标系了，这个就是世界坐标系

可以仔细看一下这部分的代码,转换的R_gimbal2imubody_.transpose()乘R之后乘了原矩阵,简单梳理一下就是,R_gimbal2imubody_是云台→IMU机体,R_imubody2imuabs是IMU机体→IMU绝对,R_gimbal2imubody_.transpose()是R_imubody2gimbal也就是imu机体到云台 那串起来就是:云台——>IMU body——>IMU abs——>云台（world） 即:R_imubody2gimbal * R_imubody2imuabs * R_gimbal2imubody 这三个矩阵组合后，得到的是 R_gimbal2world，也就是“当前云台坐标系到 world 坐标系”的旋转矩阵。

这样得到的 R_gimbal2world 就可以把云台系下的目标位置转换到 world 系下：

armor.xyz_in_world = R_gimbal2world_ * armor.xyz_in_gimbal;

转化成世界坐标系的目的是什么?就是为了消除云台本身旋转影响去掉.让后续EKF,预测,弹道解算等等都是可以在一个相对稳定的坐标系下工作

注:有一点需要注意.为什么说没办法实现跑打呢?从这里就可以看出来,我们的建立的世界坐标系原点是通过imu abs +云台的轴向定义得到的(云台坐标系原点).也就是说我们从开始就假定我们的云台是一个完全静止的一个状态,但是实际上是我们的车是会移动的云台也是会移动的,世界坐标系的原点并非完全静止的,所以就会从中引入误差. 以及还需要提出一点,我们假定云台是完全静止是错误的吗?从结果来看,我的车往前走两米和对方的车向后退两米从解算来看没有区别,因为我们解算出来的全是相对位置并非绝对位置.所以这个假设不能说是错的.但是如果从卡尔曼的角度来看.我们真正想要的是对方的车的实际位置,也就是我们放进卡尔曼的矩阵当中都是以对方的车进行估计的,但是如果我们我们两方的车开始同时运动呢.对方做匀速运动,我方车做非匀速运动.但是我们坐标系的建立逻辑是假定自己的云台完全静止的.那我们的非匀速和对方的匀速全都会叠加在EKF对地方车的估计当中.从而失真.但是还是要说的是,卡尔曼本身是对事物进行高斯分布的信念进行的.那我们的车和对方的车都是呈现高斯分布,那两个高斯融合依旧还是高斯.所以可能对最终的效果影响并不会有想象中的大(过程噪声弱化). 这部分只是作为理论进行的推导,缺少实际实验的数据进行支撑,所以只在这里提出一个可能存在的优化方向.

EKF

作为整个自瞄系统的核心.理解卡尔曼才能真正触及到核心.我们一步步来理解就好.尽量从底层开始理解.

首先我们先理解一个概念: 我们的系统每一帧确实都能通过 YOLO + PnP 得到一个目标位置。比如代码里会先得到相机系下的 tvec，再转成 armor.xyz_in_world。 但问题是：测量值不等于真实值。

即我们的得到的数值是:armor.xyz_in_world=真值+噪声.也就是我们是必定无法得到真值的,得到的所有数值都是带有噪声的.

那卡尔曼针对的核心问题就是: 我已经有一个对目标位置的预测， 现在又来了一个有噪声的测量， 我应该相信谁多一点？

所以我们不应该将卡尔曼简单的抽象成滤波这样去理解.本质是在对我们得到的数值维护以后信念: 目标现在大概在哪里？ 我对这个判断有多确定？

举个例子来说就是: 假设现在的装甲板的距离我的相机的实际位置是: X=5m 然后我们的pnp每帧都在解算出以下的数值: 4.99,5.02,5.1,5.02,5.0,4.95

原因有很多,相机内参,光线,曝光等等,都会影响我们直接获取这个实际位置,也就是我们得到的是带有噪声的数值. 那我们就可以设计一个一维的卡尔曼,从这些带有噪声中的测量中估计我们想要的真值

那回到的我们项目当中.,我们的EKF中估计的是:// x vx y vy z vz a w r l h 也就是整车的状态,位置在 x y z，速度在 vx vy vz，还有目标旋转角 a、角速度 w、半径 r 等。

也就是说EKF要做从PnP 给的是“带噪声的当前观测”，估计“连续运动中的真实状态”。

状态向量

也就是我们想要估计的数值.也就是设计卡尔曼的第一步:究竟要估计什么 可以这样理解:状态量不要把所有能想到的东西都放进去，只要放入那些能描述系统当前状态，并且能推导未来状态的变量。

简单的例子: 假设目标在一条直线上运动, 当前估计为x = 5 m,vx = 2 m/s 那过了0.1s之后,假设目标短时间内匀速运动的话,那么就是: x_new = x + vx * dt = 5 + 2 * 0.1 = 5.2 m

vx_new = vx = 2 m/s 那写成状态向量是:[x, vx] -> [x + vx * dt, vx] 这也是后面状态转移矩阵 F 的来源。

那对我们这个项目当中的话,真正想要的是整车的状态量 也就是:

x, y, z     目标车旋转中心在 world 系下的位置
vx, vy, vz  目标车旋转中心在 world 系下的速度
a           当前目标车朝向角，或者说装甲板旋转基准角
w           目标旋转角速度
r           装甲板到车体旋转中心的半径
l           长短半径差，用来描述四装甲板目标的非对称半径
h           高低装甲板高度差

初始化:

auto center_x = xyz[0] + r * std::cos(ypr[0]);
auto center_y = xyz[1] + r * std::sin(ypr[0]);
auto center_z = xyz[2];

Eigen::VectorXd x0{
  {center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, r, 0, 0}
};

注意注意注意,这里进行初始化的xyz是整车中心的xyz,不要单独的理解成把装甲板.

方差和协方差矩阵 P

首先我们前面说过,我们对所有得到的数值都是带有噪声的,不确定的.应此我们对所有得到的数值都存在一种信念分布,这个分布一般是拟成高斯分布.原因一是高斯范围确定,二是高斯融合之后仍是高斯. 那也就是我们所有得到的数值不应该直接赋予100%的信任,对所有数值都以带有一个不确定性的方式进行估算的.这个不确定性在这里就是以方差的形式表达的,拿前面的例子,我们认为目标大概在5米左右,如果误差比较小,就是:x ≈ 5m,方差小 简单理解一下就是我们这个测量值,这个变量x围绕平均值散开的程度.

那现在我们有了一个简单的状态向量,x,但是如果我们想要知道这个向量的变化趋势的话,就不能只设置一个简单的x, 而是一个完整的[x,vx],这个也是一个最简单的一维的状态向量 那按照前面的说法,这个x和vx,我们应该以一种不确定性进行描述的.即,x和vx都拥有方差.但是,他们的方差是相互独立的吗? 显然不是,他们之间一定具有存在相关性//这里简单理解就好,简单说就是他们都是描述同一物体的存在形式的变化,自然具有相关性 那协方差就是用来描述:一个变量的误差变大时，另一个变量的误差是否也会一起变？

我们的简单的一维状态量是

[x,vx]

那对应的协方差矩阵就是:

P =
[ Var(x)      Cov(x, vx)  ]
[ Cov(vx, x)  Var(vx)     ]

Var(x),Var(vx)是对我们的状态量的不确定 Cov(x, vx),Cov(vx, x)是状态量之间的不确定性的关系

那P在卡尔曼内部的作用就是,维护对整个状态估计的可信度.也就是自信程度,如果状态估计准的话,P就维护整个状态估计的存在,如果不准就用来引入观察量进行补偿 卡尔曼构建的时候会将P0保存到内部的P

ExtendedKalmanFilter::ExtendedKalmanFilter(
  const Eigen::VectorXd & x0,
  const Eigen::MatrixXd & P0,
  ...
)
: x(x0), P(P0), ...

这边就可以看出来:

ekf_.x = 当前估计的目标状态
ekf_.P = 对当前目标状态估计的不确定性

首先我们都知道,卡尔曼最核心的内容的就是不断进行融合预测和观测 我们在前面做了状态估计向量的初始化,是用来估计整车的. 那同样,估计有一个独立的向量,观测的也有.在我们的项目中,观测来源于我们对装甲板的的解算,也就是PNP解算给出的6DoF位姿 观测向量:z = {yaw, pitch, distance, armor_yaw}. 同样的,状态估计拥有对应的协方差矩阵P,观测向量也拥有对应的协方差矩阵R

Eigen::VectorXd R_dig{
    {4e-3, 4e-3, dist_var,
     log(std::abs(armor.ypd_in_world[2]) + 1) / 200 + 9e-2}
};

Eigen::MatrixXd R = R_dig.asDiagonal();

这里插入一下,dist_var在这里是做了特殊处理的

double range_scale = std::clamp(dist / 3.0, 1.0, 2.5);
double side_scale = 1.0 + 4.0 * delta_angle * delta_angle;
double dist_var = 1.0 * range_scale * range_scale * side_scale;

即,对distance的观测噪声还加入了对装甲板距离,侧视角,的实变化调整,这里的意思是说,当PNP解出来的distance越大的时候,在装甲板位于侧方向被解出来的时候(灯条之间的像素对比),算出来的dis_var就越不可靠,但是这里的不可靠不是协方差定义的,而是在输入到协方差之前就做了一次判断.PNP的distance解算受太多影响了,cv的给点,相机内参,PNP解算方式的选择,这些都会很影响距离的解算,我自己在调车的过程中由于这个问题困扰很多次.重复了很多次相机内参的标定,但是对于侧面装甲板的解算仍然出现过,5米的靶车,侧面装甲板解算出6米的情况,进而影响到EKF层对整个观测量的信念.也就是会影响到这里的R,如果频繁出现dist_var 变大 -> distance 观测噪声 R 变大 -> EKF 更新时更少相信 distance 观测的情况,这时候可能就会出现整个系统对观测层的不信任了,有可能就会被状态估计带偏导致观测拉不回来的情况. 代码里面做了

if (dist_residual > 3.0 * dist_sigma) {
  z[2] = z_pred[2];
}

如果观测方差超过了3dist_sigma的时候,就直接使用预测distance这次的观测了,也就是说如果距离长时间不准的话,基本就是靠EKF内部的预测进行距离估计了.时间越长越不准.

注:这里依旧只是提出存在的问题,实际效果一定要根据实际情况再做定夺

预测 F和Q

现在有了状态估计向量x,有了用描述状态估计和观测量的不确定性的P和R

那前面说过,要用观测和预测不断融合,我们现在还少了而预测,预测需要什么--在卡尔曼中称之为模型 实际上就是用来描述物体的运动状态的. 举个例子: 状态向量:

x_state = [x, vx]

我想要预测下一帧的x和vx是不是就是

x_new  = x + vx * dt
vx_new = vx

也就是:

[ x_new  ]   [ 1  dt ] [ x  ]
[ vx_new ] = [ 0  1  ] [ vx ]

那么这边的矩阵

F = [ 1  dt
      0  1  ]

就是F,也就状态转移矩阵.简单来说就是使用用和这个描述运动状态,以此预测下一时刻的状态的. 代码里面状态向量是11维的,因此对应的状态转移向量也是11*11维的

void Target::predict(double dt) 
{
  // 状态转移矩阵
  // clang-format off
  Eigen::MatrixXd F{
    {1, dt,  0,  0,  0,  0,  0,  0,  0,  0,  0},//x
    {0,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0},//vx
    {0,  0,  1, dt,  0,  0,  0,  0,  0,  0,  0},//y
    {0,  0,  0,  1,  0,  0,  0,  0,  0,  0,  0},//vy
    {0,  0,  0,  0,  1, dt,  0,  0,  0,  0,  0},//z
    {0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0},//vz
    {0,  0,  0,  0,  0,  0,  1, dt,  0,  0,  0},//a
    {0,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0},//w
    {0,  0,  0,  0,  0,  0,  0,  0,  1,  0,  0},//r
    {0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  0},//l
    {0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1}//h
  };

仔细看,实际上这里我们对所有运动姿态都抽象成匀速模型.也就说我们的预测都是以下一时刻会以匀速和匀角速度进行的假设进行的.

同样的预测不只是简单向前预测一个时刻,还会将不确定性向前传播一个时刻,也就是P也会随着预测向前传播

Eigen::VectorXd ExtendedKalmanFilter::predict(
  const Eigen::MatrixXd & F, const Eigen::MatrixXd & Q,
  std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f)
{
  P = F * P * F.transpose() + Q;
  x = f(x);
  return x;
}

从代码上看就知道,向前传播一个时刻之后,P也随着向前的时刻去叠加不确定性 其实理解起来也很简单,预测的步长越长,对未来的预测就越不准.

但是可以继续看代码,其中有个Q被加权到P矩阵当中了,这个Q就是我们说的过程噪声协方差矩阵

为什么要有这个?

前面我们有P,用来描述我的状态量X的不确定性. 那这个Q就类似P,用来描述F状态转移矩阵的不确定性 前面有说,我们的F将所有运动都抽象成匀速变化,以方便我们向前推,但是实际上显然不可能真的全都是匀速运动的,变速变向反转都会导致通过F预测的下一时刻完全错误,所以这个Q就是用来描述:建立的运动模型的不确定性有多高 也就是Q越小越信任建立的运动模型,认为我们的目标几乎就是匀速运动.反之亦然 Q的构建:

  Eigen::MatrixXd Q{
    {a * v1, b * v1,      0,      0,      0,      0,      0,      0, 0, 0, 0},
    {b * v1, c * v1,      0,      0,      0,      0,      0,      0, 0, 0, 0},
    {     0,      0, a * v1, b * v1,      0,      0,      0,      0, 0, 0, 0},
    {     0,      0, b * v1, c * v1,      0,      0,      0,      0, 0, 0, 0},
    {     0,      0,      0,      0, a * v1, b * v1,      0,      0, 0, 0, 0},
    {     0,      0,      0,      0, b * v1, c * v1,      0,      0, 0, 0, 0},
    {     0,      0,      0,      0,      0,      0, a * v2, b * v2, 0, 0, 0},
    {     0,      0,      0,      0,      0,      0, b * v2, c * v2, 0, 0, 0},
    {     0,      0,      0,      0,      0,      0,      0,      0, 0, 0, 0},
    {     0,      0,      0,      0,      0,      0,      0,      0, 0, 0, 0},
    {     0,      0,      0,      0,      0,      0,      0,      0, 0, 0, 0}
  };

  auto a = dt * dt * dt * dt / 4;
  auto b = dt * dt * dt / 2;
  auto c = dt * dt;

这里的构建方式可以看到,加速度会影响速度,速度又会影响位置,dt时刻越大整个影响就会越大

在代码里面可以看到

if (name == ArmorName::outpost) {
  v1 = process_noise_.outpost_accel_noise;
  v2 = process_noise_.outpost_omega_noise;
} else {
  v1 = process_noise_.normal_accel_noise;
  v2 = process_noise_.normal_omega_noise;
}

分为前哨站和运动目标两种.由于前哨战转速已知且近乎可以视为匀速运动,因此对前哨站的Q一般很小

现在我们已经简单了解EKF中比较核心的工作矩阵 X状态向量用来放入我们想要估计的状态,p协方差矩阵用来描述这些状态量之间的不确定性,Z观测向量,R观测的协方差矩阵,F状态转移矩阵,Q过程噪声矩阵

但是这时候一定会疑问,我的Z明明只有yaw,pitch,distance,armor_yaw.怎么得出X中这么多的状态量?

这就一定要搞清楚一件事情,不是用Z推到X,而是X推到Z.也就是我先假设一个状态量,然后在看Z是多少,再去修正我的预测 这就像,我在看一本书之前,我先按照我的假设里面是什么内容,然后我再翻开一页去对比我的假设,再去修正我的假设,然后再猜再翻

流程是:

当前状态 x
  ↓
h(x)
  ↓
预测观测 z_pred
  ↓
真实观测 z
  ↓
残差 z - z_pred
  ↓
Kalman Gain
  ↓
修正 x

代码里面对应着

auto h = [&](const Eigen::VectorXd &x) -> Eigen::Vector4d {
  Eigen::VectorXd xyz = h_armor_xyz(x, id);
  Eigen::VectorXd ypd = tools::xyz2ypd(xyz);
  auto angle = tools::limit_rad(x[6] + id * 2 * CV_PI / armor_num_);
  return {ypd[0], ypd[1], ypd[2], angle};
};

切记一件事:每块装甲板的位置不是z直接给的 是由目标车状态生成出来的 状态量X里面有:x, y, z：目标车旋转中心 a：目标车朝向角 r：装甲板半径 l：长短半径差 h：高度差

然后再使用这个状态量算每一块装甲板的位置

auto angle = tools::limit_rad(x[6] + id * 2 * CV_PI / armor_num_);
auto r = (use_l_h) ? x[8] + x[9] : x[8];

auto armor_x = x[0] - r * std::cos(angle);
auto armor_y = x[2] - r * std::sin(angle);
auto armor_z = (use_l_h) ? x[4] + x[10] : x[4];

装甲板位置是从X这个状态量中推算出来的,切记不是从观测中直接得到的

这里说一下,EKF对整个自瞄影响很大的数值角速度w,这个会直接影响我们自瞄的选点,因此很重要 首先,我们是没有办法观测出来速度和角加速的,所以vx,vy,vz,w这些全部都是通过连续时间推算出来的 很简单的道理,我们可以看到物体位置,可以知道时间,自然可以算出来速度 第一帧:5米 第二帧:5.2米 时间0.1 就可以算出来速度大概是2m/s 当然EKF中不是这么简单推算出来的,要设计F,P,H,K这些矩阵和增益进行修正 而且,虽然我们没有办法直接得到速度,但是我们的位置和速度和P中是有相关性的,所以哪怕只看得到位置,位置残差也能修正速度

状态量更新就是:x = x_add(x, K * z_subtract(z, h(x)));//K是Kalman Gain,卡尔曼增益,比较基础的概念

那角速度呢?角速度不能通过位置算出来吧

看我们的z中的armor_yaw,这个变量叫做装甲板朝向角 具体的指是:当前被观测到的这块装甲板，在 world 坐标系下的 yaw 朝向角 也是通过pnp解算得到的:

Eigen::Matrix3d R_armor2camera;
cv::cv2eigen(rmat, R_armor2camera);

Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera;
Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal;

armor.ypr_in_world = tools::eulers(R_armor2world, 2, 1, 0);

然后在EKF观测当中:

const Eigen::VectorXd &ypr = armor.ypr_in_world;
Eigen::VectorXd z{{ypd[0], ypd[1], ypd[2], ypr[0]}};

ypr[0] 就是 armor_yaw

有了朝向角,通过连续的朝向角度的变化,就可以算出来:

a：目标车当前旋转角
w：目标车角速度

看我们前面的状态转移向量

a_new = a + w * dt
w_new = w

如果 w 不为 0，那么下一帧预测出来的 a 就会变化。

然后在项目中第ID块装甲板的朝向角是

angle = x[6] + id * 2 * CV_PI / armor_num_;
x[6] = a
x[7] = w

所以在h(x) 预测出来的第四个观测量就是：

预测 armor_yaw = a + id * 装甲板间隔角
同时观测中也有一个
真实 armor_yaw = ypr[0]

也就是预测和观测都有装甲板的朝向角

所以角速度不是一帧算出来的是通过连续 armor_yaw 残差 + a = a + w*dt 的运动模型 估计出来的

update和Kalman Gain

前面提到过Kalman Gain,卡尔曼增益 现在有了X状态向量有了Z观测向量,现在的问题是,怎么将这两个矩阵进行融合,以及每个矩阵的占比是多少 不能只看观测,也不能只相信预测,两者之间都会有不确定性 也就是X和Z之间都有不确定性,那我们前面怎么对描述这两者之间的不确定性? 那就是---P和R协方差矩阵 如果是一个简单的一维增益

那就是:K = P / (P + R)
如果K = P / (P + R)

说明预测不可靠，观测可靠：

K 接近 1
更新时：
x_new = x_pred + K * (z - x_pred)

反之亦然 这就是卡尔曼增益的基本原理

对应项目中的卡尔曼增益部分

Eigen::MatrixXd K =
    P * H.transpose() * (H * P * H.transpose() + R).inverse();
P       : 11 x 11
H       : 4 x 11
H^T     : 11 x 4
R       : 4 x 4

K       : 11 x 4

一个11*4的矩阵

这个矩阵维度就对应着我们的观测向量和预测向量的维度,因为他本质是要进行将一个四维的向量分配到11维的向量当中的

然后就是更新部分

x = x_add(x, K * z_subtract(z, h(x)));

拆开看:

z_subtract(z, h(x))  -> 4 维观测残差
K * residual         -> 11 维状态修正量
x_add(...)           -> 把修正量加回状态 x

更新本质就是融合,通过不确定性计算出K之后将观测去修正预测

同时还会有P的更新,因为经过一次观测修正后，我对状态的不确定性也要改变

EKF的由来

如果你知道EKF那么就是知道KF,但是我们的项目当中为什么会被叫成EKF呢? 其实本质就在与一个:因为观测函数 h(x) 是非线性的。

一个简单的KF,就是状态到观测之间是线性关系

如果状态是x,x_state = [position, velocity] 观测是:z = position 那那观测矩阵就是固定的H = [1, 0]

但是我们的观测和状态直接不是简单的线性变化,状态是11维,但是观测只有4维 也就是不是直接将观测推到状态的 状态的x vx y vy z vz a w r l h和观测的yaw, pitch, distance, armor_yaw 不是线性关系 不能像KF将两者之间写成:z = Hx 只能写成z = h(x) 这样的函数对应关系,h(x) 就是非线性观测函数。

但是EKF也不是把非线性的问题解了,而是给一个近似 在当前状态 x 附近，把 h(x) 近似成线性的

至此EKF部分已经基本讲完了,其实也只是将比较重要的部分拆开简单的了解了一下背后的原理,但是已经很复杂了,其实如果只是调参使用的话,只要把noise部分领出来说就好,但是我始终认为,不对其中的原理深究,是一种傲慢,以此不管究竟需不需要,一定还是要有探究本源的精神在的.

终语

本篇只描述了EKF部分和坐标系部分,这两个部分是我认为自瞄中最重要的部分,当然还感知层,yolo,cv,pnp这些,由于思路本身比较单一,这里就不做赘述.以及很多针对实际自瞄的工程优化,比如提到过的对distance的优化,对装甲板选板的逻辑,对装甲板匹配的优化,以及发散重置的逻辑,这些都值得花时间继续探究.限于篇幅这边就不多说了.从可以用到可以用的好,这之间的差距不是一般的大,应此千万不要包有自大的想法,认为我已经懂了,我已经知道了这些想法都不可取.预祝各位可以调出一版自己满意的自瞄,让自己的队伍在场上大方光彩.