基于ROS2的双IMU融合高精度AHRS设计与实践

搞机器人姿态估计的工程师，大概率都经历过同一个循环：装好一颗IMU，观察姿态输出，调滤波参数，姿态稳了一段时间，然后又开始漂，最后无奈地重新标零。当你把整个系统的姿态信息全压在一颗IMU上时，里面就埋了一颗定时炸弹——不是它今天缓慢漂移，就是它在剧烈加减速时把姿态瞬间拉飞。这篇文章打算聊的，就是一颗IMU不够用、两颗IMU正好互补的实战方案：基于ROS2做一套双IMU驱动的高精度AHRS（姿态航向参考系统）。它能解决的，恰恰是单IMU方案里最让人头疼的三个问题：长时间漂移、运动加速度干扰、单点失效。无论你是在做AGV导航、足式机器人平衡控制，还是机械臂末端姿态观测，这套架构都可以直接参考落地。内容会覆盖系统设计、硬件选型、算法原理、ROS2工程实现和排障经验，适合有ROS2或嵌入式基础、还没想清楚多IMU融合该怎么做的朋友。

1. 为什么高精度AHRS值得用双IMU来做

1.1 三个概念一次说清

在开始讲方案之前，花两分钟把基础概念对齐，后面就不会绕。AHRS全称是Attitude and Heading Reference System，姿态航向参考系统，它要输出的是物体相对世界坐标系的姿态，一般用欧拉角或四元数表示。IMU是Inertial Measurement Unit，惯性测量单元，内部集成了三轴加速度计和三轴陀螺仪，有些还带磁力计，它只吐原始物理量。ROS2是这套系统的软件骨架，负责节点通信、时间同步、TF坐标变换和可视化。

三者之间的关系可以这样理解：IMU负责“感知”，AHRS负责“整合与修正”，ROS2负责“把感知和整合串起来”。如果拿人体打比方，IMU是内耳里的前庭系统，AHRS是小脑，ROS2就是连接前庭和小脑的神经网络。单靠前庭能感觉到运动，但如果没有小脑的整合和修正，身体很快会失去平衡。这也是为什么很多人买回一颗IMU、接上串口、打印原始数据容易，但要做成稳定可用的姿态输出，才真正开始面对问题。

1.2 单颗IMU做姿态估计的三个硬伤

第一，陀螺仪积分漂移。陀螺仪输出角速度，姿态解算要靠对时间的积分。积分会累积两个坏东西：随机噪声积分之后变成角度随机游走，数值会越来越大；同时陀螺仪的零偏随时间缓慢变化，在静止状态下，一个零点几度每秒的零偏，一分钟就能把积分结果拉偏几度。两者叠加，就是设备明明没动，虚拟世界里的姿态却像喝醉了酒。

第二，运动加速度干扰。加速度计在静止时测量的是重力矢量，用它可以校正pitch和roll。但机器人一旦加速、减速、转弯，加速度计同时测量到了重力和平动加速度。滤波器很难区分这部分“额外力”，于是姿态估计就会瞬间被带偏，速度越快、加减速越猛，误差越大。对无人机、足式机器人这种强动态应用来说，这个问题特别致命。

第三，单点失效没有退路。一颗IMU传感器出现异常，比如焊点松动、温度冲击导致零偏突变、某种振动频率激发内部谐振，姿态输出就会完全不可信。而AHRS往往是导航、控制、感知链路的下游，它一旦坏了，上层导航和控制器一概跟着崩。

1.3 双IMU的实际增益：不是玄学，是可计算的底气

双IMU并不是让精度凭空翻倍，而是针对上面三个硬伤分别给出答案。第一，随机噪声部分，如果两颗IMU安装位置非常接近，且等权重融合，理论上白噪声标准差会降低到原来的约0.707倍，姿态随机游走会显著下降。第二，通过两路观测互相校验，可以识别出某一颗IMU的零偏突变或者被外界干扰，算法自动降权或切换，相当于给姿态系统加了“健康管理”。第三，真正有一颗IMU故障时，另一颗还能兜底，对长期运行的移动机器人来说，这种冗余带来的可靠性格外重要。

