1. 四旋翼控制系统的核心挑战
四旋翼飞行器的轨迹跟踪控制本质上是一个多变量、强耦合的非线性控制问题。飞行器在空中需要同时控制六个自由度(x/y/z位置和滚转/俯仰/偏航姿态),而控制输入只有四个电机的转速。这种欠驱动特性使得控制算法设计极具挑战性。
关键难点:姿态动力学与位置控制之间存在强耦合。例如当需要向前飞行时,必须先产生俯仰角,这个过程中又会引起高度变化。这种耦合关系在代码中体现在
vx_dot和vy_dot的计算公式里。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 动力学模型深度解析
2.1 坐标系定义
- 机体坐标系(Body Frame):原点在重心,X轴向前,Y轴向左,Z轴向上
- 惯性坐标系(Inertial Frame):固定地面参考系,通常采用东北天(ENU)约定
2.2 姿态表示
欧拉角(φ,θ,ψ)存在万向节锁问题,实际工程中常配合四元数使用。代码中的姿态动力学方程:
python复制phi_dot = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta) # 滚转角速度
theta_dot = q*cos(phi) - r*sin(phi) # 俯仰角速度
psi_dot = q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta) # 偏航角速度
当θ接近±90°时会出现奇异点,这是欧拉角的固有缺陷。
2.3 电机推力分配
四个电机的总推力F和力矩τ计算:
code复制F = k_f*(ω1² + ω2² + ω3² + ω4²)
τ_φ = L*k_f*(-ω2² + ω4²) # 滚转力矩
τ_θ = L*k_f*(-ω1² + ω3²) # 俯仰力矩
τ_ψ = k_m*(ω1² - ω2² + ω3² - ω4²) # 偏航力矩
其中L是电机到重心的距离,k_f和k_m分别是推力系数和力矩系数。
3. PID控制器实现细节
3.1 串级控制结构
典型的三层控制架构:
- 外环:位置PID控制器(输出期望姿态角)
- 中环:姿态PID控制器(输出角速度)
- 内环:角速度PID控制器(输出电机指令)
3.2 参数整定方法
- 先调内环再调外环
- 先设Ki=Kd=0
