# autoAim **Repository Path**: kehaunjack/autoAim ## Basic Information - **Project Name**: autoAim - **Description**: 深圳信息职业技术大学撼龙战队2025步兵视觉代码 - **Primary Language**: C++ - **License**: MIT - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 3 - **Forks**: 0 - **Created**: 2025-05-28 - **Last Updated**: 2025-09-16 ## Categories & Tags **Categories**: Uncategorized **Tags**: RoboMaster, RM, 视觉 ## README # autoAim ## 一、项目简介 autoAim 是一个基于扩展卡尔曼滤波（EKF）的自动瞄准系统，主要用于机器人视觉识别与跟踪装甲板目标，并结合相机标定、装甲板识别、三维姿态解算等模块，实现高精度的目标跟踪。 本项目采用 C++（OpenCV/Eigen）为主，部分工具（如相机标定）使用 Python 实现。 **相关视频：** [效果演示](https://www.bilibili.com/video/BV1C5j1ztEV9) --- ## 二、目录结构 ``` autoAim/ │ ├── Armor/ # 装甲板识别与姿态解算模块（C++） │ ├── Armor.h # 装甲板相关类与接口声明 │ ├── AngleSolver.cpp # 姿态解算实现 │ ├── LightBar.cpp # 灯条相关实现 │ ├── ArmorNumClassifier.cpp # 装甲板数字识别实现 │ ├── findLights.cpp # 灯条检测实现 │ ├── matchArmors.cpp # 装甲板匹配与筛选实现 │ ├── ArmorBox.cpp # 装甲板类方法实现 │ └── ArmorDetector.cpp # 识别器主流程 │ ├── General/ # 通用工具与配置 │ ├── General.h # 通用头文件 │ ├── General.cpp # 通用方法实现 │ ├── savePic/ # 通过waitKey按键's'和'S'保存的图案分类和相机标定图片的存放目录 │ ├── saveVideo/ # 通过录制器录制的视频的存放目录 │ ├── camera/ # 参数配置文件夹 │ │ ├── camera_param.xml # 参数配置文件 │ ├── svm/ # HOG+SVM分类器的训练结果文件的存放目录 │ │ ├── hog.xml # hog配置文件 │ │ ├── svm.xml # svm训练结果文件 │ └── tools/ │ ├── 使惯性系下朝向角连续的方法.py # 使角度连续的方法（Python） │ ├── 坐标变换cam2imu.py # 坐标变换验证工具（Python） │ ├── 绘制csv波形图.py # 绘制csv波形图（Python） │ ├── pic/ # 示例图片目录 │ ├── calibration/ # 相机标定目录 │ │ ├── opencv_calibration.py # 相机标定工具（Python） │ │ ├── input.xml # 标定参数配置文件 │ │ ├── images.xml # 图片路径列表配置文件 │ │ └── 棋盘格27_27.pdf # 标定板打印文件 │ └── classifier/ # 分类器训练目录 │ ├── pre.py # 训练样本重命名工具（Python） │ ├── train.py # 训练主程序（Python） │ └── armor_Tem # 训练样本存放目录 │ ├── not_aim/ # 被误识别的负样本存放目录（标签为0） │ └── aim/ # 正样本存放目录（标签为1~8） │ ├── 1/ # 1号英雄图案样本 │ ├── 2/ # 2号工程图案样本 │ ├── 3/ # 3号步兵图案样本 │ ├── 4/ # 4号步兵图案样本 │ ├── 5/ # 5号步兵图案样本 │ ├── 6/ # 哨兵图案样本 │ ├── 7/ # 基地图案样本 │ └── 8/ # 前哨图案样本 │ ├── Tracker/ # 目标跟踪与EKF滤波模块（C++） │ ├── kalman_filter.h # EKF类声明 │ ├── kalman_filter.cpp # EKF类实现 │ ├── tracker.h # 跟踪器类声明 │ └── tracker.cpp # 跟踪器主流程 │ ├── Serial/ # 串口通信模块 │ ├── Serial.h # 串口通信头文件 │ └── Serial.cpp # 串口通信实现 │ ├── MvsCamera/ # 海康工业相机模块 │ ├── mvs_camera.h # 相机类声明 │ └── mvs_camera.cpp # 相机类实现 │ ├── Main/ # 主程序入口与演示 │ ├── main.cpp # 主程序入口 │ └── cameraDemo.cpp # 相机演示程序 │ ├── RecordAndPlay/ # 录像与回放模块 │ ├── record.h # 录像器声明 │ ├── record.cpp # 录像器实现 │ ├── play.h # 播放器声明 │ └── play.cpp # 播放器实现 │ ├── autoAim.pro # Qt配置文件 └── README.md # 项目说明文档 ``` --- ## 三、主要模块说明 ### 1. 装甲板识别与姿态解算（Armor/） #### [Armor.h](Armor/Armor.h) - **ArmorType, Color, DetectorState** 枚举类型，分别定义装甲板类型（大小）、颜色、检测状态。 - **ArmorParam** 装甲板检测参数结构体，包含灯条和装甲板的各种阈值。 - **[LightBar](Armor/LightBar.cpp)** 灯条类，描述单个灯条的几何属性。 - **[ArmorBox](Armor/ArmorBox.cpp)** 装甲板类，包含左右灯条、装甲板顶点、中心、类型、三维位姿等信息。 主要方法： - `setArmor()`：设置装甲板顶点、中心等属性。 - `isSuitableArmor()`：判断装甲板是否为有效目标。 - **[ArmorNumClassifier](Armor/ArmorNumClassifier.cpp)** 装甲板数字识别类，基于SVM和HOG特征。 - **[ArmorDetector](Armor/ArmorDetector.cpp)** 装甲板检测器，负责灯条检测、装甲板匹配、数字识别、目标筛选等。 主要方法： - `void run(cv::Mat &src)`：输入图像，执行检测。 - `bool isFoundArmor()`：返回是否检测到装甲板。 - `void setEnemyColor(Color enemyColor)`：设置敌方颜色。 - `void setROI(cv::Rect ROI)`：设置感兴趣区域。 - **[AngleSolver](Armor/AngleSolver.cpp)** 姿态解算类，负责PnP解算、三维点投影、角度计算等。 主要方法： - `bool setCameraParam(const std::string &filePath)`：加载相机参数。 - `void setArmorSize(ArmorType type, float width, float height)`：设置装甲板实际尺寸。 - `void solve(ArmorBox &armor)`：对装甲板进行PnP解算。 - `void getAngle(Point3f point, float &yaw, float &pitch)`：三维点转角度。 - `cv::Point2f projectPoint(cv::Point3f point3D)`：三维点投影到像素。 - **[findLights](Armor/findLights.cpp)** 灯条分割与灯条几何约束。 - **[matchArmors](Armor/matchArmors.cpp)** 灯条匹配与装甲板几何约束。 --- ### 2. 目标跟踪与EKF滤波（Tracker/） #### [tracker.cpp](Tracker/tracker.cpp) - **Tracker** 主要成员与方法： - `bool run(Mat src, vector armors, ...)`：主流程，输入图像和装甲板，输出补偿角度和开火信号。 - `void setBulletSpeed(int &bulletSpeed)`：设置弹速。 - `void setTargetNum(int &targetNum)`：设置目标装甲板编号。 - `cv::Mat show(float add_yaw, float add_pitch, int width)`：可视化跟踪与预测结果。 - `void initEKF(ArmorBox &armor)`：初始化EKF状态。 - `void updateArmorsNum(ArmorBox &armor)`：根据装甲板类型更新数量。 - `void armorJump(ArmorBox &armor)`：处理装甲板跳变。 - `void getR()`：计算旋转矩阵。 - `void cam2imu(ArmorBox &armor)`：相机系到惯性系坐标变换。 - `void solve(ArmorBox &armor)`：调用 AngleSolver 进行PnP解算。 - `cv::Point3f imu2cam(cv::Point3f tVec)`：惯性系到相机系。 - `cv::Point2f imu2img(cv::Point3f tVec)`：惯性系到图像像素。 - `void getAngle(Point3f point, float &yaw, float &pitch)`：三维点转角度。 - `float getArmorYaw(ArmorBox &armor)`：获取装甲板Yaw角。 - `Eigen::Vector3d getArmorPositionFromState(Eigen::VectorXd &x)`：从EKF状态获取装甲板位置。 --- ## 四、改进点 - `ROI（感兴趣区域）`：引入`ROI`（只减少图像高度，宽度需与原图一致），避免了对`全图`做处理，减少了噪声的影响，比如地面镜像反光、日光灯等干扰因素。同时加快了图像处理速度，减少了耗时。 - `灯条分割`：在`通道差分`基础上，利用`最小外接矩形`和`掩码图`截取`灰度图`中的灯条区域，分别对每个灯条区域进行`OTSU二值化`，分割出完整的灯条特征。与通道差分法相比，解决了灯条`中心过曝`只分割出灯条轮廓特征的问题，同时提高了灯条顶点的精度（实验结果：0.5~3m的像素点标准差为0.09px）。 - `图案分类`：图像来源从`RGB原图`改成了`己方颜色的单通道图`。因为图案都是白色，所以对分类没有影响，同时还能减少处理步骤，以及避免地面反光的影响（三根同颜色灯条的情况，灯条排列情况如 `| | |`，中间灯条容易被当成图案进行分类，且大概率会被识别成`1号英雄`）。 - `NMS（非极大值抑制）`：去除交叠重复的装甲板。当多个矩形框存在交叠，且图案分类结果相同（0除外）时，选择面积最小的矩形作为真正的装甲板，其他则被剔除。 - `PnP位姿估计`：IPPE解算结果为`nan`时，改用迭代法求解位姿，避免出现疯车的情况。 - `装甲板左右跳变`：通过PnP解算的姿态`rvec`不太准确，如果单纯用朝向角来判断旋转装甲板是否左右跳变的话，可能会出现漏判的情况，从而没能成功切换高低装甲板，导致滤波器的估计出错。因此，加入了`水平方向的帧间位置差`来判断跳变。 --- ## 五、一些要注意的问题 - `角度连续处理`：陀螺仪的Yaw轴角度范围为`[-180°, 180°]`，在`-179°`和`179°`之间来回转动或跳变时，当前帧Yaw减上一帧Yaw的值会特别大，会导致车子360°大幅度翻转，也可以说是疯车。可以很肯定地说，25赛季的步兵疯车大概率是算法的锅。 ```cpp // 示例 // 计算帧间云台陀螺仪Yaw轴转动的差值 float d_imu_yaw = (imu_yaw - last_imu_yaw); // 当跨过+-180度时的处理 if (abs(imu_yaw) + abs(last_imu_yaw) > 180) { if (imu_yaw < 0 && last_imu_yaw > 0) { d_imu_yaw += 360; } else if (imu_yaw > 0 && last_imu_yaw < 0) { d_imu_yaw -= 360; } } last_imu_yaw = imu_yaw; ``` - `pitch轴的重力补偿`：由于25赛季的步兵弹速为30m/s，在5m内的弹丸轨迹几乎为直线，可以不用做重力补偿，其实项目原本是有做补偿的，但补偿后反而不准了，所以去掉了。如果补偿准的话，还是有必要加上的。 - `发送的角度形式`：如果用角度增量的形式给到下位机，可能会出现云台转过头的情况，因为这个增量是由下位机上一帧给的数据计算而来，当下位机拿到增量时，下位机的数据已经处于当前帧的数据，而非上一帧数据，所以会存在当前帧角度减上一帧角度的偏差。理应直接发送惯性系下的瞄准位置给到下位机，而下位机的云台角度赋值也需要从`上位机发送的增量值（弧度制）+当前位置值（弧度制）`的形式改成`上位机发送的位置值（弧度制）`的形式。 - `装甲板yaw朝向角`：通过PnP解算的姿态`rvec`不太准确，yaw值的观测噪声可能需要调得很大。可以尝试上交23年提出的[降自由度](https://github.com/julyfun/rm.cv.fans/blob/main/aimer/auto_aim/predictor/motion/top_model.cpp)的方法，提高yaw朝向角的精度。 --- ## 六、依赖环境 - **操作系统** - Ubuntu（任意版本） - **C++部分** - ROS1/ROS2 - OpenCV >= 4.2.0 - Eigen3 - **Python工具** - Python >= 3.6 - opencv-python - numpy --- ## 七、使用流程 ### 1. 主程序部署流程（已完成的步骤可以跳过） 1. 安装 `ROS`（推荐使用鱼香ros一键安装工具） ```bash wget http://fishros.com/install -O fishros && . fishros ``` 2. 安装 `Qt Creator` ```bash sudo apt-get install qtcreator ``` 3. 安装 [海康相机客户端](https://www.hikrobotics.com/cn2/source/support/software/MVS_STD_V3.0.1_241128.zip)，根据自身PC的CPU架构使用对应的deb文件进行安装即可 4. 使用 `Qt Creator` 打开项目，在初始化项目时，可以只勾选 `Debug` 版本的编译 5. 在 [main.cpp](Main/main.cpp) 里修改配置文件路径 ```cpp // 根据自身PC的项目放置情况，将General的绝对路径复制到以下两个宏定义的内容进行替换即可 #define GENERAL_PATH "/home/jack/autoAim/General/" #define PLAY_SOURCE "/home/jack/autoAim/General/saveVideo/cam_static.mp4" ``` 6. 点击左下角的开始图标，进行编译运行。至此已完成示例的运行 7. 更多功能的使用可以查看 [main.cpp](Main/main.cpp) 中的宏定义解释 ```cpp /************************************************* * 宏定义解释: * * DEBUG_MODE 调试模式: 显示图像。 * 关闭后为纯终端模式，速度更快，上场前可关闭。 * * UART 串口通信: 与下位机通信。 * 获取陀螺仪数据和比赛状态信息等； * 发送自瞄数据，指导云台瞄准目标， * 如果没有串口硬件可关闭此定义。 * * TRACK 跟踪器: 稳定跟踪目标。 * 过滤误识别、噪声、多目标、灯条闪烁和识别不稳定等干扰， * 避免频繁切换目标，从而导致的云台晃动。 * * RECORD 录制器: 录制视频。 * 根据是否识别到装甲板来自动录制比赛现场的情况， * 视频赛后可作为回顾、回放、分析或测试的素材。 * * PLAY 播放器: 播放视频或打开USB摄像头。 * 如果没有更新视频或USB摄像头的相机内参和畸变参数， * 则关闭跟踪器，因为解算出来的姿态是错误的。 * * ***********************************************/ ``` ### 2. 相机标定工具 [更多详情](General/tools/calibration/README.md) ```bash cd General/tools/calibration python opencv_calibration.py ``` - `` 为标定配置文件，需包含棋盘格规格、图片/视频路径等参数。 - `` 为图片列表文件，需包含图片的路径。 - 需要根据不同战车所配备安装的相机和PC，分别对其进行标定和参数配置。 ### 3. 分类器训练工具 [更多详情](General/tools/classifier/README.md) ```bash cd General/tools/classifier python train.py ``` --- ## 八、串口通信数据包定义 [Serial](Serial/Serial.cpp) ```cpp // 接收 bool Serial::getData(int *rstate, int *color, int *aim_num, int *bullet_speed, float *absyaw, float *abspitch){} // 发送 bool Serial::sendData(int tstate, int fire, float yaw, float pitch, float distance){} ``` ### 1. 接收 - rstate：接收状态（0：当前数据包无效；1：当前数据包有效），也可以用来切换识别目标（1：装甲板；2：能量机关），默认是1 - color：敌方颜色（0：蓝色；2：红色） - aim_num：目标id（0：默认优先级；1~8：对应id的优先级最高），一般用不到 - bullet_speed：发射弹速（m/s） - absyaw：陀螺仪yaw轴角度（弧度制，范围[-pi, pi]） - abspitch：陀螺仪pitch轴角度（弧度制） ### 2. 发送 - tstate：发送状态（0：当前数据包无效；1：当前数据包有效） - fire：发弹标志（0：熄火；1：开火） - yaw：控制云台的yaw轴增量（弧度制） - pitch：控制云台的pitch轴增量（弧度制） - distance：当前瞄准的目标距离（m），可提供给下位机做重力补偿，当前项目没有用到，默认给0 --- ## 九、实验结果 ### 1.顶点精度实验
图1 标准差
图2 综合情况
### 2.测距精度实验
图1 均值
图2 均方根误差
--- ## 十、常见问题 - **Q: 标定工具无法打开视频/图片？** A: 检查 input.xml 中 Input 路径是否正确，确保文件存在且有权限。 - **Q: 跟踪效果不稳定？** A: 检查装甲板识别参数、滤波器参数，适当调整 `tracker.cpp` 中的阈值。 --- ## 致谢 - [吉林大学](https://gitee.com/qunshanhe/JLURoboVision) - [华南师范大学](https://github.com/chenjunnn/rm_auto_aim) - [RM弹道解算](https://github.com/CodeAlanqian/SolveTrajectory)