视觉完整形态文档

战术定位,核心研发功能点,规划调整

核心研发功能点中的“自瞄预测与控制”的规划有所修改，针对追踪与开火逻辑进行了修改，从原来的EKF预测加双环PID控制，改为运动学模型结合MPC优化，并剥离开火逻辑的方案。

自瞄方案: 由于目前开火延时有将近200ms和开源的20ms的开火延时有较大差距; 如果继续沿用原方案MPC预测近200ms延时(20个时间片)来判断提前开火极不严谨。故而将开火决策从MPC中剥离,转而由独立火控执行,由于双环PID控制存在稳态误差,而开源使用的是计算力矩算法，鉴于控制算法替换周期紧张、实现复杂度高且存在稳定性风险,故而考虑将含时延提前量的预测较大直接发给MPC进行平滑轨迹优化

算法设计

1.功能简介与pipeline

自瞄系统是 RoboMaster 机器人视觉火控的核心模块，其目标是在复杂战场环境中快速、准确地识别敌方装甲板，预测目标运动状态，解算弹道，并输出云台控制指令和开火信号，实现自动瞄准与射击。系统需适应步兵、英雄、哨兵等多种车型，支持静止、旋转、平移及小陀螺等运动模式，并在远距离、低弹速等条件下保持较高命中率。

输入: 相机图像流（实时采集，一般为 YUV 或 RGB 格式）机器人自身姿态（IMU 数据，通过串口接收）弹速、摩擦轮状态等火控信息（串口接收）

输出: 云台期望角度（pitch 和 yaw）开火标志及打弹时机调试信息（可视化图像、状态数据等）

整体 Pipeline: IMG_20260311_232713.jpg

2.重要算法原理阐述,公式推导

卡尔曼滤波:

预测： image.png

更新：

image.png

扩展卡尔曼:

image.png

熵权法:

image.png

坐标变换:

image.png

降自由度yaw角优化:

image.png

最小二乘法角点优化:

image.png

PnP算法:

image.png

弹道解算:

无空气阻力弹道解算: image.png

线性空气阻力弹道解算: image.png

MPC轨迹规划:

系统模型: image.png

MPC优化问题: image.png

参考轨迹生成: image.png

控制输出: image.png

3.算法性能,优缺点分析,优化方案

优点

多算法融合提升精度采用熵权法与扩展卡尔曼滤波（EKF）相结合，在状态估计中动态调整观测权重，有效融合多传感器信息，提升目标状态估计的鲁棒性和准确性。降自由度 yaw 角优化针对车辆旋转运动，通过降自由度方法将 yaw 角独立建模，配合最小二乘法对角点进行优化，减少了姿态解算的耦合误差，提高了对旋转目标的跟踪性能。角点特征优化引入PCA主成分分析对角点进行降维和去噪，增强了角点检测的稳定性；同时结合打表弹道补偿，将实际射击试验数据用于离线修正，有效补偿了系统误差。约束平面法误差补偿利用靶面或场地平面约束，对 PnP 解算出的位姿进行平面投影修正，减小了因深度估计不准引入的垂直方向误差。模型适配性强系统支持标准步兵、前哨站等不同运动特性的目标，可通过 YAML 配置文件灵活调整卡尔曼模型、MPC 参数和弹道系数，便于针对不同车型和场景进行调优。测试体系完善提供详细的调试指南和测试方案，涵盖从单模块验证到整车集成的全流程，确保算法在实车上的稳定部署。

缺点与局限性

