上一节：第 9 章 IMU选型：从MPU6050到内置MCU融合的演进 极低成本 · 实物上手 · 可迁移至高端人形平台
第二部分：传感器系统

第 10 章 IMU轴映射：最让人崩溃的调试 踩坑记录 · 从v5到v14的版本迭代

10.1 坐标系不一致——一切混乱的根源

如果你觉得上一章选IMU已经够折腾了，那我必须提前告诉你：选IMU只是万里长征的第一步。真正让你崩溃的，是IMU的轴映射。
IMU有自己的坐标系
每一颗IMU芯片都有自己的坐标系，遵循“右手定则”：x轴指向芯片某个方向，y轴指向另一个方向，z轴垂直于芯片表面。
问题在于，这个芯片坐标系和你的机器人坐标系，未必就完全一致。
IMU芯片是焊在电路板上的，这个位置会影响芯片的摆放方向，接线怎么走才安全，怎么固定才比较稳，这么看着感觉舒服，这些因素都会影响你安装IMU芯片。而你的机器人坐标系——x轴前进方向，y轴左侧方向，z轴向上方向——和IMU芯片的坐标系，可能差了90度、180度，甚至是一个随机的角度。
更具体地说，我的IMU物理安装方向是这样的：IMU芯片的x轴方向，对应的是机器人前进方向。但由于接线安全的限制，IMU实际上转了90度。也就是说，IMU芯片的原始x轴，对准的是机器人的y轴方向。IMU芯片的原始y轴，对准的是机器人的x轴方向。

第一次发现不对劲
我第一次发现这个问题的场景，至今记忆犹新。我让机器人放在水平地面上，用手推它的身体——向前推，正常情况应该是机器人前倾，pitch角变大。但我打开监控界面一看——pitch没怎么变，roll变成了-30°。
我当时的反应是：WTF？？？
我向前推机器人，它应该前倾（pitch变化），不应该侧倾（roll变化）。但数据显示的是roll变了。这说明IMU的roll和pitch轴，在代码里搞反了——或者说，IMU的物理安装方向和代码里的假设不一致。
我花了大概半小时才想明白：因为IMU转了90度，IMU芯片的x轴（roll）对应的是机器人的前后方向（pitch），IMU芯片的y轴（pitch）对应的是机器人的左右方向（roll）。
向前推机器人，IMU显示roll变成了-30°——不是机器人倾倒了，而是IMU的轴和机器人的轴对不上。这是所有IMU调试中最经典、最让人困惑的场景。
坐标系对齐的三种方式
方式一：物理安装对齐。 把IMU芯片的安装方向调整到和机器人坐标系完全一致。这听起来最简单，但在实际中往往不可行——因为各种限制，你可能找不到完美的安装位置。
方式二：软件层做轴映射。 在代码里把IMU的原始数据做一次变换——把x轴的数据赋值给y轴，把y轴的数据赋值给x轴，再加上必要的符号翻转。这样无论物理安装方向如何，发布到ROS2话题的数据都是标准坐标系的。
方式三：在消费端（控制器）做适配。 不对IMU原始数据做任何变换，而是在控制器里根据实际安装方向来调整。比如，如果IMU转了90度，就把roll当pitch用，把pitch当roll用。
看起来方式三最灵活——你可以在控制器里随意调整。但这也是我后来踩了无数坑的根源。下面我会详细讲。
在继续之前，我先定义一下本章要用的几个关键术语：

术语	含义
标准坐标系	机器人前=x+，左=y+，上=z+。roll=左右倾斜（正=右倾），pitch=前后倾斜（正=前倾），yaw=偏航（正=左转）
IMU原始坐标系	IMU芯片自身的坐标系，可能与标准坐标系差90度或180度
轴映射	将IMU原始数据变换到标准坐标系的操作
swap	交换两个轴的数据（如把roll当pitch用）
符号翻转	对某个轴的数据取反（如把+30°变成-30°）
驱动层	直接读取IMU硬件并发布ROS2话题的代码，即ybimu_driver_node.py
控制器层	消费IMU数据做平衡控制的代码，即zmp_walking_controller.py
舵机指令层	将控制器输出的角度指令发送给舵机的代码，即buslinker_controller.py
恒等映射	变换后结果与输入相同，即实际上没有效果的方向修正

