做无人艇控制这几年我有个很深的感触很多团队把硬件折腾得漂漂亮亮电机、电池、通讯一套下来花了不少钱结果一下水就露馅——航迹走不直、抗不了风、避障躲不过去。问题基本不在船体而在控制器。早些年大家最常用PID简单是简单但碰上横风横流、执行器饱和、动态障碍物这些场景参数调到头也压不住。后来我转到模型预测控制Model Predictive ControlMPC这条路上解决了我手上好几个项目的核心痛点包括无人艇的轨迹跟踪、航向保持和自主避障。这篇东西不是教科书复读是我在实际调试和实艇试验里攒下来的经验适合正在做无人船控制的研究生、工程师以及准备把控制器从PID往更优方案迁移的团队参考。我先把话放这儿MPC不是万能的它也有计算量大、模型依赖强、调参费神的毛病。但对你手上的无人艇来说它能帮你把“能跑”变成“跑得稳、跑得准”而且你能提前看到未来一段时间的状态把约束条件直接写进控制器里这件事在PID框架里几乎做不到。下面我把模型怎么搭、参数怎么定、软件怎么写、实艇怎么调一整套讲清楚。1. 为什么无人艇控制里大家都在聊MPC1.1 无人艇控制难点不是你想的那么简单很多人第一次接触无人艇第一反应是“这不就是水面上的遥控车吗陀螺仪加PID不就够了”。实际做下来完全不是那么回事。无人艇有三个特点直接把它跟地面无人车拉开差距。首先无人艇没有刹车。地面车辆可以靠制动力矩快速减速无人艇全速航行时你要是想让它停下来只能靠倒车或者漂移动态响应慢得让人着急。控制算法如果预见性不够等发现偏了再纠偏船早就冲过去了。其次是恶劣且持续的环境扰动。风、浪、流是无人艇的“标配”。尤其在水面上风对船体的作用力很不均匀浪会带来周期性干扰流的影响更麻烦它不是固定方向固定大小而是随空间和时间都在变的。这些扰动对控制器的考验是实打实的。再就是执行器饱和问题。你的推进器只有那么大的推力舵只有那么大的转角范围。很多实验室做仿真的时候不把饱和当回事控制器输出算出来多大就用多大结果上实艇发现压根执行不了。明明控制器让舵打20度但机械限位只有15度这时候控制误差引发的后果会直接传导到整个闭环。还有一个常被低估的难点无人艇的动力学是强非线性的。你去看水面船只的运动模型会发现纵荡、横荡和艏摇三个自由度之间有强耦合低速时还好中高速时水动力特性显著变化线性模型很容易失真。这也是为什么PID在一个航速段调得不错换个航速段又不行了的原因。1.2 PID、LQR都试试为什么最后落在MPC上我最早在一艘1.2米的小型无人艇上用的是串级PID内环航向角速率外环航向角再加一个位置环来跟踪轨迹。湖试的时候天气好微风无浪效果还行。但一遇到3级风以上偏差就明显加大尤其转弯后对准直线航迹那一段振荡能持续很久一直是调参的死循环——你调小比例系数稳定性上来了但响应慢了调大比例系数跟踪快了又开始超调怎么都不舒服。LQR也试过它是线性二次型最优控制相比PID有模型支撑理论上是漂亮。但LQR处理不了约束。你算出来的最优控制量有可能是5米每秒的加速度但你的推进器物理上限才2米每秒那这个“最优”就毫无意义。更尴尬的是LQR只能针对一个工作点设计船体状态偏离这个点之后性能下降明显尤其转弯这种大幅操作效果比PID好不到哪里去。MPC的思路则完全不同。它把约束不再当回事处理而是当成本身就必须要满足的条件直接写进优化问题里。每一时刻控制器都基于当前状态利用模型预测未来一段时间内的状态轨迹然后在这个预测轨迹上求解一个带约束的最优控制问题只执行第一个控制量下一时刻再重新来一遍。这种“预测滚动优化反馈校正”的架构天然适合无人艇这种大惯性、多约束、扰动强的被控对象。我手头的一个模拟项目X就是用MPC代替PID之后横风场景下的平均轨迹跟踪误差从原来的近两米降到了四十几厘米转弯超调量更是直接减少了一半以上。这个效果不是调两个月PID能调出来的。1.3 MPC核心思想看一眼未来再动手为了把MPC讲清楚我习惯拿开车做类比。你开车的时候绝不会只看车头跟前那一段路你会看前方几十米甚至更远的地方提前判断这个弯怎么打方向、什么时候开始减速同时还会考虑路边有没有障碍物、车道边界在哪里。这个“看远”的行为就是预测时域。MPC每走一步就重新做一次“往前看”的规划计算执行完当前这一步之后再重新预测、重新优化这就是滚动优化。它保证控制器永远基于最新信息做决策不至于一条道跑到黑。而约束就像路上的实线、护栏、限速牌你不能压线、不能撞护栏、不能超速。这些限制在MPC里是以数学不等式的形式出现的求解器会在满足所有不等式的前提下找出最优控制量。三句话总结MPC的工作流程预测用系统模型从当前状态出发推算未来N个步长的状态序列。优化在满足输入约束和状态约束的前提下最小化一个包含跟踪误差、控制量变化量的目标函数。执行与滚动把解出来的最优控制序列的第一步施加给被控对象下一时刻重复第1步。这就是MPC能在无人艇上解决实际问题的根本原因它的每一步决策都建立在对未来状态的预判上而不是对过去误差的被动反应。2. MPC控制器的模型基础与核心设计2.1 三自由度船舶运动学模型怎么搭MPC再强也逃不过一个基本事实预测全靠模型模型不准预测就是玄学。所以第一步把无人艇的运动学模型搭清楚。我常用的坐标系分两个一是大地固定坐标系用于描述船的位置和航向二是船体固定坐标系用于描述速度。这里按无人艇最常见的三自由度运动学模型来讲纵荡surge、横荡sway、艏摇yaw。在大地坐标系下船舶位置和航向的导数和船体坐标系速度的关系如下x_dot u * cos(ψ) - v * sin(ψ) y_dot u * sin(ψ) v * cos(ψ) ψ_dot r其中u是纵向速度船头方向v是横向速度垂直于船头方向r是艏摇角速度ψ是航向角。这个运动学模型是通用的没有近似。动力学部分就需要做简化了。无人艇的完整六自由度模型参数一大堆大多数情况下用不上。我通常只保留一个线性阻尼的三自由度动力学模型m * (u_dot - v*r) τ_u - d_u * u m * (v_dot u*r) τ_v - d_v * v I_z * r_dot τ_r - d_r * rm是船体质量I_z是绕z轴的转动惯量d_u、d_v、d_r分别对应三个自由度的线性阻尼系数τ_u是推进器的纵向推力τ_r是转艏力矩。τ_v非全驱动无人艇上为0也就是没有侧推。这里你可能会问为什么用线性阻尼模型因为MPC每一刻都在做滚动优化非线性模型虽然更精确但会让优化问题变成非线性规划NLP求解时间直接指数级上升。线性模型的预测精度虽然有限但胜在速度快、稳定性好实艇上跑的绝大多数MPC用的都是线性化模型然后在扰动和误差上通过反馈去补偿。这个取舍非常重要先进控制不是一味追求模型复杂而是要在精度和可算性之间找平衡点。状态空间表达式写成矩阵形式就是x(k1) A * x(k) B * u(k)其中状态向量取[x, y, ψ, u, v, r]控制输入取[τ_u, τ_r]。A和B通过对上述非线性运动学在当前工作点线性化得到。实际离散化时用零阶保持器采样时间为Ts。想提醒你的是搭建模型时尽量去水池或船模水池做几组辨识实验把阻力系数、惯性参数用最小二乘法拟合出来别完全靠理论估算。我见过好几个团队北湖试验时模型参数跟实际差了30%以上预测轨迹完全失真控制器全程在瞎指挥。2.2 预测时域、控制时域、采样时间怎么选MPC有三个时间参数任何一个选不好控制器性能都会出大问题。采样时间 Ts决定控制器每个控制周期跑一次的时间间隔。无人艇通常取0.1秒到0.5秒之间。取太短CPU来不及求解控制周期被打乱取太长预测精度下降动态响应变差。我做过的项目里小艇1米左右采样时间0.2秒居多大一些的无人艇4米到5米可以放到0.5秒。一个经验是以系统最短动态时间常数为参考采样时间取它的1/10到1/5比较合理。预测时域 Np预测未来多少步。它跟采样时间的乘积就是“向前看的时间长度”。对无人艇来说预测时域至少要覆盖一次转弯或避障动作的持续时间。我一般让 Np*Ts 保持在10秒到20秒之间。比如Ts0.2秒时Np取50到100。太小预测没有意义太大计算量上去了但增益却越来越小这个后面会提到。控制时域 Nc只对前多少步做优化。这个值小于Np一般取Np的1/5到1/3。比如Np50时Nc取10到15。控制时域之后的部分控制输入保持最后一步的值不变。这样可以大幅减少优化变量数量求解更快同时对性能的影响很小。一个重要的直觉是预测时域决定“看得多远”控制时域决定“下一步往哪走”。两者之间要有合理的比例太多优化变量会拖慢求解速度太少则控制器过于短视。2.3 约束条件与代价函数怎么设计才合理MPC的核心优势就是把约束写进优化问题里。约束分两类输入约束推进器最大推力、舵机最大转角、控制量变化率限制。以无人艇为例输入约束长这样τ_u_min ≤ τ_u ≤ τ_u_max τ_r_min ≤ τ_r ≤ τ_r_max Δτ_u_min ≤ τ_u(k) - τ_u(k-1) ≤ Δτ_u_max Δτ_r_min ≤ τ_r(k) - τ_r(k-1) ≤ Δτ_r_max第3和第4条是变化率约束。这个往往被新手忽略但对无人艇很重要。舵机是一个有物理惯性的机械结构你让它瞬间从-30度打到30度它根本反应不过来。加了变化率约束之后控制输出更平滑实艇执行更健康。状态约束安全距离不可进入的禁航区、航向或速度限制。在避障场景下通常把障碍物表示为位置空间约束(x(k) - x_obs)^2 (y(k) - y_obs)^2 ≥ R_safe^2代价函数是整个优化的目标。我用的标准形式是J Σ (x(k) - x_ref(k))^T Q (x(k) - x_ref(k)) Σ u(k)^T R u(k) Σ Δu(k)^T Rd Δu(k)简单理解就是让状态跟踪参考轨迹Q项同时让控制量尽量小R项控制变化尽量平缓Rd项。Q和R矩阵的工程意义很直白。Q矩阵权重调大控制器会更积极地去消除跟踪偏差但代价是会容易激发振荡和激进操作R矩阵权重调大控制器对控制量的“消耗”更敏感输出更温和但跟踪变慢。我自己整定的顺序是先用小Q试能跟踪但偏慢然后逐步增大Q直到出现振荡再增大Rd把控制动作变平滑。这个过程重复两三轮基本就能找到一条比较优的参数曲线。还有一个经验Q矩阵中对横向偏差和航向偏差的权重比例决定船的“脾气”。如果你发现船总是贴近参考线但方向偏着走那是航向权重偏低如果船航向很正但位移偏差一直消不掉那是位移权重偏低。实际调的时候边试边找比例不用一上来就追求精确。3. 仿真环境搭建与MPC控制器的实现落地3.1 仿真平台选择Matlab、Python还是ROS在仿真阶段选对平台能省下你后面一个月的头发。我在学习和快速原型阶段偏向用Matlab因为MPC相关的工具箱很成熟。你可以直接调用模型预测控制工具箱也可以自己用矩阵运算写一个POMDP式的小型MPC。Matlab自带的可视化和调试能力确实方便参数扫描、响应曲线查看都是一两行命令的事。但如果你要把代码部署到实艇上我更推荐Python方案。原因很直接实艇上的工控机一般跑LinuxPython的工程衔接比Matlab顺畅得多。加上casadi这个库你可以在Python里用符号化方式定义优化问题然后把它编译成高效的求解器代码。整个过程分工非常清楚casadi负责建模和求导osqp或者ipopt负责数值求解numpy负责矩阵运算。ROS其实是个中间层。如果你的无人艇系统是模块化的传感器模块、决策模块、控制模块用ROS做通讯非常合适。MPC控制器在ROS里通常作为一个独立节点订阅状态和参考轨迹发布控制指令。但ROS本身不是求解器也不是建模工具它只负责让各个模块能对话。我的建议是阶段推荐平台理由学习原理Python casadi自由度高、可验证细节快速原型Python osqp线性MPC求解快、接口简单实艇部署ROS Python节点模块化管理、日志回放方便学术验证Matlab工具箱完善、出图方便3.2 一个可复现的线性MPC控制器实现框架我在这里给你一个可直接参考的Python代码骨架。它实现的是一个线性MPC的轨迹跟踪控制器使用casadi求解器。import casadi as ca import numpy as np # 系统参数从辨识实验获得 Ts 0.2 # 采样时间(s) Np 50 # 预测时域 Nc 15 # 控制时域 # 状态[x, y, psi, u, v, r] # 控制[tau_u, tau_r] A np.array([...]) # 状态矩阵由线性化得到 B np.array([...]) # 输入矩阵 # 约束 tau_u_max 10.0 tau_r_max 2.0 delta_tau_u_max 1.0 delta_tau_r_max 0.5 # 权重矩阵 Q np.diag([10, 10, 5, 1, 1, 2]) # 状态权重 R np.diag([0.1, 0.1]) # 控制权重 Rd np.diag([0.5, 0.5]) # 控制变化率权重 # 定义优化变量 X ca.SX.sym(X, 6, Np1) # 状态序列 U ca.SX.sym(U, 2, Nc) # 控制序列 x0 ca.SX.sym(x0, 6) # 当前状态 x_ref ca.SX.sym(x_ref, 6, Np1) # 参考轨迹 # 初始约束第0步状态等于当前状态 constraints [X[:, 0] - x0] objective 0 # 预测模型约束 for k in range(Np): if k Nc: control U[:, k] else: control U[:, Nc-1] # 控制时域之后保持最后一步 constraints.append(X[:, k1] - (A X[:, k] B control)) # 代价函数 state_err X[:, k1] - x_ref[:, k1] objective ca.mtimes(state_err.T, ca.mtimes(Q, state_err)) objective ca.mtimes(control.T, ca.mtimes(R, control)) if k Nc and k 0: delta_u U[:, k] - U[:, k-1] objective ca.mtimes(delta_u.T, ca.mtimes(Rd, delta_u)) # 输入约束 for k in range(Nc): constraints.append(U[0, k] tau_u_max) constraints.append(U[0, k] -tau_u_max) constraints.append(U[1, k] tau_r_max) constraints.append(U[1, k] -tau_r_max) # 求解器配置 prob {x: ca.vertcat(X.reshape((-1, 1)), U.reshape((-1, 1))), f: objective, g: ca.vertcat(*constraints)} solver ca.qpsol(solver, osqp, prob)这段代码的核心就是做三件事建立预测模型约束、建立代价函数、配置求解器。实际运行的时候需要自己包装成MPC类输入当前状态和参考轨迹输出控制量第一个元素。有几个地方容易踩坑我提一下。一是矩阵维度匹配。在casadi里定义符号变量时维度一定要对清楚。我有一个同事栽在A矩阵维度错了一维编译时完全不报错但运行时解出来的控制量全部是垃圾值。排查了两天才找到。二是状态约束别写太紧。尤其是避障安全距离R_safe这个值比物理船体直径稍微大一点就行留太大容易导致无解。具体来说如果你的船是1.2米长、0.4米宽R_safe取1.5米就够用了。留太大控制器会认为周围处处都是障碍物走到哪里都在强行绕路跟踪精度反而变差。三是OSQP是好选择但不是万能的。对于线性MPC问题OSQP一个内点法求解器速度快、稳定性好非常适合嵌入式部署。但如果你的代价函数里有非线性项或者约束是时变的强非线性就用ipopt。代价是不同的求解器有不同的求解速度上实艇之前必须做SIL软件在环和HIL硬件在环验证。3.3 从仿真到实艇参数整定实操记录仿真跑通了别高兴太早。模型在仿真里再完美拿到实艇上大概率会有让你头疼的偏差。这里分享一个我之前做模拟项目X时完整的参数整定过程。第一步我先在水池里做推力标定实验。记录不同PWM占空比下的船体实际速度拟合出推进器模型。因为实艇的推进器存在死区和非线性这一步如果不做仿真里很漂亮的控制量到实艇上执行效果会大相径庭。第二步做自由衰减实验辨识水动力阻尼系数。把船体推离平衡位置后松手记录位移和速度的衰减曲线用最小二乘拟合出d_u、d_v、d_r。这个实验在水池里做一次精度远高于CFD仿真估算。参数初值是Ts0.2秒Np50Nc15Qdiag(8, 8, 4, 1, 1, 2)Rdiag(0.1, 0.05)Rddiag(0.5, 0.3)最大推力8N最大转艏力矩1.5N·m。湖试第一次跑的时候现象非常典型船能跟着参考轨迹走但航向振荡得很厉害偏差忽大忽小转弯处尤其明显。回看控制日志后发现控制器持续输出满转艏力矩说明Q矩阵对艏摇误差的权重过大控制器在拼命纠偏。我第一轮调整是把Q中对应艏摇角的权重从4降到2同时把Rd的艏摇变化率惩罚从0.3加大到0.6。第二次下水振荡明显减弱但跟踪响应变慢了到达参考线的距离变长。接下来我再把Q的横向位移权重从8升到12补偿响应变慢的问题。如此反复迭代了大约六轮最终参数定为Qdiag(12, 12, 2, 1, 1, 1.5)Rdiag(0.1, 0.03)Rddiag(0.8, 0.5)。实际横向跟踪误差稳定在0.3到0.5米之间转艏过程平滑全程无振荡。很多团队第一次上手MPC总指望一组参数通吃所有工况这是不太现实的。MPC的优点就是它有模型的支撑换工况时你只需要调整权重比从头调PID高效得多。从仿真到实艇的差距也恰恰是你需要花时间在辨识和实船实验上的地方。4. 轨迹跟踪与自主避障MPC的典型任务落地4.1 轨迹跟踪任务从“能跑”到“跟得住”轨迹跟踪是无人艇最基础也最重要的任务。做比赛、做巡检、做测绘本质上都是让船沿着一条规划好的参考轨迹走。实际工程中有两种常见的参考轨迹来源。一是直线航段任务比如在两个GPS航点之间走直线这时参考轨迹可以简单插值得到二是追踪一条预先规划的平滑曲线比如用圆弧过渡的航线或者B样条曲线。无论哪种参考轨迹需要至少包含参考位置和参考航向。MPC干轨迹跟踪这件事画面其实很直观。它在每一时刻“问自己”如果我现在打这么多舵、加这么多油门未来20秒内我的位置会偏离参考线多少然后把未来20秒的总偏离量降到最低。这里有一个关键细节参考航向怎么定义。新手常犯的错误是全篇用一个不变的全局航向角做参考。但船在实际转弯时参考航向是持续变化的。如果参考航向给得不对控制器就会同时受到“位置偏了”和“航向错了”两个误差的夹击操作会很矛盾。正确做法是对参考轨迹上每个点计算它的切线方向作为该时刻的参考航向。这样位置跟踪和航向跟踪就是一致的控制起来自然顺滑。好多人问过我MPC比PID到底强在哪公平地讲在直线平静水域PID经参数优化后能达到跟MPC接近的性能。但一旦遇到转弯、侧风、需要跟一段复杂曲线时PID就吃力了。MPC凭借对曲线参考轨迹的预测提前开始打舵转弯更顺出弯后偏差更小。这个区别不需要高深的数学也能理解PID是根据“现在偏了多少”来纠正MPC是根据“接下来会偏多少”来预防。4.2 障碍物避让怎么融进统一框架避障是无人艇的另一大刚需尤其是水面有人船、浮标、渔网这些实际存在的障碍物。这里说的与躲避动态障碍物类似我需要多讲几步。在MPC框架里避障根本不是单独的一个“模块”而是往优化问题里加一个不等式约束。每一时刻控制器都必须保证预测轨迹上所有点都落在安全区域之外。所有障碍物可以简化为圆形区域然后写成约束不等式。因为圆形约束在二维平面上是凸约束OSQP可以直接高效求解。不过有一个工程上的细节值得展开速度太快时硬约束可能无解。如果前方障碍物距离过近而船速又高MPC几乎不可能找出一条同时满足“跟参考轨迹”“不撞障碍物”“执行器饱和”的控制序列此时求解器会直接返回无解。这是MPC在实际避障中最常见的失败模式。应对策略是两招。第一招是给约束加松弛变量把硬约束变成软约束。数学上可以给障碍物距离约束加一项松弛变量ξ把它写进目标函数里乘一个很大的惩罚系数。这样求解器在无解时会优先松弛安全约束而不是直接罢工。同时你可以设一个ξ_max上限告诉它“你最多能越界多少”。这相当于把安全距离做成一个可伸缩的弹性边界。第二招是“先刹后躲”逻辑。检测到前方近距离障碍物时MPC无法及时绕行那就先在纵向速度上加约束让船减速同时把预测时域拉长让控制器有更充裕的时间来规划绕行路径。实测下来效果好很多船速从2米每秒降到0.5米每秒绕行成功率会大幅提升。如果障碍物是动态的处理方式会复杂一些——需要考虑对障碍物的运动轨迹进行预测。障碍物的预测轨迹可以基于恒定速度模型外推然后把随时间变化的约束写进MPC里形成时变约束。这种做法的优点是一个MPC里同时处理跟踪和避障不需要切模式避免模式切换带来的抖动。4.3 一个控制器跑完所有模式任务切换与管理无人艇在实际任务中不是永远只干一件事。它会经历离开码头、沿路径巡航、发现障碍物后绕行、重新回到路径、最后返航回收——每件事都需要不同的运行模式。传统做法是用状态机切换不同的控制器PID路径跟踪切PID避障再切PID返航。麻烦在于切换点处容易产生控制量跳变。船正在全速跟踪路径突然进入避障模式推进器指令从“全速前进”突然变成“急转”船上的人能感受到明显的顿挫机械结构也会受到冲击。MPC的好处在于你的框架是统一的。代价函数里的Q、R、Rd在这些模式下基本不变变的只是约束和参考。返航时就把参考轨迹指向回收点避障时把障碍物约束加进去路径跟踪时把参考轨迹设成规划好的路径。整个切换过程是连续的因为控制器求解出来的控制序列天然在相邻周期连续不会出现模式切换引起的跳变。我自己实现的模式管理逻辑大致是这样上层决策模块根据传感器数据判断当前任务状态生成参考轨迹和约束集合MPC下层只负责接收当前状态、参考轨迹和约束条件解出一个控制量。整个架构非常清晰而且换任务只改上层逻辑不改控制器核心代码。模式切换时一个容易忽略的细节是避障结束、恢复路径跟踪时如果船头方向偏离参考轨迹很多直接切回来会导致巨幅纠偏动作船很容易过冲。我的做法是在避障结束后的几秒内给航向偏差权重一个渐变过程让船“温和地”回到参考轨迹上。这个渐变时间一般3到5秒具体看船的回转特性。5. 常见问题排查与调试技巧实录5.1 仿真跑得好好的为什么实艇上一跑就偏说来残酷这是新手最常见的挫败时刻。问题基本出在三个地方。一是模型失配严重。仿真里用的模型参数是理论估算的实艇水动力特性你还没测导致MPC预测的是“理想船”而实际控制的是“真实船”。解决方法是先做模型辨识实验把阻尼系数、惯性参数拟合出来。另一个辅助手段是在模型里加一个缓慢变化的“扰动项”用在线估计去补偿未建模动态这属于进阶技巧。二是状态估计噪声过大。MPC是强依赖状态的算法状态不准预测就是空中楼阁。实艇上用GPS定位横穿桥底或者靠近岸边时GPS会有漂移甚至丢星导致位置反馈突然跳变。控制器瞬间以为船偏离很远疯狂输出控制量来纠偏。解决方法是加滤波但对滤波器的频带要谨慎滤得太狠会带来状态延迟这个延迟对MPC是致命的。推荐的做法是区分位置和速度的频率特性位置信号走慢速滤波速度信号走快速滤波各取所需。三是执行器延迟没有建模进去。你发了一个舵机指令舵机本身要几百毫秒才转到指定角度。仿真里指令即执行实际里却延迟几百毫秒。这个延迟在MPC里的表现是控制器决策“超前”了等你执行的时候状态已经变了。解决办法是在模型里加一个执行器一阶惯性环节让它近似反映舵机和推进器的动态特性。模型矩阵A里把执行器状态加进去虽然状态维度多了一两维但效果立竿见影。5.2 求解超时或者直接无解怎么办求解超时和求解无解是MPC工程落地中两个必须面对的问题。求解超时的原因通常是优化变量过多。比如Np100、Nc50变量数已经接近300线性求解器还能勉强跑但一旦你用了非线性模型或者ipopt这类内点法求解器延迟就会很感人。解决办法按优先级排列缩短Nc让控制变量数减少。对无人艇来说Nc10到15通常表现已经很好了再大收益很小。把问题保持为凸二次规划QP。线性模型 线性/二次约束 二次代价函数整个问题是一个QP用OSQP求解通常在十毫秒以内。如果必须用非线性模型用casadi生成的代码做代码生成优化配合C部署速度可以再提升一个量级。无解的核心原因有三类。第一类是约束定义冲突比如同时设置了“必须在1秒内到达目标点”和“最大速度2米每秒”而目标距离是10米矛盾了。第二类是没有引入松弛变量硬约束过紧导致可行域消失。第三类是预测时域太短来不及让系统在满足约束的情况下到达目标。无解的排查方法是先把所有硬约束都设成非常宽松的值用二分法逐个收紧找到是哪个约束导致问题不可行的。这个调试技巧可以帮你快速锁定冲突源比对着报错信息瞎猜效率高得多。5.3 参数整定的三个独家经验做无人艇MPC也有两年多了我把三个非常实用的整定经验分享出来。第一个经验是用饱和分析来定R矩阵的初始值。很多新手不知道R矩阵怎么给随便填个0.1。更好的做法是分析执行器的饱和情况如果控制量经常打到最大值说明R太小如果控制量波动很小但对跟踪误差的修正不够说明R太大。一个靠谱的初始值是让R矩阵的对角元素等于最大控制量的倒数的平方。比如最大推力10NR的推力项初始值取0.01最大转艏力矩2N·mR的转艏项初始值取0.25。这样保证了代价函数中控制能量和跟踪误差的单位尺度一致整定起来更快。第二个经验是从仿真到实艇的权重转换不是一个简单平移。实艇上水动力噪声和环境扰动都远大于仿真你会发现同样的Q、R在仿真里跟得很好实艇上却偏保守或者偏激原因在于模型误差放大了反馈增益的需求。我的做法是实艇第一次上水时把Q矩阵整体缩小30%让控制器温和一些先摸清实艇脾气确认状态估计可靠后再逐步加大Q直到跟踪性能满足要求。第三个经验是日志记录和回放系统一定要建。MPC内部有太多变量状态预测序列、代价函数各分量、约束裕度、求解耗时。这些都是定位问题时最重要的线索。我习惯把每个控制周期所有变量存成CSV或者SQLite数据库调试时画图比对。有一次船下水后突然频繁急转弯回看日志发现障碍物检测模块传过来的虚拟障碍物坐标一直在跳动导致约束反复触发。这类问题不看日志根本没法定位。再补充一个经验在统筹测试时永远留手动遥控通道。这个建议可能看起来跟MPC无关但实艇试验中一旦MPC出现bug你还有办法把船强制拉回来避免安全事故。我们就遇到过控制器状态估计异常导致输出持续跑偏的情况备用遥控通道救了一次试验。写在最后几点实在体会玩MPC这两年我自己最大的体会是它不会让一个烂模型变好但它能让一个好模型发挥出PID给不了的潜力。如果你手上有个无人艇项目正卡在跟踪精度和抗扰动性能上先别急着调PID花点时间把模型辨识做了把MPC框架搭出来你会发现从“能跑”到“跑得准”并没有想象中那么遥远。想把MPC真正用好我建议按这个路线图走先用小艇做湖试以最基础的轨迹跟踪任务为起点把状态估计、权重整定、控制周期这些基本功磨扎实再逐步加入避障和动态场景。直接一上来就做全功能会很难受因为问题叠加在一起你根本分不清是哪个环节出了岔子。最后分享一个小技巧MPC控制器的权重不一定要最优化但一定要物理意义清楚。只有每一组权重、每一个约束你都解释得了它为什么取这个值这台控制器才是你真正能够维护、调优、信任的东西。这个思路不仅适用于无人艇在任何基于MPC的自动控制系统里都一样。
做无人艇控制这几年我有个很深的感触很多团队把硬件折腾得漂漂亮亮电机、电池、通讯一套下来花了不少钱结果一下水就露馅——航迹走不直、抗不了风、避障躲不过去。问题基本不在船体而在控制器。早些年大家最常用PID简单是简单但碰上横风横流、执行器饱和、动态障碍物这些场景参数调到头也压不住。后来我转到模型预测控制Model Predictive ControlMPC这条路上解决了我手上好几个项目的核心痛点包括无人艇的轨迹跟踪、航向保持和自主避障。这篇东西不是教科书复读是我在实际调试和实艇试验里攒下来的经验适合正在做无人船控制的研究生、工程师以及准备把控制器从PID往更优方案迁移的团队参考。我先把话放这儿MPC不是万能的它也有计算量大、模型依赖强、调参费神的毛病。但对你手上的无人艇来说它能帮你把“能跑”变成“跑得稳、跑得准”而且你能提前看到未来一段时间的状态把约束条件直接写进控制器里这件事在PID框架里几乎做不到。下面我把模型怎么搭、参数怎么定、软件怎么写、实艇怎么调一整套讲清楚。1. 为什么无人艇控制里大家都在聊MPC1.1 无人艇控制难点不是你想的那么简单很多人第一次接触无人艇第一反应是“这不就是水面上的遥控车吗陀螺仪加PID不就够了”。实际做下来完全不是那么回事。无人艇有三个特点直接把它跟地面无人车拉开差距。首先无人艇没有刹车。地面车辆可以靠制动力矩快速减速无人艇全速航行时你要是想让它停下来只能靠倒车或者漂移动态响应慢得让人着急。控制算法如果预见性不够等发现偏了再纠偏船早就冲过去了。其次是恶劣且持续的环境扰动。风、浪、流是无人艇的“标配”。尤其在水面上风对船体的作用力很不均匀浪会带来周期性干扰流的影响更麻烦它不是固定方向固定大小而是随空间和时间都在变的。这些扰动对控制器的考验是实打实的。再就是执行器饱和问题。你的推进器只有那么大的推力舵只有那么大的转角范围。很多实验室做仿真的时候不把饱和当回事控制器输出算出来多大就用多大结果上实艇发现压根执行不了。明明控制器让舵打20度但机械限位只有15度这时候控制误差引发的后果会直接传导到整个闭环。还有一个常被低估的难点无人艇的动力学是强非线性的。你去看水面船只的运动模型会发现纵荡、横荡和艏摇三个自由度之间有强耦合低速时还好中高速时水动力特性显著变化线性模型很容易失真。这也是为什么PID在一个航速段调得不错换个航速段又不行了的原因。1.2 PID、LQR都试试为什么最后落在MPC上我最早在一艘1.2米的小型无人艇上用的是串级PID内环航向角速率外环航向角再加一个位置环来跟踪轨迹。湖试的时候天气好微风无浪效果还行。但一遇到3级风以上偏差就明显加大尤其转弯后对准直线航迹那一段振荡能持续很久一直是调参的死循环——你调小比例系数稳定性上来了但响应慢了调大比例系数跟踪快了又开始超调怎么都不舒服。LQR也试过它是线性二次型最优控制相比PID有模型支撑理论上是漂亮。但LQR处理不了约束。你算出来的最优控制量有可能是5米每秒的加速度但你的推进器物理上限才2米每秒那这个“最优”就毫无意义。更尴尬的是LQR只能针对一个工作点设计船体状态偏离这个点之后性能下降明显尤其转弯这种大幅操作效果比PID好不到哪里去。MPC的思路则完全不同。它把约束不再当回事处理而是当成本身就必须要满足的条件直接写进优化问题里。每一时刻控制器都基于当前状态利用模型预测未来一段时间内的状态轨迹然后在这个预测轨迹上求解一个带约束的最优控制问题只执行第一个控制量下一时刻再重新来一遍。这种“预测滚动优化反馈校正”的架构天然适合无人艇这种大惯性、多约束、扰动强的被控对象。我手头的一个模拟项目X就是用MPC代替PID之后横风场景下的平均轨迹跟踪误差从原来的近两米降到了四十几厘米转弯超调量更是直接减少了一半以上。这个效果不是调两个月PID能调出来的。1.3 MPC核心思想看一眼未来再动手为了把MPC讲清楚我习惯拿开车做类比。你开车的时候绝不会只看车头跟前那一段路你会看前方几十米甚至更远的地方提前判断这个弯怎么打方向、什么时候开始减速同时还会考虑路边有没有障碍物、车道边界在哪里。这个“看远”的行为就是预测时域。MPC每走一步就重新做一次“往前看”的规划计算执行完当前这一步之后再重新预测、重新优化这就是滚动优化。它保证控制器永远基于最新信息做决策不至于一条道跑到黑。而约束就像路上的实线、护栏、限速牌你不能压线、不能撞护栏、不能超速。这些限制在MPC里是以数学不等式的形式出现的求解器会在满足所有不等式的前提下找出最优控制量。三句话总结MPC的工作流程预测用系统模型从当前状态出发推算未来N个步长的状态序列。优化在满足输入约束和状态约束的前提下最小化一个包含跟踪误差、控制量变化量的目标函数。执行与滚动把解出来的最优控制序列的第一步施加给被控对象下一时刻重复第1步。这就是MPC能在无人艇上解决实际问题的根本原因它的每一步决策都建立在对未来状态的预判上而不是对过去误差的被动反应。2. MPC控制器的模型基础与核心设计2.1 三自由度船舶运动学模型怎么搭MPC再强也逃不过一个基本事实预测全靠模型模型不准预测就是玄学。所以第一步把无人艇的运动学模型搭清楚。我常用的坐标系分两个一是大地固定坐标系用于描述船的位置和航向二是船体固定坐标系用于描述速度。这里按无人艇最常见的三自由度运动学模型来讲纵荡surge、横荡sway、艏摇yaw。在大地坐标系下船舶位置和航向的导数和船体坐标系速度的关系如下x_dot u * cos(ψ) - v * sin(ψ) y_dot u * sin(ψ) v * cos(ψ) ψ_dot r其中u是纵向速度船头方向v是横向速度垂直于船头方向r是艏摇角速度ψ是航向角。这个运动学模型是通用的没有近似。动力学部分就需要做简化了。无人艇的完整六自由度模型参数一大堆大多数情况下用不上。我通常只保留一个线性阻尼的三自由度动力学模型m * (u_dot - v*r) τ_u - d_u * u m * (v_dot u*r) τ_v - d_v * v I_z * r_dot τ_r - d_r * rm是船体质量I_z是绕z轴的转动惯量d_u、d_v、d_r分别对应三个自由度的线性阻尼系数τ_u是推进器的纵向推力τ_r是转艏力矩。τ_v非全驱动无人艇上为0也就是没有侧推。这里你可能会问为什么用线性阻尼模型因为MPC每一刻都在做滚动优化非线性模型虽然更精确但会让优化问题变成非线性规划NLP求解时间直接指数级上升。线性模型的预测精度虽然有限但胜在速度快、稳定性好实艇上跑的绝大多数MPC用的都是线性化模型然后在扰动和误差上通过反馈去补偿。这个取舍非常重要先进控制不是一味追求模型复杂而是要在精度和可算性之间找平衡点。状态空间表达式写成矩阵形式就是x(k1) A * x(k) B * u(k)其中状态向量取[x, y, ψ, u, v, r]控制输入取[τ_u, τ_r]。A和B通过对上述非线性运动学在当前工作点线性化得到。实际离散化时用零阶保持器采样时间为Ts。想提醒你的是搭建模型时尽量去水池或船模水池做几组辨识实验把阻力系数、惯性参数用最小二乘法拟合出来别完全靠理论估算。我见过好几个团队北湖试验时模型参数跟实际差了30%以上预测轨迹完全失真控制器全程在瞎指挥。2.2 预测时域、控制时域、采样时间怎么选MPC有三个时间参数任何一个选不好控制器性能都会出大问题。采样时间 Ts决定控制器每个控制周期跑一次的时间间隔。无人艇通常取0.1秒到0.5秒之间。取太短CPU来不及求解控制周期被打乱取太长预测精度下降动态响应变差。我做过的项目里小艇1米左右采样时间0.2秒居多大一些的无人艇4米到5米可以放到0.5秒。一个经验是以系统最短动态时间常数为参考采样时间取它的1/10到1/5比较合理。预测时域 Np预测未来多少步。它跟采样时间的乘积就是“向前看的时间长度”。对无人艇来说预测时域至少要覆盖一次转弯或避障动作的持续时间。我一般让 Np*Ts 保持在10秒到20秒之间。比如Ts0.2秒时Np取50到100。太小预测没有意义太大计算量上去了但增益却越来越小这个后面会提到。控制时域 Nc只对前多少步做优化。这个值小于Np一般取Np的1/5到1/3。比如Np50时Nc取10到15。控制时域之后的部分控制输入保持最后一步的值不变。这样可以大幅减少优化变量数量求解更快同时对性能的影响很小。一个重要的直觉是预测时域决定“看得多远”控制时域决定“下一步往哪走”。两者之间要有合理的比例太多优化变量会拖慢求解速度太少则控制器过于短视。2.3 约束条件与代价函数怎么设计才合理MPC的核心优势就是把约束写进优化问题里。约束分两类输入约束推进器最大推力、舵机最大转角、控制量变化率限制。以无人艇为例输入约束长这样τ_u_min ≤ τ_u ≤ τ_u_max τ_r_min ≤ τ_r ≤ τ_r_max Δτ_u_min ≤ τ_u(k) - τ_u(k-1) ≤ Δτ_u_max Δτ_r_min ≤ τ_r(k) - τ_r(k-1) ≤ Δτ_r_max第3和第4条是变化率约束。这个往往被新手忽略但对无人艇很重要。舵机是一个有物理惯性的机械结构你让它瞬间从-30度打到30度它根本反应不过来。加了变化率约束之后控制输出更平滑实艇执行更健康。状态约束安全距离不可进入的禁航区、航向或速度限制。在避障场景下通常把障碍物表示为位置空间约束(x(k) - x_obs)^2 (y(k) - y_obs)^2 ≥ R_safe^2代价函数是整个优化的目标。我用的标准形式是J Σ (x(k) - x_ref(k))^T Q (x(k) - x_ref(k)) Σ u(k)^T R u(k) Σ Δu(k)^T Rd Δu(k)简单理解就是让状态跟踪参考轨迹Q项同时让控制量尽量小R项控制变化尽量平缓Rd项。Q和R矩阵的工程意义很直白。Q矩阵权重调大控制器会更积极地去消除跟踪偏差但代价是会容易激发振荡和激进操作R矩阵权重调大控制器对控制量的“消耗”更敏感输出更温和但跟踪变慢。我自己整定的顺序是先用小Q试能跟踪但偏慢然后逐步增大Q直到出现振荡再增大Rd把控制动作变平滑。这个过程重复两三轮基本就能找到一条比较优的参数曲线。还有一个经验Q矩阵中对横向偏差和航向偏差的权重比例决定船的“脾气”。如果你发现船总是贴近参考线但方向偏着走那是航向权重偏低如果船航向很正但位移偏差一直消不掉那是位移权重偏低。实际调的时候边试边找比例不用一上来就追求精确。3. 仿真环境搭建与MPC控制器的实现落地3.1 仿真平台选择Matlab、Python还是ROS在仿真阶段选对平台能省下你后面一个月的头发。我在学习和快速原型阶段偏向用Matlab因为MPC相关的工具箱很成熟。你可以直接调用模型预测控制工具箱也可以自己用矩阵运算写一个POMDP式的小型MPC。Matlab自带的可视化和调试能力确实方便参数扫描、响应曲线查看都是一两行命令的事。但如果你要把代码部署到实艇上我更推荐Python方案。原因很直接实艇上的工控机一般跑LinuxPython的工程衔接比Matlab顺畅得多。加上casadi这个库你可以在Python里用符号化方式定义优化问题然后把它编译成高效的求解器代码。整个过程分工非常清楚casadi负责建模和求导osqp或者ipopt负责数值求解numpy负责矩阵运算。ROS其实是个中间层。如果你的无人艇系统是模块化的传感器模块、决策模块、控制模块用ROS做通讯非常合适。MPC控制器在ROS里通常作为一个独立节点订阅状态和参考轨迹发布控制指令。但ROS本身不是求解器也不是建模工具它只负责让各个模块能对话。我的建议是阶段推荐平台理由学习原理Python casadi自由度高、可验证细节快速原型Python osqp线性MPC求解快、接口简单实艇部署ROS Python节点模块化管理、日志回放方便学术验证Matlab工具箱完善、出图方便3.2 一个可复现的线性MPC控制器实现框架我在这里给你一个可直接参考的Python代码骨架。它实现的是一个线性MPC的轨迹跟踪控制器使用casadi求解器。import casadi as ca import numpy as np # 系统参数从辨识实验获得 Ts 0.2 # 采样时间(s) Np 50 # 预测时域 Nc 15 # 控制时域 # 状态[x, y, psi, u, v, r] # 控制[tau_u, tau_r] A np.array([...]) # 状态矩阵由线性化得到 B np.array([...]) # 输入矩阵 # 约束 tau_u_max 10.0 tau_r_max 2.0 delta_tau_u_max 1.0 delta_tau_r_max 0.5 # 权重矩阵 Q np.diag([10, 10, 5, 1, 1, 2]) # 状态权重 R np.diag([0.1, 0.1]) # 控制权重 Rd np.diag([0.5, 0.5]) # 控制变化率权重 # 定义优化变量 X ca.SX.sym(X, 6, Np1) # 状态序列 U ca.SX.sym(U, 2, Nc) # 控制序列 x0 ca.SX.sym(x0, 6) # 当前状态 x_ref ca.SX.sym(x_ref, 6, Np1) # 参考轨迹 # 初始约束第0步状态等于当前状态 constraints [X[:, 0] - x0] objective 0 # 预测模型约束 for k in range(Np): if k Nc: control U[:, k] else: control U[:, Nc-1] # 控制时域之后保持最后一步 constraints.append(X[:, k1] - (A X[:, k] B control)) # 代价函数 state_err X[:, k1] - x_ref[:, k1] objective ca.mtimes(state_err.T, ca.mtimes(Q, state_err)) objective ca.mtimes(control.T, ca.mtimes(R, control)) if k Nc and k 0: delta_u U[:, k] - U[:, k-1] objective ca.mtimes(delta_u.T, ca.mtimes(Rd, delta_u)) # 输入约束 for k in range(Nc): constraints.append(U[0, k] tau_u_max) constraints.append(U[0, k] -tau_u_max) constraints.append(U[1, k] tau_r_max) constraints.append(U[1, k] -tau_r_max) # 求解器配置 prob {x: ca.vertcat(X.reshape((-1, 1)), U.reshape((-1, 1))), f: objective, g: ca.vertcat(*constraints)} solver ca.qpsol(solver, osqp, prob)这段代码的核心就是做三件事建立预测模型约束、建立代价函数、配置求解器。实际运行的时候需要自己包装成MPC类输入当前状态和参考轨迹输出控制量第一个元素。有几个地方容易踩坑我提一下。一是矩阵维度匹配。在casadi里定义符号变量时维度一定要对清楚。我有一个同事栽在A矩阵维度错了一维编译时完全不报错但运行时解出来的控制量全部是垃圾值。排查了两天才找到。二是状态约束别写太紧。尤其是避障安全距离R_safe这个值比物理船体直径稍微大一点就行留太大容易导致无解。具体来说如果你的船是1.2米长、0.4米宽R_safe取1.5米就够用了。留太大控制器会认为周围处处都是障碍物走到哪里都在强行绕路跟踪精度反而变差。三是OSQP是好选择但不是万能的。对于线性MPC问题OSQP一个内点法求解器速度快、稳定性好非常适合嵌入式部署。但如果你的代价函数里有非线性项或者约束是时变的强非线性就用ipopt。代价是不同的求解器有不同的求解速度上实艇之前必须做SIL软件在环和HIL硬件在环验证。3.3 从仿真到实艇参数整定实操记录仿真跑通了别高兴太早。模型在仿真里再完美拿到实艇上大概率会有让你头疼的偏差。这里分享一个我之前做模拟项目X时完整的参数整定过程。第一步我先在水池里做推力标定实验。记录不同PWM占空比下的船体实际速度拟合出推进器模型。因为实艇的推进器存在死区和非线性这一步如果不做仿真里很漂亮的控制量到实艇上执行效果会大相径庭。第二步做自由衰减实验辨识水动力阻尼系数。把船体推离平衡位置后松手记录位移和速度的衰减曲线用最小二乘拟合出d_u、d_v、d_r。这个实验在水池里做一次精度远高于CFD仿真估算。参数初值是Ts0.2秒Np50Nc15Qdiag(8, 8, 4, 1, 1, 2)Rdiag(0.1, 0.05)Rddiag(0.5, 0.3)最大推力8N最大转艏力矩1.5N·m。湖试第一次跑的时候现象非常典型船能跟着参考轨迹走但航向振荡得很厉害偏差忽大忽小转弯处尤其明显。回看控制日志后发现控制器持续输出满转艏力矩说明Q矩阵对艏摇误差的权重过大控制器在拼命纠偏。我第一轮调整是把Q中对应艏摇角的权重从4降到2同时把Rd的艏摇变化率惩罚从0.3加大到0.6。第二次下水振荡明显减弱但跟踪响应变慢了到达参考线的距离变长。接下来我再把Q的横向位移权重从8升到12补偿响应变慢的问题。如此反复迭代了大约六轮最终参数定为Qdiag(12, 12, 2, 1, 1, 1.5)Rdiag(0.1, 0.03)Rddiag(0.8, 0.5)。实际横向跟踪误差稳定在0.3到0.5米之间转艏过程平滑全程无振荡。很多团队第一次上手MPC总指望一组参数通吃所有工况这是不太现实的。MPC的优点就是它有模型的支撑换工况时你只需要调整权重比从头调PID高效得多。从仿真到实艇的差距也恰恰是你需要花时间在辨识和实船实验上的地方。4. 轨迹跟踪与自主避障MPC的典型任务落地4.1 轨迹跟踪任务从“能跑”到“跟得住”轨迹跟踪是无人艇最基础也最重要的任务。做比赛、做巡检、做测绘本质上都是让船沿着一条规划好的参考轨迹走。实际工程中有两种常见的参考轨迹来源。一是直线航段任务比如在两个GPS航点之间走直线这时参考轨迹可以简单插值得到二是追踪一条预先规划的平滑曲线比如用圆弧过渡的航线或者B样条曲线。无论哪种参考轨迹需要至少包含参考位置和参考航向。MPC干轨迹跟踪这件事画面其实很直观。它在每一时刻“问自己”如果我现在打这么多舵、加这么多油门未来20秒内我的位置会偏离参考线多少然后把未来20秒的总偏离量降到最低。这里有一个关键细节参考航向怎么定义。新手常犯的错误是全篇用一个不变的全局航向角做参考。但船在实际转弯时参考航向是持续变化的。如果参考航向给得不对控制器就会同时受到“位置偏了”和“航向错了”两个误差的夹击操作会很矛盾。正确做法是对参考轨迹上每个点计算它的切线方向作为该时刻的参考航向。这样位置跟踪和航向跟踪就是一致的控制起来自然顺滑。好多人问过我MPC比PID到底强在哪公平地讲在直线平静水域PID经参数优化后能达到跟MPC接近的性能。但一旦遇到转弯、侧风、需要跟一段复杂曲线时PID就吃力了。MPC凭借对曲线参考轨迹的预测提前开始打舵转弯更顺出弯后偏差更小。这个区别不需要高深的数学也能理解PID是根据“现在偏了多少”来纠正MPC是根据“接下来会偏多少”来预防。4.2 障碍物避让怎么融进统一框架避障是无人艇的另一大刚需尤其是水面有人船、浮标、渔网这些实际存在的障碍物。这里说的与躲避动态障碍物类似我需要多讲几步。在MPC框架里避障根本不是单独的一个“模块”而是往优化问题里加一个不等式约束。每一时刻控制器都必须保证预测轨迹上所有点都落在安全区域之外。所有障碍物可以简化为圆形区域然后写成约束不等式。因为圆形约束在二维平面上是凸约束OSQP可以直接高效求解。不过有一个工程上的细节值得展开速度太快时硬约束可能无解。如果前方障碍物距离过近而船速又高MPC几乎不可能找出一条同时满足“跟参考轨迹”“不撞障碍物”“执行器饱和”的控制序列此时求解器会直接返回无解。这是MPC在实际避障中最常见的失败模式。应对策略是两招。第一招是给约束加松弛变量把硬约束变成软约束。数学上可以给障碍物距离约束加一项松弛变量ξ把它写进目标函数里乘一个很大的惩罚系数。这样求解器在无解时会优先松弛安全约束而不是直接罢工。同时你可以设一个ξ_max上限告诉它“你最多能越界多少”。这相当于把安全距离做成一个可伸缩的弹性边界。第二招是“先刹后躲”逻辑。检测到前方近距离障碍物时MPC无法及时绕行那就先在纵向速度上加约束让船减速同时把预测时域拉长让控制器有更充裕的时间来规划绕行路径。实测下来效果好很多船速从2米每秒降到0.5米每秒绕行成功率会大幅提升。如果障碍物是动态的处理方式会复杂一些——需要考虑对障碍物的运动轨迹进行预测。障碍物的预测轨迹可以基于恒定速度模型外推然后把随时间变化的约束写进MPC里形成时变约束。这种做法的优点是一个MPC里同时处理跟踪和避障不需要切模式避免模式切换带来的抖动。4.3 一个控制器跑完所有模式任务切换与管理无人艇在实际任务中不是永远只干一件事。它会经历离开码头、沿路径巡航、发现障碍物后绕行、重新回到路径、最后返航回收——每件事都需要不同的运行模式。传统做法是用状态机切换不同的控制器PID路径跟踪切PID避障再切PID返航。麻烦在于切换点处容易产生控制量跳变。船正在全速跟踪路径突然进入避障模式推进器指令从“全速前进”突然变成“急转”船上的人能感受到明显的顿挫机械结构也会受到冲击。MPC的好处在于你的框架是统一的。代价函数里的Q、R、Rd在这些模式下基本不变变的只是约束和参考。返航时就把参考轨迹指向回收点避障时把障碍物约束加进去路径跟踪时把参考轨迹设成规划好的路径。整个切换过程是连续的因为控制器求解出来的控制序列天然在相邻周期连续不会出现模式切换引起的跳变。我自己实现的模式管理逻辑大致是这样上层决策模块根据传感器数据判断当前任务状态生成参考轨迹和约束集合MPC下层只负责接收当前状态、参考轨迹和约束条件解出一个控制量。整个架构非常清晰而且换任务只改上层逻辑不改控制器核心代码。模式切换时一个容易忽略的细节是避障结束、恢复路径跟踪时如果船头方向偏离参考轨迹很多直接切回来会导致巨幅纠偏动作船很容易过冲。我的做法是在避障结束后的几秒内给航向偏差权重一个渐变过程让船“温和地”回到参考轨迹上。这个渐变时间一般3到5秒具体看船的回转特性。5. 常见问题排查与调试技巧实录5.1 仿真跑得好好的为什么实艇上一跑就偏说来残酷这是新手最常见的挫败时刻。问题基本出在三个地方。一是模型失配严重。仿真里用的模型参数是理论估算的实艇水动力特性你还没测导致MPC预测的是“理想船”而实际控制的是“真实船”。解决方法是先做模型辨识实验把阻尼系数、惯性参数拟合出来。另一个辅助手段是在模型里加一个缓慢变化的“扰动项”用在线估计去补偿未建模动态这属于进阶技巧。二是状态估计噪声过大。MPC是强依赖状态的算法状态不准预测就是空中楼阁。实艇上用GPS定位横穿桥底或者靠近岸边时GPS会有漂移甚至丢星导致位置反馈突然跳变。控制器瞬间以为船偏离很远疯狂输出控制量来纠偏。解决方法是加滤波但对滤波器的频带要谨慎滤得太狠会带来状态延迟这个延迟对MPC是致命的。推荐的做法是区分位置和速度的频率特性位置信号走慢速滤波速度信号走快速滤波各取所需。三是执行器延迟没有建模进去。你发了一个舵机指令舵机本身要几百毫秒才转到指定角度。仿真里指令即执行实际里却延迟几百毫秒。这个延迟在MPC里的表现是控制器决策“超前”了等你执行的时候状态已经变了。解决办法是在模型里加一个执行器一阶惯性环节让它近似反映舵机和推进器的动态特性。模型矩阵A里把执行器状态加进去虽然状态维度多了一两维但效果立竿见影。5.2 求解超时或者直接无解怎么办求解超时和求解无解是MPC工程落地中两个必须面对的问题。求解超时的原因通常是优化变量过多。比如Np100、Nc50变量数已经接近300线性求解器还能勉强跑但一旦你用了非线性模型或者ipopt这类内点法求解器延迟就会很感人。解决办法按优先级排列缩短Nc让控制变量数减少。对无人艇来说Nc10到15通常表现已经很好了再大收益很小。把问题保持为凸二次规划QP。线性模型 线性/二次约束 二次代价函数整个问题是一个QP用OSQP求解通常在十毫秒以内。如果必须用非线性模型用casadi生成的代码做代码生成优化配合C部署速度可以再提升一个量级。无解的核心原因有三类。第一类是约束定义冲突比如同时设置了“必须在1秒内到达目标点”和“最大速度2米每秒”而目标距离是10米矛盾了。第二类是没有引入松弛变量硬约束过紧导致可行域消失。第三类是预测时域太短来不及让系统在满足约束的情况下到达目标。无解的排查方法是先把所有硬约束都设成非常宽松的值用二分法逐个收紧找到是哪个约束导致问题不可行的。这个调试技巧可以帮你快速锁定冲突源比对着报错信息瞎猜效率高得多。5.3 参数整定的三个独家经验做无人艇MPC也有两年多了我把三个非常实用的整定经验分享出来。第一个经验是用饱和分析来定R矩阵的初始值。很多新手不知道R矩阵怎么给随便填个0.1。更好的做法是分析执行器的饱和情况如果控制量经常打到最大值说明R太小如果控制量波动很小但对跟踪误差的修正不够说明R太大。一个靠谱的初始值是让R矩阵的对角元素等于最大控制量的倒数的平方。比如最大推力10NR的推力项初始值取0.01最大转艏力矩2N·mR的转艏项初始值取0.25。这样保证了代价函数中控制能量和跟踪误差的单位尺度一致整定起来更快。第二个经验是从仿真到实艇的权重转换不是一个简单平移。实艇上水动力噪声和环境扰动都远大于仿真你会发现同样的Q、R在仿真里跟得很好实艇上却偏保守或者偏激原因在于模型误差放大了反馈增益的需求。我的做法是实艇第一次上水时把Q矩阵整体缩小30%让控制器温和一些先摸清实艇脾气确认状态估计可靠后再逐步加大Q直到跟踪性能满足要求。第三个经验是日志记录和回放系统一定要建。MPC内部有太多变量状态预测序列、代价函数各分量、约束裕度、求解耗时。这些都是定位问题时最重要的线索。我习惯把每个控制周期所有变量存成CSV或者SQLite数据库调试时画图比对。有一次船下水后突然频繁急转弯回看日志发现障碍物检测模块传过来的虚拟障碍物坐标一直在跳动导致约束反复触发。这类问题不看日志根本没法定位。再补充一个经验在统筹测试时永远留手动遥控通道。这个建议可能看起来跟MPC无关但实艇试验中一旦MPC出现bug你还有办法把船强制拉回来避免安全事故。我们就遇到过控制器状态估计异常导致输出持续跑偏的情况备用遥控通道救了一次试验。写在最后几点实在体会玩MPC这两年我自己最大的体会是它不会让一个烂模型变好但它能让一个好模型发挥出PID给不了的潜力。如果你手上有个无人艇项目正卡在跟踪精度和抗扰动性能上先别急着调PID花点时间把模型辨识做了把MPC框架搭出来你会发现从“能跑”到“跑得准”并没有想象中那么遥远。想把MPC真正用好我建议按这个路线图走先用小艇做湖试以最基础的轨迹跟踪任务为起点把状态估计、权重整定、控制周期这些基本功磨扎实再逐步加入避障和动态场景。直接一上来就做全功能会很难受因为问题叠加在一起你根本分不清是哪个环节出了岔子。最后分享一个小技巧MPC控制器的权重不一定要最优化但一定要物理意义清楚。只有每一组权重、每一个约束你都解释得了它为什么取这个值这台控制器才是你真正能够维护、调优、信任的东西。这个思路不仅适用于无人艇在任何基于MPC的自动控制系统里都一样。
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
