四旋翼MPC控制：轨迹跟踪与多目标优化实践

1. 四旋翼MPC控制问题定义与挑战

四旋翼飞行器在5米高度保持0航向角的同时，以0.1 rad/s角速度跟踪圆形轨迹，这个看似简单的任务实际上包含了三个相互耦合的控制目标：

1. 高度控制：需要维持精确的5米飞行高度，误差需控制在±0.1米以内
2. 航向锁定：在轨迹跟踪过程中保持yaw角为0，避免出现方向偏差
3. 轨迹跟踪：以恒定角速度沿半径5米的圆形路径运动，位置误差需小于0.3米

1.1 系统动力学建模要点

在简化模型中有几个关键假设需要特别注意：

python复制# 状态变量定义 [x,y,z, vx,vy,vz, roll,pitch,yaw]
state = [0, 0, 5, 0, 0, 0, 0, 0, 0]  

# 控制输入定义 [roll_cmd, pitch_cmd, thrust_cmd]
control = [0, 0, 9.81]  # 初始悬停状态

注意：这个模型忽略了空气阻力、陀螺效应和电机动力学等实际因素，适用于初步算法验证。实际部署时需要引入更完整的动力学模型。

1.2 MPC控制的核心优势

相比传统PID控制，MPC在轨迹跟踪中展现出三大独特优势：

1. 前瞻性控制：通过预测未来N步的系统行为提前调整控制量
2. 约束处理：可显式地处理控制量幅值、变化率等约束条件
3. 多目标优化：同时优化位置、姿态、能耗等多个目标函数

2. MPC控制器详细实现

2.1 预测模型构建

采用离散化状态空间模型，采样时间dt=0.1s：

python复制def discrete_model(state, control, dt=0.1):
    k1 = state_space_model(state, control)
    k2 = state_space_model(state + 0.5*dt*k1, control)
    k3 = state_space_model(state + 0.5*dt*k2, control) 
    k4 = state_space_model(state + dt*k3, control)
    return state + (dt/6)*(k1 + 2*k2 + 2