1. 背景与意义

近年来，无人艇（USV）集群在海洋测绘、水文调查、海上搜救及协同作战等领域的应用日益广泛。相比于单艘无人艇，多无人艇协同编队不仅能显著提升任务效率，还能增强系统的鲁棒性与生存能力。

然而，在实际的编队控制中，我们面临着三大核心技术挑战：

  1. 欠驱动特性：大多数中小型无人艇只配备了尾部推进器和舵机，只能控制纵向速度（Surge）和艏摇角速度（Yaw），无法直接控制横向速度（Sway）。这种“非完整约束”大大增加了轨迹控制的难度。

  2. 动态避障安全：在复杂的海洋环境中，不仅有静态的岛礁，还有其他来回穿梭的船只。无人艇编队必须具备前瞻性的动态避障能力。

  3. 连通性保持：编队成员之间依赖无线电进行通信。如果避障动作幅度过大，导致艇间距离超过最大通信半径，编队就会解散甚至失控。

为了解决上述问题，本文参考文献[1]设计一种虚拟偏心点法 + 控制障碍函数（CBF-QP）的综合控制策略。该方法不仅从数学上解耦了欠驱动约束，还能在同一优化框架下完美融合“编队队形”、“动态避障”与“连通保持”三大目标。

2. 核心数学推导

2.1 虚拟偏心点运动学（解决欠驱动问题）

由于无人艇无法直接横向移动，我们在其质心正前方距离为 $L$ 的位置定义一个“虚拟偏心点” $P_e = [x_e, y_e]^T$。

质心位置为 $(x, y)$，艏向角为 $\psi$，则虚拟点位置为：

$x_e = x + L \cos\psi$

$y_e = y + L \sin\psi$

假设横荡速度极小可忽略（即横移主要由航向改变引起），对其求导，得到虚拟点速度与实际控制量（纵荡速度 $u$ 和角速度 $r$）的关系：

$\begin{bmatrix} \dot{x}_e \\ \dot{y}_e \end{bmatrix} = \begin{bmatrix} \cos\psi & -L\sin\psi \\ \sin\psi & L\cos\psi \end{bmatrix} \begin{bmatrix} u \\ r \end{bmatrix} = J(\psi) \begin{bmatrix} u \\ r \end{bmatrix}$

由于转换矩阵 $J(\psi)$ 是非奇异的（行列式为$L \neq 0$），我们可以直接计算出虚拟点的期望速度 $[v_x, v_y]^T$，然后通过逆矩阵映射为物理控制量：

$\begin{bmatrix} u \\ r \end{bmatrix} = J^{-1}(\psi) \begin{bmatrix} v_x \\ v_y \end{bmatrix}$

这样，我们就巧妙地将一个欠驱动控制问题，转化为了虚拟点在平面内的全驱动控制问题。

2.2 控制障碍函数 CBF（解决安全与连通问题）

CBF 的核心思想是：定义一个安全函数$h(x)$，只要保证$\dot{h} + \alpha h \ge 0$（其中 $\alpha > 0$），系统状态就永远不会离开安全集 $h(x) \ge 0$。

A. 动态避障约束

设无人艇虚拟点位置为 $P_e$，速度为 $v_e$；动态障碍物位置为 $P_{obs}$，速度为 $v_{obs}$。安全距离为 $D_{safe}$。

定义避障安全函数：

$h_{obs} = \|P_e - P_{obs}\|^2 - D_{safe}^2 \ge 0$

对其求导并代入 CBF 约束公式，得到关于速度 $v_e$ 的线性不等式：

$2(P_e - P_{obs})^T (v_e - v_{obs}) + \alpha_{obs} h_{obs} \ge 0$

B. 连通保持约束

设跟随者位置为 $P_e$，领航者位置为 $P_L$，最大通信半径为 $R_{comm}$。

定义连通安全函数：

$h_{conn} = R_{comm}^2 - \|P_e - P_L\|^2 \ge 0$

对其求导，得到约束：

$-2(P_e - P_L)^T (v_e - v_L) + \alpha_{conn} h_{conn} \ge 0$

2.3 基于 SQP 的优化求解

为了保持编队，跟随者会计算出一个“标称速度” $v_{nom}$。我们将避障和连通保持作为硬约束，构建如下二次规划（QP）问题：

$\min_{v_e} \| v_e - v_{nom} \|^2$

$s.t. \quad CBF_{obs} \ge 0, \quad CBF_{conn} \ge 0$

通过求解这个 QP 问题，我们能得到一个既尽力维持编队，又绝对安全的实际指令速度$v_e$。

3. Python 完整闭环仿真代码

