基于NMPC的无人机目标跟踪系统设计与实现

1. 项目概述：基于NMPC的无人机目标跟踪系统

在无人机自主控制领域，目标跟踪一直是个既基础又关键的课题。传统PID控制虽然计算量小，但在处理非线性强耦合系统时表现乏力。我最近用STM32MP157双核处理器搭建的六旋翼平台，实现了基于非线性模型预测控制（NMPC）的目标跟踪方案。实测数据显示，在5m×5m区域内跟踪移动目标时，位置误差比PID方案降低62%，虽然计算负载增加了3倍，但得益于STM32MPC的异构计算架构，系统仍能保持20Hz的稳定控制频率。

这个项目的核心在于三个方面：一是用Casadi符号计算框架构建无人机动力学模型，二是设计兼顾跟踪精度与能耗的多目标优化函数，三是处理各类物理约束的工程化实现。下面我会结合代码实例，详细解析每个环节的技术要点和踩坑经验。

2. 系统建模与NMPC框架搭建

2.1 无人机动力学建模

六旋翼无人机是典型的欠驱动系统，其动力学特性包含以下几个关键部分：

matlab复制% 定义系统状态量（12维）
states = MX.sym('x', 12); 
pos = states(1:3);    % 三维位置 (x,y,z)
vel = states(4:6);    % 三轴速度 
euler = states(7:9);  % 欧拉角 (roll,pitch,yaw)
omega = states(10:12);% 角速度

% 控制输入（4电机PWM）
controls = MX.sym('u',4); 

% 物理参数
k_thrust = 8.548e-6;  % 推力系数 (N/PWM^2)
A_matrix = [ -0.7071*k_thrust*arm_length,  0.7071*k_thrust*arm_length, 0.7071*k_thrust*arm_length, -0.7071*k_thrust*arm_length;
             -0.7071*k_thrust*arm_length, -0.7071*k_thrust*arm_length, 0.7071*k_thrust*arm_length,  0.7071*k_thrust*arm_length;
             k_torque/k_thrust,           -k_torque/k_thrust,           k_torque/k_thrust,           -k_torque/k_thrust ]; % 力矩分配矩阵

旋转矩阵的符号化表示是建模的关键难点。通过Casadi的符号计算，我们可以避免手动推导雅可比矩阵：

matlab复制function R = rotation_matrix(euler)
    phi = euler(1); theta = euler(2); psi = euler(3);
    
    R = [cos(psi)*cos(theta), cos(psi)*sin(phi)*sin(theta)-cos(phi)*sin(psi), sin(phi)*sin(psi)+cos(phi)*cos(psi)*sin(theta);
         cos(theta)*sin(psi), cos(phi)*cos(psi)+sin(phi)*sin(psi)*sin(theta), cos(phi)*sin(psi)*sin(theta)-cos(psi)*sin(phi);
         -sin(theta),         cos(theta)*sin(phi),                              cos(phi)*cos(theta)];
end

实际调试中发现，当俯仰角θ接近±90°时会出现万向节锁死。解决方案是在约束条件中限制俯仰角范围（|θ|<80°），并在代码中加入四元数转换的备用逻辑。

2.2 NMPC问题构建

预测控制的核心是在有限时域内求解优化问题。我们设置预测步长N=10，控制步长dt=0.05s（对应20Hz控制频率）：

matlab复制% 权重矩阵设计
Q = diag([10, 10, 20]);  % 位置误差权重（z轴权重更高）
R = diag([0.1, 0.1, 0.1, 0.1]); % 控制量权重
S = diag([2, 2, 1]);     % 姿态角权重

J = 0;  % 初始化目标函数
for k=1:N
   % 目标位置预测（二阶运动模型）
   target_pos = ref_traj(:,k) + (k-1)*dt*target_vel + 0.5*((k-1)*dt)^2*target_accel;
   
   % 多目标代价函数
   J = J + (pos_pred(:,k)-target_pos)'*Q*(pos_pred(:,k)-target_pos)...  % 跟踪误差
          + controls_pred(:,k)'*R*controls_pred(:,k)...                % 控制能耗
          + (euler_pred(:,k))'*S*euler_pred(:,k)...                    % 姿态稳定
          + 5*exp(-10*(pos_pred(3,k)-0.5))...                          % 高度软约束
          + 3*norm(vel_pred(:,k))^2;                                   % 速度惩罚
end

代价函数设计的几个经验原则：