需要说清楚的是，双IMU解决不了标定误差，如果两颗IMU本身没有做过温度补偿和轴对齐，融合后反而可能出现两路数据打架的情况。所以后面的篇幅里，标定和外参对齐会是重点。理解了双IMU的收益边界，再去看整个系统架构，思路就会清晰很多。

2. 系统架构与硬件选型

2.1 硬件拓扑：一体式MCU还是分体式串口

双IMU系统的硬件拓扑通常有两条路线。路线A是一体式MCU方案：两颗IMU共同挂在一个MCU上，由MCU统一定时采集，合并成一条数据帧，再通过单串口或USB输出给上位机。优点是时间同步天然一致，不会出现两路数据各带各的时间戳、错位严重的问题；缺点是嵌入式侧要额外写调制逻辑，后面想换传感器型号，得连固件一起改，调试成本不低。

路线B是分体式方案：两颗IMU各自通过独立的串口或USB模块接到ROS2主机，主机上分别运行两个驱动节点，各自发布sensor_msgs/Imu话题。上一篇分享里我选的正是这种方案。原因是开发阶段灵活，驱动、同步、融合逻辑全部放在ROS2侧，哪里出问题都能直接用ros2 topic echo看到，改参数不需要重新烧录固件。时间同步问题虽然稍有代价，但可以用message_filters的近似时间同步策略解决。对于研究和原型验证，分体式方案是最省力的。

IMU硬件型号方面，低成本DIY会选择MPU-6050、MPU-6500、ICM-42688-P这一类消费级芯片，几十块钱就能上手；工业级项目普遍用BMI088、ADIS16448这种温漂和噪声特性更好的产品。做双IMU时我建议尽量选同型号芯片，至少保证两路的噪声量级一致，方便后面用固定权重融合。

2.2 ROS2节点划分与消息设计

整个ROS2系统的节点划分很清晰。两个驱动节点imu1_driver和imu2_driver分别负责读取串口数据，解析加速度、角速度帧，发布到/sensor/imu1_raw和/sensor/imu2_raw。融合节点ahrs_fusion订阅这两路话题，完成外参变换、时间同步、数据级融合、姿态解算，然后输出三类消息。

• /ahrs/imu：sensor_msgs/Imu，包含融合后的姿态四元数、角速度、加速度，以及协方差矩阵。这是姿态数据的核心输出。
• /ahrs/pose：geometry_msgs/PoseStamped，把四元数封装成Pose形式，方便Rviz2显示或给导航栈直接订阅。
• /ahrs/diagnostics：项目自定义消息，保存两路IMU的方差估计、异常标志、实时融合权重。做调试时这份信息非常有用。

坐标系和TF树也要提前定好。通常定义base_link为机器人本体坐标系，imu1_link和imu2_link分别是两颗IMU的安装坐标系。TF树在base_link下面挂两个静态子坐标系，发布static_transform_publisher即可。AHRS输出的姿态，语义是base_link相对odom或world的旋转，输出前要用TF或者代码一次性把计算基准转过去。

2.3 安装约束、外参标定与杆臂效应

两颗IMU的轴向很难做到完全平行，就算同一批次芯片焊到PCB上，也会有零点几度的安装偏角，更不用说装在机械臂或车身不同位置。这种安装误差如果不处理，融合出来的数据会直接打架。解决办法是先做外参标定，确定IMU2坐标系到IMU1坐标系的旋转矩阵R21和平移向量t21。低成本做法是把机器人静态摆成多个已知姿态，分别记录两路IMU的测量值，用最小二乘拟合旋转关系；精度要求高的，可以直接引入基于Kalibr思路的IMU-IMU外参标定工具，或借用视觉IMU联合标定的外参优化思路。

旋转对齐公式很简单：

ω1_aligned = R21 * ω2

加速度对齐要麻烦一些。如果两颗IMU安装距离较远，刚体旋转时IMU2相对IMU1会感受到由杆臂效应产生的额外加速度，包括向心加速度和角加速度引起的切向加速度。变换时要把这些量按刚体运动关系扣除：

a1_等效 = R21 * (a2_meas - α × r - ω × (ω × r))

其中r是从IMU1指向IMU2的向量，ω和α是刚体的角速度和角加速度。低速场景下这一项确实可以忽略，但大家既然做双IMU、提高精度，就一定要在工程上把杆臂补偿流程写进去，哪怕是近似数值。之前遇到过一个案子，两台IMU横向距离大约15cm，天线绕z轴快速转动时刻的加速度差能到0.3g左右，不补偿，融合出来的pitch和roll在高转速下错得离谱。

