惯性传感器单元 IMU

IMU 是 Inertial Measurement Unit 的缩写, 直接翻译过来就是惯性测量单元, 常见的有单独的三轴加速度(Accelerometer)计 ADXL345, L3G4200D, L3GD20等, 单独的三轴角速度计(又称陀螺仪, Gyroscope) LIS3DH, L3GD20H, BMG160, 以及包含了加速度计和陀螺仪的六轴运动传感器 MPU6050, MPU6500, MPU6881, BMI160等, 以及带电子罗盘的九轴运动传感器 MPU9250, MPU9255等.

在判断物体在空间中的姿态以及运动轨迹时, 用得最多的是加速度和角速度传感器. 加速度传感器可以计算倾角, 陀螺仪可以计算角速度, 这两种传感器各自的特点为

• 陀螺仪: 动态特性好, 因为测量噪声(误差)的存在, 以及各向灵敏度的差异, 通过积分计算角度会累积误差, 导致结果越来越不准
• 加速度计: 不会累积误差, 所以准确度有保证, 但是动态响应差, 不适用于角度变化快速的场景.

如果设备运动速度较慢, 作用于系统的加速度力主要是重力, 可以使用加速度计来计算倾角, 利用重力矢量及其在加速度计轴上的投影(即对应读数)来确定倾角. 由于重力是恒定加速度, 如果存在额外的恒定加速度也会影响计算结果. 额外的恒定加速度包括发动机持续加速以及设备自身匀速的旋转等.

通过加速度计算倾角

静止和慢速物体因为主要加速度来源为重力, 用加速度传感器可以计算得到物体的倾角.

单轴数据计算倾角

假设X轴上测到的加速度值为 $a_x$ 定义倾角 $\alpha$ 为X轴与水平面(与重力矢量垂直的平面)的夹角, 计算为

$$\alpha =si{n}^{-1}(\frac{a_x}{g})$$

当 $a_x=0$ 时, 倾角为0, X轴处于水平位置, 当$a_x=g$ 时, 倾角90°, 处于垂直位置, 当 $a_x$ 的值很小时, 可以用近似公式 $sin(\alpha )\approx \alpha$, 于是

$$\alpha \approx k(\frac{a_x}{g})$$

比例系数k用于角度的线性近似计算

$a_x$ 的读数匹配 $sin(\alpha )$ 曲线, 读数值范围为 $-1g1g$, 在读数为0(水平位置)时灵敏度最高, 在读数为 $+-1g$ 时灵敏度最低.

双轴加速度数据计算倾角

单轴无法判断方向, 因为在倾角为 $\alpha$ 和 $180°-\alpha$ 时读数是一样的. 如果增加一个与X轴垂直的轴, 假定为Z轴, 且XZ轴形成的平面垂直于水平面, 那么XZ轴在这个平面里形成的对水平面的倾角就可以判断方向, 并且结果较为精确, 因为总会有一个轴处于灵敏度较高的区间.

当XZ轴形成的平面垂直于水平面时, X轴倾角可以用两个轴的读数进行计算.

$$\alpha =ta{n}^{-1}(\frac{a_x}{a_z})$$

$a_x$为0时, 倾角为0, X轴处于水平, 当$a_z$为0时要注意避免零除. 如果XZ轴平面不垂直于水平面, 这个结果会小于实际的倾角, 倾斜越厉害误差越大.

三轴加速度数据计算倾角

假设传感器Z轴垂直朝下, X轴朝正前方, 则X轴与水平面之间的夹角为俯仰角(pitch) $\alpha$, Y轴与水平面间的夹角为横滚角(roll) $\beta$, 航向角yaw需要地磁传感器, 无法通过加速度传感器计算. Z轴垂直于X轴和Y轴, 和两轴数值是相关的, 并没有独立性, 仅用于判断设备上下的朝向. 令Z轴与水平面的夹角为 $\gamma$

重力加速度在XYZ三个轴上的投影即为三个轴传感器的读数, 可以将三轴倾角和重力加速度想像为一个斜立的长方体, 长方体的对角线为重力加速度, 对角线就是这个角对应的三条边, 倾角的计算方法为

$$\alpha =si{n}^{-1}(\frac{a_x}{g})$$
$$\beta =si{n}^{-1}(\frac{a_y}{g})$$
$$\gamma =si{n}^{-1}(\frac{a_z}{g})$$

带运动加速度的倾角计算