测距精度不足单目视觉下 PnP 解算在远距离（>5m）时深度误差急剧增大，导致落点偏低且弹道补偿失效。虽然尝试了双目标定，但改善效果微弱；硬件上受限于成本无法采用激光测距或长焦相机。弹道模型不完善当前弹道解算未考虑空气阻力，在远距离下落点偏差严重。即使引入线性阻力模型，也对测距误差极其敏感，无法稳定工作。参考上交方案，需采用四阶龙格-库塔数值积分方法精确求解弹道。系统延时过大 ROS 节点间的图像传输延迟严重，限制了自瞄帧率；采集、识别、处理未完全多线程化，导致整体延时约 200ms，极大削弱了 MPC 的预测能力（现 MPC 命中率仅为传统方法的 25%），且云台 PID 跟随存在稳态误差。发弹延迟未补偿未考虑从决策到子弹击发的延迟（包括机械和电磁阀响应），导致实际开火时刻目标已移动。可借鉴上交的“历史查询”或同济的 MPC 提前开火策略进行补偿。装配误差未完全标定相机与枪管之间的装配误差（包括光轴平行度和枪管下垂）未经过精确标定，导致弹道补偿存在系统性偏差，需要引入弹道闭环（如上交方案）或自研弹道拟合方法。 CV 模型对平移目标跟踪不佳当前采用恒定速度（CV）模型，对于目标加速度突变（如急停、变向）存在过量预测，导致跟随超调。需引入更合理的恒定加速度（CA）模型，并结合电控时延补偿和差分前馈 PID。 MPC 尚未工程化落地虽然已集成同济开源的 MPC 算法，但因延时大、参数调优困难，目前处于半成品状态，需与电控深度协作，参考西工大等开源方案进行实车适配。哨兵全向感知能力不足哨兵节点需处理多路相机数据，当前采集与识别未分离，严重降低帧率。需实现采集和识别多线程（如共享内存），提升全向感知的实时性。图像传输延迟限制帧率 ROS 节点间的图像话题传输存在较大延迟，建议采用共享内存或零拷贝传输机制（如 ROS 2 或自研传输层）来突破瓶颈。

4.算法库介绍与接口说明:

ROS Melodic：作为通信框架，负责节点间通信、参数服务器、TF坐标变换以及日志记录。 OpenCV 3.4.5：用于图像处理、PnP位姿解算，以及基于HOG特征的SVM数字识别。 Eigen3 3.3.7：提供高效的矩阵运算，支持卡尔曼滤波和坐标变换。 yaml-cpp 0.6.3：用于解析YAML格式的配置文件。 Boost 1.65.1：提供串口通信、多线程和定时功能。海康SDK (GalaxyView)：控制相机并采集图像。 SVM分类器：自训练模型，用于数字识别（基于HOG特征）。 OpenVINO™：Intel推出的推理优化工具包，用于加速深度学习模型（如目标检测、分类）在边缘设备上的部署与推理。

5.算法结果(展示图像,图表,中间过程等):

自瞄重投影可视化界面识别步兵 image.png

识别英雄 b244b25d1059d13d4a0242697dba0853_720.jpg

俯视图调试界面 Cache_2c66856de0b533e9.jpg

软件设计

1.系统架构:

所有机器人使用同一套代码框架,其中算法库及接口主要有: opencv,yolov8,eigen,openvino,ros,tinympc,yaml,海康SDK等

rm_hikcamera: 海康相机驱动节点,调用相机驱动,发布相机图像 rm_identify: 识别节点,订阅相机图片,进行识别处理后,发布敌方装甲板信息 rm_msgs: 信息机制管理节点,对各个节点间通信内容进行管理 rm_sentry: 哨兵节点,哨兵机器人特有,里面包括哨兵机器人导航相关内容 rm_serial: 串口节点,订阅到要击打的目标信息,发布到串口,与下位机(开发型C板)通信 rm_tracker: 跟踪节点,订阅敌方装甲板信息,进行决策处理,发布期望电控瞄准的yaw角和pitch角 rm_fu: 打符节点,订阅能量机关信息,发布视觉期望瞄准的yaw角和pitch角