10.2 双重错误抵消——薛定谔的IMU
如果你觉得“坐标系不一致”这个问题已经够烦了，那接下来这个故事会让你知道什么叫“真正的绝望”。
在我发现IMU的roll和pitch轴反了之后，我做了一件“理所当然”的事——在代码里把变量名赋值对调了一下。逻辑很简单：既然传感器把roll和pitch搞反了，那我就在代码里换回来。改完之后，重新编译，重启服务，测试——方向正确了。我心想：好，搞定。然后继续调其他参数。
两周之后，我发现了一个惊天秘密。
变量命名错了，但舵机方向也错了
我在review代码时惊讶地发现——我不仅把变量名搞反了，而且舵机的方向也搞反了。
错误一： 在IMU数据读取时，我把IMU的roll数据赋值给了_imu_pitch，把IMU的pitch数据赋值给了_imu_roll。变量名和实际物理含义是反的。
错误二： 在舵机指令发送时，踝关节的roll和pitch补偿方向也反了——本来应该向左补偿的，结果变成了向右补偿；本来应该向前补偿的，结果变成了向后补偿。
两个错误互相抵消，机器人居然能站稳。
我当时的心情，只能用“薛定谔的IMU”来形容——在我不打开代码检查之前，IMU的轴映射既是正确的，也是错误的。它正确是因为两个错误抵消了，它错误是因为每一层单独看都是错的。
两个错误互相抵消，机器人居然能站稳。这不是什么好运气，而是一颗定时炸弹——你永远不知道什么时候其中一个错误会被无意中“修正”，然后机器人立刻倒地。
这个发现让我后怕了很久。如果我在后续的调试中，不小心“修正”了其中一个错误——比如，发现变量名搞反了就改过来，但没发现舵机方向也反了——那机器人会立刻失去平衡，而且我完全不知道为什么。因为从变量名上看，一切“正确”了，但从物理行为上看，一切都不对了。
为什么双重错误能抵消
假设机器人真实的倾斜角度是pitch=+5°（前倾5°），IMU因为安装方向转了90度，输出的原始数据是roll_raw=+5°, pitch_raw=0°。在控制器里，代码是这样的（简化版）：

“// 错误一：变量名赋值反了
_imu_roll = pitch_raw;  // 实际上存的是5°（前倾值）
_imu_pitch = roll_raw;  // 实际上存的是0°（侧倾值）

// 平衡补偿（简化）
ankle_roll_comp = _imu_roll * Kp;  // = 5° * Kp
ankle_pitch_comp = _imu_pitch * Kp; // = 0° * Kp

// 错误二：舵机方向反了
// 本来应该：ankle_roll_comp 用于调整左右倾斜
// 但因为舵机方向反了，ankle_roll_comp 实际调整了前后倾斜
// 而 ankle_roll_comp 里存的正好是前倾值（5°）
// 所以补偿方向恰好正确！”

