坐标转换与自瞄整体思路

坐标转换与自瞄整体思路

本文基于当前仓库中的实际源码和参数编写，目标是先建立直觉，再把代码串起来。
重点不是背公式，而是始终回答三个问题：目标现在在哪里、子弹到达时它会在哪里、云台应该转到哪里。

1. 先用一句话理解整套系统

自瞄是一个带反馈的闭环：

相机拍图
→ 检出装甲板二维角点
→ PnP 求装甲板在相机前方的三维位置
→ TF 转到不随云台转动的 odom 坐标系
→ 跟踪并预测目标未来状态
→ 选择要打的装甲板
→ 计算 yaw / pitch，并补偿弹道
→ 下发绝对云台角和开火建议
→ 下位机回传真实云台姿态和弹速
→ 更新 TF，进入下一帧

可以把它拆成三个问题：

2. 坐标系到底是什么

坐标系可以理解成一把带方向的三维尺子。同一个物理点没有变，换了一把尺子测量，三个坐标值就可能完全不同。

例如，目标始终停在场地中的同一处：

• 云台向左转时，目标会移动到图像右边，因此目标的相机坐标变了；
• 目标在场地里没有动，因此正确变换后的 odom 坐标应该基本不变。

这正是为什么不能直接拿 PnP 的相机坐标做跨帧跟踪。

2.1 本项目中的主要坐标和坐标轴

注意：图像的 (u,v) 和光学坐标系的 (X,Y,Z) 都有“右、下”的方向，但前者单位是像素、只有二维，后者单位是米、是三维，不能混为一谈。

2.2 本项目的 TF 树

odom
└── gimbal_link 动态 TF：下位机实时回传 roll/pitch/yaw
└── camera_link 静态 TF：相机相对云台的安装平移和倾斜
└── camera_optical_frame
静态 TF：把 X前Y左Z上 重排为 X右Y下Z前

对应代码和参数：

• 动态 odom → gimbal_link：rm_serial_driver/src/serial_driver_node.cpp
• 静态 gimbal_link → camera_link：launch_params.yaml 中的 odom2camera 参数，名称虽然叫 odom2camera，实际被填进了 camera_joint
• 静态 camera_link → camera_optical_frame：rm_gimbal.urdf.xacro

当前安装参数为：

xyz: 0.246 0.0 0.049
rpy: 0.00 0.275 0.015

它表示相机坐标原点相对 gimbal_link 原点的安装偏移和安装倾角。xyz 的单位是米，rpy 的单位是弧度。

2.3 光学系为什么还要旋转一次

相机算法/OpenCV 常用：

X 右，Y 下，Z 前

ROS 机体系常用：

X 前，Y 左，Z 上

URDF 中的固定旋转 rpy=(-π/2, 0, -π/2) 完成了这次轴重排。对应矩阵为：

R_gimbal_or_cameraLink_from_optical =
[ 0 0 1 ]
[-1 0 0 ]
[ 0 -1 0 ]

判断一个旋转矩阵如何映射坐标轴，要看它的列：

光学系 +X（右） → 机体系 -Y（右）
光学系 +Y（下） → 机体系 -Z（下）
光学系 +Z（前） → 机体系 +X（前）

因此相机看到的“前方 4 米” (0,0,4)，转换后大致就是云台系的“前方 4 米” (4,0,0)。

3. 怎样读懂一个 TF 变换

ROS 中：

lookupTransform(target_frame, source_frame, time)

返回的是把 source_frame 中的数据变到 target_frame 的变换，可记为：

T_target_source

对一个点：

p_target = R_target_source · p_source + t_target_source

其中：

• R 改变坐标轴方向；
• t 补偿两个坐标系原点的位置差；
• 点需要旋转和平移，方向向量只需要旋转。

本项目从相机光学系到 odom 的完整链为：

T_odom_optical
= T_odom_gimbal
· T_gimbal_cameraLink
· T_cameraLink_optical

于是：

p_odom = T_odom_optical · p_optical

矩阵相乘顺序不能交换。可以从右往左读：先把光学系数据变到 camera_link，再到 gimbal_link，最后到 odom。

3.1 一个直观例子

暂时忽略安装倾角和云台转角，只保留轴重排和相机平移。假设 PnP 得到：

p_optical = (0.5, 0.3, 4.0) m

含义是目标在光轴前方 4 m、画面右侧约 0.5 m、画面下方约 0.3 m。轴重排后约为：

p_gimbal ≈ (4.0, -0.5, -0.3) m

再加相机相对云台原点的平移 (0.246, 0, 0.049)：

p_gimbal ≈ (4.246, -0.5, -0.251) m

实车还要继续考虑安装 rpy 和当前云台动态姿态，所以最终数值由 TF2 统一计算，不建议手写分量公式。

3.2 时间戳为什么和坐标转换同样重要

相机曝光时云台角度为 yaw₁，等图像处理完后云台可能已经转到 yaw₂。如果用“当前最新姿态”去转换旧图像，就会把目标投到错误的世界位置。

当前代码有意识地用图像时间戳查询 TF：

lookupTransform(odom_frame_, "gimbal_link", img_msg->header.stamp, ...)

串口侧还有 timestamp_offset，用于校正下位机反馈和相机时间之间的偏差。时间同步不准的典型表现是：云台静止时位置正常，一转动目标的 odom 坐标就摆动或拖影。

4. PnP：怎样从二维图像得到三维位置

4.1 PnP 已知什么、求什么

已知：

求解：

p_camera = R_camera_armor · p_armor + t_camera_armor

• R_camera_armor：装甲板物体系相对相机光学系的朝向；
• t_camera_armor：装甲板原点在相机光学系中的位置；
• 本项目装甲板模型使用米，因此 tvec 也是米。

最容易说反的一点是：这里求的是“装甲板在相机中的位姿”，不是最终的世界坐标。

4.2 当前项目给 PnP 的点

传统检测路径从左右灯条生成 6 个像素点：

左灯条：bottom、center、top
右灯条：top、center、bottom

