🚁 第2课：四旋翼动力学

        前(+x)                              
         ①↺    ②↻
          \  /
           \/  机身
           /\
          /  \
         ④↻    ③↺
        后(-x)
  1,3逆时针 | 2,4顺时针

惯性系(NED)：X北 Y东 Z下

机体系：X前 Y右 Z下

欧拉角：φ(roll)绕X | θ(pitch)绕Y | ψ(yaw)绕Z

U₁ = kf(ω₁²+ω₂²+ω₃²+ω₄²)       # 总推力
U₂ = kf·L(-ω₁²+ω₂²+ω₃²-ω₄²)     # 滚转
U₃ = kf·L(-ω₁²-ω₂²+ω₃²+ω₄²)     # 俯仰
U₄ = km(-ω₁²+ω₂²-ω₃²+ω₄²)        # 偏航

矩阵形式: U = M · Ω²

class Quadrotor:
    def __init__(self):
        self.m=1.5; self.g=9.81; self.L=0.25
        self.Ixx=0.0142; self.Iyy=0.0142; self.Izz=0.0284
        self.kf=1e-5; self.km=1.5e-7
    
    def mixing_matrix(self):
        kf,km,L = self.kf,self.km,self.L
        return [[kf,kf,kf,kf],[-kf*L,kf*L,kf*L,-kf*L],
                [-kf*L,-kf*L,kf*L,kf*L],[-km,km,-km,km]]
    
    def dynamics(self, state, omega_sq):
        x,y,z,vx,vy,vz,phi,theta,psi,p,q,r = state
        U = self.mixing_matrix() @ omega_sq
        # 平移 + 旋转动力学...
        return derivatives

✅ 6自由度运动由4个电机控制(欠驱动)

✅ 混控矩阵将虚拟控制量映射到电机转速

✅ 旋转动力学含陀螺耦合项

✅ RK4数值积分实现高精度仿真

# 推力测试台实验
1. 固定电机+桨到测试台
2. 从0到最大油门采样20个点
3. 记录: PWM, RPM, 推力N, 扭矩N·m, 电流A, 电压V
4. 拟合: T = kf*ω², M = km*ω²

# 典型值(2212电机+9045桨):
kf ≈ 1.0e-5 N/(rad/s)²
km ≈ 1.5e-7 N·m/(rad/s)²
km/kf ≈ 0.015

关键公式回顾

本课涉及的核心公式和算法需要反复练习才能真正掌握。建议通过修改仿真参数、添加扰动等方式深入理解每个参数对系统行为的影响。在实际飞行中，这些参数的选择往往需要在理论分析的基础上结合实验微调。

推荐实验

• 修改关键参数，观察系统响应变化
• 添加不同类型的噪声和扰动
• 对比不同算法的精度和计算效率
• 将仿真结果与理论分析对比验证
• 尝试将多个模块组合成完整系统

🔧 工程实践要点

• 实际部署时注意数值稳定性(归一化/限幅/防除零)
• 实时性要求: 控制环1kHz+，导航环50-100Hz
• 传感器数据时间戳对齐至关重要
• 异常处理: 传感器失效需安全降级策略
• 日志记录: 飞行数据全程记录便于事后分析
• 固件升级: 始终使用稳定版本，升级前备份参数

📊 性能指标参考

450mm轴距航拍机设计

总重约1.5kg，悬停时间约25分钟，推重比约2.1:1

空气动力学效应

• 桨叶挥舞：前飞时前行桨与后行桨相对速度不同，产生不对称推力，导致俯仰力矩
• 地面效应：距地面小于1个桨径时推力增大5-15%，需在控制器中补偿
• 下洗气流：多机编队时下洗气流影响下方无人机
• 风场扰动：阵风可视为外加力矩和力的随机扰动

柔性体效应

碳纤维臂在大载荷下可能弯曲5-10mm，改变有效力臂长度。高精度控制需考虑：

# 柔性臂模型(简化)
# L_eff = L_0 - k_bend * (T^2 + tau^2)^0.5
# k_bend: 弯曲刚度系数，典型值0.001-0.005 m/(N^2)
# 高载荷时臂弯曲使控制增益偏移约5-10%

