gimbal_ahrs

课 1 作业：订阅 /offboardlink/imu_data，用 AHRS（Mahony） 解算云台姿态，以 TF world -> gimbal_link_ahrs 发布，和板载姿态 world -> gimbal_link 对比。

当前实现只用陀螺仪 + 加速度计（没有磁力计），不做陀螺标定；Mahony 的 Ki 项顺带估零偏， ki:=0 可关掉（退回纯 P 项）。

由 deepseek-falsh-v4.1 生成代码

思路

发送端已经提供角速度与加速度计（测量重力方向校准pitch,roll），通过数值积分与mahony给出gimbal_ling_ahrs的方向

注意事项

代码在ubuntu26.04.1环境下编写，python，ROS 版本与其他环境版本可能不同

做什么

• 订阅 imu_topic（默认 /offboardlink/imu_data，sensor_msgs/Imu，500 Hz）。QoS 与 bridge 一致：可靠、volatile、深度 200。
• 每帧解算（在 onImu 里，按 IMU 的 500 Hz 跑）：dt 取两帧 header.stamp 之差，做一步 Mahony。
• 定时发布（rate_hz，默认 50 Hz）：把当前姿态发成 TF，平移恒 0，header.stamp 用那帧 IMU 的 （和板载姿态同一时刻，方便逐帧对比）。走 /tf（动态），不用 /tf_static。
• 有新 IMU 才发：stamp 没前进就不发，避免 tf2 报 TF_REPEATED_DATA；stamp 倒退（MCU 复位）时同样等更大的 stamp。
• 还没收到 IMU 时只警告一次、不发 TF，否则 RViz 里坐标系会在原点乱跳。
• 每 compare_every 个 IMU 帧在终端打印一次与板载姿态的对比：参考mahony算法测量误差的方法，考虑四元数作向量点积应该为误差角度的cos值，近似为误差角度。点积 |q_ahrs · q_board| 和夹角 θ = 2·arccos|dot|。用的是同一帧的 Imu.orientation， 所以天然时间对齐（对比两条 TF 会有几毫秒的时刻差，混进真实转动）。

解算部分

include/gimbal_ahrs/mahony.hpp + src/mahony.cpp 是算法本体（纯函数，便于单测）：

1. 初始化（第一帧）：用加速度计定 roll/pitch（quatFromAccel），yaw 不可观测、取 0。

2. 每帧一步 Mahony（PI 修正）：

   • 误差 e = v × v̂：v 是实测比力归一化（静止时就是世界 +z 在机体系里的方向）， v̂ 是姿态估计出的同一个方向；
   • ω = gyro + kp·e + ki·∫e dt——积分项 ki·∫e dt 顺带把陀螺零偏也估掉了 （完整的 Mahony 是单独维护 ḃ = −ki·e 再从陀螺读数里减掉，两者等价）。 等价关系：零偏估计 b̂ = −ki·∫e dt，节点每 2 秒把它打出来，可以看它在收敛；
   • 按课时一的**离散积分更新（指数形式）**更新四元数： q ← q ⊗ (cos(|ω|Δt/2), ω/|ω|·sin(|ω|Δt/2)) —— 对固定角速度是精确解，比示例代码里的一阶泰勒 dq = (1, ½ωΔt) 精度高。

   Ki 能估出什么、估不出什么：e 恒垂直于实测重力方向，所以只有"可观测"的那部分零偏能被估出来。 云台保持水平时，绕重力轴（机体 z，也就是 yaw 轴）的零偏不可观测 —— yaw 的漂移 Ki 压不住， 那需要磁力计；Ki 的作用是消掉 roll/pitch 方向的漂移。 另外零偏估计的时间常数约 kp/ki（默认 1/0.05 = 20 s），想快就把 ki 调大，但会被加速度计噪声带着走。

3. 归一化：每 normalize_every 步（默认 10）做一次 q.normalize()。浮点误差会让模长慢慢偏离， 长时间跑下来姿态会整体缩放走样，这是最容易被漏掉的一步。