┌─────────────────────────────────────────────────────────────┐ │ 应用层
│ - 主控流程 (main.cpp)
│ - 状态机 (开火逻辑/选板)
│ - 调试与可视化接口
├─────────────────────────────────────────────────────────────┤ │ 功能模块层
│ - 识别模块 (ArmorDetector)
│ - 跟踪模块 (Tracker/KalmanFilter)
│ - 轨迹规划 (MPCController)
│ - 弹道解算 (BallisticsSolver)
│ - 坐标变换 (CoordinateTransformer)
├─────────────────────────────────────────────────────────────┤ │ 中间件层
│ - ROS Melodic (通信/消息/参数服务器)
│ - 配置管理 (yaml-cpp)
│ - 日志系统 (rosconsole/自定义)
│ - 线程池 (C++11 std::thread)
│ - 数值计算库 (Eigen3)
│ - 图像处理库 (OpenCV)
│ - 串口通信库 (boost::asio)
├─────────────────────────────────────────────────────────────┤ │ 硬件抽象层
│ - 相机驱动 (大恒SDK / V4L2)
│ - 串口驱动 (UART)
│ - 硬件时间同步 (ROS Time)
└─────────────────────────────────────────────────────────────┘

硬件抽象层相机驱动：封装大恒相机SDK，提供图像采集接口，支持参数调节（曝光、增益、白平衡）。串口驱动：基于boost::asio实现串口收发，负责与电控板（C型开发板）通信，接收IMU/弹速数据，发送云台角度和开火指令。时间同步：使用ROS Time作为统一时间基准，确保各模块时间戳一致性。

中间件层 ROS Melodic：作为核心通信中间件，提供节点间消息传递（图像、目标位置、调试数据）、参数服务器（动态调参）、TF坐标变换、日志记录功能。配置管理：采用yaml-cpp解析YAML配置文件，支持分层参数（如不同车型的卡尔曼参数）。日志系统：利用rosconsole输出分级日志（DEBUG/INFO/WARN/ERROR），并支持rqt_console实时查看。线程池：自实现简易线程池，用于并行处理图像采集、识别和多相机调度（全向感知）。数值计算：Eigen3提供高效的矩阵运算，用于卡尔曼滤波、坐标变换、MPC求解。图像处理：OpenCV实现图像预处理、灯条检测、数字分类器（HOG+SVM）等。串口通信：boost::asio提供异步串口通信，保证数据收发的实时性和稳定性。

功能模块层识别模块 (ArmorDetector)：图像预处理→灯条提取→装甲板匹配→数字识别，输出装甲板候选列表。跟踪模块 (Tracker)：核心状态估计单元，内含卡尔曼滤波器族（标准/平衡/前哨模型），负责目标滤波、预测、选板逻辑。轨迹规划 (MPCController)：基于模型预测控制生成平滑的云台期望轨迹，输出速度/加速度前馈。弹道解算 (BallisticsSolver)：根据目标距离、弹速、重力及空气阻力模型解算云台俯仰角，支持无阻力、线性阻力、四阶龙格库塔积分。坐标变换 (CoordinateTransformer)：管理相机→云台→枪管→世界坐标系的变换关系，处理装配误差补偿。

应用层主控流程 (main.cpp)：初始化各模块，启动ROS节点，循环执行“采集→识别→跟踪→解算→发送”主流程。状态机：实现自动开火逻辑（如热管冷却、弹频限制、目标切换）、选板优先级管理。调试接口：提供Rviz可视化（显示目标位置、预测轨迹、角点）、rqt动态参数配置、重投影误差曲线输出。

2.运行流程:

IMG_20260311_232713.jpg

步骤1: 图像采集和预处理步骤2: 灯条检测与装甲板匹配步骤3: 目标筛选与选板步骤4: 坐标变换步骤5: 扩展卡尔曼预测与更新步骤6: 发布可视化与调试输出步骤7: 弹道解算步骤8: 开火决策步骤9: 串口发送控制指令

3.重点功能:

3.1 多模型自适应卡尔曼滤波器解决的问题：不同车型（步兵、平衡步兵、前哨战）运动特性差异大，单一模型无法适应；旋转目标角度观测噪声大。技术方案：设计三个卡尔曼模型族：Standard（常规步兵）Outpost（前哨战，极小平移噪声）。模型参数通过熵权法结合实车数据离线优化，并支持在线动态切换。引入角度相关的距离观测噪声增益（gain参数），当目标侧对时自动增大测距噪声，避免距离跳动。提供详细的调参指南（Q/R矩阵调节原则）和调试输出（新息分析、协方差监控）。