SITL软件在环仿真

# PX4 SITL环境搭建
git clone https://github.com/PX4/PX4-Autopilot.git
cd PX4-Autopilot
make px4_sitl jmavsim  # 启动jMAVSim仿真

# 或使用Gazebo(更逼真)
make px4_sitl gazebo   # 启动Gazebo仿真

# 连接QGroundControl
# QGC自动连接UDP 14550端口

AirSim仿真

# 微软AirSim - 基于Unreal Engine的高保真仿真
# 支持多旋翼/固定翼/汽车
# 特点: 逼真视觉/物理/天气模拟
pip install airsim
# 启动Unreal环境后连接

Python动力学仿真

# 轻量级6DOF仿真(本课程使用)
# 优势: 快速验证算法，无需安装复杂环境
# 缺点: 精度有限，不含气动效应
# 适合: 控制算法开发和快速原型验证

import numpy as np
class QuadSim:
    def __init__(self):
        self.state = np.zeros(12)  # [pos,vel,attitude,rate]
        self.params = self.load_params()
    
    def step(self, motor_cmds, dt=0.005):
        # 1. 电机模型(一阶延迟)
        # 2. 推力/力矩计算
        # 3. 6DOF动力学
        # 4. RK4积分
        return self.state

选择建议：研究开发选Pixhawk(开源+灵活)，竞速穿越选F4/F7飞控(低延迟)，商业产品选DJI(可靠性)。

算法复杂度分析

在实际飞控系统中，算法必须在有限的计算资源下实时运行。以下为本课核心算法的复杂度分析：

实时系统设计原则

无人机飞控是典型的实时嵌入式系统，必须满足严格的时序约束：

• 硬实时：姿态控制环必须在1ms内完成(1kHz)，否则可能失控
• 软实时：EKF更新应在5ms内完成(200Hz)，偶尔超时可接受
• 非实时：路径规划可在后台线程运行，100ms-1s响应均可

# 典型飞控任务优先级(高→低)
# 1. IMU采样+姿态估计 (1kHz, 优先级最高)
# 2. 角速度PID控制     (1kHz)
# 3. 姿态PID控制       (500Hz)
# 4. EKF状态更新       (200Hz)
# 5. 位置/速度控制      (100Hz)
# 6. 路径规划/避障      (10-50Hz)
# 7. 通信/日志          (1-10Hz)

# RTOS调度: 优先级抢占 + 时间片轮转

代码质量与测试

飞控软件是安全关键系统，代码质量要求极高：

• 编码规范：MISRA C/C++，禁止动态内存分配
• 静态分析：Coverity/Cppcheck扫描所有警告
• 单元测试：每个算法模块100%覆盖率
• SIL测试：软件在环仿真验证功能
• HIL测试：硬件在环仿真验证时序
• 飞行测试：从悬停开始逐步扩大飞行包线

# 单元测试示例
def test_pid_output():
    pid = PID(kp=1.0, ki=0, kd=0)
    assert pid.update(1.0, 0.01) == 1.0  # P=1*1.0

def test_pid_integral_limit():
    pid = PID(kp=0, ki=1.0, kd=0, i_limit=5.0)
    for _ in range(1000):
        pid.update(1.0, 0.01)  # 大量积分
    assert abs(pid.integral) <= 5.0  # 不超过限幅

def test_kalman_convergence():
    kf = KalmanFilter(...)
    for _ in range(100):
        kf.predict()
        kf.update(measurement)
    assert kf.P[0,0] < initial_P  # 协方差下降

开源飞控架构对比

安全关键系统设计

无人机系统属于安全关键(Safety-Critical)系统，设计时必须遵循以下原则：

• Fail-safe设计：任何单点故障不应导致灾难性后果
• 冗余设计：关键传感器(IMU/GPS)至少双冗余
• 看门狗：硬件看门狗检测软件挂起，超时复位
• 状态机：明确的飞行状态转换，禁止非法状态
• 地理围栏：硬编码的飞行区域限制
• 最大倾斜角限制：防止翻机失控
• 电池保护：低压自动返航/降落
• 信号丢失保护：遥控信号丢失5s自动RTH