并与装甲板物体系中的 6 个模型点一一对应。当前尺寸为：

小装甲板：0.133 m × 0.050 m
大装甲板：0.225 m × 0.050 m

YOLO 路径也会把四角点构造成相同接口所需的灯条/关键点数据，之后仍走 PnP。

4.3 为什么 IPPE 会给多个解

装甲板近似是一个平面，只从单目图像观察平面时可能存在姿态歧义。当前代码用 SOLVEPNP_IPPE 获得候选解，再结合：

• 重投影误差；
• 灯条在图像中的倾斜方向；
• 装甲板姿态先验；

选择更合理的解。可选 BA 会在 PnP 初值上进一步优化装甲板 yaw。

重投影误差的直觉是：把求出的三维装甲板重新投回图像，如果投影点和实际检测点差得很远，说明角点、尺寸、内参、畸变或 PnP 解至少有一项不对。

4.4 Detector 发布的位姿属于哪个坐标系

相机驱动把图像 header.frame_id 设为 camera_optical_frame。Detector 复制这个 header，并直接把 PnP 的 R/t 填入 Armor.pose，所以：

armor_detector/armors 中的 pose
所属坐标系 = camera_optical_frame

Detector 阶段没有把位置手动转换到 odom；真正的完整 TF 转换发生在 Solver 的 armorsCallback() 中。

5. 为什么要把检测结果转换到 odom

在 Solver 中，每块装甲板都会执行：

armor.pose = tf2_buffer_->transform(ps, target_frame_).pose;

当前 target_frame_ 配置为 odom，因此位置和姿态都会从 camera_optical_frame 转到 odom。

这一步解决了两个问题：

一个非常有用的正确性判据：

固定装甲板 + 主动转动云台
camera_optical_frame 中的位置应该明显变化
odom 中的位置应该基本稳定

如果两边都在明显变化，优先排查 TF 方向、外参、时间戳和 pitch 正负号。

6. 跟踪：从“这一帧的一块板”恢复“整台机器人”

6.1 为什么检测结果不能直接拿去瞄准

单帧检测会遇到：

• 角点噪声导致 PnP 抖动；
• 某几帧漏检；
• 同一台机器人有多块外观相同的装甲板；
• 机器人平移的同时还会旋转；
• 处理、通信和子弹飞行都存在延迟。

因此 Solver 不只跟踪“当前看到的板”，而是估计“目标旋转中心 + 运动速度 + 装甲板绕中心的几何关系”。

6.2 普通目标的 EKF 状态

当前普通目标的 10 维状态为：

X = [xc, vxc, yc, vyc, zc, vzc, yaw, v_yaw, r, d_zc]

相机真正测到的是一块装甲板：

Z = [xa, ya, za, yaw]

EKF 的观测模型为：

xa = xc – r · cos(yaw)
ya = yc – r · sin(yaw)
za = zc + d_zc

几何直觉如下：

装甲板 (xa, ya)
●
/
r /
/
目标旋转中心 (xc,yc) ●────→ yaw 所描述的径向/板面关系

初始化时，代码先假设 r=0.26 m，再由测到的板位置反推中心位置。之后 EKF 通过连续观测修正中心、速度、角速度和半径。

6.3 每一帧 EKF 做什么

上一帧状态
→ predict：按 dt 把中心位置和 yaw 向前推
→ 数据关联：在相同数字的板中找最接近预测位置的观测
→ update：用 PnP 测量修正预测
→ 得到新的目标中心、速度、角速度、半径

代码还用状态机降低偶发检测对控制的影响：

LOST → DETECTING → TRACKING → TEMP_LOST
↑ │
└──────── 丢失超时 ───────────────┘

• DETECTING：要连续匹配若干帧才确认；
• TRACKING：正常输出目标；
• TEMP_LOST：短时漏检仍可依靠预测；
• LOST：超时后重新选目标初始化。

6.4 为什么还要“换板”

四装甲板机器人旋转时，可见板会从一块切换到相邻板。新板的 yaw、半径和高度可能突然变化，但机器人中心没有瞬移。

handleArmorJump() 会在这种情况下：

• 更新参考 yaw；
• 交换两组半径；
• 修正高度差；
• 如果几何仍严重不一致，再用当前观测重置部分状态。

所以“装甲板 yaw 跳变”不一定是误检，也可能只是视野切到了下一块板。

6.5 前哨站为什么单独处理

前哨站是 3 块板绕固定中心旋转，当前代码没有走普通目标 EKF，而是使用 OutpostTracker：

• 收集观测并估计固定圆心；
• 使用固定半径约 0.275 m；
• 判断静止或旋转状态；
• 按 120° 间隔预测三块板；
• 单独学习三块板的高度。

它和普通目标的共同点是：最终都要输出“中心、旋转角、角速度、半径”，供后面的选板和预测使用。

7. 从目标状态到瞄准点

7.1 先预测子弹到达时的目标

当前代码使用：

dt = 数据年龄 + 子弹飞行时间 + prediction_delay

然后：

目标中心未来位置 = 当前估计位置 + dt · 线速度
未来 yaw = 当前 yaw + dt · v_yaw

其中“数据年龄”是当前解算时间减检测时间戳。这样补偿的不只是子弹飞行时间，还包括图像处理和消息传输已经消耗的时间。

如果只瞄准观测时的位置，那么目标速度越快、距离越远，落点滞后越明显。

7.2 枪口不等于 odom 原点

代码从 TF 读取 gimbal_link 在 odom 中的位置和姿态，并假设枪口在云台前方 0.095 m：

p_muzzle_odom
= p_gimbal_origin_odom
+ R_odom_gimbal · (0.095, 0, 0)

然后计算：

p_target_from_muzzle = p_target_odom – p_muzzle_odom

这里有一个很容易混淆的细节：

• 这个向量的起点已经移到了枪口；
• 但它的三个分量仍沿 odom 的坐标轴表达；
• 代码没有再乘 R_odom_gimbalᵀ 把它旋转成 gimbal_link 坐标。

因此后面的 atan2(y,x) 算出的是相对 odom 的绝对目标角，不是相对当前枪口朝向的局部角度。

