姿态解算进阶:互补滤波(陀螺仪、加速度计、地磁计数据融合)

互补滤波原理:  

     在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系,动态性能好,受外部干扰小,但测量值随时间变化比较大。可以看出,它们优缺点互补,结合起来才能有好的效果。那么三者的数据如何融合呢,接下来介绍互补滤波算法。

     互补滤波就是在短时间内采用陀螺仪得到的角度做为最优,定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。即短时间内用陀螺仪比较准确,以它为主;长时间用加速度计比较准确,这时候加大它的比重,这就是互补了,不过滤波在哪里?加速度计要滤掉高频信号,陀螺仪要滤掉低频信号,互补滤波器就是根据传感器特性不同,通过不同的滤波器(高通或低通,互补的),然后再相加得到整个频带的信号,例如,加速度计测倾角,其动态响应较慢,在高频时信号不可用,所以可通过低通抑制高频;陀螺响应快,积分后可测倾角,不过由于零漂等,在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合,就将陀螺和加表的优点融合起来,得到在高频和低频都较好的信号,互补滤波需要选择切换的频率点,即高通和低通的频率。

互补滤波原理框图:


加速度计补偿:

    假设n系为地理坐标系,b系为机体坐标系,在地理坐标系中,加速度的输出为:,经过矩阵转换后的值为:。在b系中,加速度的测量值为:,现在

### 使用陀螺仪进行姿态以获得欧拉角 #### 方法概述 为了通过陀螺仪数据设备的姿态,通常采用一阶差分方法来离散化微分方程并求姿态角。然而,在某些情况下会出现分母等于零的现象,这标志着达到了所谓的“奇异性”,即无法利用该方法完成全部三个自由度上的角度测量[^1]。 #### 实现过程 对于基于6轴传感器(加速度计+陀螺仪)的姿态来说,可以先初始化各个方向的角度值为0°;之后每读取一次新的角速度样本就更新当前时刻下的pitch、roll以及yaw参数: - **Roll (横滚)** 和 **Pitch (俯仰)** 的变化率可以直接由三轴陀螺仪测得的数据积分得出; - 而由于地球重力场的存在使得沿Z轴存在固定的向量G=(0, 0,-g),因此可以通过分析X-Y平面上投影长度的变化情况间接推导出Yaw (偏航) 角度改变的信息。 需要注意的是,上述提到的两个角度——俯仰角(Pitch)和翻滚角(Roll)—的具体定义取决于所使用的硬件平台及其坐标系设置方式。例如,在一些应用场合下可能规定前者对应于绕Y轴转动而后者则是关于X轴发生的变动;而在其他场景里两者的关系可能会反过来。所以在编写具体的程序之前一定要确认好这些细节,并据此调整相应的法逻辑[^2]。 ```python import numpy as np def update_angles(gyro_data, dt): """ 更新欧拉角 参数: gyro_data : list or tuple of floats 来自IMU模块中的角速率信息 [wx, wy, wz] dt : float 时间间隔 返回: roll_pitch_yaw : ndarray shape(3,) 新的状态向量包含[roll, pitch, yaw] """ wx, wy, wz = gyro_data # 初始化状态变量 roll = 0.0 pitch = 0.0 yaw = 0.0 # 增量 d_roll = wx * dt d_pitch = wy * dt d_yaw = wz * dt # 应用修正因子处理奇异点问题 if abs(np.cos(pitch)) < 1e-7: # 当cos(pitch)=0时发生奇异性 d_roll = 0 # 更新角度 roll += d_roll pitch += d_pitch yaw += d_yaw return np.array([roll, pitch, yaw]) ```
评论 27
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值