一、核心概念对比

1.1 轨迹优化（Trajectory Optimization）

本质：开环最优控制问题，寻找从初始状态到目标状态的最优轨迹。

数学形式：

${\min }_{u(t)}{\int }_{t_0}^{t_f}L(x(t),u(t))dt+\Phi (x(t_f))$

$\text{s.t. }\dot{x}=f(x,u),g(x,u)\le 0,h(x,u)=0$

特点：

• 离线计算：提前规划完整轨迹
• 全局优化：考虑整个时间域的最优性
• 计算密集：可承受较长计算时间
• 无反馈：对扰动敏感

1.2 非线性模型预测控制（NMPC）

本质：闭环反馈控制，在线滚动优化。

数学形式：
${\min }_{u(\cdot )}{\int }_t^{t+T}L(x(\tau ),u(\tau ))d\tau +E(x(t+T))$

$\text{s.t. }\dot{x}=f(x,u),x(t)=x_{\text{meas}},u\in \mathcal{U}$

特点：

• 在线优化：实时计算控制输入
• 滚动时域：有限预测时域
• 反馈机制：基于测量状态更新
• 实时性要求：必须在采样时间内求解

二、技术实现对比

2.1 算法框架对比

特性	轨迹优化	非线性MPC
计算时机	离线，一次性	在线，每个采样周期
优化时域	完整轨迹 ([t_0, t_f])	滚动时域 ([t, t+T])
反馈机制	无（开环）	有（闭环）
计算时间	分钟/小时级	毫秒/秒级
最优性	全局最优	局部最优
鲁棒性	对扰动敏感	对扰动鲁棒
应用场景	任务规划、路径设计	实时控制、轨迹跟踪

2.2 求解方法对比

轨迹优化常用方法：

1. 直接法：将连续问题离散化为非线性规划（NLP）
   • 直接配点法（Direct Collocation）
   • 直接多重打靶法（Direct Multiple Shooting）
2. 间接法：求解庞特里亚金极大值原理
3. 伪谱法：在配点处使用高精度多项式近似

NMPC常用方法：

1. 实时迭代（Real-Time Iteration, RTI）
2. 显式NMPC：离线计算分段仿射控制律
3. 序列二次规划（SQP）在线求解
4. 内点法处理路径约束

三、MATLAB实现框架

3.1 轨迹优化实现（直接配点法）