4. dt 的异常处理：dt <= 0（重复帧 / MCU 复位）或 dt > 0.1 s（间隔过大）时这一帧不积分， 只记下新 stamp（否则一次大积分就把姿态带飞）。

5. 加速度计为零的异常帧不做修正，只按陀螺积分（避免 normalized() 出 NaN）。

完整启动命令

四个终端。[1] → [2] → [4] 有先后依赖：桥要打开虚拟云台建的 pty；本节点要在云台开始运动（[3]）之前起。

每个终端都先做这两步：

cd ~/workspace/ros/course01
source /opt/ros/lyrical/setup.bash    # 这台机器是 lyrical；课程文档写的是 humble，按自己的替换
source install/setup.bash

[1] 虚拟云台（假下位机 + 视觉）

ros2 launch virtual_gimbal virtual_gimbal.launch.py vision_mode:=corners

等它打出这一行再往下（pty 和 /tmp/ttyVGIMBAL 就是这时建的）：

[virtual_mcu]: virtual gimbal MCU up: port /tmp/ttyVGIMBAL -> /dev/pts/N

[2] 串口桥（发布 /offboardlink/imu_data 和板载姿态 TF）

# 只发数据（云台不动）
ros2 launch robo_bridge_edu bridge.launch.py port:=/tmp/ttyVGIMBAL

# 想让云台能被指令驱动，加上这个参数（还需要 [3] 持续发指令才真的会动）
ros2 launch robo_bridge_edu bridge.launch.py port:=/tmp/ttyVGIMBAL subscribe_cmd_vel:=true

确认数据在发：ros2 topic hz /offboardlink/imu_data，应该是 ~500 Hz。

[3] 让云台动起来（可选，但强烈建议）

ros2 run bridge_usage_py bridge_usage_py

向 /offboardlink/cmd_vel_gimbal 持续发小幅正弦角速度。subscribe_cmd_vel 不做保活（每条指令只转一帧），所以必须持续发。云台静止也能验证解算，但看不出跟踪效果，dot 会一直贴在 1.0000000。

[4] 本节点

ros2 run gimbal_ahrs ahrs_node
# 或者用 launch（任意参数都能这样覆盖）
ros2 launch gimbal_ahrs gimbal_ahrs.launch.py
ros2 launch gimbal_ahrs gimbal_ahrs.launch.py kp:=2.0 ki:=0.2 compare_every:=50

要在 [3] 之前起。 第一帧是用加速度计初始化的（roll/pitch 有重力这个绝对参考，yaw 取 0），而板载的 yaw 零点是它自己上电那一刻。所以在云台开始运动之前起本节点，两条 TF 的 yaw 参考才一致；否则会差一个常数角，正好等于启动时云台已经转过的角度。

[5] 看结果

• 终端：每 compare_every 帧打印一次 与板载姿态对比：dot = ... 夹角 = ... deg（默认 250 帧 ≈ 2 行/秒），另外每 2 秒打印一次估计的陀螺零偏。
• RViz（可选）：

rviz2      # Fixed Frame 改成 world，然后在 Displays 面板加 TF（Ctrl+N）

会同时看到 world -> gimbal_link（板载）和 world -> gimbal_link_ahrs（本节点）两条。

真机

不启动 [1]，并把 port:=/tmp/ttyVGIMBAL 去掉（port 默认 auto：依次尝试 RM_SIM_SERIAL → /dev/gimbal → 第一个 /dev/ttyUSB* / ttyACM*）：

ros2 launch robo_bridge_edu bridge.launch.py

串口权限和 udev 规则见 gimbal-edu-tools 的 setup/setup_serial.sh 与 setup/deps.md。

常见报错