7.3 根据中心恢复所有装甲板并选板

普通四板目标由中心、yaw、两组半径和高度差恢复出四块板的位置。概念上是：

p_armor_i = p_center + 绕中心旋转后的径向偏移 + 高度偏移

代码再结合：

• 目标相对枪口的方向；
• 各板朝向；
• 目标旋转方向和角速度；
• 子弹飞行时间；
• 最大侧向提前角；

选择现在适合瞄准、到弹丸抵达时仍合适的一块板。

高速旋转超过阈值时，Solver 可从 TRACKING_ARMOR 切到 TRACKING_CENTER，避免瞄准点在多块板之间高速跳变。

前哨站则优先选择正向射击窗口附近、且正在接近窗口的板，并带有单独的提前角和 ±30° 开火窗口。

8. yaw、pitch 和弹道补偿

8.1 不考虑弹道时

对“以枪口为起点、用 odom 轴表达”的目标向量 p=(x,y,z)：

yaw_geom = atan2(y, x)
pitch_geom = atan2(z, sqrt(x²+y²))

输出在内部先使用弧度，填入 GimbalCmd 时再转为度。

8.2 为什么主要补 pitch

子弹会因重力下坠，空气阻力还会延长飞行时间，因此实际枪口通常要比目标的几何仰角再抬高一些。

当前配置使用 quadratic_drag 二次阻力模型，主要参数包括：

• 弹速；
• 重力加速度；
• 阻力系数；
• 空气密度；
• 弹丸质量和直径。

弹道补偿器在当前实现中修正 pitch；yaw 的提前量主要来自目标运动预测和旋转选板，而不是重力模型。

弹速由串口消息中的 bullet_speed_flag 选择两档配置值。距离或弹速估计错误会同时影响：

8.3 最终下发的到底是绝对角还是角度差

Solver 同时生成：

pitch / yaw 相对 odom 的目标绝对角，单位：度
pitch_diff / yaw_diff 目标绝对角 – 当前云台角，单位：度

但当前 DefaultProtocol::send() 真正通过串口打包的是：

fire_advice、pitch、yaw、distance

也就是说，当前协议下发的是绝对 pitch/yaw，不是 pitch_diff/yaw_diff。差值字段主要用于调试、观察误差等上层用途。下位机必须和这里采用相同的“绝对角”协议约定，否则会出现越转越偏或指令尺度不对。

8.4 pitch 正负号要沿整条链看

串口接收后构造 TF 时，代码使用：

TF pitch = – receive_data.pitch

Solver 从 TF 取 RPY 后又执行一次：

rpy_[1] = -rpy_[1]

这是在 ROS 右手系和下位机 pitch 正方向之间转换。调试时不要只看其中一个负号就删除；要用“云台物理向上转时，反馈、TF、目标 pitch 和误差是否同向收敛”来验证整条符号链。

9. 什么时候给开火建议

普通目标要求当前云台角足够接近目标角：

允许 yaw 误差 ≈ atan2(射击窗口宽度/2, 距离)
允许 pitch 误差 ≈ atan2(射击窗口高度/2, 距离)

并设置了最小约 1° 的容忍角。距离越远，同样的物理窗口对应的角度越小，因此理论上要求越严格。

前哨站还要求：

• 目标板位于预测到达窗口的 ±30° 内；
• 板正在向合适的窗口方向接近；
• 当前云台角也满足普通的角度容忍条件。

fire_advice 只是视觉侧建议，最终是否真正发射还应由下位机结合比赛状态、摩擦轮状态和安全逻辑决定。

10. 把源码调用链完整走一遍

对应的反馈闭环为：

┌──────── 下位机姿态反馈 ────────┐
↓ │
相机 → Detector → PnP → TF → Tracker → Predictor → Solver → 云台控制
↑ │
└──── odom→gimbal TF ──────┘

11. 推荐的理解和调试顺序

不要一上来同时调检测、EKF和弹道。按下面的层次逐个证明正确，定位会快很多。

第 1 层：先证明坐标轴和 TF 正确

检查：

ros2 run tf2_ros tf2_echo odom gimbal_link
ros2 run tf2_ros tf2_echo gimbal_link camera_optical_frame
ros2 run tf2_ros tf2_echo odom camera_optical_frame

验证现象：

• 云台 yaw 左右转时，TF 中 yaw 同步变化；
• 云台向上抬时，pitch 的物理方向正确；
• 光学系蓝色 Z 轴指向相机前方；
• 静态相机外参不会随时间跳变。

第 2 层：证明 PnP 测距和方向正确

把已知尺寸的装甲板放在已知距离：

• 图像右侧目标应有 tvec.x > 0；
• 图像下方目标应有 tvec.y > 0；
• 正前方距离主要体现在 tvec.z > 0；
• 2 m、4 m、6 m 测距误差应有一致趋势；
• 重投影误差不能持续很大。

如果近远距离都按固定比例偏差，先怀疑内参或装甲板模型尺寸；如果偏差随图像位置变化明显，先怀疑畸变和角点。

第 3 层：证明相机系到 odom 的转换正确

固定装甲板，只转动云台：

ros2 topic echo /armor_detector/armors
ros2 topic echo /armor_solver/measurement

期望：Detector 中的相机系位置会变化，Solver 中转换到 odom 的测量位置基本稳定。

如果云台越快误差越大，优先检查 TF 时间戳和 timestamp_offset，而不是先调 EKF。

第 4 层：再看 EKF

静止目标时：

• 中心位置应收敛；
• 线速度和角速度应接近 0；
• 瞄准点不应比原始 PnP 更抖。

移动/旋转目标时：

• 预测位置应先于测量而不是落后；
• 换板时中心不应瞬移；
• 短时漏检应进入 TEMP_LOST 并继续平滑预测。

一般调参直觉：

Q 增大 → 更允许模型状态快速变化，响应快但可能更抖
R 增大 → 更不相信单帧测量，更平滑但可能更滞后

第 5 层：最后调预测和弹道

