带过异构编队项目的人都知道一个道理水里跑的船和地上跑的车放到同一个编队里困难不是“一个能控一个不能控”这么简单而是两个明明都在二维平面上运动动力学特性却差了十万八千里。我当时参与的这个多智能体协同控制项目目标很直接让一条欠驱动的无人船和几台无人车组成一个Leader-Follower编队按规划路径穿过一片带障碍物的开阔场地全程保持队形并且能自动避障。核心算法锁定两条主线MPC模型预测控制负责轨迹跟踪与避障约束一致性理论负责队形状态同步。下面我把从架构设计、公式推导到仿真调试踩过的所有关键节点都复盘一遍尤其适合正在做无人系统、多智能体方向课题的同学参考。1. 先想明白异构编队协同真正的难点在哪1.1 无人船和无人车不是同一种机器的重复部署很多人第一次接触这个题目时容易把船和车当成“两台在地面上跑一样算法的机器人”这是最大的误区。无人船和无人车虽然都在二维平面运动但它们的运动学模型、执行器特性和环境扰动方式完全是两套逻辑。先看无人船。大多数用于编队实验的无人船都是欠驱动模型什么意思就是船只在纵向有推进器推力在转艏方向靠舵或差速产生力矩但横荡方向是没有独立执行机构的。船不能像车一样“直接往侧面走一步”所有横向位移都必须靠艏向调整加纵向速度配合才能实现。再加上水动力附加质量、漂角、风浪流干扰船是一个典型的大惯性、响应慢、强耦合对象。你给一个阶跃转艏指令船要晃好几秒才能转到目标角度这个延迟在编队里会直接拉大跟随误差。再看无人车。车是典型的非完整性约束系统简化成自行车模型后控制输入是前轮转角和纵向加速度虽然也有“不能横移”的约束但车的转向响应比船快一个数量级且轮胎与地面的接触模型远比船-水相互作用好预测。真机上车速高一些、转角大一些也不会出现船的“漂角”这类动态失配问题。最麻烦的是这两类平台要在同一个编队框架下协同。编队指令下发后船要慢慢悠悠地转向车可能已经到位了。如果上层算法不区分这两种执行特性直接把统一指令发给所有平台结果一定是车围着船打转或者船根本没跟上编队参考点。所以项目第一步不是写算法而是把两个平台的动态模型差异量化清楚。我给每条船和每台车单独建了运动学/动力学模型并在仿真里分别测它们的阶跃响应时间常数后续所有协同参数的设计都以“最慢的那个平台”为基准。1.2 一致性理论把“保持队形”翻译成数学协议一致性理论是编队控制里非常经典的方法论。它的思想不复杂把每台智能体看作图中的一个节点智能体之间的通信关系看作边然后设计一个分布式协议让所有节点的某个状态随时间趋于一致。基础的一致性协议长这样$$u_i(t) -\sum_{j \in N_i} a_{ij}(t)\big(x_i(t) - x_j(t)\big)$$其中 $N_i$ 是智能体 $i$ 的邻居集合$a_{ij}$ 是通信权重。这个公式的含义是每个智能体不断把自己的状态和邻居状态做差再沿着误差的负方向微调自己。所有节点都执行这个协议整组状态就会收敛到共同值。用人话说这就是“大家都往邻居的平均位置靠拢”。代数图论在这里扮演数学底层。定义拉普拉斯矩阵 $L$一行一列对应一个智能体对角线元素是邻居数量非对角线是通信连边。一致性协议可以写成矩阵形式 $\dot{x} -Lx$系统的收敛性由拉普拉斯矩阵的特征值决定。只要通信拓扑是连通的系统就是稳定的收敛速度由代数连通度第二小特征值决定。放到编队场景还需要在第0号节点加一个虚拟领航者对应Leader-Follower结构。协议升级成带牵制项的形式$$u_i -\sum_{j \in N_i} a_{ij}(x_i - x_j) - b_i(x_i - r_i)$$这里的 $b_i$ 表示节点能否直接收到领航者信息$r_i$ 是领航者的参考状态。$b_i1$ 的节点可以是实际领航船也可以是虚拟参考点。跟随者只通过局部通信感知邻居和领航者的状态就能实现整个编队的同步移动。但要形成“队形”而不是“聚成一团”还需要在一致性协议里加入期望队形偏移量。给每个智能体定义一个固定偏差 $d_i$把状态量替换成 $x_i - d_i$一致性协议就会让 $x_i - d_i$ 趋同于是各平台之间稳定在期望相对位置上。这就是“状态一致 位置偏置”的组合拳。到这里一致性理论解决的问题是队形的数学表达、分布式通信架构下的收敛保证、以及Leader-Follower的牵引逻辑。但它也有一个致命短板——它只回答了“状态应该收敛到哪”没有回答“物理上能不能收敛到位”。轮子打滑、舵角饱和、水流动荡这些执行层因素一致性协议是完全不管的。所以项目里必须引入一个能处理约束的执行层控制器这就是MPC登场的理由。2. 为什么核心算法锁定“MPC 一致性”这套组合2.1 一致性管宏观队形MPC管微观运动如果只用一个算法解决所有问题最自然的方案是纯一致性协议加一个PID跟踪。但我们在实际仿真里试过编队队形在直道上还能维持一遇到转弯或避障跟随平台的误差立刻暴涨。原因是一致性协议生成的是期望速度或期望位置参考但期望参考和实际可执行指令之间隔着执行器约束——船的舵角不能无限大车的加速度受发动机限制还有安全避障距离这样的硬约束。PID只能根据当前误差去修正控制量它不具备“预判未来”的能力等误差已经变大时再去调整对于船这种大惯性对象已经来不及了。MPC的核心理念是解决问题而不是应对问题。它在每个控制周期内基于当前系统状态和预测模型往前滚动优化一个有限时域的控制序列同时把执行器饱和、避障距离、状态边界等约束显式放进优化问题里。算出最优序列后只执行第一步下一个周期重新求解。这样每一步都在做“带约束的全局规划”而不是“根据误差打补丁”。这是我们选择“一致性 MPC”组合的根本原因一致性负责上层协同目标保证编队数学上收敛MPC负责下层物理可实现性保证每台平台能在约束范围内真正跟上参考。两层各干各的活接口就传参考轨迹和预测状态。2.2 从公式看MPC抢占主动权的逻辑MPC的数学描述虽然写在课本里很复杂但落到代码上就是每周期解一个如下的优化问题$$\min_{u_0,\dots,u_{N-1}} \sum_{k0}^{N} \big| z_k - z_{k,\text{ref}} \big|Q^2 \sum{k0}^{N-1} \big| u_k \big|_R^2$$$$\text{s.t. } z_{k1} f(z_k, u_k),\quad z_k \in \mathcal{Z},\quad u_k \in \mathcal{U},\quad z_0 z(t_0)$$里面 $z_k$ 是预测状态$u_k$ 是控制量$N$ 是预测时域$Q$ 和 $R$ 分别是状态误差和控制量的权重矩阵。约束条件包括动力学模型 $f(z_k, u_k)$、状态边界集合 $\mathcal{Z}$速度上限、避障半径、安全走廊、控制量集合 $\mathcal{U}$舵角限制、推力限制。“预测”两个字是关键。比如船以当前航向往前冲前面20米有一块障碍物PID控制器只有船离障碍物5米时才能检测到横向误差并开始转向但MPC在第一个周期就把未来20步的船位都算了一遍发现按当前指令会在第12步撞进障碍物于是第1步就开始提前转向。这种“为未来预留动作空间”的特性对无人船这种大惯性运动体尤其宝贵。2.3 工程上比“纯一致性”得分更多在哪纯一致性协议本身不关心控制量的物理限制。真实系统里船的最大推力、车的最大前轮转角都是有限值如果一致性协议算出的期望速度是3米每秒但跟随船只能跑到2.5米每秒那队形误差永远不会收敛到零协议还一直认为“大家保持一致是可行的”这就出现了理论上的稳定和物理上的不可行之间的矛盾。MPC相当于给每台平台加了一个“带约束的本地优化器”。一致性协议给出目标后MPC会判断这个目标在当前约束下是否可达。如果不可达它会解出一个最接近这个目标的可行解而不是盲目执行不可行指令。这在工程项目里非常重要——你宁愿让跟随者以最大能力追赶得到一个明确的跟踪误差也不希望控制器输出一个超出执行器范围的值让真机在舵角饱和状态下瞎跑。此外MPC还可以很方便地处理避障。把障碍物约束写成两个圆心的距离必须大于安全半径$$(x_k - x_{\text{obs}})^2 (y_k - y_{\text{obs}})^2 \geq r_{\text{safe}}^2$$这类非线性约束放进优化问题后MPC会自动在预测时域内规划出绕行轨迹不需要额外的避障决策模块。相比纯几何避障算法MPC的避障路径更平滑和编队参考轨迹的衔接也更自然。3. 系统架构从通信拓扑到分层控制框架3.1 分层控制架构协同层、跟踪层、执行层整个系统的架构设计我们用了经典的三层结构分层的主要目的是把“异构”带来的复杂度隔绝在各自层级内。最上层是协同层。这一层运行一致性协议输入是编队队形参数和虚拟领航者的参考轨迹输出是每台平台各自的期望位置、期望速度、期望航向。协同层不关心船和车的动力学差异它操作的是抽象后的状态量统一用二维坐标和航向角表示。这个抽象的代价是协同层输出的期望指令可能是物理上不可行的但这没关系下一层会处理。中间层是轨迹跟踪层。这一层对每台平台单独运行一个MPC控制器输入是协同层下发的期望状态输出是各自平台的控制指令船是推进器推力和转艏力矩车是加速度和前轮转角。这一层的核心工作是保证“在约束条件下尽可能精确地跟踪期望轨迹”。船有船的MPC车有车的MPC模型和约束各自独立互不干扰。最下层是执行层也就是底层驱动控制。这里处理的是电机/发动机的电流环、舵机的PWM信号、推进器的功率分配等。这一层在真机上和车规/船规的电控系统打交道在仿真里就是一个执行器饱和模块加一个低通滤波器。把执行层独立出来是因为它和算法本身关系不大但它会对上层的可行性产生直接影响——比如舵角限位是30度执行层会把它硬限制在30度MPC必须提前知道这个限制才能在计算中避开饱和。三层结构的好处是改船不改车时只动跟踪层和执行层协同层完全不用动改队形策略时只动协同层两个跟踪层也不用动。项目后期我们换了一次船的推进器型号只改了执行层的推力映射参数上层代码几乎没有改动。3.2 队形设计与坐标变换让两套运动学放在一个桌面上算异构平台能协同的前提是“同一把尺子量所有状态”。最初仿真时我们直接把船的GPS坐标和车的GPS坐标塞进同一个坐标系算编队误差结果转弯工况下误差曲线乱成一团。后来定位到问题坐标都在北东地坐标系里没错但航向角的定义方式不一样。船的航向角以正北为0顺时针增加车的航向角在车体坐标系下可能以正东为0逆时针增加两套角度定义不做对齐编队偏差参考点就会在转弯时发生突变。项目里做的关键一步是坐标和姿态的统一。所有平台的状态都用世界坐标系 $(\xi, \eta)$ 表示航向角统一以世界坐标系X轴正方向为0逆时针为正。然后通过一个旋转矩阵把Leader的坐标转换到Follower的期望位置$$p_{i,\text{des}} p_L R(\psi_L) \cdot d_i$$其中 $p_L$ 是Leader位置$d_i$ 是智能体 $i$ 在Leader坐标系中的期望队形偏移$\psi_L$ 是Leader航向$R(\psi_L)$ 是旋转矩阵。这个式子看起来简单但它解决了一个实际问题当Leader转弯时Follower的期望位置不是固定在世界坐标里的某个点而是跟着Leader的朝向一起旋转。如果不加这个旋转项编队在Leader转弯时会像“甩链子”一样向外甩出去转弯结束再慢慢收回来队形误差的暂态峰值能比直道时大十倍。队形参数用一个矩阵 $F [d_1, d_2, \dots, d_n]$ 定义每一列对应一个跟随者的期望偏移。不同的任务阶段可以切换不同的队形矩阵比如窄道通行时把所有偏移纵向收拢成一条线开阔区域展开成V形或楔形。切换过程也需要平滑过渡通常是给 $d_i$ 加一个一阶低通避免Follower期望位置突变。3.3 通信拓扑选型链式、环形还是全连接一致性协议的收敛性和通信拓扑直接相关。我们在这个项目里对比过三种拓扑。链式拓扑最简单所有Follower只和前后邻居通信Leader信息从队首一路往后传。这种拓扑边的数量少通信负担小但误差会沿链路累积末端智能体的队形误差通常比前端大而且在链路某处断链时下游节点可能丢失Leader信息导致编队崩溃。环形拓扑把链的末端接回Leader形成一个环。这样可以提升代数连通度误差累积情况也明显改善但代价是通信链路数增加且每一条链路都要保证双向连通。全连接拓扑稳定性最好但工程上几乎不可行。水面通信带宽、路测电台覆盖范围都有限让所有节点互相广播编队状态会很快耗尽通信资源。我们最终选的是基于Leader的稀疏拓扑Leader直接和所有Follower通信Follower之间也保留部分紧邻链路但每条链路的状态量并不是全状态而是只传位置和航向。这样既保证了Leader信息的直接可达性又控制了通信带宽。实测下来在丢包率低于5%时编队误差能维持在可接受范围丢包率飙升到15%以上时就得靠一致性协议里的时延补偿机制救场。4. 核心实现编队保持与路径规划的MPC实现细节4.1 代价函数设计什么变量先算清楚MPC的效果好不好代价函数的设计占了六成。不是所有误差都值得用同一个权重去压对不同的平台要对症下药。我们的代价函数分三块队形误差项、控制量惩罚项、终端状态项。队形误差项里又细分了位置误差和航向误差$$J \sum_{k1}^{N} \Big( | e_{\text{pos},k} |^2_{Q_{\text{pos}}} | e_{\psi,k} |^2_{Q_{\psi}} \Big) \sum_{k0}^{N-1} \Big( | u_k |^2_R \Big) | e_N |^2_P$$其中位置误差 $e_{\text{pos},k}$ 定义为预测位置与期望位置的三维坐标差航向误差 $e_{\psi,k}$ 是预测航向与期望航向的差值做了角度归一化处理避免 $\pi$ 到 $-\pi$ 跳变导致优化出现假病态。权重设计的经验位置误差权重 $Q_{\text{pos}}$ 通常要远大于航向误差权重 $Q_{\psi}$因为编队看的是位置不是船头朝向但无人船特殊情况由于横向没有直接执行器横向位置误差只能靠艏向变化来修正所以船的 $Q_{\psi}$ 不能设得太小否则横向误差会一直存在。控制量惩罚 $R$ 的作用是让解出来的舵角或推力不要过于“暴躁”取值太小会导致控制器频繁大幅摆动取值太大会让跟踪误差偏大需要来回试。4.2 无人船的MPC实现欠驱动模型要“拐着弯走”无人船的MPC预测模型直接用三自由度运动学加一阶动力学简化$$\dot{x} u \cos\psi - v \sin\psi$$ $$\dot{y} u \sin\psi v \cos\psi$$ $$\dot{\psi} r$$ $$T \dot{r} r K \delta$$其中 $\psi$ 是艏向角$r$ 是艏摇角速度$\delta$ 是舵角指令$K$、$T$ 是Nomoto模型参数。这个模型把复杂的流体力学都压缩到两个参数里工程上足够用。我们用的双体船模型标定出来的 $K$ 大约是0.6$T$ 大约是1.4意思是舵角给定后船的转艏角速度要经过1.4秒的时间常数才慢慢跟上目标这个延迟比车大得多。MPC的控制量是期望推进力矩和期望转艏力矩。因为船没有横荡执行器位置控制策略是先保证船的实际航迹与期望路径的横向偏差尽可能小同时通过艏向调节完成转弯。一句话总结就是“拐着弯走”——你没法让船横着挪到目标横坐标只能让船头偏过去一点靠纵向速度把船带到横向目标位置。欠驱动带来的一个坑是如果MPC预测模型把横荡速度直接忽略了转弯工况下会出现持续的横向偏差。我们的做法是在模型里加一个基于漂角的横荡速度修正项。漂角可以估算成 $\beta \arctan(v/u)$虽然是个近似值但对MPC预测完全够用它让优化器能提前预测到“这个转弯指令会导致船被甩到路线的哪一侧”。4.3 无人车的MPC实现前轮转角控制实打实无人车我们用标准自行车模型$$\dot{x} v \cos\theta$$ $$\dot{y} v \sin\theta$$ $$\dot{\theta} \frac{v}{L} \tan\delta$$$L$ 是轴距$\delta$ 是前轮转角$v$ 是纵向速度。控制输入是加速度 $a$ 和前轮转角 $\delta$约束主要是前轮转角限位$|\delta| \leq \delta_{\max}$纵向速度范围。车的MPC比船好调原因是模型线性化之后误差更容易收敛。我们用的是在参考轨迹附近做线性化得到线性时变系统然后用二次规划求解器每周期解一遍。车和船在同一编队里的适配问题集中在速度上船的纵向速度上限往往远低于车如果编队的Leader是船车就必须频繁减速等待如果代价函数里纵向速度误差权重设置不当车会在编队参考点附近来回振荡。解决方法是给车的纵向速度误差项加一个死区只要实际车速与期望车速的差在0.2米/秒以内就不再惩罚用这个空间换取车的行驶平顺性。4.4 参数初值与调优从仿真里试出来的经验值参数初值设计非常重要。我们最终在仿真里确定的一组典型参数如下可以作为你调参的起点参数数值说明采样周期 Ts0.1 秒与底层控制频率一致预测时域 Np20即预测未来2秒船的惯性需要这长度控制时域 Nc5只优化前0.5秒的控制变量位置误差权重 Qposdiag(10, 10)船的XY权重相同航向误差权重 Qψ2船可以高一点车建议1~2控制量权重 R0.1数值小但能抑制抖舵舵角限位±30 度执行层硬约束车轴距 L1.2 米按车型给定调参顺序建议先把预测时域拉到足够长比如20到30步确保MPC能“看到”前方情况再把位置权重加大确保编队误差优先然后加控制量惩罚抑制抖动最后根据需要的动态响应微调预测时域和控制时域。实际做下来最难调的不是权重而是船的模型参数 $K$ 和 $T$它们直接影响MPC预测的轨迹形状调不好时控制器会在转弯处出现明显的相位滞后。5. 仿真实验与现场调试实录5.1 仿真环境搭建要点仿真层的搭建我们用的是 Matlab/Simulink 加 MPC 求解器。Simulink里搭了船和车的动力学模型、执行器饱和模型和通信模块MPC算法则写成独立的函数块每个平台一个实例。一个重要设计是通信模块不能做成理想无延迟。我们在仿真链路里加了高斯噪声和随机丢包原因是真实的水面无线通信和地面无线通信质量差异明显船的通信容易因为多径效应出现周期性信号衰减车在障碍物后面会直接丢包。提前在仿真里把这些因素加上等于提前给算法做了压力测试后面真机调试省了很多时间。场景建模方面障碍物先简化为静态圆后续再扩展为多圆近似的任意形状。航道边界建模成两侧虚拟墙约束直接写成MPC的硬约束。这样MPC在避障的同时还要保证不出界相当于同时处理动态障碍和静态边界。5.2 三个典型实验场景直线、转弯、变速我们设计了三个场景来验证整套“一致性 MPC”框架。场景一是匀速直线编队。Leader以2米/秒速度直线前进三台Follower分别布成左后、右后、正后三点队形所有人以一致的纵向速度行进。这个场景验证基础编队保持能力。实测效果是稳定后跟随误差约0.2米末端节点因为通信链路多一跳误差稍大但仍在期望范围。场景二是带障碍物的绕行。Leader提前在MPC避障约束中完成绕行Follower通过一致性协议跟随Leader的期望位置。这个场景的难点不是单机避障而是在Leader绕行时Follower的队形偏差参考点要随Leader航向一起旋转同时还要避开同一个障碍物。我们启动的队形是横向展开的横排绕行时先在协同层把队形切换为纵向窄队形通过障碍后再展开。最终绕行路径平滑队形误差暂态峰值在0.6米以内。场景三是Leader变速。Leader先匀速、再加速到3米/秒、再急减速。这个场景的观察重点是纵向防碰撞。车和船的执行器响应差异在这个场景暴露得最充分。加一致性协议后Follower的期望速度能很快跟踪Leader但船的推力上升慢瞬态速度误差最大能达到0.7米/秒不过MPC会提前预测到速度偏差并且动态调整推进指令整个过程中没有发生队内间距小于安全距离的情况。5.3 从仿真到真机三个容易翻车的地方仿真跑得再好真机测试都会暴露新问题。我们这次从仿真切真机踩了三个比较典型的坑。第一个坑是执行器饱和。仿真里的MPC约束是把舵角限制在±30度但因为Simulink里的饱和模块是硬截断MPC求解器误以为舵角能一直施加最大限制导致在真机上出现舵角在最大值位来回打的现象。解决办法是在MPC模型里加一个有效的舵角速率约束限制舵角每步变化量让舵机有足够时间跟上指令。第二个坑是坐标对齐精度。无人机车的RTK定位和船的GNSS定位在同一个全局坐标下理论上是统一的但船的航向信息如果来自磁罗经在金属结构附近会产生固定偏差这个偏差会导致编队期望位置整体偏转一个角度看起来就是队形斜了。排查了很久才发现是罗经安装位置的磁干扰问题。做法是加一个固定的航向偏差补偿同时在上层一致性协议里对航向做加权平均削弱单点偏差的影响。第三个坑是通信丢包下的数值奔溃。一致性协议在丢包时的处理如果只是简单地把丢失的包当成0会导致拉普拉斯矩阵的行和不再为零状态更新会发散。我们最后采用的是“保持最近有效值”策略某个邻居的状态没更新时沿用上一个成功接收的值并在协议中加入一个衰减因子让旧数据的影响随时间衰减。这个改动让编队在通信质量较差时依然能维持50%以上的理论性能。6. 常见问题与排查速查表最后把我调试过程中频繁遇到的问题整理成一张速查表方便你在复现时快速定位问题。现象可能原因排查方法解决方案编队整体向一侧偏移航向传感器安装偏角或磁干扰对比静止时RTK方向与航向传感器读数做一次性航向校准补偿末端Follower误差持续增大一致性协议丢包导致状态发散检查通信模块丢包率加入最近有效值保持和衰减因子Leader转弯时队形像甩链子期望偏差未随Leader航向旋转查看转弯时Follower期望位置曲线在协作层加入旋转矩阵 $R(\psi_L)$MPC求解失败或输出抖动约束过紧导致无可行解查看QP求解器的退出标志位把硬约束改成带罚函数值的软约束船转弯存在明显相位滞后预测模型里横荡速度被忽略对比预测轨迹和实际轨迹加入漂角估计修正横摆速度车频繁加减速振荡速度误差权重过高观察车MPC控制量曲线引入速度误差死区编队在直线段正常、转弯段发散预测时域太短增加Np观察预测轨迹将Np提升到20或更大真机舵角始终处于限位状态执行器速率限制未建模查看舵角变化率和限制值在MPC约束中加入舵角速率限制排查问题的通用思路我建议按“从仿真到真机”的顺序逐层定位。先在纯仿真环境关掉通信噪声能复现再开噪声仿真正常是真机异常重点排查坐标系、传感器和执行器真机异常且现象随机重点排查通信。写在最后的小经验最后分享一个我自己调试这套系统时的习惯无论仿真效果多漂亮第一轮真机试跑时我会先把所有MPC权重统一缩到原来的十分之一再把Leader速度压到理论值的一半跑通整个编队流程后再逐步把权重和速度恢复上去。因为仿真里再好看的误差曲线在第一轮真机验证时主要面对的其实是坐标对齐、通信丢包和执行器饱和这些“非算法因素”。先把全链路跑通把平台的机器特性摸清楚再回头精细调MPC参数远比一开始就追求完美编队误差要快得多。这套“一致性 MPC”的框架有一个很自然的扩展方向把当前固定的通信拓扑改成时变的比如编队转弯时自动切换拓扑结构或者在一致性协议中加入自适应权重根据实时通信质量调整邻居信任度。如果你正在做同类研究建议把基础框架跑稳定之后再往动态拓扑和自适应参数的方向延伸每一步都有明确的可验证指标不容易走偏。