这是一个完美的“负负得正”——变量名搞反了，舵机方向也搞反了，两个错误恰好抵消，机器人站得稳稳的。但这段代码，任何一个有经验的工程师看了都会皱眉头——因为它违背了“代码应该反映物理现实”的基本原则。
从v5到v8：在错误中挣扎
在发现双重错误抵消之后，我做了很多尝试来“修正”这个问题。但每一次修正，都让情况变得更糟。
v5版本：双重错误抵消，机器人能站稳，但代码不可维护。任何修改都可能导致机器人倒地。
v6版本：我发现了变量名错误，把_imu_roll和_imu_pitch的赋值修正了。但没发现舵机方向也反了。结果机器人一启动就倒——因为现在变量名对了，但舵机方向还是错的，补偿完全反了。
v7版本：发现了舵机方向问题，在控制器里加了手动swap。在PD计算时把roll和pitch的数据交换使用。理论上应该能解决问题——但实际上，因为代码里到处是“if joint_name == …”的方向判断，很多地方swap了，有些地方没swap，行为完全不可预测。
v8版本：在v7的基础上继续修修补补。每次发现某个关节的方向不对，就加一个“if joint_name == …”的特判。代码越来越臃肿，越来越不可维护。
v6-v8这段时间，是我整个项目中最痛苦的阶段。每次改一个参数，都要确认十几个方向判断是否正确。改一次，调一次，测一次——做一次修改可能需要半天时间。我后来统计了一下，v8版本中仅方向判断相关的代码就有超过80行，分布在5个不同的函数里。
v6-v8的代码里有超过80行方向判断代码，分布在5个函数中。每次修改参数都像在走钢丝——你不知道改了这一个，会不会影响其他四个地方。这就是“在消费端做轴映射”的代价。
双重错误的反向推导与调试过程 [v4新增]
为了让你更直观地理解“双重错误抵消”的机制，我把我当时的调试过程还原出来。这个过程非常经典，它展示了“在错误的地方找问题”的典型症状。
调试的第一步是验证IMU数据的正确性。我写了一个简单的ROS2节点，同时订阅/imu/euler话题并打印原始值：

# 验证IMU数据方向的脚本
def imu_callback(self, msg):
    roll, pitch, yaw = msg.data[0], msg.data[1], msg.data[2]
    # 向前推机器人 -> 应该 pitch 变大
    # 向左推机器人 -> 应该 roll 变大
    print(f"roll={roll:.2f}, pitch={pitch:.2f}")

向前推机器人，pitch没变，roll变成了-30°。这说明IMU的roll和pitch轴反了——因为IMU转了90度，roll对应的是前倾方向。
然后我在控制器里做了“简单”的修正——把变量名对调。搞反了，对调不就好了吗？以下是v5版本的修正代码：

# v5: 双重错误抵消版本
def imu_callback(self, msg):
    # 错误一: 变量名和物理含义反了
    self._imu_roll = msg.data[1]   # 存的是pitch值
    self._imu_pitch = msg.data[0]  # 存的是roll值

def balance_control(self):
    # 错误二: 踝关节补偿方向也反了
    # 本来应该: ankle_roll_comp = roll * Kp
    # 但因为舵机方向反了, 这个补偿实际作用在pitch上
    # 而 _imu_roll 里存的正好是pitch值
    # 所以 负负得正!
    ankle_roll_comp = self._imu_roll * Kp_roll
    ankle_pitch_comp = self._imu_pitch * Kp_pitch

这段代码展示了v5版本的全部问题。_imu_roll里存的是pitch值，_imu_pitch里存的是roll值。但踝关节的roll补偿实际作用在pitch方向（因为舵机方向反了），所以正好“负负得正”。
我花了整整两周才意识到这一点。因为每次单独“修正”一个错误，机器人反而会倒。修正了变量名，变量名对了但舵机方向还是反的，补偿全反了——机器人立刻倒地。修正了舵机方向，舵机方向对了但变量名还是反的，补偿也全反了——机器人也立刻倒地。只有两个错误同时存在，机器人才站得稳。
这个发现让我深刻理解了v13原则的重要性：方向转换要统一在一个地方处理。如果方向转换分散在多个地方，就会出现“你不知道哪个方向是对的”的困境。

10.3 控制器内swap的血泪史——v6到v12的教训