建议分开验证：

静止目标都打不准时，不要先用运动预测参数掩盖内参、外参、枪口偏移或弹道参数问题。

12. 最常见的理解错误

错误 1：把 PnP 输出当成世界坐标

PnP 输出属于图像 header 指定的 camera_optical_frame。只有经过 TF2 转换后才属于 odom。

错误 2：认为换坐标系只需要交换 x/y/z

真实变换通常同时包含旋转和平移，而且多个旋转的顺序不能交换。让 TF2 处理整条链。

错误 3：把 lookupTransform(A,B) 理解反

它返回把 B 中的数据转换到 A 的 T_A_B。

错误 4：用最新 TF 转换旧图像

动态场景必须尽量使用图像采集时刻的 TF，否则云台一动就产生伪运动。

错误 5：把 EKF 的 (x,y,z) 当成当前装甲板

它们是目标旋转中心；装甲板位置还要由 yaw/r/d_zc 通过观测模型恢复。

错误 6：认为减去枪口位置后就是 gimbal 坐标

减法只改变向量起点，不会改变它沿哪组坐标轴表达。当前代码减去枪口 odom 位置后仍是 odom 轴表达。

错误 7：把弧度和度混用

TF、Eigen、三角函数内部主要用弧度；串口的云台角和最终 GimbalCmd 使用度。

错误 8：认为串口下发的是 yaw_diff/pitch_diff

当前默认协议实际发送绝对 yaw/pitch。协议两端必须完全一致。

13. 当前源码中值得特别留意的细节

下面不是理解主流程的前提，但调试异常时很重要。

13.1 imu_to_camera_ 的变量名和实际查询内容不一致

ArmorDetectorNode::imageCallback() 查询：

lookupTransform(odom_frame_, "gimbal_link", image_time)

取到的旋转实际是 R_odom_gimbal，却被存入名为 imu_to_camera_ 的变量，并作为 R_imu_camera 传给 BA。BA 从变量名和公式上看，期望的是“相机到惯性系”的完整旋转。

所以如果出现以下现象，应把这条链作为重点检查项：

• use_ba=true 时 yaw 明显异常，关闭 BA 反而正常；
• 云台转动时 BA 结果出现系统性偏差；
• PnP 位置正常，但姿态/旋转中心估计异常。

完整的相机光学系到 odom 旋转理论上应包含：

R_odom_optical
= R_odom_gimbal
· R_gimbal_cameraLink
· R_cameraLink_optical

这里应结合 BA 的坐标定义进一步核实后再修改，不能只按变量名直接替换。

13.2 R_gimbal_camera_ 的源码注释不要照字面背

矩阵本身与光学轴重排一致，但源码注释写的逐轴映射和按列计算出的映射不一致。应以矩阵实际乘法结果为准：

optical +X → gimbal -Y
optical +Y → gimbal -Z
optical +Z → gimbal +X

此外，这个硬编码矩阵只表示标准轴重排，没有包含 camera_joint 的实际安装 rpy=(0,0.275,0.015)。完整位姿在下游通过 URDF/TF 链转换；但 PnP 候选解筛选和 BA 是否需要安装倾角，应结合实车结果核验。

13.3 calcYawAndPitch() 的 rpy 参数当前没有参与计算

函数签名接收了 rpy，但函数体只对输入向量做 atan2 和弹道补偿。这与当前“输入向量仍按 odom 轴表达、计算绝对角”的实现是一致的，但函数名和参数容易让人误以为它在函数内部完成了 odom → gimbal 旋转。

14. 用于复述整套逻辑的简短版本

如果需要在几分钟内讲清楚，可以这样说：

相机首先发布带内参和 camera_optical_frame 时间戳的图像。Detector 在像素平面检测灯条或关键点、识别装甲板类型和编号，再用已知装甲板尺寸、相机内参与二维角点做 PnP，得到装甲板在相机光学系中的三维位姿。Solver 用图像采集时刻的 TF，把位姿沿 camera_optical_frame → camera_link → gimbal_link → odom 转到 odom，从而剔除云台自身转动造成的伪运动。普通目标用 EKF 从单块板观测估计整车旋转中心、速度、yaw、角速度和半径，前哨站用单独的圆周模型。随后根据数据延迟和子弹飞行时间预测目标未来中心及板面角度，恢复各块装甲板的位置并选出合适目标。解算器扣除枪口偏移，计算绝对 yaw 和几何 pitch，再用弹道模型修正 pitch、用手动表修正残余误差，并根据当前云台角与目标角的误差给出开火建议。默认串口最终发送绝对 yaw、pitch、距离和开火建议；下位机回传实际云台姿态和弹速，串口节点据此更新动态 TF，形成闭环。

只要能解释清楚下面这条坐标链，整个系统就已经理解了一半：

二维像素
→ PnP
camera_optical_frame 中的装甲板位姿
→ TF2（使用图像时间戳）
odom 中的装甲板测量
→ Tracker / EKF
odom 中的目标中心和未来装甲板
→ 扣除枪口 odom 位置
以枪口为起点、沿 odom 轴表达的瞄准向量
→ atan2 + 弹道补偿
相对 odom 的绝对 yaw/pitch
→ 串口
下位机云台控制

15. 建议继续对照的源码

• src/rm_robot_description/urdf/rm_gimbal.urdf.xacro：TF 树和光学轴重排
• src/rm_hardware_driver/rm_serial_driver/src/serial_driver_node.cpp：云台反馈如何生成动态 TF
• src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp：PnP、候选解与 BA
• src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp：相机系到 odom、跟踪调用与消息发布
• src/rm_auto_aim/armor_solver/include/armor_solver/motion_model.hpp：EKF 状态转移和观测模型
• src/rm_auto_aim/armor_solver/src/armor_tracker.cpp：跟踪状态机、数据关联和换板
• src/rm_auto_aim/armor_solver/src/armor_solver.cpp：预测、选板、枪口偏移、弹道与开火
• src/rm_hardware_driver/rm_serial_driver/src/protocol/default_protocol.cpp：最终实际发送的字段