3. 双IMU融合算法原理

3.1 IMU测量模型和误差源

写融合算法前，脑子里一定要有IMU的数学模型。陀螺仪测量模型可以简化为：

ω_m = ω_true + b_g + n_g

加速度计测量模型：

a_m = R_T_wb * (a_world - g) + b_a + n_a

b_g和b_a是零偏，n_g和n_a是白噪声。零偏不是一成不变的，它随温度和时间缓慢游走，这是所有姿态漂移的总根源。陀螺仪零偏不稳会直接映射到角度随机游走；加速度计零偏会让静止时估计出的水平面歪一点，但通常比陀螺仪问题好控制。

双IMU融合之所以有价值，是因为两路传感器都有独立的噪声和零偏。如果安装位置接近，它们观测同一个刚体运动，理论上下面关系成立：

ω1 ≈ R21 * ω2 a1 ≈ R21 * a2（做杆臂补偿后）

任何一路明显偏离这个约束，就说明它出了异常。这个思想贯穿了整个融合算法的大多数逻辑。

3.2 数据级融合：时间同步、外参对齐、自适应权重

数据级融合的第一步是时间同步。两颗IMU各自有独立时钟，发布的角速度、加速度不一定在同一时刻被采样，直接拿来做融合会产生额外误差。ROS2里最简单实用的工具是message_filters的ApproximateTimeSynchronizer，它会把时间戳接近的消息打包成一组回调数据，时间差的容限通常设置5到10毫秒。如果硬件是独立MCU方案，让MCU在每条数据帧里自带一个内部的计时戳，再到主机侧补偿，精度会更高。

第二步是外参对齐和杆臂补偿。按上一节的公式，把IMU2的角速度和加速度变换到IMU1坐标系。做完以后，两路数据描述的是同一坐标系下同一个刚体运动。

第三步是计算融合权重。最简单的做法是固定等权重，各0.5。更稳健的做法是自适应权重：计算一个滑动窗口内两路IMU测量残差，比如|ω1_aligned - ω2|的均方根，用逆方差加权决定权重：

w_i = (1 / σ_i^2) / ((1 / σ1^2) + (1 / σ2^2))

一旦某一路的残差突然变大，说明它可能被干扰或掉线，权重直接降到接近0，另一路自动接管。这个机制听着简单，但实际能避免大量隐性事故，我强烈建议你们的AHRS节点至少保留一份异常检测逻辑。

3.3 Mahony互补滤波做姿态解算

融合后的角速度和加速度进入姿态解算环节。很多人问，为什么不用卡尔曼滤波？我的答案是，Mahony互补滤波在大多数工程场景下已经够用，而且调参直观、计算量小、状态不会有发散风险。它靠一个PI控制器实时修正陀螺仪零偏带来的漂移，用加速度计输出的重力方向作为修正基准。

核心更新流程如下：

q_dot = 0.5 * q ⊗ [0, ω_corrected] q += q_dot * dt

其中ω_corrected是修正后的角速度：

ω_corrected = ω_meas + Kp * e + Ki * ∫ e dt

e是加速度计方向与陀螺仪积分方向之间的误差叉积。

具体实现代码用C++写出来，核心类大概是这样的：

#include <Eigen/Core> #include <Eigen/Geometry> class MahonyAHRS { public: void Update(double gx, double gy, double gz, double ax, double ay, double az, double dt) { Eigen::Quaterniond q(qw_, qx_, qy_, qz_); Eigen::Vector3d gyro(gx, gy, gz); Eigen::Vector3d accel(ax, ay, az); accel.normalize(); // 由当前姿态估计重力方向 Eigen::Vector3d v = q.conjugate() * Eigen::Vector3d(0, 0, 1); // 误差 = 测量重力方向与估计重力方向的叉积 Eigen::Vector3d error = v.cross(accel); integral_ += error * dt; Eigen::Vector3d correction = kp_ * error + ki_ * integral_; if (kInit_) { // 初始化用加速度计校准初始水平 Eigen::Vector3d e_z(0, 0, 1); Eigen::Vector3d a_z = accel; q = Eigen::Quaterniond::FromTwoVectors(e_z, a_z); } qw_ = q.w(); qx_ = q.x(); qy_ = q.y(); qz_ = q.z(); } double qw_ = 1, qx_ = 0, qy_ = 0, qz_ = 0; double kp_ = 1.2, ki_ = 0.05; Eigen::Vector3d integral_ = Eigen::Vector3d::Zero(); bool kInit_ = true; };

