基于加速度计与陀螺仪的姿态角Kalman滤波实现问题咨询
2026-7-9
姿态角计算与卡尔曼滤波融合问题
我有三轴加速度计和三轴陀螺仪传感器，需要根据传感器数据动态计算roll/pitch角，同时用陀螺仪数据计算yaw角（后续计划加磁力计修正）。我用卡尔曼滤波做数据融合：通过get_data()读取6组传感器数据，调用delta_gyro_angle(gyroscope, delta_time)计算陀螺仪角度变化，调用compute_roll_pitch(acceleration)计算加速度计姿态角，再通过filter_angles()滤波融合。但当前实现存在4个问题：
- 卡尔曼滤波疑似忽略陀螺仪角度变化
- 加速度计偶发大幅异常值（静态下出现1.5倍跳变）
- pitch角接近90度时计算失效
- ±180度角处理异常
需要解决这些问题，获取运动（大概率非线性运动）状态下的准确姿态角，用于遥控玩具船。
当前代码实现
import numpy as np import time import math def normalize(vector): norm = np.linalg.norm(vector) if norm == 0: return vector return vector / norm def compute_roll_pitch(acceleration): # 归一化加速度 acceleration = normalize(acceleration) # 计算俯仰角(pitch)和横滚角(roll) pitch = math.asin(-acceleration[0]) roll = math.atan2(acceleration[1], acceleration[2]) # 弧度转角度 pitch_degrees = math.degrees(pitch) roll_degrees = math.degrees(roll) return roll_degrees, pitch_degrees class KalmanFilter: def __init__(self, initial_angles, gyro_noise, accel_noise): # 初始化矩阵和滤波参数 self.angle_estimate = np.array(initial_angles) # 初始角度估计值 self.angle_covariance = np.identity(3) # 初始角度协方差 self.gyro_noise = gyro_noise self.accel_noise = accel_noise def update(self, gyro_deltas, accel_angles): # 预测下一滤波状态 predicted_angle = self.angle_estimate + gyro_deltas predicted_covariance = self.angle_covariance + self.gyro_noise # 计算卡尔曼增益 kalman_gain = np.dot(predicted_covariance, np.linalg.inv(predicted_covariance + self.accel_noise)) # 根据加速度计读数更新角度估计 self.angle_estimate = predicted_angle + np.dot(kalman_gain, (accel_angles - predicted_angle)) self.angle_covariance = np.dot((np.identity(3) - kalman_gain), predicted_covariance) return self.angle_estimate.tolist() def filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise): # 创建卡尔曼滤波实例 kalman_filter = KalmanFilter(accel_angles, gyro_noise, accel_noise) # 用陀螺仪和加速度计数据滤波角度 filtered_angles = [accel_angles] for delta in gyro_deltas: filtered_angle = kalman_filter.update(delta, accel_angles) filtered_angles.append(filtered_angle) return filtered_angles[-1] roll, pitch, yaw = 0, 0, 0 time_output = time.time() start_time = time.time() total_roll = [] # 横滚角历史 total_pitch = [] # 俯仰角历史 total_data = [] # 加速度计和陀螺仪数据历史 while True: delta_time = time.time() - start_time # 迭代时间间隔 start_time = time.time() acceleration, gyroscope = get_data() # 获取加速度计和陀螺仪数据 delta_roll, delta_pitch, delta_yaw = delta_gyro_angle(gyroscope, delta_time) # 计算陀螺仪角度变化 roll_accel, pitch_accel = compute_roll_pitch(acceleration) # 计算加速度计姿态角 yaw_accel = yaw # 后续添加磁力计修正 accel_angles = [roll_accel, pitch_accel, yaw_accel] # 加速度计角度(roll, pitch, yaw) gyro_deltas = [delta_roll, delta_pitch, delta_yaw] # 陀螺仪角度变化量(roll delta, pitch delta, yaw delta) gyro_noise = np.identity(3) * 0.00001 # 陀螺仪噪声矩阵 accel_noise = np.identity(3) * 0.001 # 加速度计噪声矩阵 roll, pitch, yaw = filter_angles(accel_angles, gyro_deltas, gyro_noise, accel_noise) if time.time() - time_output > 0.01: time_output = time.time() padding = ' ' * 100 print(f"{yaw}, {pitch}, {roll}{padding}\r", end="") total_roll.append(roll) total_pitch.append(pitch) total_data.append([acceleration, gyroscope])
问题解决方案
1. 卡尔曼滤波忽略陀螺仪角度变化的修复
当前filter_angles函数每次循环都会重新创建KalmanFilter实例，导致滤波状态无法持续累积，相当于每次都用初始值重新计算，完全没利用陀螺仪的积分效果。
修复代码：
把卡尔曼滤波实例移到循环外初始化，避免每次重建：
# 全局初始化卡尔曼滤波实例 gyro_noise = np.identity(3) * 0.00001 accel_noise = np.identity(3) * 0.001 kalman_filter = KalmanFilter([0, 0, 0], gyro_noise, accel_noise) def filter_angles(accel_angles, gyro_deltas): # 直接复用全局滤波实例更新状态 return kalman_filter.update(gyro_deltas, accel_angles)
主循环中修改调用：
roll, pitch, yaw = filter_angles(accel_angles, gyro_deltas)
同时检查delta_gyro_angle函数：确保陀螺仪角速度数据乘以delta_time得到角度变化量，且单位一致（比如角速度是rad/s，乘时间后为rad，后续统一转角度）。
2. 加速度计异常值处理
静态下的异常跳变可以通过滑动窗口滤波或阈值判断过滤：
方法1：滑动窗口均值滤波
维护加速度数据滑动窗口，取均值后再计算角度：
accel_window = [] WINDOW_SIZE = 5 def compute_roll_pitch(acceleration): global accel_window accel_window.append(acceleration) if len(accel_window) > WINDOW_SIZE: accel_window.pop(0) # 计算窗口均值 avg_accel = np.mean(accel_window, axis=0) avg_accel = normalize(avg_accel) pitch = math.asin(-avg_accel[0]) roll = math.atan2(avg_accel[1], avg_accel[2]) pitch_degrees = math.degrees(pitch) roll_degrees = math.degrees(roll) return roll_degrees, pitch_degrees
方法2：阈值过滤
判断加速度模长是否在合理范围（静态下归一化后接近1），偏离过大则丢弃该数据：
def compute_roll_pitch(acceleration): norm = np.linalg.norm(acceleration) # 允许±10%误差 if not 0.9 <= norm <= 1.1: # 返回上一次的角度值，避免异常跳变 return roll, pitch acceleration = acceleration / norm pitch = math.asin(-acceleration[0]) roll = math.atan2(acceleration[1], acceleration[2]) pitch_degrees = math.degrees(pitch) roll_degrees = math.degrees(roll) return roll_degrees, pitch_degrees
3. Pitch角接近90度时计算失效的修复
当pitch接近90度时，加速度计z轴分量趋近于0，atan2(accel[1], accel[2])会因分母接近0出现计算不稳定，这是欧拉角的万向锁缺陷。改用四元数表示姿态可彻底解决该问题：
四元数姿态更新示例
# 四元数转欧拉角（避免万向锁） def quaternion_to_euler(w, x, y, z): roll = math.atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)) pitch = math.asin(2*(w*y - z*x)) yaw = math.atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)) return math.degrees(roll), math.degrees(pitch), math.degrees(yaw) # 用陀螺仪数据更新四元数 def update_quaternion(quat, gyro, delta_time): w, x, y, z = quat gyro_rad = np.radians(gyro) wx, wy, wz = gyro_rad * delta_time / 2 new_w = w - x*wx - y*wy - z*wz new_x = x + w*wx + y*wz - z*wy new_y = y + w*wy + z*wx - x*wz new_z = z + w*wz + x*wy - y*wx return normalize([new_w, new_x, new_y, new_z])
主循环中用四元数跟踪姿态，结合卡尔曼滤波融合加速度计数据校准四元数，彻底规避万向锁问题。
4. ±180度角处理异常的修复
角度在±180度切换时会出现跳变，需要将角度归一化到[-180, 180]范围，同时计算角度差时取最小距离：
角度归一化与差值计算函数
def normalize_angle(angle): # 将角度归一化到[-180, 180] angle = angle % 360 if angle > 180: angle -= 360 return angle def angle_difference(a, b): # 获取两个角度的最小差值（避免跨±180的大误差） diff = a - b return normalize_angle(diff)
在卡尔曼滤波的update函数中修改角度差计算：
# 替换原有的(accel_angles - predicted_angle) angle_diff = np.array([angle_difference(accel_angles[i], predicted_angle[i]) for i in range(3)]) self.angle_estimate = predicted_angle + np.dot(kalman_gain, angle_diff)
同时在计算加速度计角度后，对结果进行归一化处理。
内容的提问来源于stack exchange，提问作者Даниил Немов
相关产品推荐
相关产品推荐