本文基于当前仓库中的实际源码和参数编写，目标是先建立直觉，再把代码串起来。
重点不是背公式，而是始终回答三个问题：目标现在在哪里、子弹到达时它会在哪里、云台应该转到哪里。

1. 先用一句话理解整套系统

自瞄是一个带反馈的闭环：

相机拍图
→ 检出装甲板二维角点
→ PnP 求装甲板在相机前方的三维位置
→ TF 转到不随云台转动的 odom 坐标系
→ 跟踪并预测目标未来状态
→ 选择要打的装甲板
→ 计算 yaw / pitch，并补偿弹道
→ 下发绝对云台角和开火建议
→ 下位机回传真实云台姿态和弹速
→ 更新 TF，进入下一帧

可以把它拆成三个问题：

2. 坐标系到底是什么

坐标系可以理解成一把带方向的三维尺子。同一个物理点没有变，换了一把尺子测量，三个坐标值就可能完全不同。

例如，目标始终停在场地中的同一处：

• 云台向左转时，目标会移动到图像右边，因此目标的相机坐标变了；
• 目标在场地里没有动，因此正确变换后的 odom 坐标应该基本不变。

这正是为什么不能直接拿 PnP 的相机坐标做跨帧跟踪。

2.1 本项目中的主要坐标和坐标轴

注意：图像的 (u,v) 和光学坐标系的 (X,Y,Z) 都有“右、下”的方向，但前者单位是像素、只有二维，后者单位是米、是三维，不能混为一谈。

2.2 本项目的 TF 树

odom
└── gimbal_link 动态 TF：下位机实时回传 roll/pitch/yaw
└── camera_link 静态 TF：相机相对云台的安装平移和倾斜
└── camera_optical_frame
静态 TF：把 X前Y左Z上 重排为 X右Y下Z前

对应代码和参数：

• 动态 odom → gimbal_link：rm_serial_driver/src/serial_driver_node.cpp
• 静态 gimbal_link → camera_link：launch_params.yaml 中的 odom2camera 参数，名称虽然叫 odom2camera，实际被填进了 camera_joint
• 静态 camera_link → camera_optical_frame：rm_gimbal.urdf.xacro

当前安装参数为：

xyz: 0.246 0.0 0.049
rpy: 0.00 0.275 0.015

它表示相机坐标原点相对 gimbal_link 原点的安装偏移和安装倾角。xyz 的单位是米，rpy 的单位是弧度。

2.3 光学系为什么还要旋转一次

相机算法/OpenCV 常用：

X 右，Y 下，Z 前

ROS 机体系常用：

X 前，Y 左，Z 上

URDF 中的固定旋转 rpy=(-π/2, 0, -π/2) 完成了这次轴重排。对应矩阵为：

R_gimbal_or_cameraLink_from_optical =
[ 0 0 1 ]
[-1 0 0 ]
[ 0 -1 0 ]

判断一个旋转矩阵如何映射坐标轴，要看它的列：

光学系 +X（右） → 机体系 -Y（右）
光学系 +Y（下） → 机体系 -Z（下）
光学系 +Z（前） → 机体系 +X（前）

因此相机看到的“前方 4 米” (0,0,4)，转换后大致就是云台系的“前方 4 米” (4,0,0)。

3. 怎样读懂一个 TF 变换

ROS 中：

lookupTransform(target_frame, source_frame, time)

返回的是把 source_frame 中的数据变到 target_frame 的变换，可记为：

T_target_source

对一个点：

p_target = R_target_source · p_source + t_target_source

其中：

• R 改变坐标轴方向；
• t 补偿两个坐标系原点的位置差；
• 点需要旋转和平移，方向向量只需要旋转。

本项目从相机光学系到 odom 的完整链为：

T_odom_optical
= T_odom_gimbal
· T_gimbal_cameraLink
· T_cameraLink_optical

于是：

p_odom = T_odom_optical · p_optical

矩阵相乘顺序不能交换。可以从右往左读：先把光学系数据变到 camera_link，再到 gimbal_link，最后到 odom。

3.1 一个直观例子

暂时忽略安装倾角和云台转角，只保留轴重排和相机平移。假设 PnP 得到：

p_optical = (0.5, 0.3, 4.0) m

含义是目标在光轴前方 4 m、画面右侧约 0.5 m、画面下方约 0.3 m。轴重排后约为：

p_gimbal ≈ (4.0, -0.5, -0.3) m

再加相机相对云台原点的平移 (0.246, 0, 0.049)：

p_gimbal ≈ (4.246, -0.5, -0.251) m

实车还要继续考虑安装 rpy 和当前云台动态姿态，所以最终数值由 TF2 统一计算，不建议手写分量公式。

3.2 时间戳为什么和坐标转换同样重要

相机曝光时云台角度为 yaw₁，等图像处理完后云台可能已经转到 yaw₂。如果用“当前最新姿态”去转换旧图像，就会把目标投到错误的世界位置。

当前代码有意识地用图像时间戳查询 TF：

lookupTransform(odom_frame_, "gimbal_link", img_msg->header.stamp, ...)

串口侧还有 timestamp_offset，用于校正下位机反馈和相机时间之间的偏差。时间同步不准的典型表现是：云台静止时位置正常，一转动目标的 odom 坐标就摆动或拖影。

4. PnP：怎样从二维图像得到三维位置

4.1 PnP 已知什么、求什么

已知：

求解：

p_camera = R_camera_armor · p_armor + t_camera_armor

• R_camera_armor：装甲板物体系相对相机光学系的朝向；
• t_camera_armor：装甲板原点在相机光学系中的位置；
• 本项目装甲板模型使用米，因此 tvec 也是米。

最容易说反的一点是：这里求的是“装甲板在相机中的位姿”，不是最终的世界坐标。

4.2 当前项目给 PnP 的点

传统检测路径从左右灯条生成 6 个像素点：

左灯条：bottom、center、top
右灯条：top、center、bottom

并与装甲板物体系中的 6 个模型点一一对应。当前尺寸为：

