Python实现6-DOF刚体仿真器（下）——环境扰动与控制闭环

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)

python 复制代码

# sixdof/environment.py
import numpy as np

class WindModel:
    def __init__(self, wind_ned=np.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)

python 复制代码

# sixdof/controllers.py
import numpy as np

class PID:
    def __init__(self, kp, ki, kd, limit_output=None, limit_integral=None):
        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, derivative=None):
        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 = None

4.3 集成到仿真器

修改 SixDOFSimulator，加入风场和控制器逻辑。

python 复制代码

# sixdof/simulator.py (修改部分)
from .environment import WindModel
from .controllers import PID

class SixDOFSimulator:
    def __init__(self, aircraft, initial_state, wind_model=None):
        # ... 原有初始化 ...
        self.wind_model = wind_model if wind_model else WindModel()
        
        # Controllers
        self.altitude_hold_pid = PID(kp=0.05, ki=0.01, kd=0.1, limit_output=0.3, limit_integral=5.0)
        self.pitch_attitude_pid = PID(kp=-10.0, ki=0.0, kd=-2.0, limit_output=np.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的北风。

python 复制代码

# 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(mass=10.0, inertia=[0.2, 1.0, 1.0], S_ref=0.5)
    init_state = State(pn=0, pe=0, pd=0, u=25, w=-1, q0=1.0)
    
    # 2. Environment (5m/s North Wind)
    wind_model = WindModel(wind_ned=np.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_final=40.0, dt=0.02)
    print("Simulation finished.")
    
    data = sim.get_history_arrays()
    
    # 5. Visualization
    fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True)
    
    # Altitude Tracking
    axes[0].plot(data['time'], -data['pd'], label='Actual Altitude')
    axes[0].axhline(sim.h_command, color='r', linestyle='--', label='Commanded 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-', label='Pitch 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-', label='Pitch Angle')
    ax2.set_ylabel('Angle (deg)')
    ax2.legend(loc='upper left')
    ax2.grid(True)
    
    # Control Input (Elevator)
    # We need to log control inputs too. (Add to simulator history for full feature)
    # For now, let's 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控制器（高度环+姿态环），使飞机具备了自主维持飞行状态的能力。
验证了鲁棒性：在有风干扰的环境下，飞机依然能够稳定跟踪目标高度，证明了仿真系统的工程实用价值。

当前局限：

目前的飞机模型仍是"质点+转动"的理想模型，没有考虑发动机动力学（推力变化延迟）、舵机动力学（舵偏角速率限制）以及传感器噪声。此外，可视化仍停留在二维图表阶段。