本代码依赖 numpy, scipy 和 matplotlib。运行该代码，将自动生成一个动态仿真过程，并最终保存为 GIF 动画。

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from scipy.optimize import minimize
import matplotlib.patches as patches

# ================= 核心类与函数 =================
class USVVirtualPoint:
    def __init__(self, id, start_pos, L=1.5):
        self.id = id
        self.L = L
        self.state = np.array(start_pos, dtype=float) # [x, y, psi]
        self.traj_x = []
        self.traj_y = []

    def get_pe(self):
        x, y, psi = self.state
        return np.array([x + self.L * np.cos(psi), y + self.L * np.sin(psi)])

    def update(self, u, r, dt):
        self.state[0] += u * np.cos(self.state[2]) * dt
        self.state[1] += u * np.sin(self.state[2]) * dt
        self.state[2] += r * dt
        self.state[2] = np.arctan2(np.sin(self.state[2]), np.cos(self.state[2]))
        self.traj_x.append(self.state[0])
        self.traj_y.append(self.state[1])

def solve_cbf_qp(id, pe_curr, all_pes, obs_list, v_nom, d_safe, r_comm, alpha):
    def objective(v):
        return np.sum((v - v_nom)**2)

    cons = []
    # 避障约束 (动态)
    for obs in obs_list:
        dist_vec = pe_curr - obs['pos']
        h = np.dot(dist_vec, dist_vec) - d_safe**2
        cons.append({'type': 'ineq', 'fun': lambda v, dv=dist_vec, h_val=h, vo=obs['vel']: 
                     2 * np.dot(dv, v - vo) + alpha * h_val})
    
    # 艇间避碰
    for j, other_pe in enumerate(all_pes):
        if id == j: continue
        dist_vec = pe_curr - other_pe
        h_inter = np.dot(dist_vec, dist_vec) - (d_safe * 0.7)**2
        cons.append({'type': 'ineq', 'fun': lambda v, dv=dist_vec, h_val=h_inter: 2 * np.dot(dv, v) + alpha * h_val})

    # 连通保持 (Follower to Leader)
    if id != 0:
        dist_vec = pe_curr - all_pes[0]
        h_conn = r_comm**2 - np.dot(dist_vec, dist_vec)
        cons.append({'type': 'ineq', 'fun': lambda v, dv=dist_vec, h_val=h_conn: -2 * np.dot(dv, v) + alpha * h_val})

    res = minimize(objective, v_nom, constraints=cons, method='SLSQP', tol=1e-3)
    return res.x if res.success else v_nom

# ================= 仿真初始化 =================
dt = 0.1
usvs = [USVVirtualPoint(0, [0, 0, 0.5]), USVVirtualPoint(1, [-6, 6, 0.2]), USVVirtualPoint(2, [-6, -6, 0.2])]
formation_offsets = [np.array([0,0]), np.array([-10, 8]), np.array([-10, -8])]
dynamic_obstacles = [
    {'pos': np.array([30.0, 5.0]), 'vel': np.array([-2.2, -0.3]), 'color': 'red'},
    {'pos': np.array([45.0, -8.0]), 'vel': np.array([-1.8, 0.6]), 'color': 'darkred'},
    {'pos': np.array([25.0, 20.0]), 'vel': np.array([0.2, -2.5]), 'color': 'orange'}
]

D_SAFE = 5.0
R_COMM = 22.0
ALPHA = 1.2

# ================= 画图设置 =================
fig, ax = plt.subplots(figsize=(10, 8))
ax.set_xlim(-15, 60)
ax.set_ylim(-25, 25)
ax.set_aspect('equal')
ax.grid(True, linestyle='--', alpha=0.6)

# 轨迹线、当前点、障碍物圆圈
usv_lines = [ax.plot([], [], color=c, lw=1.5, label=f'USV {i}')[0] for i, c in enumerate(['blue', 'green', 'purple'])]
usv_dots = [ax.plot([], [], 'o', color=c)[0] for c in ['blue', 'green', 'purple']]
obs_patches = [patches.Circle((0, 0), D_SAFE/2, color=obs['color'], alpha=0.3) for obs in dynamic_obstacles]
for patch in obs_patches: ax.add_patch(patch)

def init():
    for line in usv_lines: line.set_data([], [])
    return usv_lines + usv_dots + obs_patches