v6到v12这段时间，我一直在“控制器内swap”这个思路上挣扎。这是全书最核心的教训——不在于技术本身，而在于架构设计的原则。
v6-v8：手工swap的混乱
v6-v8的核心问题是：在控制器里手动swap roll和pitch。因为IMU安装方向转了90度，物理上roll对应前倾、pitch对应侧倾。所以控制器里，roll应该用于pitch补偿，pitch应该用于roll补偿。
这个逻辑听起来是对的——但问题在于，它只在PD计算一个地方生效。控制器里还有ZMP计算、抬脚控制、跨步恢复、march_test等多个模块，每个模块都可能用到IMU数据。你在PD计算里swap了，但其他模块没swap，数据就不一致了。
然后你就开始打补丁：发现ZMP计算不对，在ZMP计算里也加一个swap；发现抬脚控制不对，在抬脚控制里也加一个swap；发现跨步恢复不对，在跨步恢复里也加一个swap。每个模块都加一次swap，代码就像补丁摞补丁的旧衣服——到处都是缝补的痕迹，但没有一块是完整的。
v9-v12：走向“恒等映射”
到了v12，我意识到“在控制器里swap”这条路走不通了。太乱了，太容易出错了。每次加新功能，第一件事就是确认“这个功能要不要swap IMU数据”。如果忘了，功能就不对。如果记错了swap的方向，功能也不对。
v12版本我做了一个重要的改进：把swap从多个地方集中到一个地方——在IMU数据进入控制器时统一做一次swap，所有后续代码直接消费处理后的数据。这比v8好了一些，但问题并没有根治——因为swap本身还在控制器里。
v12还有一个隐藏的问题：因为swap在控制器里，所以只有控制器能看到“正确”的数据。其他任何消费IMU数据的模块——比如监控系统、日志系统、调试工具——看到的都是未经swap的“错误”数据。这就导致一个尴尬的场景：你在监控系统里看到roll=10°，但控制器里“认为”roll=0°（因为swap了）。同一个数据源，两个不同的解读，调试起来极其痛苦。
v12的错误：swap在控制器里，只有控制器能看到“正确”数据。监控系统、日志系统看到的都是原始数据。同一个数据源，两个不同的解读——调试时你根本不知道哪个是对的。

10.4 统一到驱动层——v13方案

v13是整个项目中最重要的一个版本——不是因为加了多少新功能，而是因为终于把IMU轴映射这个困扰了我两个多月的问题彻底解决了。
v13的核心思想只有一句话，但这句话值两个月的痛苦：方向转换只在两个边界处理，其他所有代码一律不准涉及方向转换。
v13的核心原则
v13定义了三个层，以及每个层的职责：
第一层：IMU数据入口（驱动层，ybimu_driver_node.py）。 这是唯一一个可以修改IMU数据的地方。通过_map_to_standard()方法完成轴映射，把原始数据变换到标准坐标系。变换后的数据发布到ROS2话题，所有下游节点消费的都是标准坐标系的数据。
第二层：控制算法层（zmp_walking_controller.py等）。 这一层直接消费标准坐标系的数据，不做任何方向判断、不做任何轴交换、不做任何符号翻转。PD补偿、ZMP计算、抬脚控制、跨步恢复——所有算法都用标准坐标系，自然正确。
第三层：舵机指令出口（buslinker_controller.py）。 这是唯一一个处理硬件方向的地方。通过self.reverse字典，把标准坐标系的角度指令映射到舵机的实际运动方向。
这个分层设计的美妙之处在于：每一层都有自己的职责，每一层都只做一件事。驱动层负责“把数据翻译成标准语言”，控制器层负责“用标准语言做计算”，舵机层负责“把标准指令翻译成硬件指令”。就像翻译团队——有人负责把中文翻译成英文，有人负责用英文写报告，有人负责把英文翻译成法文。每个人都只做自己擅长的事，不会互相干扰。这样做以后，以后转到别的机器人上你不需要改核心代码，只需要把入口的数据拿好，出口的控制数据用好就可以，而且不管换到什么机器人，本身就是要更换硬件的。

_map_to_standard()的完整实现
v13的核心改动在ybimu_driver_node.py中的_map_to_standard()方法。以下是v13版本的完整实现，包含了欧拉角、角速度和加速度三个维度的映射：