调参经验是：Kp控制收敛速度，值太小修正慢，静止恢复时间长；值太大会把加速度计的噪声串进来，姿态高频抖动。常见Kp从1.0到2.0起步，需要自己扫参数。Ki控制对零偏的长期积分补偿，通常取Kp的1/20到1/50，太大容易让积分项在动态过程中乱飞。实际使用时，可以先静态放设备一分钟，用这段数据估计初始零偏，把初始偏移加到测量值上再送进滤波器，Mahony的负担会小很多。

3.4 进阶路线：EKF/ESKF多IMU完整状态融合

如果系统对精度和工况适应性要求更高，比如无人机在强机动下用，Mahony加数据级融合就不一定够。这时候可以把两颗IMU同时塞进一个状态向量里，用扩展卡尔曼滤波或误差状态卡尔曼滤波做整体融合。

状态向量可以设计成：

x = [q, b_g1, b_g2, b_a1, b_a2, v, p]

系统模型用IMU1的角速度驱动姿态更新，IMU2的角速度作为另一个测量来源。两路加速度计经过外参对齐和杆臂补偿后，各自作为测量残差进入更新方程。这样做的好处是两颗IMU的零偏和安装误差都能被实时估计，不会像Mahony那样只能得到一个合并过的观测值；代价是实现复杂度高，且需要仔细标定噪声矩阵Q和R。对大多数工程场景，我建议先跑通Mahony，确认双IMU融合能带来稳定增益，再考虑升级EKF，否则排查起来会非常痛苦。

4. ROS2工程实战：把方案跑起来

4.1 环境准备与功能包创建

实战阶段默认环境是Ubuntu 22.04加ROS2 Humble，新项目用Ubuntu 24.04加Jazzy也可以，命令大同小异。先确认环境变量已经source进当前shell。

创建工作区并创建功能包：

mkdir -p ~/dual_imu_ws/src cd ~/dual_imu_ws/src ros2 pkg create ahrs_fusion --build-type ament_cmake \ --dependencies rclcpp sensor_msgs geometry_msgs message_filters std_msgs tf2_geometry_msgs

如果后面要用Eigen，顺手在CMakeLists里加find_package(Eigen3 REQUIRED)，并且在package.xml里加依赖。

4.2 IMU驱动节点怎么写

驱动节点的工作流程很固定：打开串口、配置波特率、循环读取数据帧、校验帧头帧尾和CRC、把原始加速度和角速度换算成物理单位、填进sensor_msgs/Imu消息、发布出去。

不同IMU模组的协议差异很大，但有两个注意点值得重点提。一是时间戳一定要在数据解析完成后立刻打，最好用主机当前时间，这样才能和另一路IMU在公布时间上做近似同步对齐。二是驱动节点里不要做任何滤波处理，原始数据直接发布，滤波任务全部交给下游AHRS节点，否则调试时根本分不清是传感器的问题还是算法的问题。

发布消息的核心代码片段比较简单：

// 假设已经从串口解析出加速度 acc 和角速度 gyro auto msg = sensor_msgs::msg::Imu(); msg.header.stamp = now(); msg.header.frame_id = "imu1_link"; msg.linear_acceleration.x = acc[0]; msg.linear_acceleration.y = acc[1]; msg.linear_acceleration.z = acc[2]; msg.angular_velocity.x = gyro[0]; msg.angular_velocity.y = gyro[1]; msg.angular_velocity.z = gyro[2]; publisher_->publish(msg);

角速度单位统一用弧度每秒，加速度单位统一用米每秒平方，这是ROS标准，别弄混了。

4.3 AHRS融合节点的核心实现

AHRS节点要做的事情比较多，我拆成三层来看。第一层是消息同步层，用ApproximateTimeSynchronizer把两路IMU消息对齐。第二层是算法层，把外参对齐、杆臂补偿、数据级融合、Mahony姿态解算封装成独立的类。第三层是发布层，把姿态结果封装成消息发布出去。

