Loading [MathJax]/jax/output/CommonHTML/fonts/TeX/fontdata.js

惯性传感器的倾角计算

惯性传感器单元 IMU

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

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

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

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

通过加速度计算倾角

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

单轴数据计算倾角

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

α=sin1(axg)

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

αk(axg)

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

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

双轴加速度数据计算倾角

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

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

α=tan1(axaz)

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

三轴加速度数据计算倾角

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

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

α=sin1(axg)

β=sin1(ayg)

γ=sin1(azg)

带运动加速度的倾角计算

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

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

G=2ax2+ay2+az2

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

α=sin1(axG)

β=sin1(ayG)

γ=sin1(azG)

因为 ax, G, 2ay2+az2 三个矢量形成直角三角形, 上面的式子可以也可以用 tan1 计算

α=tan1(ax2ay2+az2)

β=tan1(ay2ax2+az2)

γ=tan1(az2ax2+ay2)

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

互补滤波

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

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

互补滤波的公式为

αn=k(αn1+δαdt)+(1k)α

其中

  • αn 互补计算得到的角度
  • αn1 前一次计算得到的角度
  • δα 陀螺仪得到的角速度
  • dt 两次计算的时间间隔
  • α 通过加速度计得到的倾角
  • k1k 为加权系数, 和为 1

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

α=ττ+dt

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

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

α=ττ+dt=0.490.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;
}

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

参考

posted on   Milton  阅读(1785)  评论(0编辑  收藏  举报

相关博文:
阅读排行:
· 一个费力不讨好的项目,让我损失了近一半的绩效!
· 清华大学推出第四讲使用 DeepSeek + DeepResearch 让科研像聊天一样简单!
· 实操Deepseek接入个人知识库
· 易语言 —— 开山篇
· 【全网最全教程】使用最强DeepSeekR1+联网的火山引擎,没有生成长度限制,DeepSeek本体
历史上的今天:
2016-03-03 PL/SQL Developer 11 64bit 安装和配置

导航

< 2025年2月 >
26 27 28 29 30 31 1
2 3 4 5 6 7 8
9 10 11 12 13 14 15
16 17 18 19 20 21 22
23 24 25 26 27 28 1
2 3 4 5 6 7 8
点击右上角即可分享
微信分享提示