小装甲板：0.133 m × 0.050 m
大装甲板：0.225 m × 0.050 m

YOLO 路径也会把四角点构造成相同接口所需的灯条/关键点数据，之后仍走 PnP。

4.3 为什么 IPPE 会给多个解

装甲板近似是一个平面，只从单目图像观察平面时可能存在姿态歧义。当前代码用 SOLVEPNP_IPPE 获得候选解，再结合：

• 重投影误差；
• 灯条在图像中的倾斜方向；
• 装甲板姿态先验；

选择更合理的解。可选 BA 会在 PnP 初值上进一步优化装甲板 yaw。

重投影误差的直觉是：把求出的三维装甲板重新投回图像，如果投影点和实际检测点差得很远，说明角点、尺寸、内参、畸变或 PnP 解至少有一项不对。

4.4 Detector 发布的位姿属于哪个坐标系

相机驱动把图像 header.frame_id 设为 camera_optical_frame。Detector 复制这个 header，并直接把 PnP 的 R/t 填入 Armor.pose，所以：

armor_detector/armors 中的 pose
所属坐标系 = camera_optical_frame

Detector 阶段没有把位置手动转换到 odom；真正的完整 TF 转换发生在 Solver 的 armorsCallback() 中。

5. 为什么要把检测结果转换到 odom

在 Solver 中，每块装甲板都会执行：

armor.pose = tf2_buffer_->transform(ps, target_frame_).pose;

当前 target_frame_ 配置为 odom，因此位置和姿态都会从 camera_optical_frame 转到 odom。

这一步解决了两个问题：

一个非常有用的正确性判据：

固定装甲板 + 主动转动云台
camera_optical_frame 中的位置应该明显变化
odom 中的位置应该基本稳定

如果两边都在明显变化，优先排查 TF 方向、外参、时间戳和 pitch 正负号。

6. 跟踪：从“这一帧的一块板”恢复“整台机器人”

6.1 为什么检测结果不能直接拿去瞄准

单帧检测会遇到：

• 角点噪声导致 PnP 抖动；
• 某几帧漏检；
• 同一台机器人有多块外观相同的装甲板；
• 机器人平移的同时还会旋转；
• 处理、通信和子弹飞行都存在延迟。

因此 Solver 不只跟踪“当前看到的板”，而是估计“目标旋转中心 + 运动速度 + 装甲板绕中心的几何关系”。

6.2 普通目标的 EKF 状态

当前普通目标的 10 维状态为：

X = [xc, vxc, yc, vyc, zc, vzc, yaw, v_yaw, r, d_zc]

相机真正测到的是一块装甲板：

Z = [xa, ya, za, yaw]

EKF 的观测模型为：

xa = xc – r · cos(yaw)
ya = yc – r · sin(yaw)
za = zc + d_zc

几何直觉如下：

装甲板 (xa, ya)
●
/
r /
/
目标旋转中心 (xc,yc) ●────→ yaw 所描述的径向/板面关系

初始化时，代码先假设 r=0.26 m，再由测到的板位置反推中心位置。之后 EKF 通过连续观测修正中心、速度、角速度和半径。

6.3 每一帧 EKF 做什么

上一帧状态
→ predict：按 dt 把中心位置和 yaw 向前推
→ 数据关联：在相同数字的板中找最接近预测位置的观测
→ update：用 PnP 测量修正预测
→ 得到新的目标中心、速度、角速度、半径

代码还用状态机降低偶发检测对控制的影响：

LOST → DETECTING → TRACKING → TEMP_LOST
↑ │
└──────── 丢失超时 ───────────────┘

• DETECTING：要连续匹配若干帧才确认；
• TRACKING：正常输出目标；
• TEMP_LOST：短时漏检仍可依靠预测；
• LOST：超时后重新选目标初始化。

6.4 为什么还要“换板”

四装甲板机器人旋转时，可见板会从一块切换到相邻板。新板的 yaw、半径和高度可能突然变化，但机器人中心没有瞬移。

handleArmorJump() 会在这种情况下：

• 更新参考 yaw；
• 交换两组半径；
• 修正高度差；
• 如果几何仍严重不一致，再用当前观测重置部分状态。

所以“装甲板 yaw 跳变”不一定是误检，也可能只是视野切到了下一块板。

6.5 前哨站为什么单独处理

前哨站是 3 块板绕固定中心旋转，当前代码没有走普通目标 EKF，而是使用 OutpostTracker：

• 收集观测并估计固定圆心；
• 使用固定半径约 0.275 m；
• 判断静止或旋转状态；
• 按 120° 间隔预测三块板；
• 单独学习三块板的高度。

它和普通目标的共同点是：最终都要输出“中心、旋转角、角速度、半径”，供后面的选板和预测使用。

7. 从目标状态到瞄准点

7.1 先预测子弹到达时的目标

当前代码使用：

dt = 数据年龄 + 子弹飞行时间 + prediction_delay

然后：

目标中心未来位置 = 当前估计位置 + dt · 线速度
未来 yaw = 当前 yaw + dt · v_yaw

其中“数据年龄”是当前解算时间减检测时间戳。这样补偿的不只是子弹飞行时间，还包括图像处理和消息传输已经消耗的时间。

如果只瞄准观测时的位置，那么目标速度越快、距离越远，落点滞后越明显。

7.2 枪口不等于 odom 原点

代码从 TF 读取 gimbal_link 在 odom 中的位置和姿态，并假设枪口在云台前方 0.095 m：

p_muzzle_odom
= p_gimbal_origin_odom
+ R_odom_gimbal · (0.095, 0, 0)

然后计算：

p_target_from_muzzle = p_target_odom – p_muzzle_odom

这里有一个很容易混淆的细节：

• 这个向量的起点已经移到了枪口；
• 但它的三个分量仍沿 odom 的坐标轴表达；
• 代码没有再乘 R_odom_gimbalᵀ 把它旋转成 gimbal_link 坐标。

因此后面的 atan2(y,x) 算出的是相对 odom 的绝对目标角，不是相对当前枪口朝向的局部角度。

