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-PID | LQR |
|---|---|---|
| 对模型依赖程度 | 低，凭经验调参也能工作 | 高，必须有线性状态空间模型 |
| 多变量耦合处理 | 基本忽略，靠参数硬扛 | 通过状态反馈矩阵显式处理 |
| 参数调节方式 | 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_des=5表示保持在领航者右侧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)≈epsi，cos(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_l，e_v = v_f - v_l，e_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的参数值。这里有一个常见错误是正负号搞反。判断办法很简单：当ex>0表示跟随者领先于期望位置，控制器应当减小前进速度或反向，所以纵向通道的比例增益取负号效果更好。但在实际实现里，我习惯把误差符号反过来，定义误差为期望位置减实际位置，这样增益保持为正，逻辑更顺手。代码里我直接用ex = dx_des - 实际相对位置，注意符号一致性即可。
4.2 参数整定的初值估算与迭代路线
PID参数我一开始不是完全靠猜，而是先针对每个通道做单通道阶跃响应测试：把其他通道的期望误差设为零，单独观察某一通道的误差阶跃响应，根据临界增益和振荡周期估算初始Kp、Ki、Kd。参考经验值如下：
| 通道 | Kp初值 | Ki初值 | Kd初值 | 备注 |
|---|---|---|---|---|
| 纵向位置通道 | 2.0 | 0.1 | 0.5 | 速度滞后时间常数T_u较大，Kd不宜过大 |
| 横向位置通道 | 2.0 | 0.1 | 0.5 | 与纵向通道对称，但耦合影响更大 |
| 艏摇通道 | 1.5 | 0.05 | 0.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(k+1) = uf(k) + (u_cmd - uf(k))/T_u*dt; vf(k+1) = vf(k) + (v_cmd - vf(k))/T_v*dt; rf(k+1) = rf(k) + (r_cmd - rf(k))/T_r*dt; % 跟随者位置更新 psi_f(k+1) = psi_f(k) + rf(k+1)*dt; xf(k+1) = xf(k) + uf(k+1)*cos(psi_f(k+1))*dt ... - vf(k+1)*sin(psi_f(k+1))*dt; yf(k+1) = yf(k) + uf(k+1)*sin(psi_f(k+1))*dt ... + vf(k+1)*cos(psi_f(k+1))*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 m | 0.27 m |
| 转弯结束后的恢复时间 | 约12 s | 约3 s |
| 转弯结束时航向误差 | 0.042 rad | 0.011 rad |
| 控制量峰值（艏摇通道） | 0.21 rad/s | 0.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精细化压耦合，最后根据模型可信度决定用哪套做最终交付。如果要把这套逻辑搬到实艇上，我接下来会先加欠驱动约束和通信时延，看看这两种控制器谁先掉链子。
