固定翼 NMPC 复杂仿真实验复现
本文给出一个基于 Stastny, Dash & Siegwart (2017) 固定翼 NMPC 思路的 MATLAB 复现实验。该版本不是作者原始 ACADO/RTI 代码，而是为了便于理解与复现，使用纯 MATLAB fminsearch、RK4 数值积分和 control blocking 构造的教学型实现。
实验包含：
- 多段路径：直线 → 半圆 → 直线 → 半圆；
- prediction horizon 内的路径段切换；
- 时变风场与强阵风；
- 风速估计误差；
- 空速缓慢变化；
- controller model 与 plant model mismatch；
- bank-rate 外部扰动；
- bank-angle reference 幅值约束；
- bank-angle reference slew-rate limit；
- N=40N=40N=40 与 N=80N=80N=80 prediction horizon 对比；
- previous-horizon control deviation penalty；
- prediction horizon 可视化；
- Times New Roman 大字号、加粗曲线的论文/博客风格绘图。
1. MATLAB 完整程序
matlab
function fixed_wing_nmpc_complex_visual()
clc;
close all;
% -------------------------------------------------------------------------
% 全局可视化排布部分
% -------------------------------------------------------------------------
set(groot,'defaultAxesFontName','Times New Roman'); % 修改坐标轴字体
% groot 表示根图形对象，代表整个 graphics system
set(groot,'defaultTextFontName','Times New Roman'); % 修改文字框字体
try
set(groot,'defaultLegendFontName','Times New Roman'); % 修改标签字体
catch
end
set(groot,'defaultAxesFontSize',15); % 修改坐标轴字号为15pt
set(groot,'defaultTextFontSize',15); % 修改文字框字号为15pt
try
set(groot,'defaultLegendFontSize',14); % 修改标签字号为15pt
catch
end
rng(7); % 设置随机数种子
%% ========================================================================
% 参数设定部分
% ========================================================================
cfg.g = 9.81; % 重力常数
% 标称控制模型
cfg.model.V0 = 14.0; % 模型空速
cfg.model.b0 = 13.48; % b0, a1, a0 3个都是滚转通道二阶动力学模型的参数
cfg.model.a1 = 6.577;
cfg.model.a0 = 13.97;
% 这里表示实际被控对象的"真实"参数（与标称控制模型有不同）
cfg.plant.b0 = 12.70;
cfg.plant.a1 = 6.20;
cfg.plant.a0 = 13.30;
% 仿真时间相关参数
cfg.dtPred = 0.10; % 离散时间步长参数
cfg.dtCtrl = 0.15; % 控制器更新周期
cfg.Tsim = 36.0; % 整个仿真的总时长
% 要测试的 NMPC 预测步数
cfg.Nlist = [40, 80];
% NMPC 的控制分块数量
cfg.Mlist = [7, 10];
% "滚转角参考指令限制
cfg.muMax = 30*pi/180; % 滚转参考角的最大幅值
cfg.muRateMax = 90*pi/180; % 实际施加的滚转加速度
% NMPC 目标函数中的权重
cfg.Q = [0.012, 1.6, 0.12, 0.018, 65.0];
cfg.R = 7.0;
% NMPC 终端代价权重
cfg.P = [0.15, 12.0, 0.0, 0.03, 0.0];
% 优化器配置结构体
cfg.opt = optimset( ...
'Display', 'off', ...
'MaxIter', 32, ...
'MaxFunEvals', 320, ...
'TolX', 2e-3, ...
'TolFun', 3e-2);
% 飞机需要跟踪的参考航迹
cfg.path = buildRacetrackPath();
% Initial state:
% Start west/south of the first straight with nonzero initial bank error.
cfg.x0 = [ ...
-8.0; ... % 初始北向位置
-92.0; ...% 初始东向位
4*pi/180; ... % 初始滚转角
88*pi/180; ... % 初始航向角
0.0]; % 初始滚转角速度
% 风速模拟参数
cfg.windFilter = 0.18; % 风速估计的低通滤波增益
cfg.windNoiseStd = 0.25; % 风速测量噪声的标准差
% 空速估计的低通滤波增益
cfg.VFilter = 0.15;
% 滚转扰动注入参数
cfg.disturbanceTime = 19.0; % 滚转扰动注入时间
cfg.disturbanceQmu = 9*pi/180; % 扰动速度
% 每 8 s 保存一次 NMPC 预测轨迹，用于后续画图
cfg.snapshotPeriod = 8.0;
cfg.saveOutput = true; % 开启结果保存。
cfg.outDir = fullfile(pwd, 'fixed_wing_nmpc_complex_output'); % 结果保存路径
% 这里是启动 fast mode ，快速仿真模式，这个模式就是减小仿真时长
cfg.fastMode = false;
if cfg.fastMode
cfg.Tsim = 18.0;
cfg.dtCtrl = 0.20;
cfg.opt.MaxIter = 22;
cfg.opt.MaxFunEvals = 220;
end
if cfg.saveOutput && exist(cfg.outDir, 'dir') ~= 7
mkdir(cfg.outDir);
end
fprintf('\n');
fprintf('============================================================\n');
fprintf('固定翼 NMPC 实验\n');
fprintf('============================================================\n');
%% ========================================================================
% 2. 运行全部两组测试
% ========================================================================
results = cell(numel(cfg.Nlist),1);
% 分别运行两组 NMPC 参数，并比较它们的控制效果和计算时间。
for c = 1:numel(cfg.Nlist)
N = cfg.Nlist(c); % 用来保存实验结果
M = cfg.Mlist(c);
fprintf('Case %d/%d: N=%d, M=%d\n', ...
c, numel(cfg.Nlist), N, M); % 循环两个 case
results{c} = runCase(N, M, cfg);
% 横向轨迹误差的均方根，越小表示跟踪越好
fprintf(' RMS cross-track error : %.3f m\n', rmsLocal(results{c}.et));
% 最大横向偏离距离
fprintf(' Max |cross-track| : %.3f m\n', max(abs(results{c}.et)));
% 航迹角误差 RMS，单位转换为度
fprintf(' RMS course error : %.3f deg\n', ...
180/pi*rmsLocal(results{c}.echi));
% 每次 NMPC 优化的平均求解时间
fprintf(' Mean solve time : %.2f ms\n', ...
1000*mean(results{c}.solveTime));
% 最慢一次 NMPC 优化耗时
fprintf(' Max solve time : %.2f ms\n\n', ...
1000*max(results{c}.solveTime));
end
%% ========================================================================
% 3. NMPC 仿真结果可视化
% ========================================================================
% 飞机的二维飞行轨迹、参考路径、横向误差和航迹角误差
fig1 = plotTrajectoryOverview(results, cfg);
% 跟踪与控制性能，如滚转角、控制指令、滚转角速度、目标函数等
fig2 = plotTrackingDiagnostics(results, cfg);
% 环境与计算性能，如真实/估计风速、空速、NMPC 求解时间等
fig3 = plotEnvironmentAndSolver(results, cfg);
% NMPC 在不同时刻预测出来的未来飞行轨迹
fig4 = plotPredictionSnapshots(results, cfg);
%% ========================================================================
% 4. 结果保存
% ========================================================================
if cfg.saveOutput
save(fullfile(cfg.outDir, 'complex_nmpc_results.mat'), 'results', 'cfg');
saveFigureCompat(fig1, fullfile(cfg.outDir, '01_trajectory_overview'));
saveFigureCompat(fig2, fullfile(cfg.outDir, '02_tracking_diagnostics'));
saveFigureCompat(fig3, fullfile(cfg.outDir, '03_environment_solver'));
saveFigureCompat(fig4, fullfile(cfg.outDir, '04_prediction_snapshots'));
fprintf('Results saved in:\n%s\n\n', cfg.outDir);
end
fprintf('Done.\n');
end
%% ========================================================================
% 闭环仿真
% ========================================================================
function out = runCase(N, M, cfg)
K = round(cfg.Tsim/cfg.dtCtrl); % 总控制步数
t = (0:K)*cfg.dtCtrl; % 时间戳
x = cfg.x0; % 初始飞机状态
segIdx = 1;
% 各类参数初始化
X = zeros(5,K+1); % 飞机状态
Ucmd = zeros(1,K); % 原始输出的滚转参考指令
Uapplied = zeros(1,K); % 经过幅值/变化率限制后，真正施加的控制指令
et = zeros(1,K+1); % 横向轨迹误差
echi = zeros(1,K+1); % 航迹角误差
segHist = zeros(1,K+1); % 当前位于第几段参考路径
solveTime = zeros(1,K); % 每次 NMPC 优化的求解时间
Jhist = zeros(1,K); % 每次 NMPC 优化得到的目标函数值 J
windTrueHist = zeros(2,K+1); % 真实风速历史，北向和东向两个分量
windEstHist = zeros(2,K+1); % NMPC 使用的估计风速历史
VtrueHist = zeros(1,K+1); % 真实空速历史
VestHist = zeros(1,K+1); % 估计空速历史
X(:,1) = x;
segHist(1) = segIdx;
% Initial environment
windTrue = trueWind(0); % 初始化环境状态和跟踪误差
windEst = windTrue; % 北向和东向风速分量
Vtrue = trueAirspeed(0,cfg); % 带噪声测量和低通滤波更新
Vest = Vtrue; % 计算t=0时刻的真实空速
% 把这些初始环境数据保存到历史数组的第一个位置
windTrueHist(:,1) = windTrue;
windEstHist(:,1) = windEst;
VtrueHist(1) = Vtrue;
VestHist(1) = Vest;
% 根据当前初始状态 x、参考路径、估计风速和空速，计算：
% 初始横向轨迹误差；初始航迹角误差；飞机当前应该对应第几段参考路径
[et(1), echi(1), segIdx] = ...
pathErrorAndSwitch(x, segIdx, cfg.path, windEst, Vest, true);
% 初始化历史信息、优化初值、扰动状态和预测轨迹记录
segHist(1)=segIdx;
% 保存初始时刻所在的路径段编号
Uprev = zeros(1,N);
% 用于惩罚相邻两次 NMPC 控制计划变化过大
zGuess = zeros(M,1);
% 保存上一个控制周期真正施加的滚转参考指令
uAppliedPrev = 0;
% 滚转扰动目前还没有注入
disturbanceInjected = false;
% 计算隔多少个控制周期保存一次 NMPC 预测轨迹
snapshotStep = max(1,round(cfg.snapshotPeriod/cfg.dtCtrl));
predX = {};
predSeg = {};
predTime = [];
% k 是当前控制步
for k = 1:K
% tk 是当前仿真时间
tk = (k-1)*cfg.dtCtrl;
% ---------------------------------------------------------
% 环境模拟
% ---------------------------------------------------------
windTrue = trueWind(tk); % 获得当前真实风速
noisyWindMeasurement = ...
windTrue + cfg.windNoiseStd*randn(2,1); % 模拟有噪声的风速
windEst = ...
(1-cfg.windFilter)*windEst + ...
cfg.windFilter*noisyWindMeasurement; % 低通滤波得到风速估计
% 得到真实空速和估计空速
Vtrue = trueAirspeed(tk,cfg); % 飞机当前真实空速
Vest = (1-cfg.VFilter)*Vest + cfg.VFilter*Vtrue; % 经过低通滤波后的空速估计
online.wind = windEst; % 估计风速
online.V = Vest; % 估计空速
online.segIdx = segIdx; % 当前路径段
% ---------------------------------------------------------
% NMPC最优化过程
% ---------------------------------------------------------
objective = @(z) horizonCost( ...
z, x, Uprev, N, M, online, cfg); % 构造 NMPC 目标函数
tic;
try
[zOpt,Jopt] = fminsearch(objective,zGuess,cfg.opt); % 用 fminsearch 求最优控制
% 优化失败时的保护措施
if any(~isfinite(zOpt)) || ~isfinite(Jopt)
zOpt = zGuess; % 找到的最优优化变量
Jopt = objective(zOpt); % 对应的最小目标函数值
end
catch
% Robust fallback: keep previous block sequence
zOpt = zGuess;
Jopt = objective(zOpt);
end
solveTime(k) = toc; % 求解时间
Jhist(k) = Jopt; % 目标函数值
Uopt = expandBlocks(zOpt,N,M,cfg.muMax); % 把控制块展开成完整预测控制序列
% 执行第一个控制输入
uCmd = Uopt(1);
% ---------------------------------------------------------
% 限制控制指令变化速度
% ---------------------------------------------------------
maxDu = cfg.muRateMax*cfg.dtCtrl;
uApplied = clamp( ...
uCmd, ...
uAppliedPrev-maxDu, ...
uAppliedPrev+maxDu); % 避免控制指令突然跳变
% 限制最大滚转参考角
uApplied = clamp(uApplied,-cfg.muMax,cfg.muMax);
Ucmd(k) = uCmd; % NMPC 原本想执行的控制
Uapplied(k) = uApplied; % 经过限制后真正执行的控制
uAppliedPrev = uApplied; % 留给下一控制周期使用
% ---------------------------------------------------------
% 定期保存 NMPC 的预测轨迹
% ---------------------------------------------------------
if mod(k-1,snapshotStep)==0
[Xp,Segp] = predictHorizon( ...
x,Uopt,segIdx,online,cfg); % 定期保存预测结果
predX{end+1} = Xp;
predSeg{end+1} = Segp;
predTime(end+1) = tk;
end
% ---------------------------------------------------------
% 用真实模型推进飞机
% ---------------------------------------------------------
plantOnline.wind = windTrue;
plantOnline.V = Vtrue;
% 使用真实飞机模型和 RK4 推进飞机前进
x = rk4Plant(x,uApplied,cfg.dtCtrl,plantOnline,cfg);
% 在指定时间加入外部扰动
if (~disturbanceInjected) && ...
(tk <= cfg.disturbanceTime) && ...
(tk+cfg.dtCtrl > cfg.disturbanceTime)
x(5) = x(5) + cfg.disturbanceQmu;
disturbanceInjected = true;
end
% 更新当前参考路径段
segIdx = updateSegmentIndex(x(1:2),segIdx,cfg.path);
% ---------------------------------------------------------
% 保存新的飞机状态
% ---------------------------------------------------------
X(:,k+1)=x;
[et(k+1),echi(k+1),segIdx] = ...
pathErrorAndSwitch( ...
x,segIdx,cfg.path,windEst,Vest,true); % 重新计算路径跟踪误差
segHist(k+1)=segIdx; % 当前路径段
windTrueHist(:,k+1)=windTrue; % 真实风
windEstHist(:,k+1)=windEst; % 估计风
VtrueHist(k+1)=Vtrue; % 真实空速
VestHist(k+1)=Vest; % 估计空速
% 保存这次 NMPC 控制序列
Uprev=Uopt;
% 热启动
zGuess=zOpt;
% 显示仿真进度
if mod(k,max(1,round(K/10)))==0
fprintf(' progress %3.0f %%\n',100*k/K);
end
end
out.N=N;
out.M=M;
out.t=t;
out.X=X;
out.Ucmd=Ucmd;
out.U=Uapplied;
out.et=et;
out.echi=echi;
out.seg=segHist;
out.solveTime=solveTime;
out.J=Jhist;
out.windTrue=windTrueHist;
out.windEst=windEstHist;
out.Vtrue=VtrueHist;
out.Vest=VestHist;
out.predX=predX;
out.predSeg=predSeg;
out.predTime=predTime;
end
%% ========================================================================
% NMPC COST
% ========================================================================
function J = horizonCost(z,x,Uprev,N,M,online,cfg)
% z：当前优化器尝试的控制块变量
% x：当前飞机状态
% Uprev：上一轮 NMPC 的控制序列
% N：预测步数
% M：控制块数量
% online：当前估计的风速、空速、路径段
% cfg：其他参数
U = expandBlocks(z,N,M,cfg.muMax); % 展开成 N 步控制指令
J=0; % 初始化总代价
segIdx=online.segIdx;
% 开始向未来预测 N 步
for i=1:N
u=U(i);
% 计算当前预测状态相对参考路径的横向轨迹误差、航迹角误差
[eti,echii,segIdx] = ...
pathErrorAndSwitch( ...
x,segIdx,cfg.path,online.wind,online.V,true);
% 计算当前控制计划和上一轮 NMPC 控制计划之间的差
duPrev = u-Uprev(i);
if N>1
rho=((N-i)/(N-1))^2; % 计算一个随预测步变化的权重
else
rho=1;
end
% 这就是单个预测步的 stage cost
L = ...
cfg.Q(1)*eti^2 + ...
cfg.Q(2)*echii^2 + ...
cfg.Q(3)*x(3)^2 + ...
cfg.Q(4)*x(5)^2 + ...
cfg.Q(5)*rho*duPrev^2 + ...
cfg.R*u^2;
% 把这一预测步的代价累加到总代价
J=J+cfg.dtPred*L;
x=rk4Model(x,u,cfg.dtPred,online,cfg);
if any(~isfinite(x)) || abs(x(3))>75*pi/180
J=J+1e9;
return;
end
end
% 计算预测终点的误差
[etN,echiN] = ...
pathErrorAndSwitch( ...
x,segIdx,cfg.path,online.wind,online.V,false);
% 加入终端代价
J=J + ...
cfg.P(1)*etN^2 + ...
cfg.P(2)*echiN^2 + ...
cfg.P(3)*x(3)^2 + ...
cfg.P(4)*x(5)^2;
% 加一个很小的正则化项是为了提高数值优化稳定性
J=J+1e-4*sum(z.^2);
end
%% ========================================================================
% 根据当前状态 x 和已经求出的控制序列 U，向未来预测 \(N\) 步，并把预测状态和路径段编号保存下来
% ========================================================================
function [Xp,Segp] = predictHorizon(x,U,segIdx,online,cfg)
% 输入：
%
% x：当前飞机状态
% U：未来控制序列
% segIdx：当前路径段编号
% online：当前估计风速、空速等
% cfg：系统参数
%
% 输出：
%
% Xp：未来预测状态
% Segp：每个预测点对应的路径段编号
N=numel(U); % 控制序列 U 有多少个元素，就预测多少步
% 提前创建存储空间。
Xp=zeros(5,N+1);
Segp=zeros(1,N+1);
Xp(:,1)=x;
Segp(1)=segIdx;
for i=1:N
% 检查按照当前预测位置，是否应该切换到下一段参考路径
[~,~,segIdx] = ...
pathErrorAndSwitch( ...
x,segIdx,cfg.path,online.wind,online.V,true);
x=rk4Model(x,U(i),cfg.dtPred,online,cfg);
Xp(:,i+1)=x;
Segp(i+1)=segIdx;
end
end
%% ========================================================================
% NMPC 控制器内部使用的固定翼动力学模型
% ========================================================================
function dx = modelDynamics(x,u,online,cfg)
mu=x(3);
xi=x(4);
q=x(5);
V=max(online.V,5.0);
% 核心动力学
dx=[...
V*cos(xi)+online.wind(1);...
V*sin(xi)+online.wind(2);...
q;...
cfg.g*tan(mu)/V;...
cfg.model.b0*u-cfg.model.a1*q-cfg.model.a0*mu];
end
% 利用四阶 Runge-Kutta（RK4），把连续动力学模型推进一个时间步
function xn = rk4Model(x,u,dt,online,cfg)
% 计算当前状态下的导数
k1=modelDynamics(x,u,online,cfg);
% 预测半步后的导数
k2=modelDynamics(x+0.5*dt*k1,u,online,cfg);
% 再次计算半步位置的导数
k3=modelDynamics(x+0.5*dt*k2,u,online,cfg);
% 计算完整一步末端的导数
k4=modelDynamics(x+dt*k3,u,online,cfg);
% 得到下一时刻状态
xn=x+dt/6*(k1+2*k2+2*k3+k4);
xn(4)=wrapPi(xn(4));
end
%% ========================================================================
% 真实飞机动态模拟
% ========================================================================
function dx = plantDynamics(x,u,online,cfg)
% 输入：
%
% x：当前飞机状态
% u：当前实际施加的滚转参考指令
% online：当前真实环境信息，主要包括：
% online.V：真实空速
% online.wind：真实风速
% cfg：系统参数，包括 g、plant.b0、plant.a1、plant.a0 等
%
% 输出：
%
% dx：当前状态的导数
mu=x(3);
xi=x(4);
q=x(5);
V=max(online.V,5.0);
% 核心动力学
dx=[...
V*cos(xi)+online.wind(1);...
V*sin(xi)+online.wind(2);...
q;...
cfg.g*tan(mu)/V;...
cfg.plant.b0*u-cfg.plant.a1*q-cfg.plant.a0*mu];
end
function xn = rk4Plant(x,u,dt,online,cfg)
% 输入：
%
% x：当前状态
% u：当前控制输入
% dt：积分时间步长
% online：真实风速、真实空速
% cfg：真实飞机参数
%
% 输出：
%
% xn：经过 dt 秒之后的新状态
k1=plantDynamics(x,u,online,cfg);
k2=plantDynamics(x+0.5*dt*k1,u,online,cfg);
k3=plantDynamics(x+0.5*dt*k2,u,online,cfg);
k4=plantDynamics(x+dt*k3,u,online,cfg);
xn=x+dt/6*(k1+2*k2+2*k3+k4);
xn(4)=wrapPi(xn(4));
end
%% ========================================================================
% 构造随时间变化的真实环境
% ========================================================================
function w = trueWind(t)
% 设置一个基础风
wn = 0.8;
we = -5.5;
% 平滑周期变化
wn = wn + 1.5*sin(0.20*t);
we = we - 1.8*sin(0.11*t+0.4);
% 接着构造一个在 t=21s 附近最强的阵风
gust = exp(-((t-21.0)/3.2)^2);
wn = wn + 3.0*gust;
we = we - 5.0*gust;
% 28 秒以后，北向风再逐渐发生一个额外变化
if t>28
wn = wn - 2.0*(1-exp(-(t-28)/3));
end
w=[wn;we];
end
function V = trueAirspeed(t,cfg)
% 输入
% 时间 t 和参数 cfg
% 输出
% 真实空速 V。
V = cfg.model.V0 ...
+0.65*sin(0.10*t) ...
+0.25*sin(0.31*t+0.8);
end
%% ========================================================================
% 构造参考航迹
% ========================================================================
function path = buildRacetrackPath()
% 定义 4 个关键点
A=[0;-80];
B=[0; 80];
D=[80;80];
C=[80;-80];
% 1)东向直线 A -> B
s1.type='line';
s1.p0=A;
s1.p1=B;
s1.T=(B-A)/norm(B-A);
s1.length=norm(B-A);
s1.name='Eastbound line';
% 2)B -> D东侧半圆
s2.type='arc';
s2.c=[40;80];
s2.R=40;
s2.sigma=-1;
s2.theta0=pi;
s2.sweep=pi;
s2.name='East semicircle';
% 3)西向直线 D -> C
s3.type='line';
s3.p0=D;
s3.p1=C;
s3.T=(C-D)/norm(C-D);
s3.length=norm(C-D);
s3.name='Westbound line';
% 4)C -> A西侧半圆
s4.type='arc';
s4.c=[40;-80];
s4.R=40;
s4.sigma=-1;
s4.theta0=0;
s4.sweep=pi;
s4.name='West semicircle';
path={s1,s2,s3,s4};
end
%% ========================================================================
% 路径误差 + 预测切换
% ========================================================================
function [et,echi,segIdx,chiD] = ...
pathErrorAndSwitch(x,segIdx,path,wind,V,allowSwitch)
% 输入：
%
% x：飞机当前状态
% segIdx：当前正在跟踪第几段路径
% path：完整参考路径
%
% wind：当前风速
% V：当前空速
% allowSwitch：是否允许自动切换到下一段路径，true/false
%
% 输出：
%
% et：横向轨迹误差 cross-track error
% echi：航迹角误差
% segIdx：更新后的路径段编号
% chiD：当前路径期望的航迹方向
% 检查飞机是不是已经飞完当前路径段
if allowSwitch
segIdx=updateSegmentIndex(x(1:2),segIdx,path);
end
seg=path{segIdx};
p=x(1:2);
if strcmp(seg.type,'line')
T=seg.T;
s=dot(p-seg.p0,T);
d=seg.p0+s*T;
else
r=p-seg.c;
rho=norm(r);
if rho<1e-9
rho=1e-9;
end
rhat=r/rho;
d=seg.c+seg.R*rhat;
% 计算圆弧当前位置的切线方向
T=seg.sigma*[-rhat(2);rhat(1)];
end
dp=d-p;
% 计算横向误差
et=dp(1)*T(2)-dp(2)*T(1);
chiD=atan2(T(2),T(1));
vgN=V*cos(x(4))+wind(1);
vgE=V*sin(x(4))+wind(2);
chi=atan2(vgE,vgN);
echi=wrapPi(chiD-chi);
end
function segIdx = updateSegmentIndex(p,segIdx,path)
% 输入：
%
% p：飞机当前位置 [N;E]
% segIdx：当前路径段编号
% path：完整参考路径
%
% 输出：
%
% segIdx：判断后新的路径段编号
for attempt=1:numel(path)
seg=path{segIdx};
crossed=false;
if strcmp(seg.type,'line')
progress=dot(p-seg.p0,seg.T)/seg.length;
crossed=(progress>=1.0);
else
r=p-seg.c;
% 计算已经绕圆弧转了多少角度
theta=atan2(r(2),r(1));
delta=positiveAngle(seg.sigma*(theta-seg.theta0));
progress=delta/seg.sweep;
crossed=(progress>=1.0);
end
if crossed
segIdx=segIdx+1;
if segIdx>numel(path)
segIdx=1;
end
else
break;
end
end
end
%% ========================================================================
% 控制模块化
% ========================================================================
function U = expandBlocks(z,N,M,muMax)
% 输入：
%
% z M个优化变量
% N：预测步数
% M：控制块数量
% muMax：最大滚转参考角
%
% 输出：
%
% U：长度为 N 的完整控制序列
uBlock=muMax*tanh(z(:)); % 把优化变量 z 转成控制块
U=zeros(1,N);
edges=round(linspace(1,N+1,M+1));
for j=1:M
i1=edges(j);
i2=edges(j+1)-1;
if j==M
i2=N;
end
U(i1:i2)=uBlock(j); % 同一个控制块里的多个预测步使用同一个控制值
end
end
%% ========================================================================
% 可视化1：轨迹
% ========================================================================
function fig = plotTrajectoryOverview(results,cfg)
% 输入：
%
% results：前面两组 NMPC 仿真结果，例如 N=40 和 N=80
% cfg：系统配置参数，包括参考路径、初始状态等
%
% 输出：
%
% fig：生成的 MATLAB Figure 图窗句柄
th = plotTheme();
[eRef,nRef]=samplePathForPlot(cfg.path); %
fig=figure( ...
'Color','w', ...
'Name','Complex NMPC trajectory', ...
'Position',[40 40 1560 860]);
ax=subplot(2,3,[1 2 4 5]);
hold(ax,'on');
plot(ax,eRef,nRef,'-', ...
'Color',[0.88 0.88 0.88], ...
'LineWidth',th.lwRefShadow, ...
'HandleVisibility','off');
plot(ax,eRef,nRef,'k--', ...
'LineWidth',th.lwRef, ...
'DisplayName','Reference path');
for c=1:numel(results)
R=results{c};
plot(ax,R.X(2,:),R.X(1,:), ...
'LineWidth',th.lwShadow, ...
'Color',lightenColor(th.C(c,:),0.72), ...
'HandleVisibility','off');
plot(ax,R.X(2,:),R.X(1,:), ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('NMPC N=%d',R.N));
idx=find(diff(R.seg)~=0)+1;
if ~isempty(idx)
plot(ax,R.X(2,idx),R.X(1,idx),'o', ...
'MarkerSize',th.markerLarge, ...
'LineWidth',1.8, ...
'Color',th.C(c,:), ...
'MarkerFaceColor','w', ...
'HandleVisibility','off');
end
plot(ax,R.X(2,end),R.X(1,end),'d', ...
'MarkerSize',th.markerLarge, ...
'LineWidth',1.8, ...
'Color',th.C(c,:), ...
'MarkerFaceColor',th.C(c,:), ...
'HandleVisibility','off');
addTrajectoryArrows(ax,R.X(2,:),R.X(1,:),th.C(c,:),7);
end
plot(ax,cfg.x0(2),cfg.x0(1),'ks', ...
'MarkerSize',10, ...
'LineWidth',2.0, ...
'MarkerFaceColor','w', ...
'DisplayName','Initial state');
arrowTimes=[0 12 21 31];
anchors=[-48 -17 14 45; -7 -7 -7 -7];
for j=1:numel(arrowTimes)
tj=arrowTimes(j);
w=trueWind(tj);
quiver(ax, ...
anchors(1,j),anchors(2,j), ...
2.1*w(2),2.1*w(1), ...
0, ...
'Color',th.windColor, ...
'LineWidth',th.lwSecondary, ...
'MaxHeadSize',0.75, ...
'HandleVisibility','off');
text(ax,anchors(1,j)-3,anchors(2,j)+10, ...
sprintf('t=%.0f s',tj), ...
'FontName',th.fontName, ...
'FontSize',th.fontSmall, ...
'FontWeight','bold', ...
'Color',th.windColor);
end
text(ax,-72,4,'Line 1','FontName',th.fontName,'FontSize',13,'FontWeight','bold','Color',[0.3 0.3 0.3]);
text(ax, 62,40,'Arc 1','FontName',th.fontName,'FontSize',13,'FontWeight','bold','Color',[0.3 0.3 0.3]);
text(ax, 35,82,'Line 2','FontName',th.fontName,'FontSize',13,'FontWeight','bold','Color',[0.3 0.3 0.3]);
text(ax,-82,40,'Arc 2','FontName',th.fontName,'FontSize',13,'FontWeight','bold','Color',[0.3 0.3 0.3]);
axis(ax,'equal');
xlabel(ax,'East [m]');
ylabel(ax,'North [m]');
title(ax,'Multi-segment ground track with wind, switching and prediction');
legend(ax,'Location','southoutside','Orientation','horizontal');
styleAxes(ax,th);
ax=subplot(2,3,3);
hold(ax,'on');
tBand=results{1}.t;
drawHorizontalBand(ax,tBand,-5,5,[0.55 0.75 0.55],0.11);
for c=1:numel(results)
R=results{c};
plot(ax,R.t,R.et, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawHLine(0,'k-',1.5);
drawHLine(5,'k:',1.2);
drawHLine(-5,'k:',1.2);
xlabel(ax,'Time [s]');
ylabel(ax,'e_t [m]');
title(ax,'Cross-track error');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(2,3,6);
hold(ax,'on');
drawHorizontalBand(ax,tBand,-10,10,[0.60 0.70 0.90],0.11);
for c=1:numel(results)
R=results{c};
plot(ax,R.t,180/pi*R.echi, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawHLine(0,'k-',1.5);
drawHLine(10,'k:',1.2);
drawHLine(-10,'k:',1.2);
xlabel(ax,'Time [s]');
ylabel(ax,'e_\chi [deg]');
title(ax,'Ground-course error');
legend(ax,'Location','best');
styleAxes(ax,th);
end
%% ========================================================================
% 可视化代码 2: 跟踪控制
% ========================================================================
function fig = plotTrackingDiagnostics(results,cfg)
% 输入：
%
% results：各组 NMPC 仿真结果，例如 N=40、N=80
% cfg：控制器参数，如 muMax、muRateMax、扰动时间等
%
% 输出：
%
% fig：生成的 Figure 图窗句柄
th=plotTheme();
fig=figure( ...
'Color','w', ...
'Name','Tracking diagnostics', ...
'Position',[55 40 1500 920]);
ax=subplot(3,2,1);
hold(ax,'on');
drawHorizontalBand(ax,results{1}.t,-10,10,[0.65 0.75 0.95],0.10);
for c=1:numel(results)
R=results{c};
plot(ax,R.t,180/pi*R.echi, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawHLine(0,'k-',1.5);
xlabel(ax,'Time [s]');
ylabel(ax,'e_\chi [deg]');
title(ax,'Ground-course tracking');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(3,2,2);
hold(ax,'on');
drawHorizontalBand(ax,results{1}.t,-180/pi*cfg.muMax,180/pi*cfg.muMax, ...
[0.78 0.86 0.96],0.08);
for c=1:numel(results)
R=results{c};
plot(ax,R.t,180/pi*R.X(3,:), ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawHLine(180/pi*cfg.muMax,'k--',1.6);
drawHLine(-180/pi*cfg.muMax,'k--',1.6);
xlabel(ax,'Time [s]');
ylabel(ax,'\mu [deg]');
title(ax,'Actual bank angle and allowable region');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(3,2,3);
hold(ax,'on');
for c=1:numel(results)
R=results{c};
stairs(ax,R.t(1:end-1),180/pi*R.Ucmd, ...
'--', ...
'LineWidth',th.lwSecondary, ...
'Color',lightenColor(th.C(c,:),0.15), ...
'DisplayName',sprintf('N=%d cmd',R.N));
stairs(ax,R.t(1:end-1),180/pi*R.U, ...
'-', ...
'LineWidth',th.lwMain+0.4, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d applied',R.N));
end
drawHLine(180/pi*cfg.muMax,'k--',1.6);
drawHLine(-180/pi*cfg.muMax,'k--',1.6);
xlabel(ax,'Time [s]');
ylabel(ax,'\mu_r [deg]');
title(ax,'Commanded vs rate-limited bank reference');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(3,2,4);
hold(ax,'on');
for c=1:numel(results)
R=results{c};
plot(ax,R.t,180/pi*R.X(5,:), ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawVerticalBand(ax,cfg.disturbanceTime-0.45,cfg.disturbanceTime+0.45, ...
[0.95 0.65 0.50],0.14);
yl=ylim(ax);
plot(ax,[cfg.disturbanceTime cfg.disturbanceTime],yl,'k--', ...
'LineWidth',1.7,'HandleVisibility','off');
xlabel(ax,'Time [s]');
ylabel(ax,'q_\mu [deg/s]');
title(ax,'Bank-rate response with injected disturbance');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(3,2,5);
hold(ax,'on');
for c=1:numel(results)
R=results{c};
du=[0 diff(R.U)]/cfg.dtCtrl;
plot(ax,R.t(1:end-1),180/pi*du, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
end
drawHLine(180/pi*cfg.muRateMax,'k--',1.5);
drawHLine(-180/pi*cfg.muRateMax,'k--',1.5);
xlabel(ax,'Time [s]');
ylabel(ax,'d\mu_r/dt [deg/s]');
title(ax,'Applied command slew rate');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(3,2,6);
hold(ax,'on');
for c=1:numel(results)
R=results{c};
plot(ax,R.t(1:end-1),R.J, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',R.N));
Jsm=movingAverageLocal(R.J,7);
plot(ax,R.t(1:end-1),Jsm, ...
':', ...
'LineWidth',th.lwSecondary, ...
'Color',lightenColor(th.C(c,:),0.12), ...
'HandleVisibility','off');
end
xlabel(ax,'Time [s]');
ylabel(ax,'NMPC objective J');
title(ax,'Objective value (solid) and moving average (dotted)');
legend(ax,'Location','best');
styleAxes(ax,th);
end
%% ========================================================================
% 可视化3：环境变量
% ========================================================================
function fig = plotEnvironmentAndSolver(results,cfg)
% 输入：
%
% results：不同 NMPC 配置的仿真结果
% cfg：系统参数
%
% 输出：
%
% fig：生成的 Figure 图窗句柄
th=plotTheme();
R=results{2};
fig=figure( ...
'Color','w', ...
'Name','Environment and numerical diagnostics', ...
'Position',[80 45 1500 900]);
gustStart=18;
gustEnd=24;
ax=subplot(2,2,1);
hold(ax,'on');
plot(ax,R.t,R.windTrue(1,:),'-', ...
'LineWidth',th.lwMain, ...
'Color',[0.10 0.45 0.78], ...
'DisplayName','w_N true');
plot(ax,R.t,R.windEst(1,:),'--', ...
'LineWidth',th.lwSecondary+0.3, ...
'Color',[0.35 0.65 0.90], ...
'DisplayName','w_N estimate');
plot(ax,R.t,R.windTrue(2,:),'-', ...
'LineWidth',th.lwMain, ...
'Color',[0.82 0.28 0.18], ...
'DisplayName','w_E true');
plot(ax,R.t,R.windEst(2,:),'--', ...
'LineWidth',th.lwSecondary+0.3, ...
'Color',[0.95 0.55 0.42], ...
'DisplayName','w_E estimate');
drawVerticalBand(ax,gustStart,gustEnd,[0.95 0.78 0.42],0.12);
xlabel(ax,'Time [s]');
ylabel(ax,'Wind [m/s]');
title(ax,sprintf('Wind truth vs estimate, N=%d',R.N));
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(2,2,2);
hold(ax,'on');
plot(ax,R.t,R.Vtrue,'-', ...
'LineWidth',th.lwMain, ...
'Color',[0.10 0.55 0.35], ...
'DisplayName','V true');
plot(ax,R.t,R.Vest,'--', ...
'LineWidth',th.lwSecondary+0.4, ...
'Color',[0.42 0.75 0.55], ...
'DisplayName','V estimate');
drawVerticalBand(ax,gustStart,gustEnd,[0.95 0.78 0.42],0.12);
xlabel(ax,'Time [s]');
ylabel(ax,'Airspeed [m/s]');
title(ax,'Airspeed truth vs online estimate');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(2,2,3);
hold(ax,'on');
for c=1:numel(results)
Rc=results{c};
tSolve=Rc.t(1:end-1);
ms=1000*Rc.solveTime;
plot(ax,tSolve,ms, ...
'LineWidth',1.45, ...
'Color',lightenColor(th.C(c,:),0.20), ...
'HandleVisibility','off');
msAvg=movingAverageLocal(ms,9);
plot(ax,tSolve,msAvg, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d moving avg',Rc.N));
end
drawHLine(1000*cfg.dtCtrl,'k--',1.7);
xlabel(ax,'Time [s]');
ylabel(ax,'Solve time [ms]');
title(ax,'NMPC computation time: raw traces + moving averages');
legend(ax,'Location','best');
styleAxes(ax,th);
ax=subplot(2,2,4);
hold(ax,'on');
for c=1:numel(results)
Rc=results{c};
stairs(ax,Rc.t,Rc.seg, ...
'LineWidth',th.lwMain, ...
'Color',th.C(c,:), ...
'DisplayName',sprintf('N=%d',Rc.N));
idx=find(diff(Rc.seg)~=0)+1;
if ~isempty(idx)
plot(ax,Rc.t(idx),Rc.seg(idx),'o', ...
'MarkerSize',th.markerLarge, ...
'LineWidth',1.6, ...
'Color',th.C(c,:), ...
'MarkerFaceColor','w', ...
'HandleVisibility','off');
end
end
xlabel(ax,'Time [s]');
ylabel(ax,'Path segment');
title(ax,'Path-segment switching events');
set(ax,'YTick',1:4,'YLim',[0.7 4.3]);
legend(ax,'Location','best');
styleAxes(ax,th);
end
%% ========================================================================
% 可视化代码 4: 预测部分
% ========================================================================
function fig = plotPredictionSnapshots(results,cfg)
% 输入
%
% results：各个 case 的仿真结果（例如 N=40 和 N=80）
% cfg：参数结构体，里面有参考路径、dtPred 等信息
% 输出
%
% fig：生成的图窗句柄
th=plotTheme();
[eRef,nRef]=samplePathForPlot(cfg.path);
fig=figure( ...
'Color','w', ...
'Name','NMPC prediction snapshots', ...
'Position',[95 70 1500 720]);
for c=1:numel(results)
R=results{c};
ax=subplot(1,2,c);
hold(ax,'on');
plot(ax,eRef,nRef,'-', ...
'Color',[0.90 0.90 0.90], ...
'LineWidth',th.lwRefShadow, ...
'HandleVisibility','off');
plot(ax,eRef,nRef,'k--', ...
'LineWidth',th.lwRef, ...
'DisplayName','Reference path');
plot(ax,R.X(2,:),R.X(1,:), ...
'Color',[0.55 0.55 0.55], ...
'LineWidth',th.lwSecondary, ...
'DisplayName','Closed-loop track');
nSnap=numel(R.predX);
if nSnap>0
colors=turboCompat(max(nSnap,2));
for j=1:nSnap
Xp=R.predX{j};
plot(ax,Xp(2,:),Xp(1,:), ...
'LineWidth',5.0, ...
'Color',lightenColor(colors(j,:),0.73), ...
'HandleVisibility','off');
plot(ax,Xp(2,:),Xp(1,:), ...
'LineWidth',2.25, ...
'Color',colors(j,:), ...
'HandleVisibility','off');
plot(ax,Xp(2,1),Xp(1,1),'o', ...
'MarkerSize',5.5, ...
'MarkerFaceColor',colors(j,:), ...
'MarkerEdgeColor',colors(j,:), ...
'HandleVisibility','off');
plot(ax,Xp(2,end),Xp(1,end),'x', ...
'MarkerSize',7, ...
'LineWidth',1.8, ...
'Color',colors(j,:), ...
'HandleVisibility','off');
if j==1 || j==nSnap
text(ax,Xp(2,1)+3,Xp(1,1)+3, ...
sprintf('t=%.0f s',R.predTime(j)), ...
'FontSize',th.fontSmall, ...
'FontWeight','bold', ...
'Color',colors(j,:));
end
end
end
axis(ax,'equal');
xlabel(ax,'East [m]');
ylabel(ax,'North [m]');
title(ax,sprintf( ...
'N=%d: %.1f s nonlinear prediction window', ...
R.N,R.N*cfg.dtPred));
styleAxes(ax,th);
end
end
%% ========================================================================
% 路径绘制
% ========================================================================
function [eAll,nAll] = samplePathForPlot(path)
eAll=[];
nAll=[];
for i=1:numel(path)
seg=path{i};
if strcmp(seg.type,'line')
s=linspace(0,1,120);
P=seg.p0+(seg.p1-seg.p0)*s;
else
theta= ...
seg.theta0 + ...
seg.sigma*linspace(0,seg.sweep,160);
P=[...
seg.c(1)+seg.R*cos(theta);...
seg.c(2)+seg.R*sin(theta)];
end
nAll=[nAll nan P(1,:)];
eAll=[eAll nan P(2,:)];
end
end
%% ========================================================================
% 功能函数
% ========================================================================
function th = plotTheme()
th.C=[...
0.0000 0.4470 0.7410;...
0.8500 0.3250 0.0980];
th.windColor=[0.18 0.48 0.85];
th.lwMain=3.2;
th.lwSecondary=2.2;
th.lwRef=3.0;
th.lwShadow=7.5;
th.lwRefShadow=8.5;
th.markerLarge=9;
th.fontName='Times New Roman';
th.font=15;
th.fontSmall=12;
th.labelFont=16;
th.titleFont=18;
th.legendFont=14;
th.axisWidth=1.35;
end
function styleAxes(ax,th)
set(ax, ...
'FontName',th.fontName, ...
'FontSize',th.font, ...
'LineWidth',th.axisWidth, ...
'Box','on', ...
'XGrid','on', ...
'YGrid','on', ...
'GridLineStyle','--', ...
'MinorGridLineStyle',':');
try
ax.XLabel.FontName = th.fontName;
ax.XLabel.FontSize = th.labelFont;
ax.XLabel.FontWeight = 'normal';
ax.YLabel.FontName = th.fontName;
ax.YLabel.FontSize = th.labelFont;
ax.YLabel.FontWeight = 'normal';
ax.Title.FontName = th.fontName;
ax.Title.FontSize = th.titleFont;
ax.Title.FontWeight = 'bold';
catch
end
try
lgd = get(ax,'Legend');
if ~isempty(lgd)
lgd.FontName = th.fontName;
lgd.FontSize = th.legendFont;
end
catch
end
try
set(ax,'GridAlpha',0.20,'MinorGridAlpha',0.08);
grid(ax,'minor');
catch
end
end
function addTrajectoryArrows(ax,x,y,color,nArrows)
if numel(x)<4
return;
end
idx=round(linspace(2,numel(x)-1,nArrows));
for k=1:numel(idx)
i=idx(k);
dx=x(i+1)-x(i-1);
dy=y(i+1)-y(i-1);
L=sqrt(dx^2+dy^2);
if L<1e-9
continue;
end
scale=7.0/L;
quiver(ax,x(i),y(i),scale*dx,scale*dy,0, ...
'Color',color, ...
'LineWidth',1.35, ...
'MaxHeadSize',1.4, ...
'HandleVisibility','off');
end
end
function c2 = lightenColor(c,amount)
amount=min(max(amount,0),1);
c2=c+(1-c)*amount;
end
function drawHorizontalBand(ax,t,yLow,yHigh,color,alphaValue)
x=[t(:);flipud(t(:))];
y=[yLow*ones(numel(t),1);yHigh*ones(numel(t),1)];
patch(ax,x,y,color, ...
'FaceAlpha',alphaValue, ...
'EdgeColor','none', ...
'HandleVisibility','off');
end
function drawVerticalBand(ax,x1,x2,color,alphaValue)
yl=ylim(ax);
patch(ax, ...
[x1 x2 x2 x1], ...
[yl(1) yl(1) yl(2) yl(2)], ...
color, ...
'FaceAlpha',alphaValue, ...
'EdgeColor','none', ...
'HandleVisibility','off');
ylim(ax,yl);
end
function y = movingAverageLocal(x,n)
x=x(:).';
n=max(1,round(n));
if n==1
y=x;
return;
end
kernel=ones(1,n)/n;
y=conv(x,kernel,'same');
half=floor(n/2);
for i=1:min(half,numel(x))
y(i)=mean(x(1:min(numel(x),i+half)));
end
for i=max(1,numel(x)-half+1):numel(x)
y(i)=mean(x(max(1,i-half):numel(x)));
end
end
function C = turboCompat(n)
try
C=turbo(n);
catch
C=parula(n);
end
end
function y = wrapPi(x)
y=atan2(sin(x),cos(x));
end
function a = positiveAngle(a)
a=mod(a,2*pi);
end
function y = clamp(x,lo,hi)
y=min(max(x,lo),hi);
end
function r = rmsLocal(x)
r=sqrt(mean(x(:).^2));
end
function drawHLine(y,style,lineWidth)
if nargin<3
lineWidth=1.5;
end
ax=gca;
xl=xlim(ax);
plot(ax,xl,[y y],style, ...
'LineWidth',lineWidth, ...
'HandleVisibility','off');
xlim(ax,xl);
end
function saveFigureCompat(fig,baseName)
try
figure(fig);
print(fig,[baseName '.png'],'-dpng','-r260');
print(fig,[baseName '.pdf'],'-dpdf','-painters');
catch
end
end2. 仿真结果
下面四幅图与程序自动保存的四个 PNG 文件一一对应。
CSDN 使用说明：
如果你直接把本文复制到 CSDN，建议先运行 MATLAB 得到 PNG 文件，然后在 CSDN 编辑器中上传图片，并把下面
<img src="...">中的相对路径替换为 CSDN 自动生成的图片链接。如果在本地 Markdown 中查看，则保持相对路径即可。
3.1 多段路径跟踪与轨迹误差
本部分主要展示：
- racetrack reference path；
- N=40N=40N=40 与 N=80N=80N=80 两种 prediction horizon 下的实际 ground track；
- 路径切换点；
- 飞行方向箭头；
- 不同时刻的风矢量；
- cross-track error；
- ground-course error。
从图中可以重点观察较长 prediction horizon 是否能够在进入圆弧或者强风区域之前更早采取控制动作。
3.2 跟踪与控制性能
本部分需包括
- course error：
eχ e_\chi eχ
- 实际 bank angle：
μ \mu μ
- NMPC commanded bank reference：
μrcmd \mu_r^{\mathrm{cmd}} μrcmd
- 经过 slew-rate limit 后的 applied bank reference：
μrapplied \mu_r^{\mathrm{applied}} μrapplied
- bank rate：
qμ=μ˙ q_\mu = \dot{\mu} qμ=μ˙
- applied command slew rate；
- NMPC objective value。
其中 bank-angle reference 受到：
−30∘≤μr≤30∘ -30^\circ \le \mu_r \le 30^\circ −30∘≤μr≤30∘
的约束。
同时控制输入变化率受到：
∣μ˙r∣≤90∘/s |\dot{\mu}_r| \le 90^\circ/\mathrm{s} ∣μ˙r∣≤90∘/s
的限制。
3.3 环境估计与数值求解性能
本部分主要包括：
- true wind 与 estimated wind；
- true airspeed 与 estimated airspeed；
- NMPC solve time；
- path segment switching。
本实验中真实风场写成：
w(t)=wsteady+wperiodic+wgust \mathbf{w}(t) =\mathbf{w}{\mathrm{steady}} + \mathbf{w}{\mathrm{periodic}} + \mathbf{w}_{\mathrm{gust}} w(t)=wsteady+wperiodic+wgust
而 NMPC 实际使用的是滤波后的估计：
w^k=(1−α)w^k−1+αwm,k \hat{\mathbf{w}}k =(1-\alpha) \hat{\mathbf{w}}{k-1} + \alpha \mathbf{w}_{m,k} w^k=(1−α)w^k−1+αwm,k
因此：
w^≠w \hat{\mathbf{w}} \neq \mathbf{w} w^=w
使 prediction model 与真实环境之间存在误差。
3.4 Prediction Horizon 可视化
每一条彩色 prediction curve 表示在某一个控制时刻，NMPC 根据当前状态、风估计、空速估计以及当前 path segment 预测出的未来轨迹：
x0,x1,...,xN \mathbf{x}_0, \mathbf{x}_1, \ldots, \mathbf{x}_N x0,x1,...,xN
当：
N=40 N=40 N=40
且：
Δt=0.1 s \Delta t=0.1\,\mathrm{s} Δt=0.1s
时，预测时间为：
Tp=NΔt=4 s T_p =N\Delta t =4\,\mathrm{s} Tp=NΔt=4s
而：
N=80 N=80 N=80
对应：
Tp=8 s T_p =8\,\mathrm{s} Tp=8s
因此长 horizon 能够更早看到：
- 即将进入的圆弧；
- 后续 path segment；
- 风对未来 ground track 的累计影响；
- 当前 bank-angle command 对未来轨迹的长期影响。
4. 实验中使用的 NMPC 模型
状态定义为：
x=neμξqμT \boxed{ \mathbf{x} =\begin{bmatrix} n & e & \mu & \xi & q_\mu \end{bmatrix}^T } x=neμξqμT
控制输入：
u=μr \boxed{ u=\mu_r } u=μr
连续时间模型：
x˙=Vcosξ+wnVsinξ+weqμgtanμVb0u−a1qμ−a0μ \boxed{ \dot{\mathbf{x}} =\begin{bmatrix} V\cos\xi+w_n \\ V\sin\xi+w_e \\ q_\mu \\ \dfrac{g\tan\mu}{V} \\ b_0u-a_1q_\mu-a_0\mu \end{bmatrix} } x˙= Vcosξ+wnVsinξ+weqμVgtanμb0u−a1qμ−a0μ
其中：
qμ=μ˙ q_\mu =\dot{\mu} qμ=μ˙
5. 数值积分
使用 RK4：
k1=f(xi,ui) \mathbf{k}_1 =f(\mathbf{x}_i,u_i) k1=f(xi,ui)
k2=f(xi+Δt2k1,ui) \mathbf{k}_2 =f\left( \mathbf{x}_i + \frac{\Delta t}{2} \mathbf{k}_1, u_i \right) k2=f(xi+2Δtk1,ui)
k3=f(xi+Δt2k2,ui) \mathbf{k}_3 =f\left( \mathbf{x}_i + \frac{\Delta t}{2} \mathbf{k}_2, u_i \right) k3=f(xi+2Δtk2,ui)
k4=f(xi+Δtk3,ui) \mathbf{k}_4 =f\left( \mathbf{x}_i + \Delta t \mathbf{k}_3, u_i \right) k4=f(xi+Δtk3,ui)
最终：
xi+1=xi+Δt6(k1+2k2+2k3+k4) \boxed{ \mathbf{x}_{i+1} =\mathbf{x}_i + \frac{\Delta t}{6} \left( \mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4 \right) } xi+1=xi+6Δt(k1+2k2+2k3+k4)
6. Path Error
横向路径误差：
et=(d−p)×Tˉd \boxed{ e_t =(\mathbf{d}-\mathbf{p}) \times \bar{\mathbf{T}}_d } et=(d−p)×Tˉd
参考 course：
χd=atan2(Tˉd,e,Tˉd,n) \chi_d =\operatorname{atan2} \left( \bar{T}{d,e}, \bar{T}{d,n} \right) χd=atan2(Tˉd,e,Tˉd,n)
实际 ground course：
χ=atan2(Vsinξ+we,Vcosξ+wn) \chi =\operatorname{atan2} \left( V\sin\xi+w_e, V\cos\xi+w_n \right) χ=atan2(Vsinξ+we,Vcosξ+wn)
角度误差：
eχ=atan2(sin(χd−χ),cos(χd−χ)) \boxed{ e_\chi =\operatorname{atan2} \left( \sin(\chi_d-\chi), \cos(\chi_d-\chi) \right) } eχ=atan2(sin(χd−χ),cos(χd−χ))
7. NMPC 代价函数
程序采用的 stage cost 可以写成：
J=∑i=0N−1(qtet,i2+qχeχ,i2+qμμi2+qqqμ,i2+qΔuρi(ui−ui,prev)2+Rui2)+JN \begin{aligned} J =\sum_{i=0}^{N-1} \Big( & q_t e_{t,i}^2 + q_\chi e_{\chi,i}^2 + q_\mu \mu_i^2 \\ & + q_q q_{\mu,i}^2 + q_{\Delta u} \rho_i (u_i-u_{i,\mathrm{prev}})^2 + Ru_i^2 \Big) + J_N \end{aligned} J=i=0∑N−1(qtet,i2+qχeχ,i2+qμμi2+qqqμ,i2+qΔuρi(ui−ui,prev)2+Rui2)+JN
其中：
ρi=(N−iN−1)2 \rho_i =\left( \frac{N-i}{N-1} \right)^2 ρi=(N−1N−i)2
用于使 prediction horizon 前部的 previous-horizon deviation penalty 更强，而后部逐渐减弱。
8. Control Blocking
由于直接使用：
N=80 N=80 N=80
意味着需要优化 80 个独立控制变量，对于 fminsearch 来说计算量较大，因此采用 control blocking。
令：
M≪N M\ll N M≪N
实际优化：
z=z1z2⋯zMT \mathbf{z} =\begin{bmatrix} z_1 & z_2 & \cdots & z_M \end{bmatrix}^T z=z1z2⋯zMT
然后通过：
uj=μmaxtanh(zj) u_j =\mu_{\max} \tanh(z_j) uj=μmaxtanh(zj)
得到满足约束的 bank-angle reference。
因此自动满足：
−μmax≤uj≤μmax -\mu_{\max} \le u_j \le \mu_{\max} −μmax≤uj≤μmax
程序中：
N=40⇒M=7 N=40 \Rightarrow M=7 N=40⇒M=7
N=80⇒M=10 N=80 \Rightarrow M=10 N=80⇒M=10
9. Model Mismatch
控制器使用：
(b0,a1,a0) (b_0,a_1,a_0) (b0,a1,a0)
构建 prediction model。
真实 plant 使用略有不同的：
(b0p,a1p,a0p) (b_0^p,a_1^p,a_0^p) (b0p,a1p,a0p)
因此：
fmodel≠fplant \boxed{ f_{\mathrm{model}} \neq f_{\mathrm{plant}} } fmodel=fplant
该设置用于模拟真实系统中的 system identification error。
10. 外部扰动
在飞行过程中给 bank-rate 注入一次扰动：
qμ←qμ+Δqμ q_\mu \leftarrow q_\mu + \Delta q_\mu qμ←qμ+Δqμ
然后观察：
qμ→μ→eχ→et q_\mu \rightarrow \mu \rightarrow e_\chi \rightarrow e_t qμ→μ→eχ→et
的扰动传播，以及 NMPC 的恢复过程。
11. Receding Horizon Principle
每一次 NMPC 优化都会求出：
U⋆=u0⋆u1⋆⋯uN−1⋆T \mathbf{U}^\star =\begin{bmatrix} u_0^\star & u_1^\star & \cdots & u_{N-1}^\star \end{bmatrix}^T U⋆=u0⋆u1⋆⋯uN−1⋆T
但系统只执行：
u(tk)=u0⋆ \boxed{ u(t_k) =u_0^\star } u(tk)=u0⋆
然后重新获得状态：
xk+1 \mathbf{x}_{k+1} xk+1
再次进行预测和优化。
所以完整闭环为：
xk→Prediction→Optimization→U⋆→u0⋆→xk+1→Repeat \boxed{ \mathbf{x}k \rightarrow \text{Prediction} \rightarrow \text{Optimization} \rightarrow \mathbf{U}^\star \rightarrow u_0^\star \rightarrow \mathbf{x}{k+1} \rightarrow \text{Repeat} } xk→Prediction→Optimization→U⋆→u0⋆→xk+1→Repeat
12. 实验结果应重点观察什么
建议重点比较 N=40N=40N=40 与 N=80N=80N=80 的以下指标：
| 指标 | 作用 |
|---|---|
| RMS ete_tet | 整体 lateral tracking performance |
| Max $ | e_t |
| RMS eχe_\chieχ | ground-course tracking ability |
| μ\muμ | 实际姿态变化 |
| μr\mu_rμr | NMPC 控制动作 |
| μ˙r\dot{\mu}_rμ˙r | 控制输入变化率 |
| solver time | 在线计算成本 |
| prediction trajectory | 对未来轨迹的预测能力 |
| segment switching | 对未来参考路径变化的处理能力 |
| wind estimate error | 对环境不确定性的敏感程度 |
整个复杂实验最终考察的是：
NMPC+Path Switching+Wind Disturbance+State/Environment Estimation Error+Model Mismatch+Input Constraints \boxed{ \text{NMPC} + \text{Path Switching} + \text{Wind Disturbance} + \text{State/Environment Estimation Error} + \text{Model Mismatch} + \text{Input Constraints} } NMPC+Path Switching+Wind Disturbance+State/Environment Estimation Error+Model Mismatch+Input Constraints
相比简单圆轨迹实验，这更接近真实固定翼 UAV 在动态环境中的路径跟踪问题。