%% 轨迹优化：无人机最小能量轨迹规划
function [traj_opt, info] = trajectory_optimization_direct_collocation()
    % 问题定义
    prob = struct();
    prob.n_states = 4;        % [x, y, vx, vy]
    prob.n_controls = 2;      % [ax, ay]
    prob.n_nodes = 50;        % 配点数
    prob.tf = 10;             % 终端时间
    
    % 边界条件
    prob.bounds.x0 = [0; 0; 0; 0];
    prob.bounds.xf = [10; 10; 0; 0];
    prob.bounds.u_min = [-2; -2];
    prob.bounds.u_max = [2; 2];
    prob.bounds.v_max = 3;
    
    % 代价函数：最小控制能量
    prob.cost.integrand = @(x,u) 0.1*(u'*u);
    prob.cost.terminal = @(xf) 0;
    
    % 动力学约束
    prob.dynamics = @(x,u) [x(3); x(4); u(1); u(2)];
    
    % 路径约束（避障）
    prob.path_constraints = @(x) obstacle_constraints(x);
    
    % 直接配点法求解
    [traj_opt, info] = solve_direct_collocation(prob);
    
    % 可视化结果
    if info.success
        plot_trajectory_optimization_results(traj_opt, prob);
    end
end

function [traj, info] = solve_direct_collocation(prob)
    % 使用MATLAB优化工具箱求解
    n_states = prob.n_states;
    n_controls = prob.n_controls;
    n_nodes = prob.n_nodes;
    
    % 决策变量：所有节点的状态和控制
    n_vars = n_nodes * (n_states + n_controls);
    x0 = zeros(n_vars, 1);
    
    % 设置优化问题
    options = optimoptions('fmincon', ...
        'Algorithm', 'interior-point', ...
        'Display', 'iter', ...
        'MaxIterations', 1000, ...
        'MaxFunctionEvaluations', 1e6, ...
        'OptimalityTolerance', 1e-6);
    
    % 求解
    [x_opt, fval, exitflag, output] = fmincon(...
        @(x) trajectory_cost(x, prob), ...
        x0, [], [], [], [], ...
        prob.lb, prob.ub, ...
        @(x) trajectory_constraints(x, prob), ...
        options);
    
    % 提取结果
    traj = extract_trajectory(x_opt, prob);
    info = struct('success', exitflag > 0, 'fval', fval, 'output', output);
end

3.2 非线性MPC实现（实时迭代）

%% 非线性MPC：无人机轨迹跟踪控制
function [mpc_controller] = nonlinear_mpc_controller_design()
    % NMPC控制器设计
    
    % 系统参数
    sys = struct();
    sys.dt = 0.1;                 % 采样时间
    sys.nx = 4;                   % 状态维度
    sys.nu = 2;                   % 控制维度
    sys.N = 10;                   % 预测时域
    
    % 权重矩阵
    sys.Q = diag([10, 10, 1, 1]); % 状态误差权重
    sys.R = diag([0.1, 0.1]);     % 控制权重
    sys.P = diag([20, 20, 2, 2]); % 终端权重
    
    % 非线性动力学（无人机模型）
    sys.f = @(x,u) [
        x(3);                     % dx = vx
        x(4);                     % dy = vy
        u(1) - 0.1*x(3);          % dvx = ax - 阻尼
        u(2) - 0.1*x(4)           % dvy = ay - 阻尼
    ];
    
    % 约束
    sys.constraints = struct();
    sys.constraints.x_min = [-inf; -inf; -3; -3];
    sys.constraints.x_max = [inf; inf; 3; 3];
    sys.constraints.u_min = [-2; -2];
    sys.constraints.u_max = [2; 2];
    
    % 构建NMPC控制器
    mpc_controller = struct();
    mpc_controller.sys = sys;
    mpc_controller.solver = setup_nmpc_solver(sys);
    mpc_controller.estimator = setup_state_estimator(sys);
    mpc_controller.reference = @(t) generate_reference_trajectory(t);
    
    % 实时迭代参数
    mpc_controller.rti_params = struct();
    mpc_controller.rti_params.max_iter = 3;      % 最大迭代次数
    mpc_controller.rti_params.warm_start = true; % 热启动
    
    fprintf('NMPC控制器设计完成\n');
end

function [u_opt, info] = nmpc_step(controller, x_current, t)
    % NMPC单步求解
    
    % 获取参考轨迹
    ref_traj = controller.reference(t);
    
    % 构建当前优化问题
    [nlp_problem, x0] = build_nmpc_problem(...
        controller, x_current, ref_traj);
    
    % 实时迭代求解
    if controller.rti_params.warm_start
        % 使用上一时刻解作为初始猜测
        x0 = controller.last_solution;
    end
    
    % 求解NLP（使用fmincon或专用求解器）
    options = optimoptions('fmincon', ...
        'Algorithm', 'sqp', ...
        'MaxIterations', controller.rti_params.max_iter, ...
        'Display', 'off');
    
    [x_opt, fval, exitflag] = fmincon(...
        nlp_problem.cost, x0, ...
        nlp_problem.Aineq, nlp_problem.bineq, ...
        nlp_problem.Aeq, nlp_problem.beq, ...
        nlp_problem.lb, nlp_problem.ub, ...
        nlp_problem.nonlcon, options);
    
    % 提取控制输入
    u_opt = x_opt(1:controller.sys.nu);
    
    % 保存解用于热启动
    controller.last_solution = x_opt;
    
    % 返回信息
    info = struct('fval', fval, 'exitflag', exitflag);
end

%% 主控制循环
function [results] = run_nmpc_simulation(controller, T_sim)
    % 运行NMPC仿真
    
    sys = controller.sys;
    n_steps = ceil(T_sim / sys.dt);
    
    % 初始化
    x = zeros(sys.nx, n_steps+1);
    u = zeros(sys.nu, n_steps);
    t = zeros(1, n_steps+1);
    
    x(:,1) = [0; 0; 0; 0];  % 初始状态
    t(1) = 0;
    
    % 性能记录
    solve_times = zeros(1, n_steps);
    cost_values = zeros(1, n_steps);
    
    % 主循环
    for k = 1:n_steps
        tic;
        
        % NMPC求解
        [u_opt, info] = nmpc_step(controller, x(:,k), t(k));
        
        % 记录求解时间
        solve_times(k) = toc;
        cost_values(k) = info.fval;
        
        % 应用控制输入（带模拟噪声）
        u_noisy = u_opt + 0.01*randn(sys.nu, 1);
        u(:,k) = saturate(u_noisy, sys.constraints.u_min, sys.constraints.u_max);
        
        % 模拟系统动态（使用4阶Runge-Kutta）
        x(:,k+1) = rk4_integration(sys.f, x(:,k), u(:,k), sys.dt);
        
        % 添加过程噪声
        x(:,k+1) = x(:,k+1) + 0.01*randn(sys.nx, 1);
        
        % 更新时间
        t(k+1) = t(k) + sys.dt;
        
        % 显示进度
        if mod(k, 10) == 0
            fprintf('步数 %d/%d, 求解时间: %.3f秒\n', ...
                k, n_steps, solve_times(k));
        end
    end
    
    % 整理结果
    results = struct();
    results.x = x;
    results.u = u;
    results.t = t;
    results.solve_times = solve_times;
    results.cost_values = cost_values;
    results.average_solve_time = mean(solve_times);
    
    fprintf('仿真完成，平均求解时间: %.3f秒\n', results.average_solve_time);
end

四、关键技术挑战与解决方案

4.1 共同挑战

挑战	轨迹优化	非线性MPC	解决方案
非凸性	路径约束导致非凸	动力学非线性导致非凸	凸松弛、序列凸规划（SCP）
计算效率	高维参数空间	实时性要求严格	稀疏性利用、代码生成
数值稳定性	长时间积分误差	在线优化数值问题	自适应网格、正则化
约束处理	复杂状态约束	实时约束满足	障碍函数、软约束

4.2 专用求解器与工具

MATLAB工具箱：

1. 轨迹优化：

   • fmincon：通用非线性规划
   • direct：DIRECT全局优化算法
   • gpoops：高斯伪谱法工具箱

2. 非线性MPC：

   • nlmpc：MATLAB官方NMPC工具箱
   • MPC Toolbox：支持非线性MPC
   • CasADi：符号计算与自动微分（需安装）

第三方工具：

• ACADO：自动控制与动态优化
• ROCKIT：刚体系统轨迹优化
• Drake：MIT机器人工具箱
• FORCES Pro：嵌入式优化求解器

参考代码 轨迹优化和非线性模型预测控制(MPC) www.youwenfan.com/contentcst/160631.html

五、实际应用中的协同策略

5.1 分层控制架构

┌─────────────────┐
│   任务规划层    │ ← 轨迹优化（离线）
└────────┬────────┘
         ↓
┌─────────────────┐
│   轨迹跟踪层    │ ← 非线性MPC（在线）
└────────┬────────┘
         ↓
┌─────────────────┐
│  底层控制器    │ ← PID/反馈线性化
└─────────────────┘

5.2 典型工作流程

1. 离线阶段（轨迹优化）：

   • 根据任务要求生成全局最优参考轨迹
   • 考虑障碍物、动力学约束、能量最优
   • 输出：时间参数化的轨迹 ($x_{\text{ref}}(t),u_{\text{ref}}(t)$)

2. 在线阶段（非线性MPC）：

   • 以参考轨迹为跟踪目标
   • 实时处理模型不确定性、扰动
   • 在线调整轨迹以满足新约束

3. 应急处理：

   • 当NMPC检测到无法跟踪参考轨迹时
   • 触发重新规划（在线轨迹优化）
   • 生成新的可行参考轨迹

5.3 性能评估指标

function [metrics] = evaluate_performance(results, reference)
    % 计算性能指标
    
    metrics = struct();
    
    % 跟踪误差
    tracking_error = results.x(1:2,:) - reference.x(1:2,:);
    metrics.rmse_position = sqrt(mean(sum(tracking_error.^2, 1)));
    metrics.max_position_error = max(sqrt(sum(tracking_error.^2, 1)));
    
    % 控制能量
    control_energy = sum(results.u.^2, 1);
    metrics.total_control_energy = sum(control_energy);
    
    % 实时性
    metrics.max_solve_time = max(results.solve_times);
    metrics.percent_real_time = ...
        sum(results.solve_times < results.sys.dt) / length(results.solve_times) * 100;
    
    % 约束违反
    constraint_violation = compute_constraint_violation(results);
    metrics.max_constraint_violation = max(constraint_violation);
    
    fprintf('性能评估结果:\n');
    fprintf('  位置RMSE: %.3f m\n', metrics.rmse_position);
    fprintf('  最大位置误差: %.3f m\n', metrics.max_position_error);
    fprintf('  控制总能量: %.3f\n', metrics.total_control_energy);
    fprintf('  最大求解时间: %.3f ms\n', metrics.max_solve_time*1000);
    fprintf('  实时性满足率: %.1f%%\n', metrics.percent_real_time);
end

六、前沿研究方向

6.1 学习增强方法

1. 神经网络近似：用深度学习近似NMPC优化
2. 强化学习结合：RL学习价值函数，MPC执行优化
3. 高斯过程学习：学习模型不确定性

6.2 高效求解算法

1. 实时迭代SQP：固定计算复杂度
2. 显式MPC：离线计算控制律查询表
3. 并行计算：GPU加速优化求解

6.3 鲁棒与随机NMPC

1. Tube-based MPC：处理有界扰动
2. 场景MPC：处理随机不确定性
3. 分布ally鲁棒MPC：最坏情况优化

七、实用建议与最佳实践

7.1 选择指南

• 选择轨迹优化当：

  • 需要全局最优解
  • 计算时间充裕（离线）
  • 系统确定性较强
  • 作为参考轨迹生成器

• 选择非线性MPC当：

  • 需要实时反馈控制
  • 系统存在不确定性
  • 需要处理扰动
  • 在线调整能力重要

7.2 实现建议

1. 从简单开始：先实现线性MPC，再扩展到非线性
2. 利用稀疏性：NMPC问题天然稀疏，可大幅加速
3. 代码生成：对嵌入式系统使用代码生成工具
4. 硬件在环测试：在实际硬件上验证实时性

7.3 MATLAB学习资源

1. 官方文档：

   • help nlmpc
   • help fmincon
   • Model Predictive Control Toolbox用户指南

2. 示例代码：

   • MATLAB示例：mpcNonlinearExample
   • File Exchange：搜索"NMPC"、“trajectory optimization”

3. 在线课程：

   • MathWorks培训：Model Predictive Control
   • Coursera：机器人学中的最优控制

总结

轨迹优化和非线性MPC是现代控制系统中两个强大且互补的工具。轨迹优化擅长全局路径规划，为系统提供最优参考；非线性MPC擅长实时反馈控制，确保系统在各种扰动下稳定跟踪。在实际应用中，两者常结合使用：轨迹优化提供"战略规划"，非线性MPC负责"战术执行"。