消息同步的代码大概长这样：

#include <message_filters/subscriber.h> #include <message_filters/sync_policies/approximate_time.h> #include <message_filters/synchronizer.h> using Imu = sensor_msgs::msg::Imu; using SyncPolicy = message_filters::sync_policies::ApproximateTime<Imu, Imu>; // 在类成员中： message_filters::Subscriber<Imu> sub_imu1_; message_filters::Subscriber<Imu> sub_imu2_; std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_; void Callback(const Imu::SharedPtr &imu1, const Imu::SharedPtr &imu2);

回调函数里，先把IMU2的数据通过外参变换到IMU1坐标系，然后判断两路数据是否异常，确定融合权重，最后调用Mahony类更新姿态并发布。

一个可以直接运行的融合节点框架大概这样：

#include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/imu.hpp> #include <geometry_msgs/msg/pose_stamped.hpp> #include <message_filters/subscriber.h> #include <message_filters/sync_policies/approximate_time.h> #include <message_filters/synchronizer.h> #include <Eigen/Geometry> class AhrsFusionNode : public rclcpp::Node { public: AhrsFusionNode() : Node("ahrs_fusion") { pub_imu_ = create_publisher<sensor_msgs::msg::Imu>("/ahrs/imu", 10); pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/ahrs/pose", 10); sub_imu1_.subscribe(this, "/sensor/imu1_raw"); sub_imu2_.subscribe(this, "/sensor/imu2_raw"); sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), sub_imu1_, sub_imu2_); sync_->registerCallback(&AhrsFusionNode::Callback, this); } private: void Callback(const sensor_msgs::msg::Imu::SharedPtr &imu1, const sensor_msgs::msg::Imu::SharedPtr &imu2) { // 1. 外参对齐：把 imu2 旋转到 imu1 坐标系 Eigen::Vector3d g2(imu2->angular_velocity.x, imu2->angular_velocity.y, imu2->angular_velocity.z); Eigen::Vector3d a2(imu2->linear_acceleration.x, imu2->linear_acceleration.y, imu2->linear_acceleration.z); Eigen::Vector3d g2_aligned = R21_ * g2; Eigen::Vector3d a2_aligned = R21_ * a2; // 2. 杆臂补偿（用当前角速度和角加速度） Eigen::Vector3d omega = g2_aligned; Eigen::Vector3d alpha = (OmegaFiltered_ - prev_omega_) / dt_; a2_aligned -= alpha.cross(r21_) + omega.cross(omega.cross(r21_)); prev_omega_ = omega; // 3. 简单等权重融合 double w1 = 0.5, w2 = 0.5; Eigen::Vector3d gyro = w1 * Eigen::Vector3d(imu1->angular_velocity.x, imu1->angular_velocity.y, imu1->angular_velocity.z) + w2 * g2_aligned; Eigen::Vector3d accel = w1 * Eigen::Vector3d(imu1->linear_acceleration.x, imu1->linear_acceleration.y, imu1->linear_acceleration.z) + w2 * a2_aligned; // 4. Mahony 姿态解算 double dt = (imu1->header.stamp.sec + 1e-9 * imu1->header.stamp.nanosec) - last_stamp_; last_stamp_ = imu1->header.stamp.sec + 1e-9 * imu1->header.stamp.nanosec; mahony_.Update(gyro.x(), gyro.y(), gyro.z(), accel.x(), accel.y(), accel.z(), dt); // 5. 发布 auto msg = sensor_msgs::msg::Imu(); msg.header.stamp = now(); msg.header.frame_id = "base_link"; msg.orientation.w = mahony_.qw_; msg.orientation.x = mahony_.qx_; msg.orientation.y = mahony_.qy_; msg.orientation.z = mahony_.qz_; pub_imu_->publish(msg); geometry_msgs::msg::PoseStamped pose; pose.header = msg.header; pose.pose.orientation = msg.orientation; pub_pose_->publish(pose); } rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_; rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_; message_filters::Subscriber<sensor_msgs::msg::Imu> sub_imu1_; message_filters::Subscriber<sensor_msgs::msg::Imu> sub_imu2_; std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_; Eigen::Matrix3d R21_ = Eigen::Matrix3d::Identity(); Eigen::Vector3d r21_ = Eigen::Vector3d::Zero(); double last_stamp_ = 0.0; Eigen::Vector3d prev_omega_ = Eigen::Vector3d::Zero(); MahonyAHRS mahony_; };

