惯性传感器的倾角计算
惯性传感器单元 IMU
IMU 是 Inertial Measurement Unit 的缩写, 直接翻译过来就是惯性测量单元, 常见的有单独的三轴加速度(Accelerometer)计 ADXL345, L3G4200D, L3GD20等, 单独的三轴角速度计(又称陀螺仪, Gyroscope) LIS3DH, L3GD20H, BMG160, 以及包含了加速度计和陀螺仪的六轴运动传感器 MPU6050, MPU6500, MPU6881, BMI160等, 以及带电子罗盘的九轴运动传感器 MPU9250, MPU9255等.
在判断物体在空间中的姿态以及运动轨迹时, 用得最多的是加速度和角速度传感器. 加速度传感器可以计算倾角, 陀螺仪可以计算角速度, 这两种传感器各自的特点为
- 陀螺仪: 动态特性好, 因为测量噪声(误差)的存在, 以及各向灵敏度的差异, 通过积分计算角度会累积误差, 导致结果越来越不准
- 加速度计: 不会累积误差, 所以准确度有保证, 但是动态响应差, 不适用于角度变化快速的场景.
如果设备运动速度较慢, 作用于系统的加速度力主要是重力, 可以使用加速度计来计算倾角, 利用重力矢量及其在加速度计轴上的投影(即对应读数)来确定倾角. 由于重力是恒定加速度, 如果存在额外的恒定加速度也会影响计算结果. 额外的恒定加速度包括发动机持续加速以及设备自身匀速的旋转等.
通过加速度计算倾角
静止和慢速物体因为主要加速度来源为重力, 用加速度传感器可以计算得到物体的倾角.
单轴数据计算倾角
假设X轴上测到的加速度值为 ax 定义倾角 α 为X轴与水平面(与重力矢量垂直的平面)的夹角, 计算为
当 ax=0 时, 倾角为0, X轴处于水平位置, 当ax=g 时, 倾角90°, 处于垂直位置, 当 ax 的值很小时, 可以用近似公式 sin(α)≈α, 于是
比例系数k用于角度的线性近似计算
ax 的读数匹配 sin(α) 曲线, 读数值范围为 −1g 1g, 在读数为0(水平位置)时灵敏度最高, 在读数为 +−1g 时灵敏度最低.
双轴加速度数据计算倾角
单轴无法判断方向, 因为在倾角为 α 和 180°−α 时读数是一样的. 如果增加一个与X轴垂直的轴, 假定为Z轴, 且XZ轴形成的平面垂直于水平面, 那么XZ轴在这个平面里形成的对水平面的倾角就可以判断方向, 并且结果较为精确, 因为总会有一个轴处于灵敏度较高的区间.
当XZ轴形成的平面垂直于水平面时, X轴倾角可以用两个轴的读数进行计算.
ax为0时, 倾角为0, X轴处于水平, 当az为0时要注意避免零除. 如果XZ轴平面不垂直于水平面, 这个结果会小于实际的倾角, 倾斜越厉害误差越大.
三轴加速度数据计算倾角
假设传感器Z轴垂直朝下, X轴朝正前方, 则X轴与水平面之间的夹角为俯仰角(pitch) α, Y轴与水平面间的夹角为横滚角(roll) β, 航向角yaw需要地磁传感器, 无法通过加速度传感器计算. Z轴垂直于X轴和Y轴, 和两轴数值是相关的, 并没有独立性, 仅用于判断设备上下的朝向. 令Z轴与水平面的夹角为 γ
重力加速度在XYZ三个轴上的投影即为三个轴传感器的读数, 可以将三轴倾角和重力加速度想像为一个斜立的长方体, 长方体的对角线为重力加速度, 对角线就是这个角对应的三条边, 倾角的计算方法为
带运动加速度的倾角计算
上面的计算方式适合相对静止和慢速的场景, 当物体受作用于多个外力时, 作用于传感器的综合加速度为重力与各外力的叠加, 此时加速度的方向就不是重力的方向, 上面的计算方式就不适用了, 因为综合加速度可能比 g 更大或更小.
对于一个物体, 整体加速度等于三轴加速度的矢量和, 其大小 G 可以通过三个向量的大小计算得到
由此可以得到运动状态下倾角的计算
因为 ax, G, 2√ay2+az2 三个矢量形成直角三角形, 上面的式子可以也可以用 tan−1 计算
此时的倾角并非相对重力加速度的倾角, 而是相对物体整体加速度矢量的倾角, 例如物体向前(X轴方向)加速运动时, 整体加速度方向会向后倾斜, 当物体左转时, 离心力会导致整体加速度方向向右倾斜. 计算此时的姿态倾角, 可以用于帮助物体在当前的运动状态上保持平衡.
互补滤波
通过加速度传感器(Accelerometer)可以使用反三角函数sin−1和tan−1求静止和慢速运动物体的倾角, 对于高速运动的物体, 需要结合陀螺仪的角速度读数快速响应倾角变化. 对于这两种传感器读数的结合, 通常采用互补滤波算法.
互补滤波就是在短时间内采用陀螺仪得到的角度做为最优值, 定时用加速度值来校正陀螺仪的得到的角度. 加速度计要滤掉高频信号, 陀螺仪要滤掉低频信号, 互补滤波器就是根据传感器特性不同, 通过不同的滤波器, 相加得到整个频带的信号.
互补滤波的公式为
其中
- αn 互补计算得到的角度
- αn−1 前一次计算得到的角度
- δα 陀螺仪得到的角速度
- dt 两次计算的时间间隔
- α′ 通过加速度计得到的倾角
- k 和 1−k 为加权系数, 和为 1
加权系数的确定. 在 《The Balance Filter》 中提到关于加权系数的求解公式, 先设滤波器的加权系数为 α, 时间常数为为 τ, 运行周期为 dt, 那么公式为
运行周期 dt 根据运行周期确定, 如果互补滤波器方法的调用频率为 200次每秒, 那么 dt=1000ms200=5ms
时间常数 τ 的取值根据系统的实际需求调整,
不同的系统的 τ 值不一定相同. τ取值越大则陀螺仪权重越大, τ取值越小则加速度传感器的权重越大. 通常互补滤波器对陀螺仪的权重会大些, 以降低加速度传感器中噪声的影响. 例如互补滤波器运行间隔为 10ms, 时间常数 τ=0.49, 那么此时加权系数为:
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; }
在实际应用中, 因为加速度计和角速度计读取的数据存在很大的噪音, 直接使用会造成反馈的不稳定(抖动), 需要在计算前通过卡尔曼滤波器等进行平滑.
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 一个费力不讨好的项目,让我损失了近一半的绩效!
· 清华大学推出第四讲使用 DeepSeek + DeepResearch 让科研像聊天一样简单!
· 实操Deepseek接入个人知识库
· 易语言 —— 开山篇
· 【全网最全教程】使用最强DeepSeekR1+联网的火山引擎,没有生成长度限制,DeepSeek本体
2016-03-03 PL/SQL Developer 11 64bit 安装和配置