1. z轴权重通常设为xy轴的1.5-2倍，对抗重力影响
2. 控制量权重过大会导致响应迟缓，建议从0.1开始调试
3. 指数项实现软约束，比硬约束更易收敛

3. 约束处理与求解优化

3.1 物理约束的实现

无人机的物理限制需要转化为数学约束条件：

matlab复制% 构建约束向量
g = [controls; 
     pos_pred(:); 
     euler_pred(:);
     diff(controls_pred,1,2)];  % 控制量变化率

% 上下界设置
lbg = [PWM_min*ones(4,1);       % 电机PWM下限
       -inf(3*N,1);             % 位置下限（仅z轴需设置）
       0.1*ones(N,1);           % 高度下限0.1m
       -pi/3*ones(2*N,1);       % 滚转/俯仰角限制±60度
       -inf(N,1);               % 偏航角无限制
       -0.2*ones(3*(N-1),1)];   % 控制量变化率限制
       
ubg = [PWM_max*ones(4,1);
       inf(2*N,1);              % xy位置无上限
       3.0*ones(N,1);           % 高度上限3m
       pi/3*ones(2*N,1);
       inf(N,1);
       0.2*ones(3*(N-1),1)];

特别注意：控制量变化率约束对实际飞行至关重要。实测发现，不加此约束时电机PWM信号会出现跳变，导致ESC保护性停机。建议设置为最大PWM范围的20%/控制周期。

3.2 求解器配置与加速技巧

使用IPOPT求解器时，线性求解器的选择直接影响计算速度：

matlab复制opts = struct('ipopt', struct(...
    'linear_solver', 'ma57',...  % 默认是mumps
    'max_iter', 50,...
    'tol', 1e-4,...
    'hessian_approximation', 'limited-memory'));
solver = nlpsol('solver','ipopt', nlp_prob, opts);

不同线性求解器的性能对比：

在STM32MP157上部署时遇到的两个典型问题：

1. ma57需要约200MB内存，导致内存溢出 → 改用mumps
2. 默认容差1e-8导致求解时间波动大 → 放宽至1e-4

4. 工程实现与实测优化

4.1 状态估计增强

原始方案中目标突然变速会导致跟踪滞后，改进方案：

matlab复制% 卡尔曼滤波器设计（离散时间）
A_kf = [1 dt 0.5*dt^2;
        0 1  dt;
        0 0  1];  % 三阶运动模型
H_kf = [1 0 0];
Q_kf = diag([0.1, 0.5, 1.0]);  % 过程噪声
R_kf = 0.01;                   % 观测噪声

% NMPC中增加加速度惩罚项
J = J + 0.5*(target_accel'*target_accel)...
      + 2*(vel_pred(:,k)-target_vel)'*(vel_pred(:,k)-target_vel);

实测数据显示，加入运动预测后：

• 匀速目标：跟踪误差降低12%
• 变速目标：误差降低41%
• 最大计算延迟：从15ms增至18ms

4.2 代码生成与部署

将Matlab代码部署到STM32MPC的步骤：

1. 使用Casadi的codegen功能生成C代码
2. 通过STM32CubeIDE创建Arm Cortex-A7项目
3. 关键优化点：
   • 将NLP求解放在A7核，状态估计放在M4核
   • 启用NEON指令集加速矩阵运算
   • 使用CMSIS-DSP库替换标准数学函数

内存占用分析（N=10时）：

5. 常见问题与调试技巧

5.1 求解失败处理流程

当求解器返回非零状态时的排查步骤：

1. 检查约束可行性：用full(lbg)和full(ubg)确认当前状态是否在约束范围内
2. 可视化预测轨迹：绘制pos_pred看是否超出安全区域
3. 降低求解精度：将ipopt.tol从1e-6调至1e-4
4. 简化问题：临时去掉姿态角约束测试

5.2 参数调试经验

关键参数调试顺序建议：

1. 先调位置误差权重Q，确保基本跟踪能力
2. 再调控制量权重R，平衡响应速度与能耗
3. 最后调姿态权重S，优化飞行平稳性

典型参数组合参考：

5.3 实时性保障措施

确保20Hz控制频率的实现方法：

1. 热启动：用上一周期的解作为初始猜测

   matlab复制args.x0 = [x0; reshape(u_prev,4*N,1)]; 
   

2. 提前终止：设置ipopt.max_iter=30限制迭代次数
3. 降级策略：超时后切换为PD控制，直到NMPC恢复

在树莓派4B上的性能对比（N=10）：