简介：这份资源面向无人船控制系统开发者与嵌入式学习者，聚焦船体自主航行中的运动控制、导航定位与编队通信等核心环节，适合具备单片机与C语言基础、希望动手实践的中级读者参考。压缩包共588个文件，约16.63MB，以o、d、crf等编译中间文件与h头文件、c源文件为主，另含hex烧录文件、map映射文件及Keil工程配置（uvprojx、uvoptx），可还原完整工程结构。内容围绕电机与舵机的PID控制、GPS定位与差分处理、IMU姿态解算及卡尔曼滤波融合、ZigBee编队通信展开，单片机作为控制中心整合多路传感器数据并驱动舵机。已有3309人学习下载，读者可据此理解各模块的驱动组织方式与工程配置思路，为自建无人船控制程序提供参考。
1. 无人船编队控制：一份能直接跑起来的控制系统拆解
去年帮一个做水域巡检的朋友看一套编队控制代码，他那边三艘船跑直线编队时总在转弯段“打架”——前船减速、后船追尾、队形散成一条斜线。翻完日志发现不是控制律的问题，是航向环和速度环的采样周期没对齐，后船拿到的还是两百毫秒前的前船位置。这类问题在无人船编队里太常见了，而手头这份“无人船控制系统（部分）”资源，恰好把编队控制里最容易翻车的几个模块拆开摆了出来：领航-跟随的队形保持、基于一致性理论的分布式协同、以及单船层面的航向/航速串级控制。它适合两类人：一是刚接手无人船编队项目、需要一份能对照着改的参考实现；二是已经跑通单船、想补上多船协同这一环的从业者。资源是“部分”而非全量，意味着它给的是骨架和关键算法，不是开箱即用的产品，这一点后面会反复提到。
2. 领航-跟随编队：队形几何与期望位置解算
2.1 为什么编队控制先选领航-跟随而不是全分布式
编队控制主流有三条路：领航-跟随（leader-follower）、基于行为（behavior-based）、虚拟结构（virtual structure）。这份资源走的是领航-跟随，理由很实际——它把多船协同降维成“每艘跟随船相对领航船解算一个期望位置，再交给单船控制器去追”，工程上最容易落地。全分布式一致性方法虽然理论上更优雅，但需要每艘船都跑一遍邻居状态融合，通信丢包时收敛性分析会变得很麻烦，调试阶段你很难判断是控制律发散了还是通信延迟导致的。
领航-跟随的核心是一个几何关系：给定领航船位姿 $(x_L, y_L, \psi_L)$，跟随船 $i$ 的期望位置由队形参数（相对距离 $d_i$、相对方位角 $\theta_i$）决定。资源里用的是船体坐标系下的偏移量再旋转到大地坐标系，这个顺序不能反，反了在领航船航向跨过 ±180° 时队形会瞬间翻转。
2.2 期望位置解算的代码实现
import numpy as np def follower_desired_pose(leader_pose, d_i, theta_i): """ 解算跟随船在领航船体坐标系下的期望位姿 leader_pose: [x_L, y_L, psi_L] 大地坐标系，psi 为航向角(rad) d_i: 跟随船相对领航船的距离(m)，正值为前方 theta_i: 相对方位角(rad)，0 表示正前方，逆时针为正 返回: [x_d, y_d, psi_d] 期望位姿（大地坐标系） """ x_L, y_L, psi_L = leader_pose # 体坐标系偏移 -> 大地坐标系，旋转矩阵 R(psi_L) dx_body = d_i * np.cos(theta_i) dy_body = d_i * np.sin(theta_i) x_d = x_L + dx_body * np.cos(psi_L) - dy_body * np.sin(psi_L) y_d = y_L + dx_body * np.sin(psi_L) + dy_body * np.cos(psi_L) # 期望航向：跟随船朝向与领航船一致（队形保持模式） psi_d = psi_L return np.array([x_d, y_d, psi_d])这段代码的关键在旋转矩阵那两行：dx_body/dy_body是体坐标系下的偏移，乘上 $\cos\psi_L$、$\sin\psi_L$ 组合才是大地坐标。参数d_i的符号约定要统一——资源里正值为领航船前方，如果你的队形是“跟随船在后方”，传负值即可，但整个项目里所有队形配置必须遵守同一约定，否则编队会前后颠倒。theta_i决定横向偏移，三船三角形队形时通常取 ±30° 到 ±45°。
2.3 队形参数配置与常见队形
资源里队形参数是外部配置的，常见几种摆法如下表。注意d_i是沿领航船航向的纵向距离，theta_i是相对该航向的方位角，两者共同决定跟随船落点。
| 队形 | 船数 | d_i (m) | theta_i (deg) | 适用场景 |
|---|---|---|---|---|
| 纵队 | 3 | -8 / -16 | 0 / 0 | 狭窄航道通过 |
| 横队 | 3 | 0 / 0 | -6 / +6 | 宽水域扫测 |
| 三角形 | 3 | -8 / -8 | -35 / +35 | 区域覆盖巡检 |
| 菱形 | 4 | -8 / -8 / -16 | -35 / +35 / 0 | 抗单点失效 |
配置时有个血泪经验：d_i的绝对值不要小于船长加安全距离。资源默认船长 2.5 m，纵队间距 8 m 是留了余量的；如果你把间距压到 4 m，转弯时前船尾流和跟随船的航向振荡会叠加，队形抖得厉害，这不是控制参数能救回来的，得改几何。
3. 一致性协同与单船串级控制：从期望位姿到舵角指令
3.1 一致性协议在编队里的角色
领航-跟随解决了“跟到哪”，但多船之间还需要一个协同层来平滑队形误差、处理某艘船暂时掉线的情况。资源里用了一致性协议做队形误差的分布式修正：每艘船把自己的位置误差和邻居的误差做加权平均，修正量再叠加到期望位置上。协议形式是经典的 $\dot{\xi}i = -\sum{j \in N_i} a_{ij}(\xi_i - \xi_j)$，其中 $\xi_i$ 是第 $i$ 艘船的队形误差向量，$a_{ij}$ 是通信拓扑的邻接权重。
这里有个选型细节：资源没有用全连接拓扑，而是按通信距离阈值建邻接矩阵。原因是水面通信（常见做法是数传电台或 4G/5G 模块）在船距超过一定范围后丢包率陡增，全连接拓扑下一条链路断了整个一致性迭代就卡住。按距离阈值建图，掉一条边还有冗余路径，收敛慢一点但不会崩。
def consensus_correction(errors, adj_matrix, dt, gain=0.5): """ 一致性协议修正队形误差 errors: N x 2 数组，每艘船的队形误差 [ex, ey] adj_matrix: N x N 邻接矩阵，a_ij 为通信权重（0 表示无连接） dt: 控制周期(s) gain: 收敛增益，过大振荡、过小收敛慢 """ N = errors.shape[0] correction = np.zeros_like(errors) for i in range(N): for j in range(N): if adj_matrix[i, j] > 0: correction[i] += adj_matrix[i, j] * (errors[j] - errors[i]) return errors + gain * dt * correctiongain这个参数是调出来的，资源默认 0.5，实际跑的时候如果队形在稳态附近来回摆，先降 gain 到 0.2 看是否收敛，再往上加。dt必须和实际控制周期一致，写成 0.1 但实际循环跑 0.15，积分项会隐性漂移，这种玄学问题查起来很费劲。
3.2 单船串级控制：航向环套航速环
编队层给出期望位姿后，落到单船就是经典的串级控制：外环航向 PID 输出期望角速度，内环航速 PID 输出推力指令。资源里航向环用 PD（微分项抑制转向超调），航速环用 PI（积分项消除稳态速度误差），这个组合在欠驱动无人船上比较稳。
class HeadingController: def __init__(self, kp=1.2, kd=0.35, max_r=0.6): self.kp, self.kd = kp, kd self.max_r = max_r # 最大角速度指令(rad/s) self.prev_err = 0.0 def update(self, psi_d, psi, dt): # 角度误差归一化到 [-pi, pi]，防止跨 ±180° 时跳变 err = np.arctan2(np.sin(psi_d - psi), np.cos(psi_d - psi)) d_err = (err - self.prev_err) / dt r_cmd = self.kp * err + self.kd * d_err self.prev_err = err return np.clip(r_cmd, -self.max_r, self.max_r)角度误差那行arctan2(sin, cos)是必须的，直接相减在航向从 179° 转到 -179° 时会得到 -358° 的巨大误差，舵机直接打满，船原地画圈。max_r限幅对应实际舵机的最大转艏能力，设大了控制器以为能转那么快，实际执行跟不上，积分饱和后又是一轮振荡。航速环同理，max_thrust要按推进器实际推力上限设。
3.3 控制周期与时间戳对齐
第 1 章提到的“打架”问题，根子就在这里。编队层解算期望位置、单船控制器执行、通信接收邻居状态，三个环节各有各的周期。资源里的做法是统一到一个基准时钟，所有模块的时间戳用同一个time.monotonic()源，通信收到的邻居状态带发送时间戳，解算时按时间戳做插值或至少做延迟补偿。
def compensate_delay(neighbor_state, recv_time, now, max_age=0.5): """邻居状态延迟补偿：超过 max_age 直接丢弃，避免用过期数据""" age = now - neighbor_state['timestamp'] if age > max_age: return None # 简单一阶外推：用邻居速度估计当前位置 x_comp = neighbor_state['x'] + neighbor_state['vx'] * age y_comp = neighbor_state['y'] + neighbor_state['vy'] * age return {'x': x_comp, 'y': y_comp, 'timestamp': now}max_age设 0.5 s 是个经验值，超过这个延迟的数据宁可不用——用过期数据做编队解算，比短暂丢一艘船的状态更危险，因为错误的位置会被一致性协议放大到整个队形。
4. 避坑与排查：编队跑起来之后最容易翻车的五件事
4.1 队形在转弯段散架
现象：直线段队形保持良好，一进转弯跟随船就往外甩，队形从三角形拉成一条弧线。原因多半是期望航向直接取了领航船航向psi_d = psi_L，但转弯时跟随船需要提前对准切线方向，否则位置误差和航向误差耦合。解决：在期望航向里叠加一个前馈项，按跟随船到期望点的横向偏差算一个修正角，或者简单点，把psi_d改成指向“当前位置到期望位置”的方位角，转弯段会明显改善。
4.2 通信丢包导致队形周期性抖动
现象：队形每隔几秒抖一下，日志里能看到邻居状态更新间隔不均匀。原因是一致性协议在丢包时用了上一帧的旧误差，修正量突然跳变。解决：给邻居状态加时间戳和max_age过滤（见 3.3），丢包时保持上一拍修正量不变而不是清零，同时把一致性增益临时降一半，等链路恢复再升回去。
4.3 航向跨 ±180° 时舵机打满
现象：船在正北附近航向来回摆，舵角指令在正负最大值之间跳。原因是角度误差没做归一化，psi_d - psi直接相减。解决：所有角度误差统一走arctan2(sin(Δ), cos(Δ))，包括一致性协议里的方位角计算，一处漏了就会复现。
4.4 编队间距设太小导致互相干扰
现象：队形几何上没问题，但跟随船航向持续小幅振荡，速度上不去。原因是间距小于船长加安全距离，前船尾流和跟随船自身控制振荡叠加。解决：纵队间距至少 3 倍船长，横队至少 2 倍船长；如果任务要求密集队形，把航速降下来，振荡幅度和航速正相关。
4.5 仿真跑通实船翻车：模型参数没标定
现象：仿真里队形完美，实船一跑就偏。原因是仿真用的船体模型（质量、转动惯量、阻尼系数）和实船对不上，PID 参数是在错误模型上调的。解决：先做单船航向阶跃响应标定，记录实际转艏速率和舵角的关系，反推等效时间常数，再回头调 PID。这一步没有捷径，资源里的默认参数只能当起点。
5. 进阶：把编队误差做成可观测指标
跑通编队只是第一步，真正难的是判断“现在队形到底好不好”。我一般会在系统里加一个队形误差监控层，把每艘船的期望位置和实际位置的偏差、队形几何的形变程度实时算出来，而不是等肉眼看出散架再回头查日志。
具体做法是维护一个队形误差向量：对每艘跟随船，算实际位置到期望位置的距离误差 $e_{pos}$ 和航向误差 $e_{psi}$，再算一个队形形变指标——所有船两两之间实际距离与期望距离的偏差的均方根。这个形变指标比单船误差更能反映“队形整体是否还成立”，因为单船误差可能被一致性协议互相抵消，但形变指标藏不住。
def formation_metrics(actual_poses, desired_poses, pair_pairs): """ actual_poses/desired_poses: N x 3 [x, y, psi] pair_pairs: [(i, j, d_expected), ...] 期望间距对 """ pos_err = np.linalg.norm(actual_poses[:, :2] - desired_poses[:, :2], axis=1) psi_err = np.abs(np.arctan2(np.sin(actual_poses[:,2]-desired_poses[:,2]), np.cos(actual_poses[:,2]-desired_poses[:,2]))) deform = [] for i, j, d_exp in pair_pairs: d_act = np.linalg.norm(actual_poses[i,:2] - actual_poses[j,:2]) deform.append((d_act - d_exp) ** 2) return {'pos_rmse': float(np.sqrt(np.mean(pos_err**2))), 'psi_rmse': float(np.sqrt(np.mean(psi_err**2))), 'deform_rms': float(np.sqrt(np.mean(deform)))}这三个指标建议按控制周期打日志，跑完一段任务回放曲线：pos_rmse在转弯段抬升是正常的，但如果抬升后不回落说明一致性增益不够；deform_rms持续爬升基本就是通信或时间戳问题；psi_rmse单独飙高而位置误差正常，多半是期望航向解算的耦合项没处理好。有了这三条曲线，调参从“凭感觉”变成“看指标”，效率差好几倍。
还有个习惯值得养成：每次改完队形参数或控制增益，先在仿真里跑一段包含直线、转弯、变速的复合轨迹，把三个指标的曲线存下来做基线，实船跑完再对比。有一次我把一致性增益从 0.5 调到 0.8，仿真里收敛快了，实船deform_rms反而翻倍——增益大了对通信延迟更敏感，仿真没建模延迟所以看不出来。从那以后我每次调协同层参数，都强制在仿真里加一段 200 ms 的通信延迟再验证，这个习惯帮我省了不少实船返工。希望这套拆解和排查思路，能让你在编队调试时少走几个弯路。
本文还有配套的精品资源，点击获取
