跳转至

IMU 与朝向保持 — 九轴传感器、欧拉角与 PID 回正

这一章讲什么: 编码器回答「走了多远」,回答不了「头朝哪」——尤其轮子打滑时, 编码器连「有没有转」都在撒谎。补位的是 IMU(惯性测量单元): 本章装 BNO055,搞懂它内部陀螺仪/加速度计/磁力计三家 sensor 怎么互补, 把融合出的朝向(yaw)接进 PID,做成「face direction」行为: 机器人被掰走,自动转回你指定的方向。校准数据存进 config store,重启不再重校。

1. 先看现象:为什么又要一个传感器

第 12 章的里程计有一个致命盲区,书里在第 13 章开篇直说: 编码器被轮子打滑骗、只给相对运动、误差快速累积成弯曲的假路径; 要知道绝对朝向,就得有个「罗盘」——但室内磁场干扰又会骗罗盘4。

两个说法各有漏洞,答案当然是:把好几个有漏洞的传感器合在一起,互相补台。 这就是 IMU 的哲学,也是本章的行文主线:每装一个传感器,先讲它「不能」干什么。

2. 顶层全景:四步闭环

 ① 装      BNO055 上架:3×20 mm 铜柱垫高(远离电机磁场),独占软件 I2C 总线 2
 ② 读      校准(sys=3)后发布 sensors/imu/euler(yaw/pitch/roll)+ 状态
 ③ 存      校准值进 config store——重启自动灌回,免再校准
 ④ 控      face_direction:yaw 误差 → PID → 两轮一正一反 → 转回目标朝向

 图说:①② 给系统一个「罗盘」,④ 让罗盘数据变成动作;③ 是体验的复利。

3. 核心原理之一:IMU 内部——三个传感器,三条盲区

IMU把多种传感器合在一起,给出机器人在三维空间中的绝对朝向; 现代 IMU 用 MEMS(微机电系统)——把带活动部件的机械结构刻进微米级芯片, 血统来自飞机上的大陀螺仪5。

关键知识是三件套各自的盲区,照原书接住6:

传感器 量什么 盲区
陀螺仪(gyroscope) 旋转的速率 不能测绝对角度;单独用会随时间漂移
加速度计(accelerometer) 加速度(含重力)→ 知道哪边是「上」 测不出 yaw(绕竖直轴的转)
磁力计(magnetometer) 磁场 → 可当罗盘 噪声大、怕倾斜、怕磁干扰

融合的分工:陀螺仪抓快速转动、加速度计定上下轴、磁力计给绝对朝向(受磁干扰制约); 模块上常再加一个温度传感器,补偿温度对其他传感器的影响6。 「degrees-of-freedom(DOF,自由度)」是这类模块的规格词——9 轴 = 三 sensor × 三轴7。

为什么选 BNO055:分立的加速度计+陀螺+磁力计也能拼,但线多复杂; BNO055 是单芯片方案,融合算法做在芯片里,机器人这边只读结果—— 第 02 章那句「选它省大量编程复杂度」在此兑现7。

4. 核心原理之二:安装与总线——磁干扰和时钟拉伸两个坑

安装的三条纪律:

  1. 轴向对齐 ENU 约定:X 朝车头、Z 朝天、Y 朝左——和 pose 的坐标一致, 读数才有意义(Tip:ENU 里 0 弧度 = 东,逆时针增大)8。
  2. 垫高:6 根 20 mm 铜柱三层叠起,把 IMU 架离电机和编码器的磁场 (第 02 章布局图里那句「IMU 离电机远点」的落实)8。
  3. 独占一条总线:BNO055 和 VL53L5CX 距离传感器争总线带宽; 更麻烦的是 BNO055 在 I²C 上玩 clock stretching(时钟拉伸), 会干扰同总线的其他传感器——Pi 的硬件 I²C 对付不了它。 解法:用软件 I²C(i2c-gpio overlay)单独开一条总线 2, 走 GPIO 23(SDA)、24(SCL),重启生效9。