上面的计算方式适合相对静止和慢速的场景, 当物体受作用于多个外力时, 作用于传感器的综合加速度为重力与各外力的叠加, 此时加速度的方向就不是重力的方向, 上面的计算方式就不适用了, 因为综合加速度可能比 $g$ 更大或更小.

对于一个物体, 整体加速度等于三轴加速度的矢量和, 其大小 $G$ 可以通过三个向量的大小计算得到

$$G=\sqrt[2]{{a_x}^2+{a_y}^2+{a_z}^2}$$

由此可以得到运动状态下倾角的计算

$$\alpha =si{n}^{-1}(\frac{a_x}{G})$$
$$\beta =si{n}^{-1}(\frac{a_y}{G})$$
$$\gamma =si{n}^{-1}(\frac{a_z}{G})$$

因为 $a_x$, $G$, $\sqrt[2]{{a_y}^2+{a_z}^2}$ 三个矢量形成直角三角形, 上面的式子可以也可以用 $ta{n}^{-1}$ 计算

$$\alpha =ta{n}^{-1}(\frac{a_x}{\sqrt[2]{{a_y}^2+{a_z}^2}})$$
$$\beta =ta{n}^{-1}(\frac{a_y}{\sqrt[2]{{a_x}^2+{a_z}^2}})$$
$$\gamma =ta{n}^{-1}(\frac{a_z}{\sqrt[2]{{a_x}^2+{a_y}^2}})$$

此时的倾角并非相对重力加速度的倾角, 而是相对物体整体加速度矢量的倾角, 例如物体向前(X轴方向)加速运动时, 整体加速度方向会向后倾斜, 当物体左转时, 离心力会导致整体加速度方向向右倾斜. 计算此时的姿态倾角, 可以用于帮助物体在当前的运动状态上保持平衡.

互补滤波

通过加速度传感器(Accelerometer)可以使用反三角函数$si{n}^{-1}$和$ta{n}^{-1}$求静止和慢速运动物体的倾角, 对于高速运动的物体, 需要结合陀螺仪的角速度读数快速响应倾角变化. 对于这两种传感器读数的结合, 通常采用互补滤波算法.

互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值, 定时用加速度值来校正陀螺仪的得到的角度. 加速度计要滤掉高频信号, 陀螺仪要滤掉低频信号, 互补滤波器就是根据传感器特性不同, 通过不同的滤波器, 相加得到整个频带的信号.

互补滤波的公式为

$${\alpha }_n=k\ast ({\alpha }_{n-1}+{\delta }_{\alpha }\ast dt)+(1-k)\ast {\alpha }^{\prime }$$

其中

• ${\alpha }_n$ 互补计算得到的角度
• ${\alpha }_{n-1}$ 前一次计算得到的角度
• ${\delta }_{\alpha }$ 陀螺仪得到的角速度
• $dt$ 两次计算的时间间隔
• ${\alpha }^{\prime }$ 通过加速度计得到的倾角
• $k$ 和 $1-k$ 为加权系数, 和为 1

加权系数的确定. 在 《The Balance Filter》 中提到关于加权系数的求解公式, 先设滤波器的加权系数为 $\alpha$, 时间常数为为 $\tau$, 运行周期为 $dt$, 那么公式为

$$\alpha =\frac{\tau }{\tau +dt}$$

运行周期 dt 根据运行周期确定, 如果互补滤波器方法的调用频率为 200次每秒, 那么 $dt=\frac{1000ms}{200}=5ms$

时间常数 $\tau$ 的取值根据系统的实际需求调整,
不同的系统的 $\tau$ 值不一定相同. $\tau$取值越大则陀螺仪权重越大, $\tau$取值越小则加速度传感器的权重越大. 通常互补滤波器对陀螺仪的权重会大些, 以降低加速度传感器中噪声的影响. 例如互补滤波器运行间隔为 10ms, 时间常数 $\tau =0.49$, 那么此时加权系数为：

$$\alpha =\frac{\tau }{\tau +dt}=\frac{0.49}{0.49+0.01}=0.98$$

C语言代码

// a = tau / (tau + dt)  
// acc = 加速度传感器数据 
// gyro = 陀螺仪数据 
// dt = 运行周期float angle;
float a;float ComplementaryFliter(float acc, float gyro, float dt)
{a = 0.98;angle = a * (angle + gyro * dt) + (1 - a) * (acc);return angle;
}

在实际应用中, 因为加速度计和角速度计读取的数据存在很大的噪音, 直接使用会造成反馈的不稳定(抖动), 需要在计算前通过卡尔曼滤波器等进行平滑.

参考

• 倾角计算 https://www.analog.com/cn/resources/app-notes/an-1057.html