惯性传感器的倾角计算

惯性传感器单元 IMU

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

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

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

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

通过加速度计算倾角

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

单轴数据计算倾角

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

\[α = sin^{-1}(\frac{a_x}{g}) \]

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

\[α ≈ k(\frac{a_x}{g}) \]

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

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

双轴加速度数据计算倾角

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

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

\[α = tan^{-1}(\frac{a_x}{a_z}) \]

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

三轴加速度数据计算倾角

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

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

\[α = sin^{-1}(\frac{a_x}{g}) \]

\[β = sin^{-1}(\frac{a_y}{g}) \]

\[γ = sin^{-1}(\frac{a_z}{g}) \]

带运动加速度的倾角计算

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

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

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

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

\[α = sin^{-1}(\frac{a_x}{G}) \]

\[β = sin^{-1}(\frac{a_y}{G}) \]

\[γ = sin^{-1}(\frac{a_z}{G}) \]

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

\[α = tan^{-1}(\frac{a_x}{\sqrt[2]{{a_y}^2 + {a_z}^2}}) \]

\[β = tan^{-1}(\frac{a_y}{\sqrt[2]{{a_x}^2 + {a_z}^2}}) \]

\[γ = tan^{-1}(\frac{a_z}{\sqrt[2]{{a_x}^2 + {a_y}^2}}) \]

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

互补滤波

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

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

互补滤波的公式为

\[α_n = k * (α_{n-1} + \delta_α ∗ dt) + (1 - k) ∗ α^{\prime} \]

其中

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

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

\[α = \frac{τ}{τ+dt} \]

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

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

\[α = \frac{τ}{τ+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