验证照旧 i2cdetect -y 2:总线 2 上应只有一个设备,地址 28(别和总线 1 的 17/29 混)10。软件侧装 Adafruit 三件套:Blinka(让 CircuitPython 库跑在 CPython)、 extended-bus(用我们额外的总线)、adafruit-circuitpython-bno05511。

5. 核心原理之三:欧拉角与校准——朝向的三联表示

IMU 输出的是欧拉角(Euler angles):绕三个轴的转角—— yaw(偏航,绕竖直轴)是机器人最关心的「头朝哪」; pitch(俯仰)、roll(横滚)对这台平地机器人基本是常数12。

BNO055 上机后需要校准:四个分量(sys/gyro/acc/mag)各有一个 0–3 的状态码, sys = 3 = 全校准。校准靠芯片收集运动数据——转一转机器人、平移几下, 直到状态爬满。这件事烦到值得自动化,于是有了本章的第二个亮点:

校准持久化:BNO055 允许读出/写入校准值(各传感器的 offsets 和 radii)。 流程:状态变到 3 的那一刻,把校准字典存进 config store (imu/calibration 键,JSON 字符串);开机时 config store 回发, 芯片还没校准就先灌回旧值13。两个实现细节见功底:

  • 读写校准必须先把芯片切到 CONFIG 模式,读完务必在 finally 里切回 NDOF 模式(9 自由度融合模式)——状态不能留在半成品13;
  • 主循环里用手动的 client.loop(0.05) 而不是后台线程—— 避免 MQTT 回调和芯片操作并发(同时发生),这是给所有「单线程硬件对象」的通用教训13。

加速度计的可视化用的是 babylon.js 做 3D:一个箭头向量(一串既有方向又有大小的数),长度贴着 9.8 m/s² (重力),姿态随手转机器人而变——「传感器融合」第一次变成看得见的画面14。

6. 核心原理之四:face direction——yaw 误差进 PID

行为设计照原书的信息流图接住15:

 目标 yaw(期望朝向)                实际 yaw(IMU)
        │                              │
        └────── 相减:error = 目标 − 实际 ◀────┘
                       │
                       ▼
              PID(带 dt)输出控制量
                       │
        一轮 = +控制量,另一轮 = −控制量 → 原地转向
                       │
              新 yaw 回来,误差变小,直到 ≈ 0
 图说:与第 11 章轮控同构,只是被控量从「轮速」换成「朝向」。

代码里的三个真问题:

① 角度环绕(wrap-around)。 yaw 在 2π 和 0 之间跳变——目标 0.1 rad、 实际 6.2 rad 的机器人,朴素误差是 −6.1(绕远路),其实只差 0.18(绕近路)。 解法:把误差夹到 ±π 区间(大于 π 减 2π,小于 −π 加 2π)——机器人永远抄近道转向16。

② 增益标度。 PID 初值 (0.3, 0.2) 比第 11 章大两三个数量级—— 因为误差单位是弧度(0–6.28),输出要撑满 ±1 的电机量程16。

③ 校准门控。 行为不急着收 yaw 数据,而是先订 sensors/imu/status; sys 到 3 才订阅 euler 开转——用未校准的罗盘开车,等于蒙眼开17。 退出时 atexit 连 IMU 服务一起停(它是这个行为拉起来的)。

7. 核心原理之五:smart_reset——给积分加刹车

上机症状:把机器人按住不让转几秒再松开——积分项攒着旧账, 松手瞬间猛蹿,冲过目标。这就是第 11 章警告过的积分饱和的另一种形态 (误差被「人为持续」喂大)18。

书中解法是给 PID 加一个 smart_reset 开关:

def control(self, error, time_difference=1):
    if self.smart_reset and np.sign(error) != np.sign(self.integral):
        self.reset()          # 误差与积分异号 → 清积分
    self.integral += error * time_difference
    ...