def _map_to_standard(self, ybimu_roll, ybimu_pitch, ybimu_yaw,
                      ybimu_gx, ybimu_gy, ybimu_gz,
                      ybimu_ax, ybimu_ay, ybimu_az):
    # YbImu物理安装方向转了90度(绕Z轴)
    # YbImu roll 增加 = 机器人前倾 = 标准 Pitch +
    # YbImu pitch 增加 = 机器人左倾 = 标准 Roll -
    # 故: standard_roll = -ybimu_pitch

    # 欧拉角映射 (YbImu 返回值单位: 度)
    std_roll = -ybimu_pitch    # 右倾为正
    std_pitch = ybimu_roll     # 前倾为正
    std_yaw = ybimu_yaw        # 左转为正

    # 角速度映射 (YbImu 返回值单位: rad/s)
    std_gx = -ybimu_gy   # 右倾角速度
    std_gy = ybimu_gx    # 前倾角速度
    std_gz = ybimu_gz    # 左转角速度

    # 加速度映射 (YbImu 返回值单位: g)
    std_ax = -ybimu_ay   # 前向加速度
    std_ay = ybimu_ax    # 左向加速度
    std_az = ybimu_az    # 垂直加速度

    return (std_roll, std_pitch, std_yaw,
            std_gx, std_gy, std_gz,
            std_ax, std_ay, std_az)

这段代码看起来很简单——只有二十行左右。但为了写出这二十行正确的代码，我花了两个多月。因为在此之前，我一直试图在控制器层做这件事，结果越做越乱。
这二十行代码完成之后，所有发布到ROS2话题的数据都是标准坐标系的。控制器里不再需要任何swap、不再需要任何if判断、不再需要任何“方向修正”。控制器代码里关于方向判断的80多行代码，全部删掉了。
v13最让我开心的事情不是加了多少新功能，而是删掉了多少旧代码。80多行方向判断代码，全部删除。代码变少了，bug变少了，维护成本降低了。这才是好的重构。
v13到v14：从轴映射到恒等映射的演进
v13之后，我做了一次重要的升级——v14。v14的改动看似简单，但背后有深刻的物理验证。
在v13版本中，_map_to_standard()做了轴交换和符号翻转。但经过更仔细的实测，我发现YbImu的物理安装方向并不是严格转了90度——实际上IMU模块的安装方向已经接近标准坐标系。在v14中，_map_to_standard()变成了恒等映射——所有数据直接透传，不做任何变换。

def _map_to_standard(self, ybimu_roll, ybimu_pitch, ybimu_yaw,
                      ybimu_gx, ybimu_gy, ybimu_gz,
                      ybimu_ax, ybimu_ay, ybimu_az):
    # v14: 恒等映射 - IMU安装方向已对齐标准坐标系
    std_roll = ybimu_roll     # 右倾为正 (恒等)
    std_pitch = ybimu_pitch   # 前倾为正 (恒等)
    std_yaw = ybimu_yaw       # 左转为正 (恒等)

    std_gx = ybimu_gx   # 右倾角速度 (恒等)
    std_gy = ybimu_gy   # 前倾角速度 (恒等)
    std_gz = ybimu_gz   # 左转角速度 (恒等)

    std_ax = ybimu_ax   # 前向加速度 (恒等)
    std_ay = ybimu_ay   # 左向加速度 (恒等)
    std_az = ybimu_az   # 垂直加速度 (恒等)

    return (std_roll, std_pitch, std_yaw,
            std_gx, std_gy, std_gz,
            std_ax, std_ay, std_az)

v14的恒等映射并不意味着_map_to_standard()方法是多余的——恰恰相反，它保留了架构的灵活性。如果将来换了IMU型号或改了安装方向，只需要修改这个方法，不需要动任何控制器代码。这就是“在边界处理方向转换”原则的威力。
舵机指令出口的self.reverse字典
v13的另一个边界是舵机指令出口。在buslinker_controller.py中，有一个self.reverse字典，它定义了每个关节的正反方向。例如：

self.reverse = {
    'r_ankle_pitch': 1,   // 正方向
    'r_ankle_roll': -1,   // 反方向
    'l_ankle_pitch': -1,  // 反方向
    'l_ankle_roll': 1,    // 正方向
    // ... 其他关节
}

这个字典是硬件方向的唯一权威来源。控制器输出的角度指令都是标准坐标系下的——正角度表示“标准方向的正方向”。但具体到每个关节的舵机，标准方向的正方向可能对应舵机的正转，也可能对应舵机的反转。这个关系由self.reverse字典定义。
v13的原则是：控制器层不关心self.reverse的内容。控制器只管输出标准坐标系的指令，具体的硬件方向映射完全交给buslinker_controller。这意味着，如果你换了一颗舵机，或者换了一个安装方向，你只需要修改self.reverse字典，不需要动控制器代码。
v13的标准坐标系定义
v13定义了明确的标准坐标系，所有算法都基于这个坐标系：