这个框架已经足够跑通第一版，实际项目里还需要加两路健康状态判断和权重自适应。融合权重先固定0.5/0.5，等确认数据一致性没问题，再上异常检测逻辑。

4.4 启动、可视化和验证方法

写完节点后，用launch文件把三个节点拉起来。launch文件里先启动两个驱动节点，再启动AHRS融合节点，最后用static_transform_publisher把TF树设置好。命令行提示：

ros2 launch ahrs_fusion dual_imu_ahrs.launch.py

Rviz2里做可视化时，重点关注三类显示。一是/ahrs/pose用Axes或Pose显示，直接看姿态朝向；二是/sensor/imu1_raw和/sensor/imu2_raw用Imu显示，看两路原始数据的轴方向是否一致；三是TF显示，确认base_link和imu_link的关系正确。

验证流程可以按四步走。第一步静态验证：设备放桌上静止一个小时，记录姿态四元数变成欧拉角后的最大漂移，目标小于1度。第二步慢速旋转验证：手持设备缓慢绕各轴旋转，观察姿态跟随是否平滑、是否有明显滞后抖动。第三步动态验证：做几次快速加减速和急停，观察pitch和roll是否被拉飞出几度，以及能否在2到3秒内收敛回去。第四步冗余验证：运行中拔掉其中一颗IMU的USB线或者给其中一路人为置零，确认AHRS依然输出接近正常的姿态，这一项是双IMU方案最值钱的试金石。

5. 常见问题与排障心得

5.1 高频问题速查表

5.2 三个值得深挖的排查案例

第一个案例是双IMU融合后比单IMU还抖。当时现象很直观，融合后的姿态比单独听IMU1的还要乱。查到最后发现是两路数据时间戳偏差到了几十毫秒，一颗IMU的主机时钟分布抖动太大，ApproximateTimeSynchronizer虽然把消息凑成了一对，但真实采样时刻差了很远。解决方案是给驱动节点加了一层“时间戳平滑”，用最近一次收到的硬件帧序号估算真实采样时间，效果立刻改善。这件事说明一个道理：融合算法的精度上限，很大程度上取决于时间同步的质量。

第二个案例是加速瞬间姿态被拉飞。单独看IMU1和IMU2的原始数据都很正常，但融合后的姿态总是在急加速时偏转三四度。后来把两路加速度差值打印出来，发现差值方向和角速度方向高度相关，认定是杆臂效应。把r21向量用卡尺量出来，在代码里补上向心加速度补偿之后，急加速时的姿态误差从三四度降到了零点几度。理论看着很虚，实际操作一次才会真正理解为什么两个IMU不能随便焊在结构件两端。

第三个案例是通电初始阶段yaw和roll乱跳。原因是Mahony滤波器初始姿态被设成单位四元数，而设备实际不是水平放置，一开机就有一大段误差需要PI慢慢纠正。解决起来也简单，开机后用第一帧加速度计数据做FromTwoVectors初始化，先把水平姿态掰正，再进主循环。这个小优化能让设备上电后立刻进入可用状态。

5.3 从单IMU到多IMU，最大的变化其实是心态

最后聊点项目之外的体会。从单颗IMU切换到双IMU融合，刚开始总下意识地想找一整套完美公式把问题一次性解决。真正做下来以后，你会发现算法只是其中一半的工作量，剩下的一半在数据对齐、标定、异常处理这些看起来很基础的地方。双IMU方案之所以能被称为“双剑合璧”，不是因为算法多花哨，而是两路独立的信息互相印证，让系统在恶劣工况下有了容错兜底的能力。

如果后续想继续扩展，可以考虑把两个IMU和相机、轮式里程计做联合标定与融合，类似视觉惯性方案里常用的滑窗优化思路，把IMU零偏和相机外参一并估计。也可以把当前这套AHRS输出送给Nav2或者控制器做反馈，替代传统的底盘里程计姿态。这套双IMU架构我自己跑了小半年，最大的感受是：只要时间同步和异常降权做扎实，后续再叠加其他传感器都非常顺。希望这篇记录能帮少走一段弯路。