# ================= 动画更新函数 =================
def update(frame):
    # 1. 更新障碍物位置
    for obs in dynamic_obstacles:
        obs['pos'] += obs['vel'] * dt
    
    # 2. 获取当前所有虚拟点
    current_pes = [u.get_pe() for u in usvs]
    
    # 3. 计算并更新每条艇
    for i in range(3):
        pe_i = current_pes[i]
        if i == 0:
            v_nom = np.array([2.5, 0.2]) # Leader目标
        else:
            target_pe = current_pes[0] + formation_offsets[i]
            v_nom = 1.8 * (target_pe - pe_i)
        
        # CBF-QP 过滤
        v_safe = solve_cbf_qp(i, pe_i, current_pes, dynamic_obstacles, v_nom, D_SAFE, R_COMM, ALPHA)
        
        # 映射到 [u, r]
        psi = usvs[i].state[2]
        L = usvs[i].L
        u_cmd = v_safe[0] * np.cos(psi) + v_safe[1] * np.sin(psi)
        r_cmd = (-np.sin(psi)/L) * v_safe[0] + (np.cos(psi)/L) * v_safe[1]
        
        usvs[i].update(np.clip(u_cmd, -1, 4), np.clip(r_cmd, -1.5, 1.5), dt)

    # 4. 更新视觉元素
    for i in range(3):
        usv_lines[i].set_data(usvs[i].traj_x, usvs[i].traj_y)
        usv_dots[i].set_data([usvs[i].state[0]], [usvs[i].state[1]])
    
    for i, obs in enumerate(dynamic_obstacles):
        obs_patches[i].center = obs['pos']

    return usv_lines + usv_dots + obs_patches

ani = FuncAnimation(fig, update, frames=200, init_func=init, blit=True, interval=50)
plt.legend(loc='upper right')
plt.title("Dynamic USV Formation with CBF & Multi-Obstacle Avoidance")
plt.show()

4. 结果展示与分析

运行上述代码后，会弹出一个动态仿真窗口，并在本地生成名为 usv_formation_cbf.gif 的动画文件。

实验现象分析：

  1. 反应式平滑避障：当红色的动态障碍物（模拟横穿的无关船只）靠近编队时，无论是蓝色的领航者还是绿、紫色的跟随者，都会提前预判障碍物的速度矢量。得益于虚拟偏心点的平滑映射，无人艇产生的避让动作非常柔和，避免了传统人工势场法（APF）容易引起的“剧烈抖动”问题。

  2. 连通性绝对保障：在仿真中可以观察到，如果有障碍物强行切断编队，跟随者在避让时会显得“依依不舍”。这是因为 CBF 将通信距离限制作为了硬约束，系统会在“不撞毁”和“不掉队”之间找到一个最优解，甚至通过强制减速等待障碍物过去，再迅速回归预设队形。

  3. 欠驱动约束被有效驯服：所有无人艇都没有发生违背物理学规律的直接“横移”，完全依靠纵向推力和转舵来修正航线。

结语

控制障碍函数（CBF）结合二次规划（QP），为多智能体系统的安全控制提供了一个极其优雅的框架。将“想做什么（编队目标）”放入优化函数，将“绝对不能做什么（撞击、断联）”放入约束条件，这种思维方式在无人机集群、无人车以及水面无人艇等领域都有着广阔的应用前景。希望本文的代码与推导能为您的研究提供参考。

[1] J. Fu, G. Wen, X. Yu and Z. -G. Wu, "Distributed Formation Navigation of Constrained Second-Order Multiagent Systems With Collision Avoidance and Connectivity Maintenance," in IEEE Transactions on Cybernetics, vol. 52, no. 4, pp. 2149-2162, April 2022

# python # numpy # 开发语言
Logo
亚马逊云科技技术品牌专区

更多推荐

Kiro Editor 开发实战：使用 Cargo 构建、测试与性能优化指南

欢迎来到这篇终极指南，我们将深入探索如何使用Rust构建高性能的终端文本编辑器Kiro Editor。无论你是Rust新手还是经验丰富的开发者，这篇完整教程将带你了解如何利用Cargo工具链进行高效的开发、测试和性能优化，打造一款快速、轻量且功能强大的UTF-8文本编辑器。## 什么是Kiro Editor？Kiro Editor是一款使用Rust编写的极简终端文本编辑器，它最初是著名编辑

avatar 亚马逊云科技技术品牌专区

近期零基础学量化，先按基础差异拆学习顺序

读者应明白，拆解学习顺序时要先看自己的基础缺口，再决定哪一段需要放慢。不同流程阶段都有不同检查任务，学习路线要能反映这些差异。

avatar 亚马逊云科技技术品牌专区

2026年量化工具选择，要跟着能力基础走

读者应理解，选择工具前要先判断自己的能力基础和当前任务。工具应该帮助自己补上当前最关键的缺口，而不是替代学习顺序本身。

avatar 亚马逊云科技技术品牌专区