我玩的尽管是玩具级别的“智能车”,但要在单片机里写下一段得到小车的俯仰角的代码也并不容易。——除非复制粘贴,那轻松多了。

本文的姿态指的是三维空间上的方向姿态. 姿态解算即通过常用的陀螺仪乃至加速度计、磁强计,算出仪器在三维空间中的姿态. 要得到姿态, 那么就必须对手头传感器数据的物理意义有一个初步的认识.

传感器芯片

常见的有 InvenSense MPU-6000 / MPU-6050 (Datasheet), 6表示六轴 (三个加速度, 三个角速度). InvenSense 公司的后续换代产品 (六轴选型链接) 编号换成 ICM-206xx 开头了.

这类的传感器通常称之为惯性测量单元, IMU (Inertial Measurement Unit), 这得名于其基本原理. 这种芯片级的产品又称之为 MEMS IMU (Microelectromechanical Systems, 微机电系统), 人们通过光刻和沉积等工艺在单晶硅表面制造出微小的 (如今普遍是微米尺度) 硅晶体或是金属材质的弹簧、振子、齿轮等一系列机械结构和与之连结的梳状电容器等, 并依靠观察电容等参数的改变, 测定机械结构之间的距离, 从而完成机械到电信号的转换. 推荐搜索一下 MEMS 相关的图片直观感受一下.

它的代表是加速度计, 而陀螺仪其实是多个加速度计的复合产品. 它们通过测量外壳相对于中心的一个被弹性悬挂的重物的位移, 从而得到加速度等信息. "九轴" IMU 或者具体来说外加的三轴 磁强计实际上不是传统的惯性测量单元也不是一个核心部分, 磁强计是用来确定绝对方位姿态的辅助传感器 (比如说地磁北).

常常令人困惑的是陀螺仪的名字, 尽管大多数人都知道角动量守恒即: 无外力作用下陀螺不改变旋转轴和旋转速度. 有人可能会想: 那既然陀螺会被"固定"在某个姿态上, 那我们测一下它的姿态不就推断出外壳的姿态 (角度) 了吗? 实际上, 你几乎找不到真正利用角动量守恒原理制作的陀螺仪. 航空航天领域使用的激光陀螺仪利用光行程改变引起的相位相干来测定角速度 (此时惯性难道是指光的"惯性"? 滑稽) ; 实际使用的芯片尺寸的 IMU 里, 陀螺仪则是利用对称分布的多个加速度计结构来实现的角速度计.

尽管机械原理方面, 加速度计是最基础的, 但用于三维姿态解算的主要器件是陀螺仪. 它得到的是载具坐标系下三个坐标轴旋转的角速度, 我们要做的是通过这个角速度算出姿态角.

总体路线

在流程上看起来非常简单, 就像回到了高中.

用这么正经的科学文献英文句点排版和下面看起来很宏伟的积分号,大言不惭说回到高中感觉有点不太好

利用陀螺仪, 高速反复采集角速度, 如此不断更新当前角度姿态即可:

  1. 转过的角度 = 角速度 × 间隔时间, $\Delta \theta = \omega \Delta t,$
  2. 当前角度 = 对转过的角度求和, $\theta = \sum{\omega \Delta t}.$

写成微积分形式:
$$
\theta = \int{\omega \mathrm dt}.
$$

这么一来相对于初始位置的姿态就已经拿到手了. 然后再加一点华彩:

  1. 假设物体只感受到一个重力加速度, 即物体匀速或静止或运动加速度远小于重力加速度, 那么我们可以用三个轴上的加速度 (即重力加速度在三个轴上的分解) 反推出重力加速度的指向, 从而独立得到一部分的绝对姿态 (因为地平面上的东南西北依然分不清).
  2. 如果你有九轴的传感器 (3轴加速度, 3轴角速度, 3轴磁强) 东南西北绝对姿态就拜托磁强计了. 假设只有地磁或干扰磁强远小于地磁.

如上得到的不靠谱绝对姿态角同一直在计算的陀螺仪的靠谱相对姿态角相互融合融合, 比如取一下平均, 搞个卡尔曼滤波之类的 (开始胡说) 就可以付诸应用了.

顺带一提: 跑得贼快还一冲一冲的, 怎么可能没有运动加速度. 旁边放一个大电机, 怎么可能没有强磁干扰. 其他的传感器测出运动加速度可以很大程度上消去这个误差. 至于磁强的问题举高一点离强磁体远一点, 或者干脆不用. 你看六轴不也照样卖的这么好.

下节预告

三维空间里, 怎么表示角度?

角度加角度哪有那么容易就算出来. 数学教你旋转不是用加的, 是用乘的.