坐标轴	含义	正方向
Roll	左右倾斜	正=右倾（机身向右倾斜）
Pitch	前后倾斜	正=前倾（机身前倾，脚尖下压方向）
Yaw	偏航角	正=左转
gyro_x	绕x轴角速度	正=右倾角速度
gyro_y	绕y轴角速度	正=前倾角速度
gyro_z	绕z轴角速度	正=左转角速度

这个标准坐标系一旦确定，所有后续的算法设计都基于它。PD补偿的自然映射是：ankle_pitch_comp = -(Kp * pitch + Kd * gyro_y)，ankle_roll_comp = Kp * roll + Kd * gyro_x。不需要任何额外的方向判断。
v13的正确性验证
v13完成后，我做了系统的验证来确保轴映射正确：
验证一：向前推机器人。 机器人前倾，pitch应为正，gyro_y应为正。实测：pitch=+5.2°，gyro_y=+0.8°/s。正确。
验证二：向左推机器人。 机器人右倾，roll应为正，gyro_x应为正。实测：roll=+3.8°，gyro_x=+0.6°/s。正确。
验证三：静止站立。 PD补偿方向正确：前倾时ankle_pitch应该让脚趾向下压（负补偿），右倾时ankle_roll应该让脚踝向左压（负补偿）。实测：机器人受到推力后能自主恢复平衡。正确。
验证四：监控系统一致性。 监控系统显示的roll/pitch数据与物理行为一致。推机器人前倾，监控界面pitch增加。推机器人右倾，监控界面roll增加。正确。
四项验证全部通过。v13的IMU轴映射，终于对了。
数据流验证的代码示例 [v4新增]
v13的验证不仅仅是“推一推机器人看看数据对不对”，我还写了一套完整的验证脚本，确保数据从IMU芯片到控制器消费的整个链路都是正确的。
以下是验证脚本的核心逻辑——它同时订阅两个话题，对比驱动层和控制器层的数据是否一致：

# 数据流验证: 确保驱动层和控制器层数据一致
class DataFlowVerifier(Node):
    def __init__(self):
        # 订阅驱动层发布的欧拉角
        self.driver_sub = self.create_subscription(
            Float32MultiArray, '/imu/euler',
            self.driver_callback, 10)
        # 订阅控制器接收到的角度 (调试话题)
        self.ctrl_sub = self.create_subscription(
            Float32MultiArray, '/debug/ctrl_imu',
            self.ctrl_callback, 10)
        self.driver_data = None
        self.ctrl_data = None

    def driver_callback(self, msg):
        self.driver_data = [msg.data[0], msg.data[1], msg.data[2]]
        self.check_consistency()

    def ctrl_callback(self, msg):
        self.ctrl_data = [msg.data[0], msg.data[1], msg.data[2]]
        self.check_consistency()

    def check_consistency(self):
        if self.driver_data and self.ctrl_data:
            diff = [abs(self.driver_data[i] - self.ctrl_data[i])
                    for i in range(3)]
            if max(diff) > 0.01:
                self.get_logger().error(
                    f"DATA MISMATCH! driver={self.driver_data}, "
                    f"ctrl={self.ctrl_data}, diff={diff}")

这个验证脚本的核心思想是：如果驱动层和控制器层看到的数据一致，说明数据在传输过程中没有发生意外的变换。如果数据不一致（差值超过0.01°），说明某个环节出了问题——可能是话题映射错误、可能是控制器内部做了不该做的swap、可能是ROS2消息序列化出了问题。
v5-v13各版本代码对比 [v4新增]
为了让你直观感受版本演进的过程，我把v5到v13每个版本的核心代码差异整理出来。这些代码说明了一个道理：架构的改进胜于代码的修补。
v5（双重错误抵消）： 变量名反了，舵机方向也反了，两个错误互相抵消。代码“能跑”但不可维护。

