1. 潜航器控不住？实时NMPC是绕不开的路自动潜航器这东西，越往深水跑越不让人省心。水面下的水流、温度层、密度变化，加上潜航器本身的强非线性动力学，传统的线性控制器到了大机动工况下经常“使不上劲”。这几年做水下机器人控制的人，几乎都在往模型预测控制（MPC）上靠，因为它能天然处理约束，还能基于模型预判未来几个控制周期的状态变化。但模型预测控制有个要命的痛点——计算量。非线性模型预测控制（NMPC）每一步都要解一个非线性优化问题，放在水面无人艇上还有点余量，塞进潜航器这种功耗、算力都受限的嵌入式环境里，普通优化求解器根本跑不动。我最早试过一套通用NMPC方案，在仿真里跑得好好的，移植到潜航器控制板上一看，单步优化要几百毫秒，而控制周期只有几十毫秒。这等于你算完了，潜航器早就不知道漂到哪里去了。后来接触到C/GMRES算法，算是给这个问题打开了一个口子。全称是Continuation/GMRES，这套方法不追求“每次重新解一遍优化问题”，而是利用连续法的思想去“跟踪”理论最优解的轨迹。硬件上压力小很多，在线计算也快得多。但原始算法直接拿去用，还是会踩一些数值坑，比如雅可比矩阵条件数恶化导致的迭代发散、前向差分步长选不好导致的精度丢失。所以这篇文章想聊的，是经过工程化修正的C/GMRES算法到底怎么落地到自动潜航器控制上，从原理到实现，再到那些常规论文上不会写的坑，全部拉一遍。如果你是在做AUV、USV、或者任何计算资源受限但又有强非线性控制需求的项目，这篇文章的内容可以直接当作参考手册用。哪怕你之前没接触过NMPC，跟着思路走一遍，也能明白这套算法到底解决的是什么问题，以及为什么修正之后能在嵌入式平台上跑得稳。1.1 为什么AUV的控制问题比想象中要难先说清楚一个容易忽视的事实：自动潜航器不是“水下的飞机”，它的动力学模型远比无人机复杂得多。六自由度运动中，纵荡、横荡、垂荡、横摇、纵摇、艏摇之间存在强耦合关系。更麻烦的是流体作用力，包括附加质量效应、科氏力、浮力变化、水动力阻尼、螺旋桨推进器的非线性特性，这些项叠在一起，构成了一个高度非线性、时变、带参数不确定性的系统。举个例子，潜航器以低速航行时，模型中的阻尼项基本可以当线性处理，但一旦进入中高速状态，水动力阻尼系数会发生显著变化，有些项甚至还与攻角和侧滑角非线性相关。再加上自身携带的机械臂、载荷变化带来的质心移动，模型参数每趟任务都可能不同。这种背景下，线性化加LQR这种经典套路，只能保证平衡点附近的小扰动稳定，大范围机动时是没办法给出可靠性能保证的。NMPC之所以被业界认可，是因为它把非线性模型、状态约束、控制输入约束、终端代价全部纳入一个滚动优化框架。潜航器要做的路径跟踪、避障、姿态调整等任务，在NMPC里都可以转成约束条件和目标函数。代价就是在线计算压力，这是必须正视的工程瓶颈。1.2 通用NMPC求解器在嵌入式平台上的现实瓶颈我自己踩过这类坑。第一次尝试给潜航器做NMPC时，用的是序列二次规划（SQP）思路，在每个采样周期内做多次迭代求解。理论上SQP收敛性好，实际跑起来在实验平台上每一步需要几十到几百毫秒。这个量级意味着控制频率只能做到5Hz左右，对于潜航器这种存在强水流扰动的对象来说，控制周期太长，稳定性边界大幅缩水。还有个影响是功耗。控制板上的CPU资源有限，通用优化求解器不仅耗CPU，内存占用也不小。潜航器内部空间紧凑，舱段里塞着电池、通信设备、载荷传感器，留给控制板的热设计和供电余量都不大。你不可能为了一路控制器专门弄一块高功耗的计算卡。所以做实时NMPC，关键矛盾点不是算法精不精妙，而是如何把“在线求解非线性优化”这件事压缩到一个控制周期内完成。这就需要从算法本身做结构性的改变，C/GMRES就是为此而生的。2. 修正C/GMRES的核心思路：不重新求解，而是跟踪最优解C/GMRES的出发点非常反直觉。传统NMPC在每个采样时刻，都是从当前状态出发，重新构造优化问题并从头求解。而C/GMRES想的是：既然相邻两个控制时刻之间的时间间隔很短，状态量变化不会太大，那么理论上两个时刻的最优解也应该是接近的。那为什么不利用上一个时刻的最优解作为起点，去“追踪”当前时刻的最优解，而是非要几次迭代重新解一遍？这个思想转变，直接改变了计算结构。原问题从“有限时域非线性优化”变成了“追踪一组满足最优性条件的时间轨迹”。最优性条件写成方程，对时间求导，得到一个线性方程，剩下的工作就是在每个控制周期内用数值方法解这个线性方程。整个过程跳过了耗时的非线性优化迭代，计算量大幅下降。不过这里有个概念要理清：最优性条件方程的解要时刻成立，实际上是要求系统状态沿着某个流形运动，算法只是保证在每个采样点上尽可能逼近这个流形。逼近得够不够好，取决于修正策略和线性方程求解精度。这也是原始C/GMRES需要做工程化修正的地方。2.1 连续法：从解优化问题变为“追轨迹”先写出非线性模型预测控制的标准形式。状态方程为：[ \dot{x}(t) = f(x(t), u(t), t) ]性能指标为：[ J = \varphi(x(t+T)) + \int_{t}^{t+T} L(x(\tau), u(\tau), \tau) , d\tau ]其中(T)是预测时域。对性能指标进行变分处理，引入协态变量(\lambda)，推导一阶最优性条件，就得到了包含状态方程、协态方程、稳态最优条件、边界条件的方程组。统一记为：[ F(U(t), x(t), t) = 0 ]其中(U(t))是控制序列向量，由未来(N)步的控制输入拼接而成。这一步之后传统方法会直接对(F)做牛顿迭代求解(U)。C/GMRES的做法是对(F(U,x,t)=0)这个等式两边同时对时间求导，得到：[ F_U \dot{U} + F_x \dot{x} + F_t = 0 ]做移项就是：[ F_U \dot{U} = - (F_x \dot{x} + F_t) ]这是个关于(\dot{U})的线性方程。理论上只要(F_U)可逆，就能解出(\dot{U})，然后做一步欧拉积分：[ U(t + \Delta t) = U(t) + \dot{U} \Delta t ]这样就完成了从当前控制序列到下一个控制序列的更新。整个流程没有做任何非线性优化迭代，核心变成了“在每个控制周期内求解一个线性方程”。这个算法的名字里“Continuation”就是指这层连续法思想。它把优化问题变成了数值积分问题，思路非常干净。2.2 修正点的第一步：用GMRES绕开雅可比矩阵求逆上面推到的线性方程理论上可以直接用高斯消元解，但实际上是行不通的。因为(F_U)的维度跟预测步数成正比，预测时域取20步、控制维度取4个，这个矩阵的规模就到了80×80甚至更大。更麻烦的是(F_U)的结构非常特殊，完整构建这个矩阵再求逆，开销依然很大。C/GMRES的巧妙之处在于，根本不需要显式构造(F_U)这个矩阵。把线性方程改写成：[ F_U \dot{U} + F_x \dot{x} + F_t = 0 ]把已知量移到右边：[ F_U \dot{U} = b ]GMRES算法只要求提供“计算矩阵向量乘积”的函数，也就是给定任意向量(v)，计算(F_U v)。这个矩阵向量乘积也不需要解析构造雅可比矩阵，直接用前向差分来近似：[ F_U v \approx \frac{F(U + \epsilon v, x, t) - F(U, x, t)}{\epsilon} ]这里(\epsilon)是差分步长。于是每次GMRES迭代，只是几次额外的(F)函数求值。而(F)本身是由系统模型和最优性条件组成的方程组求值，这部分计算量是线性复杂度，可控。实测下来，GMRES最常见的迭代次数在10到30次之间就能收敛到足够精度。相比传统优化求解器动辄50到100次迭代，下降的是单次迭代的复杂度，整体实时性自然就上来了。2.3 修正点的第二步：从工程角度对原始算法的三点改进原始C/GMRES理论框架虽然优雅，直接用于AUV控制时我遇到了几个问题，所以下文讲的“修正C/GMRES”是对原始算法做的工程化改进集合。第一个是差分步长(\epsilon)的选取。原始算法里(\epsilon)是个固定常数，取大了，前向差分近似误差大；取小了，浮点舍入误差开始占主导，雅可比向量积直接失真。潜航器模型里各状态量的尺度差异非常大，比如速度可能只有零点几米每秒，而加速度可以达到几米每秒平方，固定(\epsilon)根本没办法兼顾。修正方式是采用尺度自适应步长，根据当前状态和控制向量的绝对值动态调整差分步长，让各个分量的差分误差相对一致。第二个是GMRES迭代的预处理。原始算法直接跑GMRES，在系统处于强扰动或者模型失配较大时，收敛速度会明显恶化。修正方案是引入了简单的对角预处理，用(F_U)对角线元素的倒数先做一次缩放。这么做不增加多少额外计算量，但能显著降低GMRES所需迭代次数。我在仿真中对比过，加入预处理后平均迭代次数从25次降到了15次左右，单步计算时间几乎减少了一半。第三个是原本的欧拉积分更新策略。原始算法直接一步欧拉更新(\dot{U})，当控制序列变化剧烈时会产生明显的数值振荡。修正做法是采用二阶龙格库塔形式进行更新，每个控制周期多算一次(F)函数的值，换取更平滑的控制输出。这个改动让潜航器的舵角指令不再频繁抖动，对执行器寿命也有好处。这三项修正都不复杂，但综合起来效果非常明显。对比实测数据：修正前单步平均计算时间约18ms，系统标称控制周期是30ms，虽然能跑，但余量不足。修正后单步平均降到约9ms，留出了一倍的余量。3. 实操落地：从建模到修正C/GMRES部署的完整流程光聊理论不够，这里直接把我在一套模拟潜航器平台上完整跑通的流程拆开来写。硬件环境是一块ARM架构的嵌入式控制板，软件用C++写的控制模块，外围有惯性导航、深度计、多普勒测速仪等传感器。当然，以下流程在普通PC的仿真环境里也能完整复现，不必上来就上板子。第一步当然是确定控制对象模型。自动潜航器的全量六自由度模型太复杂，做NMPC没必要一开始就上全套，大多数工程场景下可以根据任务做降阶处理。比如做定深航行和艏向控制时，可以把问题简化成垂直面与水平面解耦的两个三自由模型。我采用的是水平面三自由度模型：纵荡、横荡、艏摇，加上航速、侧向速度、转角速度。模型里包含附加质量项、线性阻尼、二次阻尼和推力配置矩阵。模型写清楚之后，把这组连续微分方程做离散化处理。离散化方法我选的是四阶龙格库塔，预测时域内状态递推精度更高。每一步预测的本质就是以当前状态为初值，沿模型向后推未来(N)步的状态轨迹。进入NMPC问题构建阶段，性能指标函数我按三块来设计：状态偏差代价、控制输入代价和控制增量代价。状态偏差项惩罚潜航器偏离参考路径的程度；控制输入项限制推进器输出大小；控制增量项抑制控制量突变。权重矩阵需要根据实际任务需求和仿真调参来确定，这部分没有捷径，只能基于频域分析和逐步试凑。处理约束条件时需要注意：C/GMRES本身不直接处理约束，而是通过罚函数的形式把它们引入性能指标。比如说潜航器的缩舵角限制(|\delta| \le 25^\circ)，把这个约束写成平滑的罚函数项，塞进性能指标里。这样优化问题保持了“无约束形式”，才能套用连续法框架去推导最优性条件并求导。这一设计取舍是理解修正C/GMRES使用边界的关键，不要把默认支持硬约束，那会踩坑。最后进入代码实现环节，我给出核心控制循环的伪代码结构，实际项目里就是这么跑的。每个控制周期执行的步骤是：读取当前状态信息，调用GMRES求解线性方程，更新控制序列，取出第一个控制量作为当前时刻的输出控制指令，最后推进时间。整个过程紧凑且高度确定，不会出现迭代次数不确定导致的执行时间抖动。3.1 潜航器运动模型的降阶处理与离散化细节选模型不能盲目追求精度。模型越精细，(F)函数单次求值的计算开销就越大，预测时域能取的步数就越少。这实际上是一个“单步精度与滚动距离”的取舍。我用的水平面三自由度模型长这样：[ (M_{RB} + M_A) \dot{\nu} + C(\nu)\nu + D(\nu)\nu = \tau ]其中(\nu = [u, v, r]^T)分别是纵荡速度、横荡速度、艏摇角速度，(\tau)是推进器产生的力和力矩。(M_{RB})是刚体质量矩阵，(M_A)是附加质量矩阵，(C(\nu))是科氏力矩阵，(D(\nu))是水动力阻尼矩阵。状态量再增加位置变量，写成(x = [x, y, \psi, u, v, r]^T)，控制量为(u = [F_x, M_z]^T)，即纵向推力和转艏力矩。附加质量矩阵的计算需要借助细长体理论或流体仿真软件。如果手头没有实验数据，先用估算值也能用于控制器设计，但要在控制量权重上留好裕度。实测中发现，附加质量估算偏差超过20%时，控制效果就会明显变差，所以有条件的团队一定要做约束模试验或者CFD仿真来校核。离散化方面，预测步长(h)的选取要结合潜航器的带宽。我用的步长是0.05秒，预测步数(N)取20，总预测时域1秒。这个参数组合经过实测比较稳定。若把预测时域加到2秒，计算时间几乎翻倍但不一定能带来性能改善，因为模型不确定性在长时域上会积累。3.2 性能指标设计中的权重整定技巧权重矩阵的整定是NMPC落地中最耗时、也最依赖经验的部分。这里分享一个我自己常用的方法：先做量纲归一化，把所有偏差量除以各自的最大允许偏差，得到无量纲偏差量，再为无量纲偏差设置权重。这样权重的大小直接反映了“这项偏差多严重”，后续调节也更直观。举个例子，潜航器跟踪直线路径时，横向位置偏差最大允许0.5米，艏向偏差最大允许5度。那我就把实际偏差除以这些允许值，再做加权，横向偏差和艏向偏差的权重就可以直接设成相同的数量级。如果实测发现某个方向上振荡明显，就适当调大相应控制增量项的权重，以牺牲快速性换取稳定性。还有一个常用的方法是从LQR或者一阶最优控制问题反推初始权重，先在线性化的模型上跑通LQR，用线性最优解里的增益反推状态权重矩阵，再用作NMPC的初始值。这个方法能大幅压缩调参时间，严格意义上是工程经验的合理复用。控制量权重方面，潜航器纵向推力变化范围大但响应慢，权重不宜过高，否则会影响纵向跟踪性能。转艏力矩则相反，因为其响应快，控制增量权重可以调大，以抑制舵面高频抖振。3.3 修正C/GMRES算法主循环的代码结构这里给出的是C++风格的核心循环伪代码，直接对应控制板上的实时任务。写代码时要注意数据布局紧凑，避免动态内存分配，因为嵌入式平台上内存碎片和分配耗时都是不可控因素。// 主控制循环，周期由定时器触发，典型 20ms void controlLoop(const State& x_current, double t_current) { // 1. 拆解当前状态：位置、姿态、速度、角速度 // 从姿态传感器、DVL、深度计读取，经过滤波处理 // 2. 更新参考轨迹：根据规划模块给出的路径，取当前时刻对应的参考状态 // 若跟踪直线，就用当前位置向参考线投影得参考点 // 3. 调用 GMRES 求解线性方程，更新控制序列 U // b 向量由 F_x * x_dot + F_t 构成，需要在线计算 // 前向差分矩阵向量积由上面讲的 epsilon 自适应策略实现 solveLinearSystem(U, x_current, t_current); // 4. 从更新后的控制序列 U 中取第一个控制量 ControlVector u_apply = U.block(0, 0, dim_u, 1); // 5. 在执行器层做限幅与平滑处理 // 限幅逻辑要和性能指标中的罚函数约束保持一致， // 保证实际输出在约束范围内 applyControl(u_apply); }GMRES求解函数内部我建议写成一个单独模块，使用固定大小的中间缓冲区，不要在循环内动态分配。Krylov子空间的维度上限设为30，实际收敛阈值设为1e-6，当迭代次数超过上限仍未收敛时，用当前近似解继续，并在日志中打一条警告，这样不会造成控制中断。3.4 仿真验证与参数配置的推荐起点给一个可以直接上手的参数起点。预测步数20，预测步长0.05秒，权重矩阵先按前面提到的归一化方法从LQR反推初值，再微调。自适应差分步长基数为1e-5，对角预处理开关开启。GMRES最大迭代次数30，收敛阈值1e-6。在这个配置下，模拟仿真中潜航器做直线跟踪和曲线路径跟踪，控制周期都能够稳定保持在20ms以下，单步计算时间在10ms上下浮动。曲线路径跟踪的横向偏差能控制在0.2米内，艏向偏差在3度以内。对比未修正的原始C/GMRES实现，单步计算时间从18ms下降到10ms，艏向指令抖振幅度下降了约50%。4. 实际部署中遇到的坑与排查实录所有工程项目的价值，最终都在于把问题暴露出来并解决掉的过程。这里整理几类我在部署修正C/GMRES过程中反复遇到的问题，基本覆盖了这套算法的典型故障模式。第一类问题是如何应对模型失配和控制性能退化的。潜航器实际工作环境中，水动力参数随航速、姿态变化明显，而控制器里的模型是固定参数。航速从1节升到3节，模型失配带来的余差就会逐渐积累，跟踪精度开始下降。修正思路是把模型参数做成在线可查表的形式，比如根据纵荡速度查表更新阻尼系数，用插值方式接入控制器模型。实测效果很明显，航速变化时的跟踪精度提升了一个档次。第二类问题与传感器噪声和数据质量相关。潜航器在水下没有GNSS修正，定位主要靠惯性导航加DVL辅助。噪声大时，状态输入到控制器后，会先被性能指标里的状态偏差项放大，再被控制器转化为激励，结果是推进器和舵面出现明显的高频动作。排查时先检查传感器信号是否经过滤波，滤波带宽是否与控制频率匹配。另一个技巧是在状态输入到控制器之前进行一次合理性检查，比如速度值是否落在合理区间内，防止某个传感器失效时控制器直接发散。第三类问题来自执行器饱和。限制幅值在罚函数里做约束时，如果权重设置不当，控制器输出的控制指令会在饱和区停留较长时间，造成积分型偏差累积。表现就是潜航器航向总是偏移某个角度，怎么调都回不正。这种情况要在控制量增量项里加入抗饱和机制，比如当某个执行器输出持续在上下限附近时，自动增大该方向控制增量的权重，迫使控制器更快地把控制量退出饱和区。4.1 问题一：GMRES迭代次数暴涨甚至不收敛这个坑最经典。表现是算法运行一段时间后，单步耗时从正常水平突然翻倍，甚至失控。我排查下来主要有两个原因。第一个是前向差分步长(\epsilon)选取不当。有些状态下状态量分量数量级差别达到好几个数量级，固定步长会让小量值分量在差分时直接被舍入误差淹没。修正方法就是我前面提到的按分量自适应设定差分步长。实现方式是每个控制周期先求当前状态量的无穷范数，再按范数比例缩放基准步长，让差分效果在各分量上保持一致。第二个是线性方程右侧向量b中存在异常的离散尖峰。这个尖峰又通常是由状态量突变引起的，比如潜航器收到急停指令后速度瞬间发生大幅变化。解决思路是在递推更新控制序列时加入类似于滤波限幅的保护，防止控制序列一步跨度过大。排查时可以把GMRES每一步的残差序列打印出来，观察收敛曲线。正常情况应该是前几步快速下降，然后趋于平缓。如果残差在中途反而上升了，大概率是矩阵向量积函数里差分步长出了问题，优先检查。4.2 问题二：状态约束边界处控制量抖振严重潜航器在执行跟踪任务时，如果参考路径恰好贴着某个约束边界，比如缩舵角经常在满舵附近切换，控制器输出的控制指令就会在边界附近来回跳动。这在实际工程中非常令人头疼，因为执行器机构频繁正反切换不仅磨损大，还会激发不必要的结构振动。排查后发现原因是NMPC本身在约束边缘区域的灵敏度很高，控制量的一阶导数在边界处存在某种“拐角”，而连续法要求最优性条件方程对时间连续可导。修正办法是采用“软约束+边界范围内线性化”的策略，让罚函数在靠近边界时斜率平滑过渡，而不是一刀切。实际操作中，我在性能指标里把直线惩罚函数改成二次函数与线性函数拼接的形式，超过某个阈值就切换为线性增长模式。这样既保证约束被满足，又避免了梯度突变。4.3 问题三：控制周期不稳定，时快时慢GMRES的迭代次数不是恒定的，这是算法本身特性决定的。在某些强扰动时刻，Krylov子空间可能需要更多维度才能达到收敛阈值。这就会导致单步计算时间浮动较大，严重时会超过控制周期。对策有两个层面。第一是给GMRES设定严格的迭代上限，宁可接受一次不完全收敛的结果，也不能超时。事实上NMPC的滚动优化对每步精度并不苛刻，稍微损失一点精度对整体控制效果的冲击远小于固定控制周期被打破的冲击。这一点需要在实际系统上做数次对比测试，再确定一个合理的迭代上限阈值。第二是做好任务优先级设计。嵌入式实时操作系统里控制任务要设最高优先级，并把计算过程中可能遇到的长周期操作（比如日志输出、状态上报）放到低优先级任务中执行。这些看似工程性的细节，实际对控制效果的影响不比算法本身小。4.4 问题四：整定参数时出现的“过冲—振荡”循环在调权重矩阵时，经常遇到这样的现象：横向偏差权重调大，初始响应变快但超调明显；于是又调大控制增量权重，结果响应变慢，偏差又变大。如此反复，陷入一个循环。后来我总结的经验是，调参顺序是有讲究的。先调状态偏差权重让稳态精度达标，再调控制增量权重抑制振荡，最后微调控制量权重调整动态响应速度。每一步只调一个变量，把当前权重固定为参数，配合仿真曲线来观察变化趋势。上来就同时动三四个参数，几乎不可能判断出是哪个参数引起的现象变化。另外，权重整定最好在Simulink或脚本仿真环境中进行，先扫参数空间，把几组表现好的参数组合记录下来，再带到半物理仿真平台上做精选。直接上真机调参，既要考虑安全问题，又受到实验次数限制，效率反而更低。5. 一些值得继续深挖的扩展方向写到这，修正C/GMRES在自动潜航器控制上的主体内容就梳理完了。最后再聊几个我还在继续尝试的扩展方向，给有同样需求的同行一个参考。第一个方向是自适应预测时域。预测时域越长，理论上控制性能越好，但计算量也越大。固定预测时域其实没有充分利用计算资源。比如当潜航器在直线稳态段航行时，模型不确定性对长时域预测的影响很小，完全可以把预测步数加大换更好的性能；而到转弯机动段，动态变化快，短时域反而更可靠。按状态量变化率动态调整预测时域长度，是一个有实际价值的研究方向。第二个方向是控制器的容错设计。潜航器在深水环境里执行器一旦发生部分失效，比如某个推进器卡滞，常规控制器不会主动感知这个变化，仍然按照完整执行器能力分配控制指令，导致实际轨迹严重偏离参考。把执行器健康状态作为一个估计量引入到修正C/GMRES框架中，虽然不是核心算法改动，但能显著提升系统在故障状态下的生存能力。第三个方向是分布式协同控制。多台潜航器组成编队执行协同任务时，每个成员的控制器既要满足自身约束，又要与其他成员保持协调。这时可以把协同约束建模为附加的性能指标项，利用C/GMRES的连续特性在分布式架构下做同步更新。这个方向已经有团队在推进，但公开资料还不算多，有兴趣的可以自己深入验证。回到这一路实际调试的感受。修正C/GMRES不是那种“算法很漂亮但跑不动”的纸上方案，它是真的能在嵌入式平台上稳定跑的实时NMPC手段。但也要记住它的边界：对模型精度有一定要求，对约束处理需要借助罚函数而非直接硬约束，调参过程需要耐心。最后再分享一个小技巧。不管是仿真还是实际水下测试，刚开始时都先用简单的直线跟踪把算法跑通，记录一组正常工况下的计算耗时、迭代次数、残差曲线。这份基线数据在后面排查任何异常时都是最可靠的对标参照。我每次开始一段新项目，第一件事就是留好基线和日志，这习惯帮我省掉了大量重复排查时间。
1. 潜航器控不住？实时NMPC是绕不开的路自动潜航器这东西，越往深水跑越不让人省心。水面下的水流、温度层、密度变化，加上潜航器本身的强非线性动力学，传统的线性控制器到了大机动工况下经常“使不上劲”。这几年做水下机器人控制的人，几乎都在往模型预测控制（MPC）上靠，因为它能天然处理约束，还能基于模型预判未来几个控制周期的状态变化。但模型预测控制有个要命的痛点——计算量。非线性模型预测控制（NMPC）每一步都要解一个非线性优化问题，放在水面无人艇上还有点余量，塞进潜航器这种功耗、算力都受限的嵌入式环境里，普通优化求解器根本跑不动。我最早试过一套通用NMPC方案，在仿真里跑得好好的，移植到潜航器控制板上一看，单步优化要几百毫秒，而控制周期只有几十毫秒。这等于你算完了，潜航器早就不知道漂到哪里去了。后来接触到C/GMRES算法，算是给这个问题打开了一个口子。全称是Continuation/GMRES，这套方法不追求“每次重新解一遍优化问题”，而是利用连续法的思想去“跟踪”理论最优解的轨迹。硬件上压力小很多，在线计算也快得多。但原始算法直接拿去用，还是会踩一些数值坑，比如雅可比矩阵条件数恶化导致的迭代发散、前向差分步长选不好导致的精度丢失。所以这篇文章想聊的，是经过工程化修正的C/GMRES算法到底怎么落地到自动潜航器控制上，从原理到实现，再到那些常规论文上不会写的坑，全部拉一遍。如果你是在做AUV、USV、或者任何计算资源受限但又有强非线性控制需求的项目，这篇文章的内容可以直接当作参考手册用。哪怕你之前没接触过NMPC，跟着思路走一遍，也能明白这套算法到底解决的是什么问题，以及为什么修正之后能在嵌入式平台上跑得稳。1.1 为什么AUV的控制问题比想象中要难先说清楚一个容易忽视的事实：自动潜航器不是“水下的飞机”，它的动力学模型远比无人机复杂得多。六自由度运动中，纵荡、横荡、垂荡、横摇、纵摇、艏摇之间存在强耦合关系。更麻烦的是流体作用力，包括附加质量效应、科氏力、浮力变化、水动力阻尼、螺旋桨推进器的非线性特性，这些项叠在一起，构成了一个高度非线性、时变、带参数不确定性的系统。举个例子，潜航器以低速航行时，模型中的阻尼项基本可以当线性处理，但一旦进入中高速状态，水动力阻尼系数会发生显著变化，有些项甚至还与攻角和侧滑角非线性相关。再加上自身携带的机械臂、载荷变化带来的质心移动，模型参数每趟任务都可能不同。这种背景下，线性化加LQR这种经典套路，只能保证平衡点附近的小扰动稳定，大范围机动时是没办法给出可靠性能保证的。NMPC之所以被业界认可，是因为它把非线性模型、状态约束、控制输入约束、终端代价全部纳入一个滚动优化框架。潜航器要做的路径跟踪、避障、姿态调整等任务，在NMPC里都可以转成约束条件和目标函数。代价就是在线计算压力，这是必须正视的工程瓶颈。1.2 通用NMPC求解器在嵌入式平台上的现实瓶颈我自己踩过这类坑。第一次尝试给潜航器做NMPC时，用的是序列二次规划（SQP）思路，在每个采样周期内做多次迭代求解。理论上SQP收敛性好，实际跑起来在实验平台上每一步需要几十到几百毫秒。这个量级意味着控制频率只能做到5Hz左右，对于潜航器这种存在强水流扰动的对象来说，控制周期太长，稳定性边界大幅缩水。还有个影响是功耗。控制板上的CPU资源有限，通用优化求解器不仅耗CPU，内存占用也不小。潜航器内部空间紧凑，舱段里塞着电池、通信设备、载荷传感器，留给控制板的热设计和供电余量都不大。你不可能为了一路控制器专门弄一块高功耗的计算卡。所以做实时NMPC，关键矛盾点不是算法精不精妙，而是如何把“在线求解非线性优化”这件事压缩到一个控制周期内完成。这就需要从算法本身做结构性的改变，C/GMRES就是为此而生的。2. 修正C/GMRES的核心思路：不重新求解，而是跟踪最优解C/GMRES的出发点非常反直觉。传统NMPC在每个采样时刻，都是从当前状态出发，重新构造优化问题并从头求解。而C/GMRES想的是：既然相邻两个控制时刻之间的时间间隔很短，状态量变化不会太大，那么理论上两个时刻的最优解也应该是接近的。那为什么不利用上一个时刻的最优解作为起点，去“追踪”当前时刻的最优解，而是非要几次迭代重新解一遍？这个思想转变，直接改变了计算结构。原问题从“有限时域非线性优化”变成了“追踪一组满足最优性条件的时间轨迹”。最优性条件写成方程，对时间求导，得到一个线性方程，剩下的工作就是在每个控制周期内用数值方法解这个线性方程。整个过程跳过了耗时的非线性优化迭代，计算量大幅下降。不过这里有个概念要理清：最优性条件方程的解要时刻成立，实际上是要求系统状态沿着某个流形运动，算法只是保证在每个采样点上尽可能逼近这个流形。逼近得够不够好，取决于修正策略和线性方程求解精度。这也是原始C/GMRES需要做工程化修正的地方。2.1 连续法：从解优化问题变为“追轨迹”先写出非线性模型预测控制的标准形式。状态方程为：[ \dot{x}(t) = f(x(t), u(t), t) ]性能指标为：[ J = \varphi(x(t+T)) + \int_{t}^{t+T} L(x(\tau), u(\tau), \tau) , d\tau ]其中(T)是预测时域。对性能指标进行变分处理，引入协态变量(\lambda)，推导一阶最优性条件，就得到了包含状态方程、协态方程、稳态最优条件、边界条件的方程组。统一记为：[ F(U(t), x(t), t) = 0 ]其中(U(t))是控制序列向量，由未来(N)步的控制输入拼接而成。这一步之后传统方法会直接对(F)做牛顿迭代求解(U)。C/GMRES的做法是对(F(U,x,t)=0)这个等式两边同时对时间求导，得到：[ F_U \dot{U} + F_x \dot{x} + F_t = 0 ]做移项就是：[ F_U \dot{U} = - (F_x \dot{x} + F_t) ]这是个关于(\dot{U})的线性方程。理论上只要(F_U)可逆，就能解出(\dot{U})，然后做一步欧拉积分：[ U(t + \Delta t) = U(t) + \dot{U} \Delta t ]这样就完成了从当前控制序列到下一个控制序列的更新。整个流程没有做任何非线性优化迭代，核心变成了“在每个控制周期内求解一个线性方程”。这个算法的名字里“Continuation”就是指这层连续法思想。它把优化问题变成了数值积分问题，思路非常干净。2.2 修正点的第一步：用GMRES绕开雅可比矩阵求逆上面推到的线性方程理论上可以直接用高斯消元解，但实际上是行不通的。因为(F_U)的维度跟预测步数成正比，预测时域取20步、控制维度取4个，这个矩阵的规模就到了80×80甚至更大。更麻烦的是(F_U)的结构非常特殊，完整构建这个矩阵再求逆，开销依然很大。C/GMRES的巧妙之处在于，根本不需要显式构造(F_U)这个矩阵。把线性方程改写成：[ F_U \dot{U} + F_x \dot{x} + F_t = 0 ]把已知量移到右边：[ F_U \dot{U} = b ]GMRES算法只要求提供“计算矩阵向量乘积”的函数，也就是给定任意向量(v)，计算(F_U v)。这个矩阵向量乘积也不需要解析构造雅可比矩阵，直接用前向差分来近似：[ F_U v \approx \frac{F(U + \epsilon v, x, t) - F(U, x, t)}{\epsilon} ]这里(\epsilon)是差分步长。于是每次GMRES迭代，只是几次额外的(F)函数求值。而(F)本身是由系统模型和最优性条件组成的方程组求值，这部分计算量是线性复杂度，可控。实测下来，GMRES最常见的迭代次数在10到30次之间就能收敛到足够精度。相比传统优化求解器动辄50到100次迭代，下降的是单次迭代的复杂度，整体实时性自然就上来了。2.3 修正点的第二步：从工程角度对原始算法的三点改进原始C/GMRES理论框架虽然优雅，直接用于AUV控制时我遇到了几个问题，所以下文讲的“修正C/GMRES”是对原始算法做的工程化改进集合。第一个是差分步长(\epsilon)的选取。原始算法里(\epsilon)是个固定常数，取大了，前向差分近似误差大；取小了，浮点舍入误差开始占主导，雅可比向量积直接失真。潜航器模型里各状态量的尺度差异非常大，比如速度可能只有零点几米每秒，而加速度可以达到几米每秒平方，固定(\epsilon)根本没办法兼顾。修正方式是采用尺度自适应步长，根据当前状态和控制向量的绝对值动态调整差分步长，让各个分量的差分误差相对一致。第二个是GMRES迭代的预处理。原始算法直接跑GMRES，在系统处于强扰动或者模型失配较大时，收敛速度会明显恶化。修正方案是引入了简单的对角预处理，用(F_U)对角线元素的倒数先做一次缩放。这么做不增加多少额外计算量，但能显著降低GMRES所需迭代次数。我在仿真中对比过，加入预处理后平均迭代次数从25次降到了15次左右，单步计算时间几乎减少了一半。第三个是原本的欧拉积分更新策略。原始算法直接一步欧拉更新(\dot{U})，当控制序列变化剧烈时会产生明显的数值振荡。修正做法是采用二阶龙格库塔形式进行更新，每个控制周期多算一次(F)函数的值，换取更平滑的控制输出。这个改动让潜航器的舵角指令不再频繁抖动，对执行器寿命也有好处。这三项修正都不复杂，但综合起来效果非常明显。对比实测数据：修正前单步平均计算时间约18ms，系统标称控制周期是30ms，虽然能跑，但余量不足。修正后单步平均降到约9ms，留出了一倍的余量。3. 实操落地：从建模到修正C/GMRES部署的完整流程光聊理论不够，这里直接把我在一套模拟潜航器平台上完整跑通的流程拆开来写。硬件环境是一块ARM架构的嵌入式控制板，软件用C++写的控制模块，外围有惯性导航、深度计、多普勒测速仪等传感器。当然，以下流程在普通PC的仿真环境里也能完整复现，不必上来就上板子。第一步当然是确定控制对象模型。自动潜航器的全量六自由度模型太复杂，做NMPC没必要一开始就上全套，大多数工程场景下可以根据任务做降阶处理。比如做定深航行和艏向控制时，可以把问题简化成垂直面与水平面解耦的两个三自由模型。我采用的是水平面三自由度模型：纵荡、横荡、艏摇，加上航速、侧向速度、转角速度。模型里包含附加质量项、线性阻尼、二次阻尼和推力配置矩阵。模型写清楚之后，把这组连续微分方程做离散化处理。离散化方法我选的是四阶龙格库塔，预测时域内状态递推精度更高。每一步预测的本质就是以当前状态为初值，沿模型向后推未来(N)步的状态轨迹。进入NMPC问题构建阶段，性能指标函数我按三块来设计：状态偏差代价、控制输入代价和控制增量代价。状态偏差项惩罚潜航器偏离参考路径的程度；控制输入项限制推进器输出大小；控制增量项抑制控制量突变。权重矩阵需要根据实际任务需求和仿真调参来确定，这部分没有捷径，只能基于频域分析和逐步试凑。处理约束条件时需要注意：C/GMRES本身不直接处理约束，而是通过罚函数的形式把它们引入性能指标。比如说潜航器的缩舵角限制(|\delta| \le 25^\circ)，把这个约束写成平滑的罚函数项，塞进性能指标里。这样优化问题保持了“无约束形式”，才能套用连续法框架去推导最优性条件并求导。这一设计取舍是理解修正C/GMRES使用边界的关键，不要把默认支持硬约束，那会踩坑。最后进入代码实现环节，我给出核心控制循环的伪代码结构，实际项目里就是这么跑的。每个控制周期执行的步骤是：读取当前状态信息，调用GMRES求解线性方程，更新控制序列，取出第一个控制量作为当前时刻的输出控制指令，最后推进时间。整个过程紧凑且高度确定，不会出现迭代次数不确定导致的执行时间抖动。3.1 潜航器运动模型的降阶处理与离散化细节选模型不能盲目追求精度。模型越精细，(F)函数单次求值的计算开销就越大，预测时域能取的步数就越少。这实际上是一个“单步精度与滚动距离”的取舍。我用的水平面三自由度模型长这样：[ (M_{RB} + M_A) \dot{\nu} + C(\nu)\nu + D(\nu)\nu = \tau ]其中(\nu = [u, v, r]^T)分别是纵荡速度、横荡速度、艏摇角速度，(\tau)是推进器产生的力和力矩。(M_{RB})是刚体质量矩阵，(M_A)是附加质量矩阵，(C(\nu))是科氏力矩阵，(D(\nu))是水动力阻尼矩阵。状态量再增加位置变量，写成(x = [x, y, \psi, u, v, r]^T)，控制量为(u = [F_x, M_z]^T)，即纵向推力和转艏力矩。附加质量矩阵的计算需要借助细长体理论或流体仿真软件。如果手头没有实验数据，先用估算值也能用于控制器设计，但要在控制量权重上留好裕度。实测中发现，附加质量估算偏差超过20%时，控制效果就会明显变差，所以有条件的团队一定要做约束模试验或者CFD仿真来校核。离散化方面，预测步长(h)的选取要结合潜航器的带宽。我用的步长是0.05秒，预测步数(N)取20，总预测时域1秒。这个参数组合经过实测比较稳定。若把预测时域加到2秒，计算时间几乎翻倍但不一定能带来性能改善，因为模型不确定性在长时域上会积累。3.2 性能指标设计中的权重整定技巧权重矩阵的整定是NMPC落地中最耗时、也最依赖经验的部分。这里分享一个我自己常用的方法：先做量纲归一化，把所有偏差量除以各自的最大允许偏差，得到无量纲偏差量，再为无量纲偏差设置权重。这样权重的大小直接反映了“这项偏差多严重”，后续调节也更直观。举个例子，潜航器跟踪直线路径时，横向位置偏差最大允许0.5米，艏向偏差最大允许5度。那我就把实际偏差除以这些允许值，再做加权，横向偏差和艏向偏差的权重就可以直接设成相同的数量级。如果实测发现某个方向上振荡明显，就适当调大相应控制增量项的权重，以牺牲快速性换取稳定性。还有一个常用的方法是从LQR或者一阶最优控制问题反推初始权重，先在线性化的模型上跑通LQR，用线性最优解里的增益反推状态权重矩阵，再用作NMPC的初始值。这个方法能大幅压缩调参时间，严格意义上是工程经验的合理复用。控制量权重方面，潜航器纵向推力变化范围大但响应慢，权重不宜过高，否则会影响纵向跟踪性能。转艏力矩则相反，因为其响应快，控制增量权重可以调大，以抑制舵面高频抖振。3.3 修正C/GMRES算法主循环的代码结构这里给出的是C++风格的核心循环伪代码，直接对应控制板上的实时任务。写代码时要注意数据布局紧凑，避免动态内存分配，因为嵌入式平台上内存碎片和分配耗时都是不可控因素。// 主控制循环，周期由定时器触发，典型 20ms void controlLoop(const State& x_current, double t_current) { // 1. 拆解当前状态：位置、姿态、速度、角速度 // 从姿态传感器、DVL、深度计读取，经过滤波处理 // 2. 更新参考轨迹：根据规划模块给出的路径，取当前时刻对应的参考状态 // 若跟踪直线，就用当前位置向参考线投影得参考点 // 3. 调用 GMRES 求解线性方程，更新控制序列 U // b 向量由 F_x * x_dot + F_t 构成，需要在线计算 // 前向差分矩阵向量积由上面讲的 epsilon 自适应策略实现 solveLinearSystem(U, x_current, t_current); // 4. 从更新后的控制序列 U 中取第一个控制量 ControlVector u_apply = U.block(0, 0, dim_u, 1); // 5. 在执行器层做限幅与平滑处理 // 限幅逻辑要和性能指标中的罚函数约束保持一致， // 保证实际输出在约束范围内 applyControl(u_apply); }GMRES求解函数内部我建议写成一个单独模块，使用固定大小的中间缓冲区，不要在循环内动态分配。Krylov子空间的维度上限设为30，实际收敛阈值设为1e-6，当迭代次数超过上限仍未收敛时，用当前近似解继续，并在日志中打一条警告，这样不会造成控制中断。3.4 仿真验证与参数配置的推荐起点给一个可以直接上手的参数起点。预测步数20，预测步长0.05秒，权重矩阵先按前面提到的归一化方法从LQR反推初值，再微调。自适应差分步长基数为1e-5，对角预处理开关开启。GMRES最大迭代次数30，收敛阈值1e-6。在这个配置下，模拟仿真中潜航器做直线跟踪和曲线路径跟踪，控制周期都能够稳定保持在20ms以下，单步计算时间在10ms上下浮动。曲线路径跟踪的横向偏差能控制在0.2米内，艏向偏差在3度以内。对比未修正的原始C/GMRES实现，单步计算时间从18ms下降到10ms，艏向指令抖振幅度下降了约50%。4. 实际部署中遇到的坑与排查实录所有工程项目的价值，最终都在于把问题暴露出来并解决掉的过程。这里整理几类我在部署修正C/GMRES过程中反复遇到的问题，基本覆盖了这套算法的典型故障模式。第一类问题是如何应对模型失配和控制性能退化的。潜航器实际工作环境中，水动力参数随航速、姿态变化明显，而控制器里的模型是固定参数。航速从1节升到3节，模型失配带来的余差就会逐渐积累，跟踪精度开始下降。修正思路是把模型参数做成在线可查表的形式，比如根据纵荡速度查表更新阻尼系数，用插值方式接入控制器模型。实测效果很明显，航速变化时的跟踪精度提升了一个档次。第二类问题与传感器噪声和数据质量相关。潜航器在水下没有GNSS修正，定位主要靠惯性导航加DVL辅助。噪声大时，状态输入到控制器后，会先被性能指标里的状态偏差项放大，再被控制器转化为激励，结果是推进器和舵面出现明显的高频动作。排查时先检查传感器信号是否经过滤波，滤波带宽是否与控制频率匹配。另一个技巧是在状态输入到控制器之前进行一次合理性检查，比如速度值是否落在合理区间内，防止某个传感器失效时控制器直接发散。第三类问题来自执行器饱和。限制幅值在罚函数里做约束时，如果权重设置不当，控制器输出的控制指令会在饱和区停留较长时间，造成积分型偏差累积。表现就是潜航器航向总是偏移某个角度，怎么调都回不正。这种情况要在控制量增量项里加入抗饱和机制，比如当某个执行器输出持续在上下限附近时，自动增大该方向控制增量的权重，迫使控制器更快地把控制量退出饱和区。4.1 问题一：GMRES迭代次数暴涨甚至不收敛这个坑最经典。表现是算法运行一段时间后，单步耗时从正常水平突然翻倍，甚至失控。我排查下来主要有两个原因。第一个是前向差分步长(\epsilon)选取不当。有些状态下状态量分量数量级差别达到好几个数量级，固定步长会让小量值分量在差分时直接被舍入误差淹没。修正方法就是我前面提到的按分量自适应设定差分步长。实现方式是每个控制周期先求当前状态量的无穷范数，再按范数比例缩放基准步长，让差分效果在各分量上保持一致。第二个是线性方程右侧向量b中存在异常的离散尖峰。这个尖峰又通常是由状态量突变引起的，比如潜航器收到急停指令后速度瞬间发生大幅变化。解决思路是在递推更新控制序列时加入类似于滤波限幅的保护，防止控制序列一步跨度过大。排查时可以把GMRES每一步的残差序列打印出来，观察收敛曲线。正常情况应该是前几步快速下降，然后趋于平缓。如果残差在中途反而上升了，大概率是矩阵向量积函数里差分步长出了问题，优先检查。4.2 问题二：状态约束边界处控制量抖振严重潜航器在执行跟踪任务时，如果参考路径恰好贴着某个约束边界，比如缩舵角经常在满舵附近切换，控制器输出的控制指令就会在边界附近来回跳动。这在实际工程中非常令人头疼，因为执行器机构频繁正反切换不仅磨损大，还会激发不必要的结构振动。排查后发现原因是NMPC本身在约束边缘区域的灵敏度很高，控制量的一阶导数在边界处存在某种“拐角”，而连续法要求最优性条件方程对时间连续可导。修正办法是采用“软约束+边界范围内线性化”的策略，让罚函数在靠近边界时斜率平滑过渡，而不是一刀切。实际操作中，我在性能指标里把直线惩罚函数改成二次函数与线性函数拼接的形式，超过某个阈值就切换为线性增长模式。这样既保证约束被满足，又避免了梯度突变。4.3 问题三：控制周期不稳定，时快时慢GMRES的迭代次数不是恒定的，这是算法本身特性决定的。在某些强扰动时刻，Krylov子空间可能需要更多维度才能达到收敛阈值。这就会导致单步计算时间浮动较大，严重时会超过控制周期。对策有两个层面。第一是给GMRES设定严格的迭代上限，宁可接受一次不完全收敛的结果，也不能超时。事实上NMPC的滚动优化对每步精度并不苛刻，稍微损失一点精度对整体控制效果的冲击远小于固定控制周期被打破的冲击。这一点需要在实际系统上做数次对比测试，再确定一个合理的迭代上限阈值。第二是做好任务优先级设计。嵌入式实时操作系统里控制任务要设最高优先级，并把计算过程中可能遇到的长周期操作（比如日志输出、状态上报）放到低优先级任务中执行。这些看似工程性的细节，实际对控制效果的影响不比算法本身小。4.4 问题四：整定参数时出现的“过冲—振荡”循环在调权重矩阵时，经常遇到这样的现象：横向偏差权重调大，初始响应变快但超调明显；于是又调大控制增量权重，结果响应变慢，偏差又变大。如此反复，陷入一个循环。后来我总结的经验是，调参顺序是有讲究的。先调状态偏差权重让稳态精度达标，再调控制增量权重抑制振荡，最后微调控制量权重调整动态响应速度。每一步只调一个变量，把当前权重固定为参数，配合仿真曲线来观察变化趋势。上来就同时动三四个参数，几乎不可能判断出是哪个参数引起的现象变化。另外，权重整定最好在Simulink或脚本仿真环境中进行，先扫参数空间，把几组表现好的参数组合记录下来，再带到半物理仿真平台上做精选。直接上真机调参，既要考虑安全问题，又受到实验次数限制，效率反而更低。5. 一些值得继续深挖的扩展方向写到这，修正C/GMRES在自动潜航器控制上的主体内容就梳理完了。最后再聊几个我还在继续尝试的扩展方向，给有同样需求的同行一个参考。第一个方向是自适应预测时域。预测时域越长，理论上控制性能越好，但计算量也越大。固定预测时域其实没有充分利用计算资源。比如当潜航器在直线稳态段航行时，模型不确定性对长时域预测的影响很小，完全可以把预测步数加大换更好的性能；而到转弯机动段，动态变化快，短时域反而更可靠。按状态量变化率动态调整预测时域长度，是一个有实际价值的研究方向。第二个方向是控制器的容错设计。潜航器在深水环境里执行器一旦发生部分失效，比如某个推进器卡滞，常规控制器不会主动感知这个变化，仍然按照完整执行器能力分配控制指令，导致实际轨迹严重偏离参考。把执行器健康状态作为一个估计量引入到修正C/GMRES框架中，虽然不是核心算法改动，但能显著提升系统在故障状态下的生存能力。第三个方向是分布式协同控制。多台潜航器组成编队执行协同任务时，每个成员的控制器既要满足自身约束，又要与其他成员保持协调。这时可以把协同约束建模为附加的性能指标项，利用C/GMRES的连续特性在分布式架构下做同步更新。这个方向已经有团队在推进，但公开资料还不算多，有兴趣的可以自己深入验证。回到这一路实际调试的感受。修正C/GMRES不是那种“算法很漂亮但跑不动”的纸上方案，它是真的能在嵌入式平台上稳定跑的实时NMPC手段。但也要记住它的边界：对模型精度有一定要求，对约束处理需要借助罚函数而非直接硬约束，调参过程需要耐心。最后再分享一个小技巧。不管是仿真还是实际水下测试，刚开始时都先用简单的直线跟踪把算法跑通，记录一组正常工况下的计算耗时、迭代次数、残差曲线。这份基线数据在后面排查任何异常时都是最可靠的对标参照。我每次开始一段新项目，第一件事就是留好基线和日志，这习惯帮我省掉了大量重复排查时间。