🤖 惯性测量与姿态

import math, random class IMUSimulator: def __init__(self, dt=0.01): self.dt = dt self.g = 9.81 self.gravity = [0, 0, self.g] # True state self.roll = 0.0 self.pitch = 0.0 self.yaw = 0.0 # Noise parameters self.accel_noise = 0.05 self.gyro_noise = 0.01 self.gyro_bias = [0.001, -0.002, 0.001] def true_rotation_matrix(self): cr, sr = math.cos(self.roll), math.sin(self.roll) cp, sp = math.cos(self.pitch), math.sin(self.pitch) cy, sy = math.cos(self.yaw), math.sin(self.yaw) R = [[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr], [sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr], [-sp, cp*sr, cp*cr]] return R def read_accel(self): R = self.true_rotation_matrix() g_body = [sum(R[i][j]*self.gravity[j] for j in range(3)) for i in range(3)] noisy = [g_body[i] + random.gauss(0, self.accel_noise) for i in range(3)] return noisy def read_gyro(self): omega = [random.gauss(0, self.gyro_noise) + self.gyro_bias[i] for i in range(3)] return omega def complementary_filter(self, accel, gyro, alpha=0.98): # Gyro integration self.roll += gyro[0] * self.dt self.pitch += gyro[1] * self.dt # Accel-based attitude roll_a = math.atan2(accel[1], accel[2]) pitch_a = math.atan2(-accel[0], math.sqrt(accel[1]**2 + accel[2]**2)) # Complementary filter self.roll = alpha * self.roll + (1-alpha) * roll_a self.pitch = alpha * self.pitch + (1-alpha) * pitch_a return self.roll, self.pitch imu = IMUSimulator(dt=0.01) print("=" * 55) print(" IMU and Attitude Estimation Simulation") print("=" * 55) # Simulate tilting the robot print("\n [Tilt Response Test]") true_roll = 0.0 true_pitch = 0.0 for step in range(100): if step < 20: true_roll += 0.5 * 0.01 # slowly tilt imu.roll = true_roll imu.pitch = true_pitch accel = imu.read_accel() gyro = imu.read_gyro() est_roll, est_pitch = imu.complementary_filter(accel, gyro) if step % 20 == 0: print(f" Step {step:3d}: true=({true_roll:.3f},{true_pitch:.3f}) " f"est=({est_roll:.3f},{est_pitch:.3f}) " f"accel=({accel[0]:.2f},{accel[1]:.2f},{accel[2]:.2f})") # Static attitude from accelerometer print("\n [Static Attitude from Accelerometer]") test_tilts = [(0, 0), (0.1, 0), (0, 0.1), (0.2, -0.1), (-0.15, 0.2)] for true_r, true_p in test_tilts: imu.roll = true_r imu.pitch = true_p readings = [imu.read_accel() for _ in range(100)] avg_accel = [sum(r[i] for r in readings)/100 for i in range(3)] roll_est = math.atan2(avg_accel[1], avg_accel[2]) pitch_est = math.atan2(-avg_accel[0], math.sqrt(avg_accel[1]**2 + avg_accel[2]**2)) err_r = abs(roll_est - true_r) err_p = abs(pitch_est - true_p) print(f" True ({true_r:.3f},{true_p:.3f}) -> Est ({roll_est:.3f},{pitch_est:.3f}) " f"err=({err_r*180/math.pi:.2f},{err_p*180/math.pi:.2f}) deg") # Gyro drift analysis print("\n [Gyroscope Drift Analysis]") imu2 = IMUSimulator(dt=0.01) imu2.roll = 0 imu2.pitch = 0 drift_angles = [] for step in range(1000): accel = imu2.read_accel() gyro = imu2.read_gyro() est_r, est_p = imu2.complementary_filter(accel, gyro, alpha=0.98) if step % 200 == 0: print(f" t={step*0.01:.2f}s: est_roll={est_r*180/math.pi:.2f}deg " f"est_pitch={est_p*180/math.pi:.2f}deg") print() print(" OK - IMU simulation complete")