# v5: 两个错误互相抵消
self._imu_roll = msg.data[1]  # 存pitch
self._imu_pitch = msg.data[0] # 存roll
# 再加上舵机方向反了, 负负得正
v6-v8（控制器内swap）： 在控制器里手动swap roll和pitch。每个模块都要单独处理，代码混乱。
# v8: 到处是方向判断, 代码臃肿
if joint_name == 'r_ankle_roll':
    comp = roll * Kp  # 这个模块swap了
if joint_name == 'l_ankle_pitch':
    comp = -pitch * Kp  # 这个模块取反了
# 80+行这样的代码, 分布在5个函数中
v12（集中swap）： 把swap集中到一个入口，但仍然在控制器里。监控系统看到的是原始数据，与控制器看到的不一致。
# v12: 集中swap, 但监控系统数据不一致
def imu_callback(self, msg):
    # 入口处统一swap
    self._imu_roll = -msg.data[1]  # 交换并取反
    self._imu_pitch = msg.data[0]  # 交换
    # 但/imu/euler话题还是原始数据!
    # 监控系统看到roll=10, 控制器认为roll=0
v13（驱动层统一映射）： 在驱动层做一次映射，所有下游节点消费标准坐标系数据。监控系统和控制器看到的完全一致。
# v13: 驱动层映射, 所有下游数据一致
def _map_to_standard(self, ybimu_roll, ybimu_pitch, ...):
    std_roll = -ybimu_pitch    # 一次映射
    std_pitch = ybimu_roll     # 标准坐标系
    # 发布到/imu/euler的就是标准数据
    # 监控系统和控制器消费的都是同一个数据源

v5到v13的演进路径，本质上是“把方向转换从多个地方集中到一个地方”的过程。v5到v8在控制器里零散处理，v12集中到控制器入口，v13集中到驱动层。每集中一次，代码就简化一次，bug就减少一次。
从代码量来看：v8有80+行方向判断代码，v12有约20行，v13有约15行（只在_map_to_standard中）。代码量从80行降到15行，减少了80%，但功能完全正常。代码越少，出错的可能性越小。

10.5 恒等映射的发现

v13做完之后，我重新审视了所有历史代码中的“方向修正”逻辑。结果是惊人的：v8版本中80多行的方向判断代码，在v13的标准坐标系下，绝大部分都变成了恒等映射——也就是说，它们没有任何实际效果，只是把数据原封不动地传过去了。
什么是恒等映射
恒等映射的意思是：你做了一个变换，但变换的结果和输入一模一样。比如，你把一个数乘以1，或者加上0，或者把一个正数取反两次——结果不变。
在v8中，很多“方向修正”其实就是在做恒等映射。因为在v8中，数据在多个地方被反复swap和取反。为了抵消这些swap和取反的效果，你需要在新加入的模块中再做一次swap和取反。但如果你搞错了次数，就会多一次或少一次。同时，你也不知道前面的swap是否正确——所以你会加一些“防御性”的swap，即使它们可能已经是恒等映射了。
v8的代码中，很多“方向修正”其实是恒等映射——它们只是把数据原封不动地传过去。但因为数据流经过了多次swap，你已经不知道哪个方向是“对”的了，只能再加一层“保险”。结果就是：代码越来越长，但实际效果没变。
清理恒等映射
v13做完之后，我系统性地清理了所有恒等映射。清理的原则是：任何在标准坐标系下不需要额外处理的代码，全部删除。具体来说：
删除所有控制器内的swap。 因为IMU驱动层已经发布了标准坐标系的数据，控制器不需要再做任何swap。
删除所有“if joint_name == …”的方向判断。 这些判断在标准坐标系下都是多余的。每个关节的补偿方向自然正确。
删除所有硬编码的“* -1”翻转。 这些翻转是v8时代的遗留物，在v13的标准坐标系下已经不必要了。
清理完之后，控制器的代码行数减少了约15%。不是删掉了功能，而是删掉了冗余。
恒等映射的发现让我深刻理解了一个道理：复杂的系统往往不是因为它本身复杂，而是因为你一直在错误的地方解决问题。
如果你在控制器里解决IMU轴映射的问题，你会不断引入新的swap和判断，代码会越来越复杂。但如果你在驱动层一次性解决，控制器的代码会变得非常简单——因为所有数据都已经在标准坐标系下了，你只需要做最自然的计算。
四元数转换：euler_to_quaternion
在v13的驱动层改造中，还有一个重要的辅助函数——_euler_to_quaternion()。它负责将标准坐标系下的欧拉角转换为四元数，用于发布ROS2标准的/imu/data话题。
这个函数使用ZYX旋转顺序（Yaw绕Z、Pitch绕Y、Roll绕X），与ROS REP-103标准一致。关键点在于：它接收的是已经映射到标准坐标系的欧拉角，所以转换结果自然也是标准坐标系的。如果映射在控制器层做，那四元数转换就只能在控制器里做——这会导致/imu/data话题中的四元数和/imu/euler话题中的欧拉角不一致。