7.3 根据中心恢复所有装甲板并选板

普通四板目标由中心、yaw、两组半径和高度差恢复出四块板的位置。概念上是：

p_armor_i = p_center + 绕中心旋转后的径向偏移 + 高度偏移

代码再结合：

• 目标相对枪口的方向；
• 各板朝向；
• 目标旋转方向和角速度；
• 子弹飞行时间；
• 最大侧向提前角；

选择现在适合瞄准、到弹丸抵达时仍合适的一块板。

高速旋转超过阈值时，Solver 可从 TRACKING_ARMOR 切到 TRACKING_CENTER，避免瞄准点在多块板之间高速跳变。

前哨站则优先选择正向射击窗口附近、且正在接近窗口的板，并带有单独的提前角和 ±30° 开火窗口。

8. yaw、pitch 和弹道补偿

8.1 不考虑弹道时

对“以枪口为起点、用 odom 轴表达”的目标向量 p=(x,y,z)：

yaw_geom = atan2(y, x)
pitch_geom = atan2(z, sqrt(x²+y²))

输出在内部先使用弧度，填入 GimbalCmd 时再转为度。

8.2 为什么主要补 pitch

子弹会因重力下坠，空气阻力还会延长飞行时间，因此实际枪口通常要比目标的几何仰角再抬高一些。

当前配置使用 quadratic_drag 二次阻力模型，主要参数包括：

• 弹速；
• 重力加速度；
• 阻力系数；
• 空气密度；
• 弹丸质量和直径。

弹道补偿器在当前实现中修正 pitch；yaw 的提前量主要来自目标运动预测和旋转选板，而不是重力模型。

弹速由串口消息中的 bullet_speed_flag 选择两档配置值。距离或弹速估计错误会同时影响：

8.3 最终下发的到底是绝对角还是角度差

Solver 同时生成：

pitch / yaw 相对 odom 的目标绝对角，单位：度
pitch_diff / yaw_diff 目标绝对角 – 当前云台角，单位：度

但当前 DefaultProtocol::send() 真正通过串口打包的是：

fire_advice、pitch、yaw、distance

也就是说，当前协议下发的是绝对 pitch/yaw，不是 pitch_diff/yaw_diff。差值字段主要用于调试、观察误差等上层用途。下位机必须和这里采用相同的“绝对角”协议约定，否则会出现越转越偏或指令尺度不对。

8.4 pitch 正负号要沿整条链看

串口接收后构造 TF 时，代码使用：

TF pitch = – receive_data.pitch

Solver 从 TF 取 RPY 后又执行一次：

rpy_[1] = -rpy_[1]

这是在 ROS 右手系和下位机 pitch 正方向之间转换。调试时不要只看其中一个负号就删除；要用“云台物理向上转时，反馈、TF、目标 pitch 和误差是否同向收敛”来验证整条符号链。

9. 什么时候给开火建议

普通目标要求当前云台角足够接近目标角：

允许 yaw 误差 ≈ atan2(射击窗口宽度/2, 距离)
允许 pitch 误差 ≈ atan2(射击窗口高度/2, 距离)

并设置了最小约 1° 的容忍角。距离越远，同样的物理窗口对应的角度越小，因此理论上要求越严格。

前哨站还要求：

• 目标板位于预测到达窗口的 ±30° 内；
• 板正在向合适的窗口方向接近；
• 当前云台角也满足普通的角度容忍条件。

fire_advice 只是视觉侧建议，最终是否真正发射还应由下位机结合比赛状态、摩擦轮状态和安全逻辑决定。

10. 把源码调用链完整走一遍

对应的反馈闭环为：

┌──────── 下位机姿态反馈 ────────┐
↓ │
相机 → Detector → PnP → TF → Tracker → Predictor → Solver → 云台控制
↑ │
└──── odom→gimbal TF ──────┘

11. 推荐的理解和调试顺序

不要一上来同时调检测、EKF和弹道。按下面的层次逐个证明正确，定位会快很多。

第 1 层：先证明坐标轴和 TF 正确

检查：

ros2 run tf2_ros tf2_echo odom gimbal_link
ros2 run tf2_ros tf2_echo gimbal_link camera_optical_frame
ros2 run tf2_ros tf2_echo odom camera_optical_frame

验证现象：

• 云台 yaw 左右转时，TF 中 yaw 同步变化；
• 云台向上抬时，pitch 的物理方向正确；
• 光学系蓝色 Z 轴指向相机前方；
• 静态相机外参不会随时间跳变。

第 2 层：证明 PnP 测距和方向正确

把已知尺寸的装甲板放在已知距离：

• 图像右侧目标应有 tvec.x > 0；
• 图像下方目标应有 tvec.y > 0；
• 正前方距离主要体现在 tvec.z > 0；
• 2 m、4 m、6 m 测距误差应有一致趋势；
• 重投影误差不能持续很大。

如果近远距离都按固定比例偏差，先怀疑内参或装甲板模型尺寸；如果偏差随图像位置变化明显，先怀疑畸变和角点。

第 3 层：证明相机系到 odom 的转换正确

固定装甲板，只转动云台：

ros2 topic echo /armor_detector/armors
ros2 topic echo /armor_solver/measurement

期望：Detector 中的相机系位置会变化，Solver 中转换到 odom 的测量位置基本稳定。

如果云台越快误差越大，优先检查 TF 时间戳和 timestamp_offset，而不是先调 EKF。

第 4 层：再看 EKF

静止目标时：

• 中心位置应收敛；
• 线速度和角速度应接近 0；
• 瞄准点不应比原始 PnP 更抖。

移动/旋转目标时：

• 预测位置应先于测量而不是落后；
• 换板时中心不应瞬移；
• 短时漏检应进入 TEMP_LOST 并继续平滑预测。

一般调参直觉：

Q 增大 → 更允许模型状态快速变化，响应快但可能更抖
R 增大 → 更不相信单帧测量，更平滑但可能更滞后

第 5 层：最后调预测和弹道

建议分开验证：

