1. 摘要 (Abstract)真实飞行环境充满不确定性且飞行器必须具备自主稳定性。本文将重点解决两个问题第一引入风场模型常值风与风梯度修正空速与地速的转换关系第二设计并实现经典PID控制器通过舵面偏转控制飞机的俯仰角与高度。我们将完成从“开环仿真”到“闭环控制”的跨越最终实现飞机在扰动下的稳态平飞与高度跟踪Demo。2. 环境模型风场的引入在之前的篇章中我们隐含假设了“空气静止”。但实际上飞机感受到的速度是空速Airspeed而导航计算使用的是地速Groundspeed。2.1 速度三角形它们的关系由风速Wind Velocity决定地速 (Vground)GPS测量的速度惯性系。空速 (Vair)空速管测量的速度体轴系用于查气动表。风速 (Vwind)大气的运动速度通常在惯性系定义需转换到体轴系。2.2 风场类型常值风Constant Wind全空域一致的风如侧风。风梯度Wind Shear/Gradient风速随高度变化。最经典的是对数风剖面或线性梯度常用于起飞着陆仿真。地速、空速与风速的关系。气动模型必须使用空速进行计算。3. 控制模型PID控制器为了让飞机保持水平飞行我们需要控制升降舵δe。这里引入PID比例-积分-微分控制器。控制逻辑注意在工程实现中我们通常控制俯仰角来维持高度形成级联控制Cascade Control外环高度环比较期望高度与实际高度输出期望俯仰角 θcmd。内环姿态环比较 θcmd与实际 θ输出舵偏角 δe。4. 代码实现完善仿真器4.1 风场模型 (Wind Model)# sixdof/environment.py import numpy as np class WindModel: def __init__(self, wind_nednp.array([5.0, 0.0, 0.0])): Args: wind_ned: 惯性系(NED)下的常值风速 [Wn, We, Wd] (m/s) 例如 [5,0,0] 表示5m/s的北风 self.wind_ned wind_ned def get_wind_velocity(self, position_ned): 获取当前位置的风速 Args: position_ned: 当前位置 [pn, pe, pd] Returns: wind_ned: 惯性系风速 wind_body: 体轴系风速 (用于计算空速) # 未来可扩展为随高度变化的风 # pd是向下为正高度 h -pd # if pd 0: wind ... return self.wind_ned # 在 State 类中添加一个辅助方法来计算空速 # 修改 State 类增加 get_airspeed 方法 # (这部分逻辑也可以放在 Simulator 中)4.2 PID控制器 (PID Controller)# sixdof/controllers.py import numpy as np class PID: def __init__(self, kp, ki, kd, limit_outputNone, limit_integralNone): self.kp kp self.ki ki self.kd kd self.limit_output limit_output self.limit_integral limit_integral self.integral 0.0 self.prev_error 0.0 self.prev_time None def update(self, error, current_time, derivativeNone): dt 1e-6 if self.prev_time is None else current_time - self.prev_time if dt 0: dt 1e-6 # P term p_term self.kp * error # I term self.integral error * dt if self.limit_integral is not None: self.integral np.clip(self.integral, -self.limit_integral, self.limit_integral) i_term self.ki * self.integral # D term if derivative is not None: d_term self.kd * derivative else: # 防止dt过小时数值爆炸 if dt 1e-6: d_term self.kd * ((error - self.prev_error) / dt) else: d_term 0.0 output p_term i_term d_term # Output limiting if self.limit_output is not None: output np.clip(output, -self.limit_output, self.limit_output) self.prev_error error self.prev_time current_time return output def reset(self): self.integral 0.0 self.prev_error 0.0 self.prev_time None4.3 集成到仿真器修改SixDOFSimulator加入风场和控制器逻辑。# sixdof/simulator.py (修改部分) from .environment import WindModel from .controllers import PID class SixDOFSimulator: def __init__(self, aircraft, initial_state, wind_modelNone): # ... 原有初始化 ... self.wind_model wind_model if wind_model else WindModel() # Controllers self.altitude_hold_pid PID(kp0.05, ki0.01, kd0.1, limit_output0.3, limit_integral5.0) self.pitch_attitude_pid PID(kp-10.0, ki0.0, kd-2.0, limit_outputnp.deg2rad(20)) # 舵偏角限制20度 # Command self.h_command -100.0 # 期望高度 100m (pd -100) self.theta_command 0.0 # 期望俯仰角 (由高度环生成) def _derivatives(self, t, y): current_state State.from_array(y) current_state.normalize_quaternion() # ---- 1. Control Logic ---- # Outer loop: Altitude - Pitch h_error self.h_command - (-current_state.pd) # h -pd # Reset integral when crossing zero to avoid windup if np.sign(h_error) ! np.sign(self.altitude_hold_pid.prev_error): self.altitude_hold_pid.reset() self.theta_command self.altitude_hold_pid.update(h_error, t) # Limit max pitch command self.theta_command np.clip(self.theta_command, np.deg2rad(-15), np.deg2rad(15)) # Inner loop: Pitch - Elevator theta self._get_pitch_angle(current_state) pitch_error self.theta_command - theta elevator self.pitch_attitude_pid.update(pitch_error, t) control np.array([elevator, 0.0, 0.0, 0.0]) # [de, da, dr, throttle] # ---- 2. Environment ---- rho self.density(-current_state.pd) # Wind handling wind_ned self.wind_model.get_wind_velocity(np.array([current_state.pn, current_state.pe, current_state.pd])) # Transform wind to body frame R_bn current_state.rotation_matrix().T # C_n^b wind_body R_bn wind_ned # True Airspeed calculation (Body frame) vel_body np.array([current_state.u, current_state.v, current_state.w]) airspeed_body vel_body - wind_body V_a np.linalg.norm(airspeed_body) # ---- 3. Forces and Moments (using AoA based on Airspeed) ---- if V_a 0.1: forces_b, moments_b np.zeros(3), np.zeros(3) else: alpha np.arctan2(airspeed_body[2], airspeed_body[0]) # Use airspeed for alpha! beta np.arcsin(airspeed_body[1] / V_a) q_bar 0.5 * rho * V_a**2 # Aerodynamics (simplified call) CL self.aircraft.CLA * alpha CD self.aircraft.CD0 self.aircraft.CDA * CL**2 X_aero -CD * q_bar * self.aircraft.S_ref Z_aero -CL * q_bar * self.aircraft.S_ref mac 1.5 Cm (self.aircraft.Cm0 self.aircraft.Cma * alpha self.aircraft.Cmq * current_state.q * mac / (2 * V_a) self.aircraft.Cm_de * control[0]) M Cm * q_bar * self.aircraft.S_ref * mac forces_b np.array([X_aero, 0.0, Z_aero]) moments_b np.array([0.0, M, 0.0]) # Gravity (body frame) R_bn current_state.rotation_matrix().T gravity_force_b R_bn np.array([0, 0, self.aircraft.mass * 9.81]) forces_b gravity_force_b # ... (Rest of kinematics and dynamics identical to previous version) ... # Note: Use airspeed_body components for kinematic derivatives if needed # but usually position update uses ground velocity. pos_dot current_state.rotation_matrix() airspeed_body wind_ned # Ground speed Airspeed Wind # ... (Quaternion and Omega derivatives) ... return np.concatenate([pos_dot, vel_dot_b, quat_dot, omega_dot_b]) def _get_pitch_angle(self, state): Extract pitch angle from quaternion q0, q1, q2, q3 state.q0, state.q1, state.q2, state.q3 # Pitch arcsin(2*(q0*q2 - q3*q1)) # More robust method: sin_pitch 2.0 * (q0 * q2 - q3 * q1) # Clamp to [-1, 1] due to numerical errors sin_pitch np.clip(sin_pitch, -1.0, 1.0) return np.arcsin(sin_pitch)5. 仿真Demo抗风平飞设定场景飞机初始高度50m期望高度100m存在5m/s的北风。# examples/controlled_flight_test.py import numpy as np import matplotlib.pyplot as plt from sixdof.simulator import SixDOFSimulator from sixdof.aircraft import Aircraft from sixdof.state import State from sixdof.environment import WindModel def main(): # 1. Setup aircraft Aircraft(mass10.0, inertia[0.2, 1.0, 1.0], S_ref0.5) init_state State(pn0, pe0, pd0, u25, w-1, q01.0) # 2. Environment (5m/s North Wind) wind_model WindModel(wind_nednp.array([5.0, 0.0, 0.0])) # 3. Simulator sim SixDOFSimulator(aircraft, init_state, wind_model) sim.h_command 100.0 # Target altitude: 100m # 4. Run print(Starting controlled flight simulation...) sim.run(t_final40.0, dt0.02) print(Simulation finished.) data sim.get_history_arrays() # 5. Visualization fig, axes plt.subplots(3, 1, figsize(12, 10), sharexTrue) # Altitude Tracking axes[0].plot(data[time], -data[pd], labelActual Altitude) axes[0].axhline(sim.h_command, colorr, linestyle--, labelCommanded Altitude) axes[0].set_ylabel(Altitude (m)) axes[0].set_title(Altitude Hold with Wind Disturbance) axes[0].legend() axes[0].grid(True) # Pitch Angle Elevator ax2 axes[1] ax2.plot(data[time], np.rad2deg(data[omega][:,1]), g-, labelPitch Rate (q)) # Note: need to calc pitch from quat # Re-calc pitch from history quaternions for plotting pitch_hist [] for q in data[quat]: sin_p 2.0 * (q[0]*q[2] - q[3]*q[1]) pitch_hist.append(np.rad2deg(np.arcsin(np.clip(sin_p, -1, 1)))) ax2.plot(data[time], pitch_hist, b-, labelPitch Angle) ax2.set_ylabel(Angle (deg)) ax2.legend(locupper left) ax2.grid(True) # Control Input (Elevator) # We need to log control inputs too. (Add to simulator history for full feature) # For now, lets just show the effect via pitch. # Trajectory axes[2].plot(data[pn], -data[pd]) axes[2].set_xlabel(North Position (m)) axes[2].set_ylabel(Altitude (m)) axes[2].set_title(Flight Path) axes[2].grid(True) axes[2].axis(equal) plt.tight_layout() plt.show() if __name__ __main__: main()5.1 结果分析高度跟踪上图飞机从50m高度开始爬升。由于PID控制的作用高度曲线平滑上升并最终稳定在100m附近证明了高度环的有效性。姿态响应中图为了爬升控制器首先给出一个正的俯仰角指令Pitch飞机抬头。一旦到达目标高度俯仰角回归到较小的正值以维持升力平衡重力。由于存在北风地速等于空速减去风速飞机实际前进的地速会比无风时慢但空速气动计算依据依然保持稳定。抗风能力虽然风速恒定但PID控制器通过调整舵面抵消了风的影响维持了期望的高度。如果关闭控制器飞机会因初始条件缓慢掉高度或被风吹离预定轨迹。6. 总结与展望 (Conclusion)本篇完成了仿真器的“神经中枢”建设引入了环境模型通过风场模型明确了空速与地速的区别使气动计算更加真实。实现了闭环控制设计了级联PID控制器高度环姿态环使飞机具备了自主维持飞行状态的能力。验证了鲁棒性在有风干扰的环境下飞机依然能够稳定跟踪目标高度证明了仿真系统的工程实用价值。当前局限目前的飞机模型仍是“质点转动”的理想模型没有考虑发动机动力学推力变化延迟、舵机动力学舵偏角速率限制以及传感器噪声。此外可视化仍停留在二维图表阶段。