def _euler_to_quaternion(self, roll_deg, pitch_deg, yaw_deg):
    roll = math.radians(roll_deg)
    pitch = math.radians(pitch_deg)
    yaw = math.radians(yaw_deg)

    cy = math.cos(yaw * 0.5)
    sy = math.sin(yaw * 0.5)
    cp = math.cos(pitch * 0.5)
    sp = math.sin(pitch * 0.5)
    cr = math.cos(roll * 0.5)
    sr = math.sin(roll * 0.5)

    qw = cr * cp * cy + sr * sp * sy
    qx = sr * cp * cy - cr * sp * sy
    qy = cr * sp * cy + sr * cp * sy
    qz = cr * cp * sy - sr * sp * cy

    return (qw, qx, qy, qz)

10.6 避坑清单

避坑清单：

1. 轴映射要在驱动层做，不要在控制器层做。驱动层是数据的唯一入口，在这里做一次映射，所有下游节点都受益。

2. 方向转换只在两个边界处理——IMU数据入口和舵机指令出口。其他所有代码一律不准涉及方向转换。这是v13的核心原则。

3. 不要在控制器里swap IMU数据。如果你发现需要在控制器里swap，说明你的驱动层映射没有做好——回到驱动层去修正，而不是在控制器里打补丁。

4. 双重错误抵消是定时炸弹。两个错误互相抵消，看似“能用”，但随时可能因为修改其中一个错误而崩溃。发现双重错误抵消，必须立即修正两个错误。

5. 验证方法：推机器人看数据。向前推，pitch应为正；向左推，roll应为正。如果数据不对，说明轴映射有问题。这是最直观的验证方法。

6. 监控系统的数据必须和控制器消费的数据一致。如果监控系统显示roll=10°，但控制器只知道roll=0°，调试会非常困难。

7. 清理恒等映射。如果某个“方向修正”在标准坐标系下没有效果，删除它。多余的代码不会让系统更好，只会让系统更难理解。

8. 代码变少了不是坏事。v13删掉了80多行方向判断代码，但功能完全正常。好的代码不是越多越好，而是越少越对。

9. 不要用“if joint_name == ...”来判断方向。这种代码是脆弱的——加一个新关节，忘了加if判断，方向就错了。方向判断应该统一在self.reverse字典中。

10. 如果你的IMU轴映射经历了超过3个版本还没搞定，停下来，重新审视你的架构。你可能是在错误的地方解决问题。

11. 即使映射变成了恒等映射，也不要删掉_map_to_standard()方法。它保留了架构的灵活性——将来换IMU型号或改安装方向时，只需要改这一个方法。

到目前为止，我们已经完成了IMU的选型和轴映射——数据有了，方向对了。但还有一个问题没解决：数据质量。IMU的数据有噪声，有零偏，需要标定和滤波。这就是下一章要讲的内容。
下一章，我会带你完成IMU的标定和数据滤波——怎么标定零位，怎么选择滤波算法，怎么确保数据频率和同步。这三章合在一起，构成了IMU三部曲——选型、映射、滤波。读完这三章，你就能从零开始，在你的机器人上搭建一套可靠的IMU系统。