机制:误差和积分符号相反 = 系统在「还债」——过去的累积正在把输出往反方向推, 这时旧积分没有信息量、只有惯性,清掉它。效果书里描述得很形象: 「按住时你能听到电机凭积分越拧越用力;松手会略冲过头,但很快回来。」18 这是抗积分饱和(anti-windup)的手作版——教科书方法的一碟小菜。

8. 作者的判断与证据

  • 判断: 多传感器融合的哲学贯穿全章:陀螺漂移、加速度计盲区 yaw、磁力计怕扰—— 「每个传感器先说它不能干什么」是选型和融合的第一课6。
  • 判断: 软件 I²C 隔离 BNO055 是典型的「按器件脾气布线」,不是折腾: clock stretching 干扰其他传感器是芯片行为,绕开比驯服便宜9。
  • 事实层: 总线 2 地址 28、CONFIG/NDOF 模式切换、校准四状态码、 增益 0.3/0.2、±π 夹角——都可对照 BNO055 数据手册复核101316。
  • 遗留诚实: face_direction 的 PID 和 target_yaw 还是写死在代码里, 挑战题第一条就是「把它们接进 config store」19。

9. 边界与局限

  • 室内磁场干扰仍是 yaw 的暗伤。 校准缓解,不根除;第 17 章用「与编码器融合」 再补一刀——单传感器的账,终究要融合来还。
  • 俯仰/横滚基本没用上。 平地机器人只用 yaw;真正的 3D 姿态估计超出本书。
  • NDOF 融合是黑箱。 芯片内部怎么融合三家数据,本书只消费结果; 想深入得读数据手册和 Paul McWhorter 的视频(Further reading)19。
  • face direction 只管朝向不管位置。 「朝东开 1 米」要它和第 11 章的 里程行为拼接——挑战题里出了这道组合题19。

10. 可带走的

  1. IMU 三件套互补:陀螺快但漂、加速度计定「上」不管 yaw、磁力计给绝对朝向怕磁扰6。
  2. 选模块看集成度:单芯片带融合(BNO055)省的是主机代码7。
  3. 磁敏感器件远离电机;clock stretching 器件独占软件 I²C 总线89。
  4. 校准是资产:读出来、存 config store、开机灌回——体验的复利13。
  5. 单线程硬件对象:手动 client.loop() 防并发访问13。
  6. 角度运算先处理环绕:误差夹 ±π,永远抄近道16。
  7. 增益跟着单位走:弧度误差配 0.3/0.2,轮速误差配 0.001——换被控量必须重标度。
  8. 按住测试积分饱和;smart_reset(误差与积分异号即清零)是手作抗饱和18。

补遗:校准流程与朝向可视化的细节

校准的「舞蹈」。 BNO055 到校准 3 需要把三轴的传感数据各喂一遍: 让机器人在每个轴上都转一转、平移几下(说明书里叫 calibration dance)。 本章真正修好的痛点是这件事只做一次:状态爬到 3 的瞬间, 芯片内部的校准参数(offsets 与 radii)被读出、打包成 JSON 存进 config store; 下次开机芯片还没校准,旧参数先灌回去,几秒钟内恢复满校准状态1。 两个实现细节别处复用得上:读写校准期间芯片必须停在 CONFIG 模式, 而 finally 块保证不管读写是否出错都切回 NDOF 融合模式; MQTT 改用手动 client.loop(0.05) 步进,让芯片操作绝不被消息回调打断。

朝向的三联显示。 数据上 MQTT 之后,网页端的可视化分两层: 先是 babylon.js 的 3D 箭头——加速度向量(模长贴着 9.8,方向随手转机器人而变化), 直观感受「传感器融合」是什么;随后是专门一页显示欧拉角(yaw/pitch/roll 三个数), 排错口诀就一句:0 是东不是北——ENU 约定里 0 弧度朝东,逆时针增大2。

face direction 的收尾调参。 正文给了初值 P 0.3、I 0.2(弧度误差专用大增益)。 上机后的两个典型症状:目标附近来回晃 = P 太大;被按住再松开冲过头、回得慢 = I 攒了旧账。 后者用 smart_reset 治(误差与积分异号即清零)——按住时你能听见电机凭积分越拧越用力, 松手略冲、很快回正,这就是积分在还债的声音3。

