🤖 姿态平衡控制

import math class AttitudeController: def __init__(self, mass=6.2, Ixx=0.05, Iyy=0.1, Izz=0.1, dt=0.001): self.mass = mass self.Ixx, self.Iyy, self.Izz = Ixx, Iyy, Izz self.dt = dt self.g = 9.81 def pd_controller(self, roll, pitch, roll_ref, pitch_ref, roll_dot, pitch_dot, kp=50, kd=10): tau_roll = kp * (roll_ref - roll) - kd * roll_dot tau_pitch = kp * (pitch_ref - pitch) - kd * pitch_dot return tau_roll, tau_pitch def simulate_balancing(self, initial_roll=0.1, initial_pitch=0.05, kp=50, kd=10, duration=2.0): roll = initial_roll pitch = initial_pitch roll_dot = 0 pitch_dot = 0 trajectory = [] t = 0 while t < duration: tau_r, tau_p = self.pd_controller(roll, pitch, 0, 0, roll_dot, pitch_dot, kp, kd) # Simplified dynamics: I*alpha = tau - m*g*h*sin(angle) h = 0.2 roll_ddot = (tau_r - self.mass * self.g * h * math.sin(roll)) / self.Ixx pitch_ddot = (tau_p - self.mass * self.g * h * math.sin(pitch)) / self.Iyy roll_dot += roll_ddot * self.dt pitch_dot += pitch_ddot * self.dt roll += roll_dot * self.dt pitch += pitch_dot * self.dt trajectory.append((t, roll, pitch, tau_r, tau_p)) t += self.dt return trajectory ctrl = AttitudeController() print("=" * 55) print(" Attitude Balance Control Simulation") print("=" * 55) # PD controller response print("\n [PD Balance Controller - Kp=50, Kd=10]") traj = ctrl.simulate_balancing(initial_roll=0.15, initial_pitch=0.1) for i in range(0, len(traj), max(1, len(traj)//12)): t, r, p, tr, tp = traj[i] print(f" t={t:.3f}s roll={r*180/math.pi:+6.2f}deg pitch={p*180/math.pi:+6.2f}deg " f"tau=({tr:+.2f},{tp:+.2f})Nm") # Different gains comparison print("\n [Gain Comparison - initial roll=10deg]") gains = [(20, 5), (50, 10), (100, 20), (200, 40)] for kp, kd in gains: traj = ctrl.simulate_balancing(initial_roll=10*math.pi/180, initial_pitch=0, kp=kp, kd=kd, duration=1.0) max_roll = max(abs(r) for _, r, _, _, _ in traj) final_roll = traj[-1][1] settling = 0 for t, r, _, _, _ in traj: if abs(r) < 0.001 and settling == 0: settling = t print(f" Kp={kp:3d}, Kd={kd:2d}: max_roll={max_roll*180/math.pi:.2f}deg, " f"final={final_roll*180/math.pi:.4f}deg, settle={settling:.3f}s") # Disturbance rejection print("\n [Disturbance Rejection]") traj = ctrl.simulate_balancing(initial_roll=0, initial_pitch=0, kp=80, kd=15, duration=1.0) # Apply impulse at t=0.3 for i, (t, r, p, tr, tp) in enumerate(traj): if abs(t - 0.3) < 0.001: # Apply impulse pass # Simpler: just show the balancing with initial offset print(" Applying 5deg lateral push at t=0...") traj2 = ctrl.simulate_balancing(initial_roll=5*math.pi/180, initial_pitch=0, kp=80, kd=15, duration=0.5) for i in range(0, len(traj2), max(1, len(traj2)//8)): t, r, p, tr, tp = traj2[i] print(f" t={t:.3f}s roll={r*180/math.pi:+6.2f}deg tau_roll={tr:+.2f}Nm") # Stability margin analysis print("\n [Maximum Recoverable Tilt]") for kp in [30, 50, 80, 120]: max_tilt = 0 for tilt_deg in range(1, 45): tilt = tilt_deg * math.pi / 180 traj = ctrl.simulate_balancing(initial_roll=tilt, initial_pitch=0, kp=kp, kd=kp/5, duration=2.0) final_roll = abs(traj[-1][1]) if final_roll < 0.01: max_tilt = tilt_deg else: break print(f" Kp={kp:3d}: max recoverable tilt = {max_tilt}deg") print() print(" OK - Attitude balance control simulation complete")