静止目标都打不准时，不要先用运动预测参数掩盖内参、外参、枪口偏移或弹道参数问题。

12. 最常见的理解错误

错误 1：把 PnP 输出当成世界坐标

PnP 输出属于图像 header 指定的 camera_optical_frame。只有经过 TF2 转换后才属于 odom。

错误 2：认为换坐标系只需要交换 x/y/z

真实变换通常同时包含旋转和平移，而且多个旋转的顺序不能交换。让 TF2 处理整条链。

错误 3：把 lookupTransform(A,B) 理解反

它返回把 B 中的数据转换到 A 的 T_A_B。

错误 4：用最新 TF 转换旧图像

动态场景必须尽量使用图像采集时刻的 TF，否则云台一动就产生伪运动。

错误 5：把 EKF 的 (x,y,z) 当成当前装甲板

它们是目标旋转中心；装甲板位置还要由 yaw/r/d_zc 通过观测模型恢复。

错误 6：认为减去枪口位置后就是 gimbal 坐标

减法只改变向量起点，不会改变它沿哪组坐标轴表达。当前代码减去枪口 odom 位置后仍是 odom 轴表达。

错误 7：把弧度和度混用

TF、Eigen、三角函数内部主要用弧度；串口的云台角和最终 GimbalCmd 使用度。

错误 8：认为串口下发的是 yaw_diff/pitch_diff

当前默认协议实际发送绝对 yaw/pitch。协议两端必须完全一致。

13. 当前源码中值得特别留意的细节

下面不是理解主流程的前提，但调试异常时很重要。

13.1 imu_to_camera_ 的变量名和实际查询内容不一致

ArmorDetectorNode::imageCallback() 查询：

lookupTransform(odom_frame_, "gimbal_link", image_time)

取到的旋转实际是 R_odom_gimbal，却被存入名为 imu_to_camera_ 的变量，并作为 R_imu_camera 传给 BA。BA 从变量名和公式上看，期望的是“相机到惯性系”的完整旋转。

所以如果出现以下现象，应把这条链作为重点检查项：

• use_ba=true 时 yaw 明显异常，关闭 BA 反而正常；
• 云台转动时 BA 结果出现系统性偏差；
• PnP 位置正常，但姿态/旋转中心估计异常。

完整的相机光学系到 odom 旋转理论上应包含：

R_odom_optical
= R_odom_gimbal
· R_gimbal_cameraLink
· R_cameraLink_optical

这里应结合 BA 的坐标定义进一步核实后再修改，不能只按变量名直接替换。

13.2 R_gimbal_camera_ 的源码注释不要照字面背

矩阵本身与光学轴重排一致，但源码注释写的逐轴映射和按列计算出的映射不一致。应以矩阵实际乘法结果为准：

optical +X → gimbal -Y
optical +Y → gimbal -Z
optical +Z → gimbal +X

此外，这个硬编码矩阵只表示标准轴重排，没有包含 camera_joint 的实际安装 rpy=(0,0.275,0.015)。完整位姿在下游通过 URDF/TF 链转换；但 PnP 候选解筛选和 BA 是否需要安装倾角，应结合实车结果核验。

13.3 calcYawAndPitch() 的 rpy 参数当前没有参与计算

函数签名接收了 rpy，但函数体只对输入向量做 atan2 和弹道补偿。这与当前“输入向量仍按 odom 轴表达、计算绝对角”的实现是一致的，但函数名和参数容易让人误以为它在函数内部完成了 odom → gimbal 旋转。

14. 用于复述整套逻辑的简短版本

如果需要在几分钟内讲清楚，可以这样说：

相机首先发布带内参和 camera_optical_frame 时间戳的图像。Detector 在像素平面检测灯条或关键点、识别装甲板类型和编号，再用已知装甲板尺寸、相机内参与二维角点做 PnP，得到装甲板在相机光学系中的三维位姿。Solver 用图像采集时刻的 TF，把位姿沿 camera_optical_frame → camera_link → gimbal_link → odom 转到 odom，从而剔除云台自身转动造成的伪运动。普通目标用 EKF 从单块板观测估计整车旋转中心、速度、yaw、角速度和半径，前哨站用单独的圆周模型。随后根据数据延迟和子弹飞行时间预测目标未来中心及板面角度，恢复各块装甲板的位置并选出合适目标。解算器扣除枪口偏移，计算绝对 yaw 和几何 pitch，再用弹道模型修正 pitch、用手动表修正残余误差，并根据当前云台角与目标角的误差给出开火建议。默认串口最终发送绝对 yaw、pitch、距离和开火建议；下位机回传实际云台姿态和弹速，串口节点据此更新动态 TF，形成闭环。

只要能解释清楚下面这条坐标链，整个系统就已经理解了一半：

二维像素
→ PnP
camera_optical_frame 中的装甲板位姿
→ TF2（使用图像时间戳）
odom 中的装甲板测量
→ Tracker / EKF
odom 中的目标中心和未来装甲板
→ 扣除枪口 odom 位置
以枪口为起点、沿 odom 轴表达的瞄准向量
→ atan2 + 弹道补偿
相对 odom 的绝对 yaw/pitch
→ 串口
下位机云台控制

15. 建议继续对照的源码

• src/rm_robot_description/urdf/rm_gimbal.urdf.xacro：TF 树和光学轴重排
• src/rm_hardware_driver/rm_serial_driver/src/serial_driver_node.cpp：云台反馈如何生成动态 TF
• src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp：PnP、候选解与 BA
• src/rm_auto_aim/armor_solver/src/armor_solver_node.cpp：相机系到 odom、跟踪调用与消息发布
• src/rm_auto_aim/armor_solver/include/armor_solver/motion_model.hpp：EKF 状态转移和观测模型
• src/rm_auto_aim/armor_solver/src/armor_tracker.cpp：跟踪状态机、数据关联和换板
• src/rm_auto_aim/armor_solver/src/armor_solver.cpp：预测、选板、枪口偏移、弹道与开火
• src/rm_hardware_driver/rm_serial_driver/src/protocol/default_protocol.cpp：最终实际发送的字段