水下机器人编队控制：PID与LQR算法实践

1. 无人水下航行器编队控制概述

水下机器人协同作业正成为海洋资源勘探、海底管线巡检等领域的核心技术手段。去年参与某海底观测网项目时，我们团队就曾面临多台UUV协同布放传感器的挑战——既要保持队形稳定，又要应对复杂洋流干扰。传统单机控制方案在这里完全失效，而基于PID和LQR的编队控制算法最终帮我们解决了这个难题。

UUV编队控制本质上要解决三个核心问题：如何描述个体间的相对位置关系（队形建模）、如何设计控制器维持期望队形（控制算法）、如何处理环境扰动带来的不确定性（鲁棒性设计）。这就好比一支水下芭蕾舞团，每位舞者既要记住自己的走位路线，又要实时感知队友位置，还要在湍急水流中保持平衡。

2. 编队控制核心算法解析

2.1 SISO-PID控制方案实现

在南海某次UUV集群试验中，我们曾用经典PID控制实现过5台航行器的菱形编队。PID控制器之所以在水下场景依然有效，关键在于其误差驱动的本质特性——不需要精确的动力学模型，这对建模困难的水下环境尤为重要。

具体实现时，每个自由度（如深度、航向）都对应一组独立的PID控制器。以水平面控制为例：

matlab复制% 水平面PID控制器参数
Kp_yaw = 1.2;   % 航向角比例系数
Ki_yaw = 0.05;  % 积分系数  
Kd_yaw = 0.3;   % 微分系数

Kp_surge = 0.8; % 前向速度比例系数
Ki_surge = 0.02;
Kd_surge = 0.15;

关键经验：水下环境的积分项容易累积误差，建议采用抗积分饱和算法。我们曾在300米深度测试时，因积分项失控导致航行器持续振荡，后通过限制积分上限解决。

2.2 LQR最优控制设计

当需要同时优化能耗与队形精度时，LQR就显示出独特优势。去年为某型观测UUV设计的LQR控制器，在保持0.5米间距精度的同时，比PID方案节能17%。

建立状态空间模型是关键第一步。以领航-跟随模式为例：

code复制状态变量 x = [位置误差, 速度误差, 角度误差, 角速度误差]^T
控制输入 u = [推进器推力, 舵角]^T
系统矩阵 A 来自UUV动力学线性化
控制矩阵 B 与推进器配置相关

代价函数中的Q、R矩阵选取直接影响控制效果：

matlab复制Q = di