现象	原因 / 处理
打开串口失败：open /tmp/ttyVGIMBAL: No such file or directory（退出码 3）	[1] 虚拟云台没起，pty 不存在。先起 [1]；或给桥加 respawn:=true，它会在设备文件出现前每秒重试
打开串口失败 / EBUSY	串口被别的进程占着（又起了一个桥、或 virtual_gimbal 的 demo_tracker）。一个串口同时只能一个进程打开
终端一直 还没收到 IMU、不打印对比	桥没起，或起在别的 port 上。用 ros2 topic hz /offboardlink/imu_data 确认

参数

参数	默认	说明
imu_topic	/offboardlink/imu_data	订阅的 IMU 话题
parent_frame	world	父坐标系
child_frame	gimbal_link_ahrs	子坐标系。不能用 gimbal_link：那个父节点已被 bridge 占用，同一坐标系两个发布者会让 TF 树断
rate_hz	50.0	TF 发布频率，Hz。<= 0 报错
kp	1.0	Mahony 比例增益：先给经验值，再照实测调（大了会在有运动加速度时被带偏，小了收敛慢）
ki	0.05	Mahony 积分增益，顺带估陀螺零偏。给 0 表示只用 P 项、不估零偏；< 0 报错
normalize_every	10	每多少步归一化一次。< 1 报错
compare_every	250	每多少个 IMU 帧在终端打印一次与板载姿态的对比（点积 + 夹角），0 关掉。500 Hz 输入时 250 = 2 行/秒；想密一点就调小（10 = 50 行/秒，会刷屏）

和板载姿态比（作业要求的"差多少"）

• 姿态夹角（最标准）：θ = 2·arccos(|q_ahrs · q_board|)，单位度；给出 RMS 和最大值。
• 分轴 RPY 差：两边各转 roll/pitch/yaw 再相减，yaw 要归一化到 ±180°（跨过时会跳变）。

预期结果，写报告时要用得上：

• roll/pitch 会收敛，和板载（也是互补滤波）应该差得不远；
• yaw 会缓慢漂移——只用 accel + gyro 时 yaw 不可观测：绕机体 z 轴转，重力方向在机体系里不变， 加速度计一个字都给不出来，yaw 只能靠陀螺积分，零偏会一直积累。这是物理限制，不是 bug。
• 静止时如果两条 TF 差很多（尤其 roll/pitch），先检查四元数方向：静止时应该基本重合， 反着转就说明该取共轭。

还能怎么改（课时三的坑）

• 调 ki 看零偏估计的收敛速度：时间常数约 kp/ki。调大收敛快，但加速度计噪声会被积分进 b̂ （也会让"运动加速度"被误当成零偏），调小则要等更久。节点每 2 秒打印一次 b̂，可以直接看曲线。
• 换 Madgwick：同样是互补滤波，做法是用梯度下降那一步代替这里的 P 项，参数更少。
• 加速度计动态加权：运动时 |a| 明显偏离 9.8（有运动加速度）就降 kp 或跳过修正， 否则急加速时 roll/pitch 会被带偏，还会把运动加速度积分进 b̂。
• 归一化频率实测对比：把 normalize_every 从 10 改成 1，看模长和姿态角差随时间怎么变。

测试

colcon test --packages-select gimbal_ahrs && colcon test-result --verbose

覆盖（19 个用例，failures=0）：

• 加速度计初始化：水平 / roll / pitch，yaw 取 0；
• 指数积分：恒定角速度下与解析解一致到 1e-9（一阶泰勒近似会差 1e-3，容差刚好区分）；
• 零加速度帧只按陀螺积分、不出 NaN；
• P 项收敛：从水平出发，加速度计说 roll = 30°，8 秒后估计重力方向与实测一致；
• Ki 项估零偏：陀螺绕 x 有 0.01 rad/s 常值零偏时，只用 P 项稳态还差 bias/kp = 0.01 rad 的倾斜（droop），加上 Ki 后倾斜被压到 1e-3 rad 以内，且 ∫e dt → −bias/ki；
• 限频判断（纯函数）；
• 话题级：没收到 IMU 时不发 TF、第一帧的姿态/父子坐标系/平移/stamp、陀螺积分的 dt 用两帧 stamp 之差、 同一帧不重复发、非法参数（rate_hz、kp、ki、normalize_every）报错。