3.2 降自由度yaw角优化解决的问题：PnP解算出的yaw角在0°附近剧烈跳变，影响跟踪稳定性。技术方案：实现上交三分法：将旋转分解为三个自由度分别估计，降低耦合。备用同济暴力搜索：在局部范围内搜索最优yaw使重投影误差最小（适用于近距离）。结合fitLine最小二乘法优化灯条角点，提升输入数据质量。

3.3 高精度弹道解算解决的问题：远距离重力补偿不足导致落点偏低；空气阻力造成弹道非线性。技术方案：集成三种弹道模型：无阻力解析解、一阶线性阻力解析解、四阶龙格-库塔数值积分（上交方案）。通过复写纸测试统计弹着点，实现弹道闭环：将实际落点与理论弹道匹配，在线修正阻力系数和弹速。考虑枪管装配误差（pitch/yaw补偿），通过激光笔微调确定补偿值。

3.4 基于MPC的轨迹规划解决的问题：电控PID跟随存在稳态误差和延时，导致高速目标脱靶。技术方案：建立云台角加速度模型，状态量为角度和角速度，控制量为角加速度。设计带参考轨迹的MPC，参考轨迹来自卡尔曼预测的未来目标位置。输出期望角度序列的同时，输出速度/加速度前馈，辅助电控PID。参数可调（预测步长、权重矩阵），以适应不同机械响应特性。当前局限性：因开火延时过大（200ms），MPC效果受限，需与电控协同优化。

3.5 坐标变换与误差补偿解决的问题：相机安装位置与枪管不重合导致瞄准偏差；C板安装倾斜导致世界系歪斜。技术方案：维护四个坐标系：相机、云台、枪管、世界，通过齐次变换矩阵关联。相机→枪管平移量由机械图纸提供，旋转量通过手眼标定或误差函数测定。实现角点坐标变换：将装甲板角点从像素系转到枪管系，确保PnP输入正确。支持在线修正补偿值（pitch/yaw offset），通过射击测试微调。

3.6 火控: 解决的问题：如何在目标运动、弹道下落和系统延迟的综合影响下，精确计算射击时机与射击角度，提高命中率。

技术方案：目标状态预测：基于运动模型预测目标在未来时刻的位置和速度，为弹道解算提供准确输入。弹道模型集成：无阻力解析解：适用于近距离快速解算。一阶线性阻力模型：考虑空气阻力对弹道的非线性影响。发弹延迟补偿：通过串口接收下位机实际发弹时刻，结合子弹飞行时间，采用历史查询或MPC提前预测将瞄准点对准子弹到达时的目标位置。射击决策：根据目标距离、速度、装甲板朝向及是否进入射程，结合安全阈值（如最小开火角度、正对性）控制ShootFlag输出。对前哨站、哨兵等特殊目标设置差异化射击策略。

3.7 装甲板识别: 解决的问题：在复杂光照和运动背景下，稳定、准确地检测装甲板并识别数字，为PnP解算提供高精度角点。

技术方案：灯条检测：基于颜色阈值（红/蓝）对图像进行二值化，提取轮廓。通过轮廓面积、长宽比、填充率、倾斜角度等几何特征筛选灯条）。使用最小二乘法拟合灯条直线，精确计算上下端点及角度。灯条匹配：根据左右灯条长度比、中心距（归一化到灯条长度）、连线角度等约束进行配对。区分大小装甲板（’s’/’l’），为后续透视变换提供依据。数字识别：对配对后的装甲板区域进行透视变换（extractNumbers），提取固定尺寸ROI（28×28）。采用LeNet/MLP模型（ONNX格式）进行分类，输出数字1~8，并过滤背景（类别9）。角点优化：对灯条区域进行PCA分析，提取对称轴沿轴向基于亮度梯度精确定位灯条上下角点，提升PnP解算的输入精度。颜色判断：统计灯条轮廓内红色/蓝色像素和，确定目标颜色并与自方颜色比对