11. 原文地图

主题 原书位置
三传感器盲区 text/114-fm-technical-requirements.txt:41(搜「gyroscope, which measures」) · text/114-fm-technical-requirements.txt:44(搜「cannot detect yaw」)
MEMS 与单芯片选型 text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:191(搜「MEMS」) · text/114-fm-technical-requirements.txt:61(搜「single chip」)
安装:轴向与垫高 text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:44(搜「X axis should face forward」)
软件 I2C 隔离 text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:99(搜「clock stretching」) · text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:103(搜「i2c-gpio」)
总线 2 验址 28 text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:130(搜「address 28」)
MQTT 发布与 3D 可视化 text/116-fm-reading-the-euler-vector-with-mqtt.txt:31(搜「sensors/imu/accelerometer」)
face direction 设计 text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:29(搜「expected yaw」) · text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:194(搜「yaw_error」)
角度环绕 text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:209(搜「wraps around between 2 pi and 0」)
smart_reset text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:334(搜「smart_reset」)
章节总结 text/118-fm-summary.txt:31(搜「yaw, pitch, and roll」)

  1. 出处:「Driving a robot from IMU data and PID」第 60 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:60,搜「store_calibration」)。原文:状态变 3 时读校准参数存 config;开机 config/get 灌回;CONFIG/NDOF 模式切换;手动 client.loop。 ↩

  2. 出处:「Troubleshooting」(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:5,搜「0 is East」)。原文排错:记住 0 是东不是北;确认已校准;查 journalctl 与 MQTT 里的欧拉值。 ↩

  3. 出处:「Driving a robot from IMU data and PID」第 334 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:334,搜「smart_reset」)。原文:误差与积分异号即 reset;按住时听电机凭积分越拧越用力,松手略冲后快回。 ↩

  4. 出处:「13」章首(text/113-ch13.txt:5,搜「wheels slipping」)。原文:编码器会被轮子打滑骗、只给相对运动、误差快速不对称累积成假弯曲路径;要绝对朝向需罗盘,而室内磁干扰影响航向精度——这正是 IMU 要解决的问题。(注:该段出自第 13 章章首简介。) ↩

  5. 出处:「Technical requirements」第 31 段(text/114-fm-technical-requirements.txt:31,搜「MEMS」)。原文:IMU 结合传感器给出三维绝对朝向;源于航空大陀螺仪;用 MEMS(微机电系统)——微米级芯片上有活动部件;原始信号可能是模拟量,模块常带 ADC,经 I²C/SPI 通信。 ↩

  6. 出处:「Technical requirements」第 41 段(text/114-fm-technical-requirements.txt:41,搜「gyroscope, which measures」)与第 38 段(搜「cannot detect yaw」)。原文:陀螺测旋转速率,不能测绝对旋转、单用会漂移;加速度计测加速度(含重力)指示「上」,测不出 yaw;磁力计测磁场可当罗盘,噪声大、怕倾斜;另有温度传感器补偿;分工:陀螺抓快转、加速度计定上下、磁力计给绝对朝向(受磁干扰制约)。 ↩↩↩↩

  7. 出处:「Technical requirements」第 61 段(text/114-fm-technical-requirements.txt:61,搜「single chip」)。原文:分立三件可拼但线多复杂;BNO055 单芯片 + 少量外围;DOF(自由度)表示传感器轴数;融合在芯片内完成。 ↩↩↩

  8. 出处:「Connecting an IMU to a Raspberry Pi robot」第 44 段(text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:44,搜「X axis should face forward」)。原文:X 朝前、Z 朝上、Y 朝左,对齐第 12 章 pose 的 ENU 约定;Tip:ENU 中 0 弧度 = 东、逆时针增;3×20 mm 铜柱把 IMU 垫离电机/编码器磁场。 ↩↩↩

  9. 出处:「Connecting an IMU to a Raspberry Pi robot」第 99 段(text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:99,搜「clock stretching」)。原文:BNO 与 VL53L5CX 争总线带宽;BNO055 的 I²C 时钟拉伸干扰其他传感器、需软件 i2c 支持——故隔离到自定义 I²C 总线 2;dtoverlay=i2c-gpio,bus=2,i2c_gpio_sda=23,i2c_gpio_scl=24;pyinfra 下发后重启。 ↩↩↩

  10. 出处:「Connecting an IMU to a Raspberry Pi robot」第 130 段(text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:130,搜「address 28」)。原文:sudo i2cdetect -y 2 应见唯一设备在 28;见到多个说明查错了总线;没有则断电查 SDA/SCL。 ↩↩

  11. 出处:「Connecting an IMU to a Raspberry Pi robot」第 144 段(text/115-fm-connecting-an-imu-to-a-raspberry-pi-robot.txt:144,搜「Adafruit-Blinka」)。原文:Blinka 是 CircuitPython 库的 CPython 兼容层;extended-bus 让它能用额外的 I²C 总线;再装 adafruit-circuitpython-bno055。 ↩

  12. 出处:「Reading the Euler vector with MQTT」(text/116-fm-reading-the-euler-vector-with-mqtt.txt:13,搜「Euler」)。原文:芯片给出 yaw(偏航)、pitch(俯仰)、roll(横滚)三个欧拉角;本章行为主要用 yaw。 ↩

  13. 出处:「Driving a robot from IMU data and PID」第 60 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:60,搜「store_calibration」)。原文:校准 = 各传感器 offsets + radii;读前切 CONFIG 模式、finally 里切回 NDOF;整体 JSON 存 config 键 imu/calibration;只在状态「变为」3 时存;收到回发且已校准则跳过(不响应自己);手动 client.loop(0.05) 防并发;断电重启后自动灌回,无需重校。 ↩↩↩↩↩↩

  14. 出处:「Reading the Euler vector with MQTT」第 93 段(text/116-fm-reading-the-euler-vector-with-mqtt.txt:93,搜「babylon」)。原文:babylon.js 3D 场景(FreeCamera + HemisphericLight);加速度向量箭头,模长贴着 9.8 m/s²(重力);随手转机器人姿态跟着变。 ↩

  15. 出处:「Driving a robot from IMU data and PID」第 29 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:29,搜「expected yaw」)。原文:期望 yaw − 实际 yaw = 误差;dt 来自 time stepper;PID 输出一轮正一轮负实现转向;调对了就转到误差近零。 ↩

  16. 出处:「Driving a robot from IMU data and PID」第 194 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:194,搜「yaw_error」)与第 170 段(搜「wraps around between 2 pi and 0」)。原文:PID(0.3, 0.2)——弧度误差(0–2π)配大增益;误差 > π 减 2π、< −π 加 2π,夹到 ±π,免得绕远路。 ↩↩↩↩

  17. 出处:「Driving a robot from IMU data and PID」第 227 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:227,搜「on_status_update」)。原文:订阅 status,sys==3 才订阅 euler 接 on_imu_data;start 里用 launcher 拉起 imu_service,atexit 注册 stop(停车 + 停 IMU 服务)。 ↩

  18. 出处:「Driving a robot from IMU data and PID」第 334 段(text/117-fm-driving-a-robot-from-imu-data-and-pid.txt:334,搜「smart_reset」)。原文:误差与积分异号时 reset()——过零瞬间清掉旧累积;按住机器人能听见电机凭积分越拧越用力,松手略冲后快回。 ↩↩↩

  19. 出处:「Challenges」第 7 段(text/119-fm-challenges.txt:7,搜「configurable from the page」)。原文挑战:把 target yaw 和 PID 参数接进页面可配;组合朝向保持与避障(遇障绕开、障碍清除后回到原方向)。Further reading:BNO055 数据手册、CircuitPython bno055 文档、babylon.js 教程、Paul McWhorter 的 IMU 数学视频。 ↩↩↩