测试用自己的名字（话题 /gimbal_ahrs_test/imu、子坐标系 gimbal_link_ahrs_test），所以别的节点正在跑也互不干扰。

课 2：摄像头 ArUco 检测节点

aruco_detection_node 使用 OpenCV 实时读取摄像头，检测 DICT_4X4_50 标记， 并在窗口中绘制边框和 ID。使用相机标定和标记四角估计位姿，并绘制红色 X、绿色 Y、蓝色 Z 坐标轴。 发布光学坐标系和 world 下的位姿数组，以及带 ID 的 RViz 坐标轴；不发布摄像头图像。 原来的 ahrs_node 可继续单独运行。

在 course02 工作区编译并启动：

cd ~/workspace/ros/course02
source /opt/ros/lyrical/setup.bash
colcon build --packages-select gimbal_ahrs
source install/setup.bash
ros2 run gimbal_ahrs aruco_detection_node

选择摄像头或关闭显示窗口：

ros2 run gimbal_ahrs aruco_detection_node --ros-args -p camera_id:=1
ros2 run gimbal_ahrs aruco_detection_node --ros-args -p show_image:=false
ros2 launch gimbal_ahrs aruco_detection.launch.py camera_id:=0 show_image:=true

camera_id（默认 0）和 show_image（默认 true）是只读启动参数。 显示窗口时按 Esc、Q 或关闭窗口退出；也可在终端按 Ctrl+C。 无桌面显示环境时使用 show_image:=false。 摄像头打不开、读取失败或参数非法时，程序打印错误并以非零状态退出。

节点采用约 30 Hz 的定时回调读取画面，每 30 帧打印标记数量和检测耗时。 检测器已启用 CORNER_REFINE_SUBPIX 亚像素角点细化；窗口、迭代次数等使用 OpenCV 默认值。 PnP 使用细化后的角点，启动日志会显示当前角点细化方法。 依赖 OpenCV 4.7 或更新的 4.x（使用 ArucoDetector）；工作区的 colcon_defaults.yaml 已指定系统的 OpenCV 4.10。

标记坐标轴

默认 estimate_pose:=true、marker_length:=0.015（单个标记黑色外框边长 15 mm，不含白边）。 每个标记的坐标系原点位于中心，三条轴长为边长的 1.5 倍，即 22.5 mm。 marker_length 使用米；它表示单个标记的边长，而不是整张标记板或白色外边框的尺寸。 默认标定文件 config/camera.yaml 来自 course02 的 calibrationdata/ost.yaml， 包含 640×480 分辨率下的相机内参及 plumb_bob 畸变参数。 这些参数必须对应当前使用的摄像头；换摄像头后需要重新标定。

ros2 launch gimbal_ahrs aruco_detection.launch.py camera_id:=0 marker_length:=0.015
# 使用自己的 ROS camera_info 格式标定文件：
ros2 launch gimbal_ahrs aruco_detection.launch.py calibration_file:=/path/to/camera.yaml
# 只检测标记边框，不加载标定或绘制坐标轴：
ros2 launch gimbal_ahrs aruco_detection.launch.py estimate_pose:=false

节点按标定分辨率请求摄像头画面；如果摄像头实际输出的尺寸不匹配，保留标记边框并提示， 跳过坐标轴绘制，避免使用不匹配的相机内参。参数均为只读启动参数。

TF 位姿和 RViz2

PnP 使用 OpenCV 光学坐标系：X 向右、Y 向下、Z 向前。 工程的实际名称为 camera_optical_frame，相机前方的目标具有正的 Z 距离。 标记原点仍在黑色正方形的中心；输出的是标记在目标坐标系中的完整位姿。

