简介面向自主水下航行器AUV定位控制研究学习者本资源提供基于线性参数变化LPV与模型预测控制MPC的Matlab实现方案能够解决AUV在水下复杂环境中定位控制与速度形式优化的时变参数控制问题并适用于计算机、电子信息工程、数学等专业课程设计、期末大作业及毕业设计场景。资源包共11个文件以8个.m主程序为核心辅以1个.py辅助脚本、1个.txt说明和1个.md文档压缩包整体仅12KB结构紧凑便于快速部署。已有49人学习下载。代码采用参数化编程参数可方便更改注释详细清晰并兼容Matlab 2014/2019a/2024a等多个版本案例程序可直接运行便于读者对照LPV-MPC算法流程、理解预测模型构建与求解器调用方式也可作为AUV控制方向进一步研究与二次开发起点。该实现兼顾理论推导与工程实践能够帮助读者从仿真层面直观掌握LPV-MPC的在线优化过程。1. AUV定位控制里为什么LPV-MPC要写成“速度形式”自主水下航行器的定位控制实际工程里最难的不是把位置算准而是让推进器在保持位置的同时不抖、不撞饱和、不因海流突变而失稳。常规PID在固定工作点调好之后一旦艏向角变化或者海流流速翻倍增益余量就不再匹配。LPV-MPC把非线性AUV模型按调度变量展开成线性参数变化模型在每个采样周期在线求解带约束的最优控制问题速度形式则把决策变量从推力u换成推力增量Δu让控制器自带积分作用并直接约束推力变化率。本文把这个方案从模型构造、MATLAB实现到参数整定完整讲透适合已经做过PID想转MPC的工程师也适合刚接触动力定位控制的研究生快速落地一套可仿真的定位控制器。2. 速度形式LPV-MPC的模型构造与离散化2.1 水平面AUV动力学中的调度变量选择AUV在水平面的三自由度动力学方程通常写作M \dotν C(ν)ν D(ν)ν τ其中ν [u, v, r]^T是载体坐标系下的纵荡速度、横荡速度和艏摇角速度C(ν)是科氏力矩阵D(ν)是水动力阻尼τ是推进器产生的广义力。直接拿这个非线性模型做MPC每个采样周期都要处理非线性约束求解时延不可控而且没有线性的可达性保证。LPV方法的思路是把上述非线性模型改写成\dotν A(θ)ν Bτ其中调度变量θ必须是可在线测量的并且能反映系统动态变化。对自主水下航行器定位控制来说最自然的调度变量是合成速度U sqrt((u - V_cx)² (v - V_cy)²)和艏摇角速度r。注意这里用的是相对海流的速度不是载体对地的绝对速度。海流V_c常被当作外部扰动但如果调度变量取绝对速度模型在海流变化时失配会很严重。把科氏力和阻尼矩阵在当前工作点冻结得到A(θ) -M⁻¹(C(ν₀) D(ν₀))B M⁻¹θ [U₀, r₀]^T这里θ₀代表当前测量到的合成速度和艏摇速度。LPV和简单变增益控制的区别在于变增益只切换控制器增益而LPV-MPC切换的是整个预测模型每一个采样周期都用当前最接近工作点的线性模型做滚动优化。调度变量的合理取值区间必须覆盖实际作业范围超出网格范围时需要通过外扩模型处理不能直接外推。2.2 速度形式预测模型的状态扩展位置形式MPC的模型是x(k1)Ad x(k)Bd u(k)决策变量是u。问题在于AUV的推进器接受的是“推力增量”指令且系统存在常值海流扰动时位置形式MPC会留下稳态误差除非另外加积分器。速度形式的核心是把决策变量换成Δu(k)u(k)-u(k-1)同时把u(k-1)扩为状态量。扩展后的状态定义为ξ(k) [x(k); u(k-1)]对应的离散模型为ξ(k1) [Ad Bd; 0 I] ξ(k) [Bd; I] Δu(k)其中I是单位矩阵。这个写法让MPC的预测方程里带上上一时刻的推力优化过程会连续调整Δu来消除误差原理上等同于在控制律中内嵌了一个积分环节。离散化采用零阶保持器采样时间在AUV动力定位中一般取0.1s到0.5s。采样时间太小会放大传感器噪声太大会丢失动态实测中0.2s是大部分浅水AUV的折中选择。% 连续LPV模型由调度变量实时生成 sys_c ss(Acont, Bcont, eye(nx), 0); sys_d c2d(sys_c, Ts, zoh); % 零阶保持离散化 Ad sys_d.A; Bd sys_d.B; % 速度形式状态扩展 n_x size(Ad, 1); n_u size(Bd, 2); Ae [Ad, Bd; zeros(n_u, n_x), eye(n_u)]; Be [Bd; eye(n_u)]; Ce eye(n_x n_u);这里的Ae和Be就是滚动优化所用的扩展模型。注意扩展之后的n_u维度与推进器数量一致对于水平面三自由度模型通常是3个推进器或2个主推加1个艏侧推。矩阵块左上角的Ad与右上角的Bd对应一条从上一时刻控制量到当前状态变化的路径这是积分作用的来源。2.3 代价函数与约束的写入方式速度形式MPC的代价函数写成J Σ (y - y_ref)ᵀ Q (y - y_ref) Σ Δuᵀ R Δu Σ uᵀ S u其中Q对应定位误差权重R对应推力变化率权重S对应推力绝对值权重。三个权重各管一件事Q决定位置保持的优先级R抑制不必要的动作S防止推力在饱和边界附近抖动。约束分两类都必须在优化问题中显式写出。推力限幅约束是umin ≤ u(ki) ≤ umax因为u已经进入扩展状态这个约束可以直接从ξ中取出。推力变化率约束是Δumin ≤ Δu(ki) ≤ Δumax它直接约束决策变量这正是速度形式相比位置形式的结构性优势。位置形式如果想限变化率需要额外引入一阶惯性环节或对u做差分再约束不仅增加状态维度还会造成约束保守性过大。对比项速度形式位置形式决策变量Δuu积分作用状态扩展自带需外部积分器推力变化率约束直接约束Δu需差分后间接约束稳态误差结构性抑制存在偏置求解变量维度多一块u状态较少典型场景AUV动力定位、气象扰动抵抗轨迹跟踪用MATLAB的quadprog或OSQP求解时代价函数要转成标准二次型。工程上建议用YALMIP或者OSQP直接搭避免手推H矩阵时出错。% 用YALMIP描述速度形式MPC u_prev sdpvar(n_u, 1); % 上一时刻推力 du sdpvar(n_u, Nc); % 决策变量: 推力增量 u zeros(n_u, Nc); u(:,1) u_prev du(:,1); constraints [umin u(:,1) umax, dumin du(:,1) dumax]; objective 0; for i 2:Nc u(:,i) u(:,i-1) du(:,i); constraints [constraints, umin u(:,i) umax, dumin du(:,i) dumax]; end这段代码演示了Nc步控制时域内的变量展开方式。第i步的推力等于上一步推力加当前增量约束同时作用于推力绝对值与增量。代价部分还需要把状态预测误差加进去这里省略了状态更新以突出“速度形式”的变量链式关系。实际使用时控制时域Nc一般取3到8控制增量步数过大会让优化问题维度过高且动作过于激进。3. 在MATLAB/Simulink中搭建速度形式LPV-MPC3.1 水平面模型矩阵与参数初始化搭建仿真环境时被控对象可以用完整的非线性模型控制器内部用冻结LPV模型这样能模拟模型失配。下面这组参数对应一台中等体积的浅水AUV质量约30kg最大航速约2m/s。% AUV水平面标称参数示例值可按实际船模替换 m 30; % 质量 I_z 3.6; % 艏摇转动惯量 Xu -3.5; Yv -6; Nr -3.8; % 线性阻尼系数 M diag([m, m, I_z]); % 简化质量矩阵 D diag([Xu, Yv, Nr]); % 线性阻尼矩阵 % 海流环境参数 Vcx 0.3; Vcy 0.1; % 大地坐标系下的海流分量 % 控制参数 Ts 0.2; % 采样周期 0.2s Np 30; % 预测时域 Nc 5; % 控制时域M取恒定矩阵的合理性在于定位控制中AUV深度由垂直面控制器单独保持水平面运动对附加质量的影响很小。如果仿真中需要更精细的效果可以在M中加入速度相关的附加质量项但那样调度变量还要额外增加一个维度。阻尼系数和转动惯量这些参数可以从CFD或自由衰减试验获得博客场景下先用标称值完全够用。真正影响定位控制效果的是阻尼与质量矩阵的相对比例不必追求绝对精确。3.2 调度网格与矩阵插值将调度变量U和r分别取网格点。U在0到2 m/s之间取0、1、2三个点r在-0.5到0.5 rad/s之间取-0.5、0、0.5三个点形成9个局部模型。每个采样周期根据当前调度变量对系统矩阵做插值。% 预计算9个离散模型 theta1 [0 1 2]; theta2 [-0.5 0 0.5]; n_theta1 numel(theta1); n_theta2 numel(theta2); Ad_cell cell(n_theta1, n_theta2); Bd_cell cell(n_theta1, n_theta2); for i 1:n_theta1 for j 1:n_theta2 % 冻结工作点构造连续LPV矩阵 Acont -M \ (C_matrix(theta1(i), theta2(j)) D); Bcont inv(M); sys_d c2d(ss(Acont, Bcont, eye(3), 0), Ts, zoh); Ad_cell{i,j} sys_d.A; Bd_cell{i,j} sys_d.B; end end % 在线插值按矩阵元素插值而非对象插值 function A_interp interp_matrix(Ad_cell, U, r, theta1, theta2) A_interp zeros(size(Ad_cell{1,1})); for row 1:size(A_interp, 1) for col 1:size(A_interp, 2) mat cellfun((Ad) Ad(row,col), Ad_cell); mat reshape(mat, numel(theta1), numel(theta2)); A_interp(row,col) interp2(theta2, theta1, mat, r, U, linear); end end end这里展示的是逐元素插值直观但效率不高。实际项目中可先把9个模型的所有元素排列成三维数组再用interp2做批量插值避免逐元素循环。插值之后要检查A矩阵的特征值如果插值点靠近网格边界出现了负阻尼特征值说明网格间距太大或边界模型不够稳定需要加密网格。在线插值用的是滤波后的调度变量不要直接使用原始传感器信号。这一点在第4章会详细展开。3.3 用OSQP求解滚动优化并输出推力增量在线求解部分采用OSQP求解器它的优势是支持热启动和矩阵稀疏结构。速度形式MPC相当于在每一个采样周期求解一个中规模QP预测时域Np取30、控制时域Nc取5时决策变量维度只有15OSQP可以在1ms左右完成求解在0.2s采样周期下留有充足裕量。% 初始化求解器只做一次 persistent prob if isempty(prob) prob osqp; end % 每个采样周期更新代价和约束 % H: 二次项矩阵, f: 一次项向量, Aineq, l, u: 不等式约束 prob.setup(H, f, Aineq, l, u, warm_starting, true, verbose, 0); res prob.solve(); % 检查求解状态非最优解时降级处理 if res.info.status_val ~ 1 du zeros(n_u, 1); % 输出零增量保持上一时刻推力 else du_seq reshape(res.x, n_u, Nc); du du_seq(:, 1); end % 关键步骤推力通过累加得到 u_current u_previous du;代码里有三个容易踩坑的点。第一个是persistentOSQP对象在Simulink的MATLAB Function里必须声明为persistent否则每个仿真步重建求解器耗时增加十倍以上。第二个是warm_starting必须开启相邻两个采样周期的解变化很小热启动可以显著减少迭代次数。第三个是求解失败的处理不能把上次结果继续输出而应输出零增量并报警否则控制器会带着失配的预测模型继续工作。推力更新采用u_current u_previous du这就是标题中“速度形式”的直接体现。推力指令被当作控制量的积分MPC只给出增量。这个累加环节放在控制器本体里不在执行机构里意味着需要对推进器零点偏置做标定否则累加出来的推力会一直偏。仿真中接收推力的推进器模型是连续域积分与离散累加器的零点一致性也是验证重点。4. 预测时域、权重与调度信号滤波的调参要点4.1 Np、Nc、Q、R、S的初始基准与标称化速度形式LPV-MPC的参数整定不能直接用位置形式的经验值。下面给出针对AUV定位控制的一组初始范围。参数初始范围整定方向Np20 ~ 50覆盖定位响应5到10s对应5到10个采样周期Nc3 ~ 8过大会导致控制量剧烈变化Q位置权重1000级艏向100级位置误差优先R0.1 ~ 1对应推力变化率抑制强度S0.01 ~ 0.1倍的R抑制推力高频抖动位置权重比艏向高一个数量级是定位控制的基本设定。自主水下航行器在执行定点观测任务时位置误差直接影响数据质量艏向只要不持续偏转通常可以容忍。如果任务需要同时控制艏向再把Qψ调上去。R和S的单位在速度形式下与位置形式明显不同。位置形式的R惩罚的是推力大小速度形式的R惩罚的是每秒推力变化量。直接照搬位置形式权重会导致变化率被压制得过死表现为定位响应迟钝。解决方法是先标称化处理位置误差除以定位精度指标推力除以最大推力然后权重在各个标称变量的同一量级上调整。4.2 饱和约束与变化率约束的兼容性问题仿真中常看到一种现象定位误差还很大但推力已经顶在饱和值上同时在饱和点附近Δu持续向反方向波动。原因是优化求解时同时写了推力上下限和变化率上下限当推力到达上限、误差仍然要求更大推力时MPC只能输出零增量解停在边界上而S项又会试图把推力拉回来造成齿形抖动。判断这个问题可以观察优化解对应的KKT余量工程上更简单的方法是画出推力曲线和Δu曲线。如果推力在一段时间内等幅贴边且Δu在正负间快速切换说明S权重不够需要把S从0.01R逐步提到0.1R。另一个处理手段是给末端松弛变量加大权重。预测时域末端的状态误差会通过代价函数“倒逼”前端的Δu如果末端没有松弛整个预测序列可能为了满足远端的几何约束而牺牲近端动作这在速度形式中尤其明显。在OSQP的约束向量中把末端状态约束的松弛权重设为其他约束权重的10倍以上。4.3 调度变量一阶滤波模型切换与失配的折中调度变量直接来自传感器。艏摇角速度r在海洋环境中噪声和高频分量很大如果直接拿测量值代入LPV模型相邻两个采样周期的模型矩阵可能差别很大预测序列在每一步的模型基准都不一致控制品质会明显恶化。工程中几乎必做的一步是给调度变量加一阶低通滤波有时还要叠加限速环节。% 调度变量一阶低通滤波 persistent theta_f if isempty(theta_f) theta_f theta; % 初始值直接采用第一个测量值 end alpha 0.2; % 滤波系数Ts0.2s时对应约1s时间常数 theta_f alpha * theta (1 - alpha) * theta_f; % 插值时使用 theta_f 而不是 theta A_interp interp_matrix(Ad_cell, theta_f(1), theta_f(2), theta1, theta2);alpha的取值要和采样周期放在一起看。Ts0.2s、alpha0.2时等效滤波时间常数约1s对AUV动力定位来说可以接受。alpha太大会让模型快速切换控制器对噪声敏感alpha太小会滤掉海流突变等真实动态表现为对阶跃海流的响应慢半拍。调度滤波的整定应该排在Q和R之前因为模型匹配性决定了后续优化是否建立在一个可靠的基础上。滤波还会影响推进器负载的评估如果调度变量中的合成速度U有延迟模型中的阻尼项会偏小MPC预测跟随误差变乐观。实际项目中可以把高频调度信号与滤波后信号的差值作为诊断信号差值长期超标说明滤波深度不足或者传感器噪声异常。5. 用推力活动度与误差指标验证速度形式LPV-MPC的边界5.1 三组基准试验与失效判据验证一套速度形式LPV-MPC是否合格建议跑三组基准试验而不是只测一组工况。第一组是海流常值0.4m/s、方向从0°到90°每60秒阶跃重点观察位置误差是否在预测时域内收敛。第二组是海流流速在0.2到0.8m/s之间正弦变化周期2分钟观察调度变量滤波器相位延迟对定位误差的影响。第三组是在艏摇角速度r上叠加幅值0.05rad/s的高频噪声观察推力是否出现采样周期量级的抖动。% 仿真结束后统计关键指标 e_rms rms(error_position.signals.values); % 定位误差RMS sat_ratio sum(abs(tau_data) tau_max - 1e-3) / length(tau_data); qp_fail sum(exit_flag ~ 1); % OSQP非最优解次数定位误差RMS超过设定作业精度的1.5倍时优先检查调度网格覆盖范围。如果海流方向变化时qp_fail次数明显增多说明调度变量落在了网格边界甚至网格外矩阵插值变成了外推。处理办法是把调度网格最外层各加一圈重复模型让插值在边界处退化为最近邻模型避免外推产生负阻尼特征值。5.2 推力活动度与PID输出对比最后给一个可复用的验证技巧用推力活动度判定S权重和预测时域是否匹配。推力活动度定义为act Σ|Δu| / Σ|u|这个指标在速度形式MPC里特别有效因为Δu本来就是决策变量。act过大说明推进器动作频繁机械磨损严重act过小则说明动作被权重压得太死定位误差必然偏大。把act随Np增大画出来会出现一个膝盖点Np取在膝盖点右侧10%到20%处是比较理想的选择因为继续增大Np对减少活动度帮助不大反而增加计算量。对比PID时可以同时记录绝对误差积分IAE。跑完三组试验后用下面的方式快速计算l1_error trapz(t, abs(error_position.signals.values));速度形式LPV-MPC的IAE应比调好的PID小30%以上。如果IAE反而更大先检查调度滤波alpha是不是把动态压过头再把S权重调低0.3倍看响应。推力活动度与IAE的比值还可以作为执行机构负荷效率的评价指标用于与其他控制方案做横向对比。验证时还要记录每个采样周期的求解耗时。OSQP热启动状态下单次求解耗时通常小于采样周期的5%。如果偶发耗时会跳变到采样周期的20%以上需要降低Nc而不是Np因为Nc直接决定决策变量维度对求解耗时影响更大。把耗时临时变量导出到MATLAB工作空间仿真结束后绘制耗时曲线出现尖峰的位置往往就是调度变量快速穿过网格边界的位置这类位置要作为调度滤波参数调整的重点。本文还有配套的精品资源点击获取 AUV动力定位中的速度形式LPV-MPC：从模型构造到MATLAB实现
简介面向自主水下航行器AUV定位控制研究学习者本资源提供基于线性参数变化LPV与模型预测控制MPC的Matlab实现方案能够解决AUV在水下复杂环境中定位控制与速度形式优化的时变参数控制问题并适用于计算机、电子信息工程、数学等专业课程设计、期末大作业及毕业设计场景。资源包共11个文件以8个.m主程序为核心辅以1个.py辅助脚本、1个.txt说明和1个.md文档压缩包整体仅12KB结构紧凑便于快速部署。已有49人学习下载。代码采用参数化编程参数可方便更改注释详细清晰并兼容Matlab 2014/2019a/2024a等多个版本案例程序可直接运行便于读者对照LPV-MPC算法流程、理解预测模型构建与求解器调用方式也可作为AUV控制方向进一步研究与二次开发起点。该实现兼顾理论推导与工程实践能够帮助读者从仿真层面直观掌握LPV-MPC的在线优化过程。1. AUV定位控制里为什么LPV-MPC要写成“速度形式”自主水下航行器的定位控制实际工程里最难的不是把位置算准而是让推进器在保持位置的同时不抖、不撞饱和、不因海流突变而失稳。常规PID在固定工作点调好之后一旦艏向角变化或者海流流速翻倍增益余量就不再匹配。LPV-MPC把非线性AUV模型按调度变量展开成线性参数变化模型在每个采样周期在线求解带约束的最优控制问题速度形式则把决策变量从推力u换成推力增量Δu让控制器自带积分作用并直接约束推力变化率。本文把这个方案从模型构造、MATLAB实现到参数整定完整讲透适合已经做过PID想转MPC的工程师也适合刚接触动力定位控制的研究生快速落地一套可仿真的定位控制器。2. 速度形式LPV-MPC的模型构造与离散化2.1 水平面AUV动力学中的调度变量选择AUV在水平面的三自由度动力学方程通常写作M \dotν C(ν)ν D(ν)ν τ其中ν [u, v, r]^T是载体坐标系下的纵荡速度、横荡速度和艏摇角速度C(ν)是科氏力矩阵D(ν)是水动力阻尼τ是推进器产生的广义力。直接拿这个非线性模型做MPC每个采样周期都要处理非线性约束求解时延不可控而且没有线性的可达性保证。LPV方法的思路是把上述非线性模型改写成\dotν A(θ)ν Bτ其中调度变量θ必须是可在线测量的并且能反映系统动态变化。对自主水下航行器定位控制来说最自然的调度变量是合成速度U sqrt((u - V_cx)² (v - V_cy)²)和艏摇角速度r。注意这里用的是相对海流的速度不是载体对地的绝对速度。海流V_c常被当作外部扰动但如果调度变量取绝对速度模型在海流变化时失配会很严重。把科氏力和阻尼矩阵在当前工作点冻结得到A(θ) -M⁻¹(C(ν₀) D(ν₀))B M⁻¹θ [U₀, r₀]^T这里θ₀代表当前测量到的合成速度和艏摇速度。LPV和简单变增益控制的区别在于变增益只切换控制器增益而LPV-MPC切换的是整个预测模型每一个采样周期都用当前最接近工作点的线性模型做滚动优化。调度变量的合理取值区间必须覆盖实际作业范围超出网格范围时需要通过外扩模型处理不能直接外推。2.2 速度形式预测模型的状态扩展位置形式MPC的模型是x(k1)Ad x(k)Bd u(k)决策变量是u。问题在于AUV的推进器接受的是“推力增量”指令且系统存在常值海流扰动时位置形式MPC会留下稳态误差除非另外加积分器。速度形式的核心是把决策变量换成Δu(k)u(k)-u(k-1)同时把u(k-1)扩为状态量。扩展后的状态定义为ξ(k) [x(k); u(k-1)]对应的离散模型为ξ(k1) [Ad Bd; 0 I] ξ(k) [Bd; I] Δu(k)其中I是单位矩阵。这个写法让MPC的预测方程里带上上一时刻的推力优化过程会连续调整Δu来消除误差原理上等同于在控制律中内嵌了一个积分环节。离散化采用零阶保持器采样时间在AUV动力定位中一般取0.1s到0.5s。采样时间太小会放大传感器噪声太大会丢失动态实测中0.2s是大部分浅水AUV的折中选择。% 连续LPV模型由调度变量实时生成 sys_c ss(Acont, Bcont, eye(nx), 0); sys_d c2d(sys_c, Ts, zoh); % 零阶保持离散化 Ad sys_d.A; Bd sys_d.B; % 速度形式状态扩展 n_x size(Ad, 1); n_u size(Bd, 2); Ae [Ad, Bd; zeros(n_u, n_x), eye(n_u)]; Be [Bd; eye(n_u)]; Ce eye(n_x n_u);这里的Ae和Be就是滚动优化所用的扩展模型。注意扩展之后的n_u维度与推进器数量一致对于水平面三自由度模型通常是3个推进器或2个主推加1个艏侧推。矩阵块左上角的Ad与右上角的Bd对应一条从上一时刻控制量到当前状态变化的路径这是积分作用的来源。2.3 代价函数与约束的写入方式速度形式MPC的代价函数写成J Σ (y - y_ref)ᵀ Q (y - y_ref) Σ Δuᵀ R Δu Σ uᵀ S u其中Q对应定位误差权重R对应推力变化率权重S对应推力绝对值权重。三个权重各管一件事Q决定位置保持的优先级R抑制不必要的动作S防止推力在饱和边界附近抖动。约束分两类都必须在优化问题中显式写出。推力限幅约束是umin ≤ u(ki) ≤ umax因为u已经进入扩展状态这个约束可以直接从ξ中取出。推力变化率约束是Δumin ≤ Δu(ki) ≤ Δumax它直接约束决策变量这正是速度形式相比位置形式的结构性优势。位置形式如果想限变化率需要额外引入一阶惯性环节或对u做差分再约束不仅增加状态维度还会造成约束保守性过大。对比项速度形式位置形式决策变量Δuu积分作用状态扩展自带需外部积分器推力变化率约束直接约束Δu需差分后间接约束稳态误差结构性抑制存在偏置求解变量维度多一块u状态较少典型场景AUV动力定位、气象扰动抵抗轨迹跟踪用MATLAB的quadprog或OSQP求解时代价函数要转成标准二次型。工程上建议用YALMIP或者OSQP直接搭避免手推H矩阵时出错。% 用YALMIP描述速度形式MPC u_prev sdpvar(n_u, 1); % 上一时刻推力 du sdpvar(n_u, Nc); % 决策变量: 推力增量 u zeros(n_u, Nc); u(:,1) u_prev du(:,1); constraints [umin u(:,1) umax, dumin du(:,1) dumax]; objective 0; for i 2:Nc u(:,i) u(:,i-1) du(:,i); constraints [constraints, umin u(:,i) umax, dumin du(:,i) dumax]; end这段代码演示了Nc步控制时域内的变量展开方式。第i步的推力等于上一步推力加当前增量约束同时作用于推力绝对值与增量。代价部分还需要把状态预测误差加进去这里省略了状态更新以突出“速度形式”的变量链式关系。实际使用时控制时域Nc一般取3到8控制增量步数过大会让优化问题维度过高且动作过于激进。3. 在MATLAB/Simulink中搭建速度形式LPV-MPC3.1 水平面模型矩阵与参数初始化搭建仿真环境时被控对象可以用完整的非线性模型控制器内部用冻结LPV模型这样能模拟模型失配。下面这组参数对应一台中等体积的浅水AUV质量约30kg最大航速约2m/s。% AUV水平面标称参数示例值可按实际船模替换 m 30; % 质量 I_z 3.6; % 艏摇转动惯量 Xu -3.5; Yv -6; Nr -3.8; % 线性阻尼系数 M diag([m, m, I_z]); % 简化质量矩阵 D diag([Xu, Yv, Nr]); % 线性阻尼矩阵 % 海流环境参数 Vcx 0.3; Vcy 0.1; % 大地坐标系下的海流分量 % 控制参数 Ts 0.2; % 采样周期 0.2s Np 30; % 预测时域 Nc 5; % 控制时域M取恒定矩阵的合理性在于定位控制中AUV深度由垂直面控制器单独保持水平面运动对附加质量的影响很小。如果仿真中需要更精细的效果可以在M中加入速度相关的附加质量项但那样调度变量还要额外增加一个维度。阻尼系数和转动惯量这些参数可以从CFD或自由衰减试验获得博客场景下先用标称值完全够用。真正影响定位控制效果的是阻尼与质量矩阵的相对比例不必追求绝对精确。3.2 调度网格与矩阵插值将调度变量U和r分别取网格点。U在0到2 m/s之间取0、1、2三个点r在-0.5到0.5 rad/s之间取-0.5、0、0.5三个点形成9个局部模型。每个采样周期根据当前调度变量对系统矩阵做插值。% 预计算9个离散模型 theta1 [0 1 2]; theta2 [-0.5 0 0.5]; n_theta1 numel(theta1); n_theta2 numel(theta2); Ad_cell cell(n_theta1, n_theta2); Bd_cell cell(n_theta1, n_theta2); for i 1:n_theta1 for j 1:n_theta2 % 冻结工作点构造连续LPV矩阵 Acont -M \ (C_matrix(theta1(i), theta2(j)) D); Bcont inv(M); sys_d c2d(ss(Acont, Bcont, eye(3), 0), Ts, zoh); Ad_cell{i,j} sys_d.A; Bd_cell{i,j} sys_d.B; end end % 在线插值按矩阵元素插值而非对象插值 function A_interp interp_matrix(Ad_cell, U, r, theta1, theta2) A_interp zeros(size(Ad_cell{1,1})); for row 1:size(A_interp, 1) for col 1:size(A_interp, 2) mat cellfun((Ad) Ad(row,col), Ad_cell); mat reshape(mat, numel(theta1), numel(theta2)); A_interp(row,col) interp2(theta2, theta1, mat, r, U, linear); end end end这里展示的是逐元素插值直观但效率不高。实际项目中可先把9个模型的所有元素排列成三维数组再用interp2做批量插值避免逐元素循环。插值之后要检查A矩阵的特征值如果插值点靠近网格边界出现了负阻尼特征值说明网格间距太大或边界模型不够稳定需要加密网格。在线插值用的是滤波后的调度变量不要直接使用原始传感器信号。这一点在第4章会详细展开。3.3 用OSQP求解滚动优化并输出推力增量在线求解部分采用OSQP求解器它的优势是支持热启动和矩阵稀疏结构。速度形式MPC相当于在每一个采样周期求解一个中规模QP预测时域Np取30、控制时域Nc取5时决策变量维度只有15OSQP可以在1ms左右完成求解在0.2s采样周期下留有充足裕量。% 初始化求解器只做一次 persistent prob if isempty(prob) prob osqp; end % 每个采样周期更新代价和约束 % H: 二次项矩阵, f: 一次项向量, Aineq, l, u: 不等式约束 prob.setup(H, f, Aineq, l, u, warm_starting, true, verbose, 0); res prob.solve(); % 检查求解状态非最优解时降级处理 if res.info.status_val ~ 1 du zeros(n_u, 1); % 输出零增量保持上一时刻推力 else du_seq reshape(res.x, n_u, Nc); du du_seq(:, 1); end % 关键步骤推力通过累加得到 u_current u_previous du;代码里有三个容易踩坑的点。第一个是persistentOSQP对象在Simulink的MATLAB Function里必须声明为persistent否则每个仿真步重建求解器耗时增加十倍以上。第二个是warm_starting必须开启相邻两个采样周期的解变化很小热启动可以显著减少迭代次数。第三个是求解失败的处理不能把上次结果继续输出而应输出零增量并报警否则控制器会带着失配的预测模型继续工作。推力更新采用u_current u_previous du这就是标题中“速度形式”的直接体现。推力指令被当作控制量的积分MPC只给出增量。这个累加环节放在控制器本体里不在执行机构里意味着需要对推进器零点偏置做标定否则累加出来的推力会一直偏。仿真中接收推力的推进器模型是连续域积分与离散累加器的零点一致性也是验证重点。4. 预测时域、权重与调度信号滤波的调参要点4.1 Np、Nc、Q、R、S的初始基准与标称化速度形式LPV-MPC的参数整定不能直接用位置形式的经验值。下面给出针对AUV定位控制的一组初始范围。参数初始范围整定方向Np20 ~ 50覆盖定位响应5到10s对应5到10个采样周期Nc3 ~ 8过大会导致控制量剧烈变化Q位置权重1000级艏向100级位置误差优先R0.1 ~ 1对应推力变化率抑制强度S0.01 ~ 0.1倍的R抑制推力高频抖动位置权重比艏向高一个数量级是定位控制的基本设定。自主水下航行器在执行定点观测任务时位置误差直接影响数据质量艏向只要不持续偏转通常可以容忍。如果任务需要同时控制艏向再把Qψ调上去。R和S的单位在速度形式下与位置形式明显不同。位置形式的R惩罚的是推力大小速度形式的R惩罚的是每秒推力变化量。直接照搬位置形式权重会导致变化率被压制得过死表现为定位响应迟钝。解决方法是先标称化处理位置误差除以定位精度指标推力除以最大推力然后权重在各个标称变量的同一量级上调整。4.2 饱和约束与变化率约束的兼容性问题仿真中常看到一种现象定位误差还很大但推力已经顶在饱和值上同时在饱和点附近Δu持续向反方向波动。原因是优化求解时同时写了推力上下限和变化率上下限当推力到达上限、误差仍然要求更大推力时MPC只能输出零增量解停在边界上而S项又会试图把推力拉回来造成齿形抖动。判断这个问题可以观察优化解对应的KKT余量工程上更简单的方法是画出推力曲线和Δu曲线。如果推力在一段时间内等幅贴边且Δu在正负间快速切换说明S权重不够需要把S从0.01R逐步提到0.1R。另一个处理手段是给末端松弛变量加大权重。预测时域末端的状态误差会通过代价函数“倒逼”前端的Δu如果末端没有松弛整个预测序列可能为了满足远端的几何约束而牺牲近端动作这在速度形式中尤其明显。在OSQP的约束向量中把末端状态约束的松弛权重设为其他约束权重的10倍以上。4.3 调度变量一阶滤波模型切换与失配的折中调度变量直接来自传感器。艏摇角速度r在海洋环境中噪声和高频分量很大如果直接拿测量值代入LPV模型相邻两个采样周期的模型矩阵可能差别很大预测序列在每一步的模型基准都不一致控制品质会明显恶化。工程中几乎必做的一步是给调度变量加一阶低通滤波有时还要叠加限速环节。% 调度变量一阶低通滤波 persistent theta_f if isempty(theta_f) theta_f theta; % 初始值直接采用第一个测量值 end alpha 0.2; % 滤波系数Ts0.2s时对应约1s时间常数 theta_f alpha * theta (1 - alpha) * theta_f; % 插值时使用 theta_f 而不是 theta A_interp interp_matrix(Ad_cell, theta_f(1), theta_f(2), theta1, theta2);alpha的取值要和采样周期放在一起看。Ts0.2s、alpha0.2时等效滤波时间常数约1s对AUV动力定位来说可以接受。alpha太大会让模型快速切换控制器对噪声敏感alpha太小会滤掉海流突变等真实动态表现为对阶跃海流的响应慢半拍。调度滤波的整定应该排在Q和R之前因为模型匹配性决定了后续优化是否建立在一个可靠的基础上。滤波还会影响推进器负载的评估如果调度变量中的合成速度U有延迟模型中的阻尼项会偏小MPC预测跟随误差变乐观。实际项目中可以把高频调度信号与滤波后信号的差值作为诊断信号差值长期超标说明滤波深度不足或者传感器噪声异常。5. 用推力活动度与误差指标验证速度形式LPV-MPC的边界5.1 三组基准试验与失效判据验证一套速度形式LPV-MPC是否合格建议跑三组基准试验而不是只测一组工况。第一组是海流常值0.4m/s、方向从0°到90°每60秒阶跃重点观察位置误差是否在预测时域内收敛。第二组是海流流速在0.2到0.8m/s之间正弦变化周期2分钟观察调度变量滤波器相位延迟对定位误差的影响。第三组是在艏摇角速度r上叠加幅值0.05rad/s的高频噪声观察推力是否出现采样周期量级的抖动。% 仿真结束后统计关键指标 e_rms rms(error_position.signals.values); % 定位误差RMS sat_ratio sum(abs(tau_data) tau_max - 1e-3) / length(tau_data); qp_fail sum(exit_flag ~ 1); % OSQP非最优解次数定位误差RMS超过设定作业精度的1.5倍时优先检查调度网格覆盖范围。如果海流方向变化时qp_fail次数明显增多说明调度变量落在了网格边界甚至网格外矩阵插值变成了外推。处理办法是把调度网格最外层各加一圈重复模型让插值在边界处退化为最近邻模型避免外推产生负阻尼特征值。5.2 推力活动度与PID输出对比最后给一个可复用的验证技巧用推力活动度判定S权重和预测时域是否匹配。推力活动度定义为act Σ|Δu| / Σ|u|这个指标在速度形式MPC里特别有效因为Δu本来就是决策变量。act过大说明推进器动作频繁机械磨损严重act过小则说明动作被权重压得太死定位误差必然偏大。把act随Np增大画出来会出现一个膝盖点Np取在膝盖点右侧10%到20%处是比较理想的选择因为继续增大Np对减少活动度帮助不大反而增加计算量。对比PID时可以同时记录绝对误差积分IAE。跑完三组试验后用下面的方式快速计算l1_error trapz(t, abs(error_position.signals.values));速度形式LPV-MPC的IAE应比调好的PID小30%以上。如果IAE反而更大先检查调度滤波alpha是不是把动态压过头再把S权重调低0.3倍看响应。推力活动度与IAE的比值还可以作为执行机构负荷效率的评价指标用于与其他控制方案做横向对比。验证时还要记录每个采样周期的求解耗时。OSQP热启动状态下单次求解耗时通常小于采样周期的5%。如果偶发耗时会跳变到采样周期的20%以上需要降低Nc而不是Np因为Nc直接决定决策变量维度对求解耗时影响更大。把耗时临时变量导出到MATLAB工作空间仿真结束后绘制耗时曲线出现尖峰的位置往往就是调度变量快速穿过网格边界的位置这类位置要作为调度滤波参数调整的重点。本文还有配套的精品资源点击获取