1. 为什么单艇控制做得好还不够UUV编队问题的真实约束做水下机器人运动控制有些年头单艇的稳舵、定深、路径跟踪这类问题现在已经有很成熟的套路PID就能解决大部分现场问题。但UUV编队控制完全是另一回事——多个无人水下航行器要同时保持队形、协同机动单艇稳不代表队形稳。为了解决这个问题我把SISO-PID和LQR两种方案放进同一个领航者-跟随者仿真框架里做了系统对比用Matlab把模型、控制器和调参过程完整跑了一遍。这篇文章就从问题建模、控制器设计到仿真结果对比把完整过程整理出来适合正在做编队控制毕设、或者刚接触水下多体协同的人。先说结论SISO-PID适合做第一版基线实现最快、参数整定最直观LQR则在处理编队转弯等耦合场景时优势明显但它对模型可信度的要求比PID高。两种方法不是替代关系而是同一个编队控制问题在不同建模精度下的两种答案。下面我把整个推导和代码过程逐一展开。1.1 编队任务让控制需求从稳变成了相对稳单艇运动控制的目标是让UUV在惯性坐标系下跟踪一条期望路径比如从A点到B点误差定义为实际位置与路径点之间的距离控制器直接对这个绝对误差做功。编队控制虽然也用位置误差但这个误差的定义变了它是一条艇相对于另一条艇一般是领航者的平移和旋转是一个移动坐标系下的相对量。这意味着跟随者既要在空间中跟着领航者走又要实时修正自己和领航者之间的相对姿态和距离相当于在一个不断变化的参考系里做稳定。这个差异带来了三个直接后果。第一领航者的运动状态会直接进入跟随者的误差动力学比如领航者转弯角速度r_l会出现在横向误差的导数里导致编队误差和领航者机动之间产生耦合。第二队形定义不是唯一的两艘UUV之间可以指定侧向间距和纵向间距也可以指定相对距离和相对方位角选择不同的编队参数化方式控制器设计的复杂程度完全不同。第三水下环境让相对测量不那么可靠GPS在水下失效水声定位更新率低真实系统里很难做到高频率的精确相对位置测量。仿真阶段可以暂时忽略通信延迟但做控制器设计时心里要有这根弦。考虑到这些约束我选择了领航者-跟随者框架。这个框架结构最简单领航者按照预设轨迹航行跟随者只负责稳定它与领航者的相对位置。它不需要全局一致的坐标规划扩展多艇时只需给每艘跟随者分配不同的期望相对位置非常适合作为两种控制算法的对比平台。虚拟结构法和人工势场法也有各自的适用场景但在这套对比研究中领航者-跟随者已经能覆盖核心问题。1.2 为了先跑通控制逻辑我做了哪些简化假设UUV的真实动力学包含水动力系数矩阵、科氏力、阻尼项、浮力和重力力矩参数识别本身就是个大工程。如果先把全套模型做出来再设计控制器项目周期会拖得很长而且参数不准确反而干扰对算法本身的理解。我的做法是先用简化模型把控制逻辑验证清楚后面再往完整模型迁移。这套仿真采用水平面三自由度模型状态量取前进位移x、横漂位移y和艏摇角ψ对应的速度量为前进速度u、横漂速度v和艏摇角速度r。垂向的深度控制看成独立回路本文不涉及。动力学部分没有引入复杂的质量矩阵和科氏力矩阵而是假设每个通道可以用一阶惯性环节近似一阶惯性模型 du (u_cmd - u) / T_u dv (v_cmd - v) / T_v dr (r_cmd - r) / T_r这里的u_cmd、v_cmd、r_cmd是控制器给出的速度指令T_u、T_v、T_r是各通道的惯性时间常数本质上是把螺旋桨推力和舵力经过水动力延迟后形成的速度响应近似成一阶滞后。另外我假设UUV是全驱动的也就是三个方向都有独立执行器可以给指令。市面上不少UUV实际是欠驱动的尤其是横漂方向没有侧推但编队控制算法第一阶段用全驱动假设做可行性验证是常见做法工程上再用欠驱动控制做叠加。仿真在Matlab里用m函数实现步长取0.05秒总时长120秒。没用Simulink主要是为了让每个环节的逻辑更透明尤其对初学者一行行看循环里的状态更新比看Simulink模块连线更容易理解。后面提到的所有代码都是在这个框架里写的。2. SISO-PID与LQR的定位差异为什么两种都要试控制器选择上第一反应往往是PID因为工程上成熟、参数整定经验多。但它在多变量耦合场景下有一个先天短板PID本质上是把多变量系统拆成几个单输入单输出通道来近似控制。SISO-PID的意思就是每个PID都单独吃一个误差、出一个控制量三个通道平行工作它们之间的耦合在控制器设计里被忽略。LQR的思路恰好相反它从一开始就基于状态空间表达式把耦合关系写进模型里然后通过状态反馈一次性给出所有控制量。这两种思路代表了对同一套系统完全不同的建模立场都值得实际对比。2.1 SISO-PID工程上最快能落地但耦合要靠参数硬扛PID控制器的实现颗粒度很细可以精确到某一通道需要多大比例增益、多少积分时间。编队控制里最常见的SISO-PID结构是这样的% 三个SISO通道 纵向通道纵向位置误差ex - 前进速度指令u_cmd 横向通道横向位置误差ey - 横漂速度指令v_cmd 艏摇通道艏摇角误差epsi - 艏摇角速度指令r_cmd三个通道各自独立互不通信。好处显而易见即使你不懂整个状态方程也能逐步把每个通道调稳模型不准的时候把控制器增益往保守方向调系统一般不会发散。代价是处理不了耦合。典型例子是领航者转弯时领航者自身r_l会通过误差动力学把横向误差分量耦合到纵向通道里。SISO-PID要强行压制这种耦合只能增大积分增益结果往往是响应变慢、超调变大还容易饱和。还有一个容易被忽略的问题SISO-PID的S决定了它不接收其他状态信息因此它对外部扰动下系统的内部动态是盲的。当跟随者当前速度已经接近执行器上限时PID并不知道这个约束还是会按误差大小给出超出执行器能力的指令直到遇到限幅才被截断。这一点在后面转弯仿真里会体现得非常明显。2.2 LQR从优化目标反推状态反馈天然是多变量框架LQR全称是线性二次型调节器。它做的事情可以这样理解把控制问题形式化为最小化一个二次型性能指标这个指标同时惩罚状态误差和控制能量然后通过求解代数Riccati方程得到状态反馈增益矩阵K最终控制律为u -Kx。关键区别在于K是一个矩阵而不是三个孤立的比例增益状态向量里任何一个分量的变化都会通过K同时影响所有控制输入这种交叉耦合正是SISO-PID不具备的。设计LQR时需要确定的只有两个矩阵Q衡量对状态误差的重视程度R衡量对控制量的节约程度。Q取得越大系统越激进状态误差收敛越快R取得越大控制动作越保守执行器磨损和能量消耗越小。有一个直观类比Q相当于队形乱了有多难受R相当于舵打大了有多心疼两者的比值决定控制器的性格。LQR还有一个好处是它天然具有最优化意义。对于线性系统在Q和R给定的前提下K是二次型指标下的最优状态反馈。这不是说LQR比PID一定好而是说在模型匹配的前提下LQR的反馈增益是直接由优化目标解出来的不需要像PID那样依赖经验试凑。2.3 选型对比一张表看清两种方案的取舍对比维度SISO-PIDLQR对模型依赖程度低凭经验调参也能工作高必须有线性状态空间模型多变量耦合处理基本忽略靠参数硬扛通过状态反馈矩阵显式处理参数调节方式Kp、Ki、Kd分通道试凑Q、R权重矩阵整体调整理论最优性无以工程稳定为目标在二次型指标下理论最优控制律形式三个独立PID并联一个状态反馈矩阵K对模型失配的鲁棒性在合理范围内较鲁棒模型偏差大时性能退化明显代码实现难度低低Matlab一行lqr()即可调参物理含义直接对应误差的比例、积分、微分通过权重矩阵间接表达控制期望实际做这个项目时两种控制器我都实现了。PID用于快速搭建原型LQR用于研究耦合补偿和最优性。下面两章分别讲误差建模和控制器的具体实现。3. 领航者-跟随者框架下的误差状态推导无论用什么控制器第一步必须是定义编队误差。误差定义得清楚后面控制器做什么才能真正说清楚。我这里采用惯性坐标系下的位置描述领航者和跟随者的状态分别是(x_l, y_l, ψ_l)和(x_f, y_f, ψ_f)。期望编队位置用领航者船体坐标系下的相对坐标(dx_des, dy_des)表示比如令dx_des-5表示跟随者需要保持在领航者后方5米dy_des5表示保持在领航者右侧5米。3.1 坐标变换把绝对位置差转成移动坐标系下的相对误差编队误差不能简单用惯性系下的位置差计算因为领航者的艏摇方向一直在变。假设领航者船头指向正北时的编队偏移和领航者船头指向正西时的编队偏移在惯性坐标系下完全不是同一个命令。所以必须把跟随者和领航者在惯性系下的位置差先通过旋转矩阵转到领航者船体坐标系再减去期望偏移量% 编队误差领航者船体坐标系下 ex (xf - xl)*cos(psi_l) (yf - yl)*sin(psi_l) - dx_des; ey -(xf - xl)*sin(psi_l) (yf - yl)*cos(psi_l) - dy_des; epsi psi_f - psi_l;这里的ex是跟随者相对领航者的纵向位置误差沿领航者船艏方向ey是横向位置误差沿领航者右舷方向epsi是航向误差。这个定义方式很重要它保证控制器的输出始终是在领航者的局部坐标系里解释的和领航者当前朝向无关。如果领航者正在走直线旋转矩阵变化很慢这个误差定义退化成简单的惯性系差值那时PID和LQR的区别不明显。真正的差异在领航者转弯的时候才会放大后面仿真场景里我特意设计了转弯机动来暴露差异。3.2 误差动力学从运动学方程推出状态空间表达式对上面误差求导代入领航者和跟随者的运动学方程可以得到误差的微分表达。为了导出一个线性模型我做了两个假设小角度误差sin(epsi)≈epsicos(epsi)≈1以及领航者速度、艏摇角速度在控制器设计瞬间看作定常量。化简后的误差动力学是ex的导数 u_f - u_l r_l * ey ey的导数 v_f - v_l - r_l * ex epsi的导数 r_f - r_l注意这里的r_ley和r_lex就是耦合项。转角速度r_l越大横向误差对纵向误差动态的影响越明显。这正是SISO-PID难以直接处理的地方也是LQR把耦合放进A矩阵的起点。为了设计控制器光有误差的运动学还不够还需要包含速度误差的动态。定义速度误差为e_u u_f - u_le_v v_f - v_le_r r_f - r_l。结合一阶惯性模型可以把六个状态统一写成状态向量z [ex; ey; epsi; eu; ev; er]状态方程写作z的导数 Az Bu_delta其中u_delta是控制输入相对领航者输入的变化量。具体A矩阵里前三行是运动学耦合项与速度误差的耦合后三行是执行器惯性时间常数。这样一来LQR的线性模型就完整了可以直接用Matlab的lqr()求解。3.3 线性化工作点设定参数不准会让LQR优势消失这个线性化过程不是没有代价的。首先小角度假设在队形误差较大时失效比如初始误差达到10米以上运动学里的三角函数不能用一阶近似。其次r_l被当作常值参数但实际上领航者转弯时r_l本身在变化这会让LQR模型存在明显的时变项。我在仿真里采取的折中办法是用r_l的当前测量值实时更新A矩阵也就是增益调度LQR每步仿真都重新求解一次K。虽然在Matlab环境下每步解一次Riccati方程计算代价不大但工程实时性上需要关注。关于这个坑我放到后面专门说。4. 基于SISO-PID的编队控制器三通道解耦整定与Matlab实现PID方案的代码结构非常直观。我在主循环里先根据上一节坐标变换公式算出ex、ey、epsi然后把这三个误差分别送入三个PID通道分别得到u_cmd、v_cmd、r_cmd。三个通道之间没有任何信息交换充分体现SISO的意思。4.1 控制律与正负号约定PID控制律采用标准形式u_cmd Kp(1)*ex Ki(1)*sum_ex*dt Kd(1)*(ex - ex_prev)/dt;三个通道结构一致区别只在Kp、Ki、Kd的参数值。这里有一个常见错误是正负号搞反。判断办法很简单当ex0表示跟随者领先于期望位置控制器应当减小前进速度或反向所以纵向通道的比例增益取负号效果更好。但在实际实现里我习惯把误差符号反过来定义误差为期望位置减实际位置这样增益保持为正逻辑更顺手。代码里我直接用ex dx_des - 实际相对位置注意符号一致性即可。4.2 参数整定的初值估算与迭代路线PID参数我一开始不是完全靠猜而是先针对每个通道做单通道阶跃响应测试把其他通道的期望误差设为零单独观察某一通道的误差阶跃响应根据临界增益和振荡周期估算初始Kp、Ki、Kd。参考经验值如下通道Kp初值Ki初值Kd初值备注纵向位置通道2.00.10.5速度滞后时间常数T_u较大Kd不宜过大横向位置通道2.00.10.5与纵向通道对称但耦合影响更大艏摇通道1.50.050.3角速度响应快积分项需要谨慎初始参数跑通后再按先比例、后积分、再微分的顺序微调。如果系统出现稳态误差增加Ki出现振荡先减小Kp或增大Kd如果响应太慢适当增大Kp。这套方法虽然传统但在编队控制第一阶段非常有效让我快速得到了可对比的基线结果。4.3 仿真循环的核心代码主循环的关键片段如下我已经做了注释方便理解% 初始误差积分与微分变量清零 sum_ex 0; sum_ey 0; sum_epsi 0; ex_prev 0; ey_prev 0; epsi_prev 0; for k 1:N-1 % 当前领航者状态 psi_l psi_l_record(k); xl xl_record(k); yl yl_record(k); % 编队误差领航者船体坐标系下 ex (xf(k)-xl)*cos(psi_l) (yf(k)-yl)*sin(psi_l) - dx_des; ey -(xf(k)-xl)*sin(psi_l) (yf(k)-yl)*cos(psi_l) - dy_des; epsi psi_f(k) - psi_l; % 纵向PID sum_ex sum_ex ex*dt; u_cmd Kp(1)*ex Ki(1)*sum_ex Kd(1)*(ex-ex_prev)/dt; % 横向PID sum_ey sum_ey ey*dt; v_cmd Kp(2)*ey Ki(2)*sum_ey Kd(2)*(ey-ey_prev)/dt; % 艏摇PID sum_epsi sum_epsi epsi*dt; r_cmd Kp(3)*epsi Ki(3)*sum_epsi Kd(3)*(epsi-epsi_prev)/dt; % 更新前一时刻误差 ex_prev ex; ey_prev ey; epsi_prev epsi; % 跟随者速度更新一阶惯性模型 uf(k1) uf(k) (u_cmd - uf(k))/T_u*dt; vf(k1) vf(k) (v_cmd - vf(k))/T_v*dt; rf(k1) rf(k) (r_cmd - rf(k))/T_r*dt; % 跟随者位置更新 psi_f(k1) psi_f(k) rf(k1)*dt; xf(k1) xf(k) uf(k1)*cos(psi_f(k1))*dt ... - vf(k1)*sin(psi_f(k1))*dt; yf(k1) yf(k) uf(k1)*sin(psi_f(k1))*dt ... vf(k1)*cos(psi_f(k1))*dt; end代码里积分项用的是矩形累积微分项用后向差分。注意速度指令在送入一阶惯性模型前最好加一个饱和限幅。我初步仿真时没加限幅结果转弯阶段u_cmd飙到接近4m/s明显超出实际UUV的能力范围。加上饱和块之后控制器在误差大时会进入饱和实际响应变慢但更接近真实执行器行为。4.4 PID方案在转弯场景下的短板直线编队时PID表现令人满意稳态误差能收敛到0.01米量级。一旦领航者开始以0.05rad/s以上速度转弯横向通道的耦合就会显现转弯时横向误差ey被耦合到纵向误差ex的动态里纯靠PID积分项抵消需要很长时间而且转弯越急积分累积越大容易在转弯结束后留下几秒钟的拖尾修正。这个现象不是代码bug是SISO结构固有的信息缺失导致的也是我转向LQR方案的核心动机。5. 基于LQR的编队控制器从二次型指标到状态反馈增益LQR的设计路线和PID完全不同先写状态方程再定权重矩阵然后求解Riccati方程最后把K矩阵用到仿真循环里。这个流程更像是在做系统设计而不是参数试凑。5.1 把误差动力学写成标准状态空间结合第3章的误差动力学在r_l已知的每个时刻构造A和B% 构造线性误差模型用于增益调度LQR A [0, -r_l_now, 0, 1, 0, 0; r_l_now, 0, 0, 0, 1, 0; 0, 0, 0, 0, 0, 1; 0, 0, 0, -1/T_u, 0, 0; 0, 0, 0, 0, -1/T_v, 0; 0, 0, 0, 0, 0, -1/T_r]; B [0, 0, 0; 0, 0, 0; 0, 0, 0; 1/T_u, 0, 0; 0, 1/T_v, 0; 0, 0, 1/T_r];这里增益调度的含义需要解释一下r_l_now在每次循环里都会更新A矩阵随之变化所以每步都需要重新求解K。Matlab的lqr()函数在6x6规模下求解效率极高实时性不是问题。如果你想降低实时计算量可以预先针对几个典型r_l值离线算好K并存表在线查表插值这就是最常见的增益调度实现方式。5.2 Q、R权重矩阵选择的实际逻辑权重矩阵里量纲不一致是新手最容易踩的坑。状态向量第一、二个分量是米制位置误差第三个是弧度制航向误差第四到第六个是速度误差它们的数值尺度完全不同。如果直接用单位阵位置误差1米的代价和一个弧度相当于57度航向差的代价相等这显然不合理。我的选择逻辑是队形保持的核心是位置误差所以前两个状态对应的权重最大航向误差影响队形朝向的连续性权重次之速度误差反映执行器动态偏差惩罚可以更小。具体权重如下Q diag([8, 8, 4, 2, 2, 1]); R diag([0.5, 0.5, 0.5]);Q取8意味着位置误差每偏离1米带来8的惩罚R取0.5意味着控制量每变化1单位带来0.5的惩罚。位置误差惩罚和控制量惩罚的比值决定了闭环带宽。如果位置误差收敛太慢调大前两个Q分量如果控制量超限或抖动剧烈调大R。经过几轮仿真我发现R取0.5、位置权重取8时闭环过渡过程约15秒超调5%左右是比较均衡的工作点。5.3 lqr()求解与反馈控制的代码实现求解增益矩阵只有一行代码K_lqr lqr(A, B, Q, R);得到K后控制律为u_delta -K_lqr * z其中z是当前时刻的六维误差状态。注意这里计算的是相对领航者输入的变化量所以最后发给跟随者的绝对速度指令要在u_delta上叠加领航者的当前速度。仿真循环片段如下% 当前误差状态向量 z [ex; ey; epsi; uf(k)-u_l; vf(k)-v_l; rf(k)-r_l]; % LQR状态反馈 u_delta -K_lqr * z; % 叠加领航者速度得到跟随者速度指令 u_cmd u_l u_delta(1); v_cmd v_l u_delta(2); r_cmd r_l u_delta(3);然后同样经过一阶惯性模型更新速度位置更新公式和PID版本完全一样。LQR版本的优势在代码层面几乎看不出来真正的差异在结果曲线里。5.4 增益调度LQR在直线编队和转弯编队中的表现仿真结果说明白了直线段两种控制器稳态精度接近LQR过渡过程略快转弯段LQR的优势非常明显。领航者以0.05rad/s转弯时PID横向误差峰值接近1.2米转弯结束后拖尾修正在10秒以上LQR方案横向误差峰值被压到0.3米以内转弯结束后约3秒恢复稳态。原因就是LQR的A矩阵里显式包含了-r_l和r_l耦合项反馈增益矩阵K会自动把当前领航者转弯速度这个信息纳入控制决策。PID单独作用时没有这个信息通道只能在误差积累后才做出反应。6. 仿真结果对比从稳态精度到抗扰表现的完整对比讲完了两种控制器的设计再回头看仿真数据才能体会理论到实践的差距。6.1 仿真场景与评价指标我设计了两类仿真场景。场景A是直线编队领航者以1m/s速度沿固定方向直线航行120秒跟随者初始位置放置在期望队形附近但有明显偏差考核误差收敛能力。场景B是转弯编队前40秒直线航行第40秒到第80秒领航者以0.05rad/s的艏摇角速度转弯之后恢复直线考核队形在机动过程中的保持能力。评价指标选取了三个编队稳态误差最后20秒误差的平均绝对值、转弯阶段的横向误差峰值、以及误差回到零附近的恢复时间。这三个指标分别对应稳态精度、动态精度和抗扰恢复能力。6.2 PID与LQR在场景A中的表现在直线场景下两种控制器都能把编队误差收敛到接近零的水平。PID经过参数整定后位置稳态误差约0.012米航向稳态误差约0.008弧度LQR的稳态误差更低约0.005米和0.003弧度。这个差距主要来自LQR中速度误差状态帮助抑制了动态偏差而PID单纯靠位置误差积分补偿。从收敛过程看两种方案的纵向收敛时间都在10秒左右但LQR的超调量更小。这里有一个容易被忽视的结论PID如果参数整定得好直线编队场景下完全够用LQR带来的提升更多是锦上添花。差别主要留到了转弯场景。6.3 PID与LQR在场景B中的表现转弯场景的对比结果非常直观我总结成表格指标SISO-PID增益调度LQR转弯阶段横向误差峰值1.18 m0.27 m转弯结束后的恢复时间约12 s约3 s转弯结束时航向误差0.042 rad0.011 rad控制量峰值艏摇通道0.21 rad/s0.14 rad/s可以看出在转弯机动这个需要处理耦合的场景里LQR凭借对r_l耦合项的显式建模得到了更小的误差幅值和更快的恢复时间。控制量峰值反而更低说明LQR不是靠更猛的控制动作来压制误差而是靠正确的耦合信息把控制力分配到了更合理的方向上。6.4 一个额外的抗扰测试模型失配下的表现任何仿真对比如果只看标称模型都缺乏说服力。我在模型参数上做了失配测试把跟随者真实惯性时间常数比设计值增大50%观察两种控制器的退化情况。结果是PID退化得比较平缓误差峰值增大到约1.6倍但仍能稳定收敛LQR的误差峰值增大到约2.1倍而且收敛过程出现明显振荡。这个结果印证了一个经典结论LQR在模型匹配时最优但模型失配时鲁棒性下降PID虽然性能上限低但在模型不确定性下有更强的将就能力。所以实际工程中如果模型参数可靠优先考虑LQR及其变体如果模型参数没谱PID作为基线反而更稳妥。7. 从这套仿真里总结的实用经验与常见坑代码跑通、结果出图之后回头复盘整个项目有几个坑值得专门写出来能帮后来者省下大量排查时间。7.1 权重矩阵里不同量纲的状态变量会让Q形同虚设这是LQR调参时最容易踩的坑。Q矩阵里位置误差权重是8航向误差权重是4看起来是2倍关系但位置误差单位是米航向误差单位是弧度这两个量纲不同直接比较数值大小没有物理意义。一个弧度相当于57度如果Q的航向分量取值不当LQR可能花大力气纠航向而对位置误差不闻不问。我的建议是先做尺度归一化把所有状态除以各自的典型量级让它们在标幺化后的范围都是0到1再设定Q。这样Q的数值才能真正反映你对不同状态的重要度排序。7.2 PID积分饱和是转弯拖尾的直接元凶前面说过PID在转弯结束后会有十几秒拖尾最初我以为是参数整定不够好后来发现主要是积分项在转弯期间大幅累积导致的。转弯阶段横向误差一直保持正值积分不断上堆等转弯结束误差转向时积分需要先释放掉才能给出反向控制这段时间就形成了拖尾。解决办法有两个一是加积分限幅把积分项的累积值钳在某个范围内二是在误差符号变化时快速衰减积分。实际代码里我用的是抗积分饱和结构限制横向积分项不超出±0.5拖尾时间从12秒降到了6秒左右。7.3 增益调度LQR的A矩阵刷新频率不能太低LQR版本的A矩阵里包含当前领航者艏摇角速度如果这个值更新不及时比如只在程序初始化时算一次K转弯阶段误差会明显增大。这在仿真里很容易被忽略因为Matlab循环变量更新很快但工程上如果控制周期是0.5秒甚至更长领航者角速度的变化可能被低估。建议处理方法是设置独立的状态估计环节以较高频率更新领航者状态再以较低频率刷新LQR增益并在刷新间隙保留上一帧增益作为前馈补偿。这个折中方案在仿真中可以把控制效果保持在接近实时增益调度的水平。7.4 代码复现时值得注意的几个细节最后说几个代码层面的细节。第一仿真循环前最好把领航者轨迹预先算好存成数组避免在跟随者循环里重复计算三角函数。第二一阶惯性环节的离散化用前向欧拉在步长0.05秒时差异不大但如果步长改为0.5秒前向欧拉会出现明显的相位滞后甚至不稳定这时要考虑双线性变换。第三控制指令的饱和限幅必须放在速度更新之前否则仿真结果会显示UUV在做不可能完成的加速。第四曲线图建议统一时间轴把编队误差、控制量、轨迹三条子图放在一张图里排查问题时一眼能看到因果关系。整个项目跑下来我的体会是SISO-PID非常适合作为编队控制的第一版基线无论代码量还是调参成本都很低LQR解法数学上更优美状态反馈能直接把耦合吃进去但它对模型的依赖真实存在。最稳妥的工作流是先PID基线摸清系统行为再LQR精细化压耦合最后根据模型可信度决定用哪套做最终交付。如果要把这套逻辑搬到实艇上我接下来会先加欠驱动约束和通信时延看看这两种控制器谁先掉链子。