话题	消息类型	内容
/aruco/poses_optical	geometry_msgs/msg/PoseArray	默认 camera_optical_frame 下的有效标记位姿
/aruco/poses_world	geometry_msgs/msg/PoseArray	world 下的有效标记位姿
/aruco/markers_optical	visualization_msgs/msg/MarkerArray	光学系下的三色坐标轴和 ID 标签
/aruco/markers_world	visualization_msgs/msg/MarkerArray	world 下的三色坐标轴和 ID 标签

PoseArray 按成功解算的检测顺序排列，MarkerArray 的 id 保留 ArUco 标记 ID。 这些话题使用 reliable、volatile QoS。关闭 OpenCV 显示窗口（show_image:=false） 仍然解算和发布位姿；estimate_pose:=false 则只检测边框，发布空位姿。 无标记、分辨率不匹配或缺少某个目标坐标系的 TF 时，发布空数组并清理该坐标系的可视化。 可视化的三色坐标轴还有 0.5 秒生命周期，节点停止后会自动消失。

启动节点及已配置的 RViz2：

cd ~/workspace/ros/course02
source /opt/ros/lyrical/setup.bash
source install/setup.bash
ros2 launch gimbal_ahrs aruco_detection.launch.py rviz:=true

RViz 默认 Fixed Frame 为 world，已启用 /aruco/markers_world 和 TF 显示。 若希望查看原始光学位姿，启用 Optical 显示项；在没有 world 变换时， 把 Fixed Frame 改成 camera_optical_frame 即可查看光学系结果。

必须由已有节点提供完整的相机 TF 链，例如课程工程中的：

world -> gimbal_link -> camera_link -> camera_optical_frame

robo_bridge_edu 发布动态 world -> gimbal_link， gimbal_edu_description 发布静态的相机安装外参和光学坐标系旋转。 若这些包来自 course01，在启动它们的终端先 source course01 的 install/setup.bash。 相机安装外参须与实物一致；本检测节点读取已有 TF，不创建相机与 world 的外参；另外广播检测到的标记 TF。 检查是否已连通：

ros2 run tf2_ros tf2_echo world camera_optical_frame

变换使用帧读取完成时间，最长等待 tf_timeout（默认 0.03 秒）。 缺少、过期或未来时间戳的 TF 时提示并跳过该坐标系的有效位姿，光学系结果可继续发布。

启动参数	默认值	含义
camera_frame	camera_optical_frame	PnP 的原始光学坐标系
optical_frame	camera_optical_frame	光学位姿的目标坐标系；不同于原始系时通过 TF 变换
world_frame	world	世界位姿的目标坐标系
tf_timeout	0.03	等待当前图像时间戳的 TF，单位秒
use_sim_time	false	是否使用 ROS 仿真时钟，检测节点与 TF 发布者须一致
rviz	false	是否启动配置好的 RViz2

若 TF 树中的名称与默认值不同，通过上述参数传入实际名称； 这些参数使用 TF frame 名，不带开头的 /。

在 RViz 的 TF Frames 中显示 ArUco

检测到有效标记后，节点在 /tf 广播：

camera_optical_frame -> aruco_marker_<ID>

例如 ID 0 对应 aruco_marker_0。TF 的平移、旋转和时间戳与该帧 PnP 位姿一致， 子坐标系只有相机光学系这个父节点。world 下的标记位姿沿既有相机 TF 链计算。 marker_frame_prefix 默认 aruco_marker_，可用于区分多个检测节点。

重新启动检测节点后，RViz 的 TF → Frames 会在检测成功时出现相应条目。 坐标轴和 ID 也继续通过 MarkerArray 显示。

ros2 run tf2_ros tf2_echo camera_optical_frame aruco_marker_0
ros2 run tf2_ros tf2_echo world aruco_marker_0

如果没有 Frames 条目，先检查检测节点终端的“标记数量 / 有效位姿”是否大于 0， 以及标定分辨率是否匹配、estimate_pose 是否开启。TF 没有删除消息；目标丢失后停止 更新该标记的动态 TF，旧条目会按 RViz 的 Frame Timeout 过期。MarkerArray 可视化会立即清理。