用Python和NumPy手把手实现多智能体仿射队形控制（附完整代码与避坑指南）

用Python和NumPy手把手实现多智能体仿射队形控制（附完整代码与避坑指南）

当无人机编队需要快速变换队形时，如何确保每个智能体都能准确计算自己的目标位置？仿射队形控制为解决这一问题提供了数学框架。本文将绕过复杂的理论推导，直接带您用Python实现核心算法，并分享调试过程中积累的实战经验。

1. 环境配置与基础矩阵运算

在开始前，请确保已安装以下Python库：

pip install numpy matplotlib scipy

仿射控制的核心是处理configuration矩阵。假设我们有n个智能体在d维空间，其位置矩阵构造如下：

import numpy as np def build_configuration_matrix(positions): """ 构建增广configuration矩阵 :param positions: (n,d)维numpy数组，每行代表一个智能体的坐标 :return: 增广矩阵P_bar = [P | 1_n] """ n = positions.shape[0] P = positions # 原始位置矩阵 ones_column = np.ones((n, 1)) P_bar = np.hstack((P, ones_column)) return P_bar

关键点检查：

• 位置矩阵的维度必须满足n ≥ d+1才能保证仿射无关性
• 增广矩阵的最后一列必须是全1向量

2. Stress Matrix的构建与验证

Stress Matrix（Ω）是实现仿射控制的核心工具。我们通过奇异值分解(SVD)来构建：

def compute_stress_matrix(positions, edges): """ 计算满足平衡条件的stress matrix :param positions: (n,d)位置矩阵 :param edges: 列表形式的边集合，如[(0,1), (1,2)] :return: stress matrix Ω """ n, d = positions.shape P_bar = build_configuration_matrix(positions) # 构建incidence matrix H m = len(edges) H = np.zeros((m, n)) for idx, (i,j) in enumerate(edges): H[idx, i] = 1 H[idx, j] = -1 # 构建约束矩阵E E_blocks = [] for i in range(n): diag_h = np.diag(H[:,i]) E_block = P_bar.T @ H.T @ diag_h E_blocks.append(E_block) E = np.vstack(E_blocks) # 计算E的零空间基 _, s, Vh = np.linalg.svd(E) tol = max(E.shape) * np.spacing(s[0]) null_mask = s <= tol null_basis = Vh[null_mask].T # 通过半正定约束确定系数 U, _, _ = np.linalg.svd(P_bar) U2 = U[:, d+1:] M_matrices = [] for i in range(null_basis.shape[1]): z = null_basis[:, i] diag_z = np.diag(z) M = U2.T @ H.T @ diag_z @ H @ U2 M_matrices.append(M) # 使用半正定规划求解系数(简化示例) from scipy.optimize import minimize def objective(c): return np.sum(c**2) # 最小化系数范数 cons = [{'type': 'ineq', 'fun': lambda c: np.min(np.linalg.eigvals(sum(c[i]*M_matrices[i] for i in range(len(c)))))}] c0 = np.ones(len(M_matrices)) res = minimize(objective, c0, constraints=cons) omega = null_basis @ res.x Omega = H.T @ np.diag(omega) @ H return Omega

常见错误排查：

1. 矩阵维度不匹配：确保H矩阵的行数等于边数
2. 半正定性不满足：检查U2.T @ Ω @ U2的特征值是否全为正
3. 数值稳定性问题：对SVD结果设置合理的容差阈值

3. 仿射队形控制算法实现

基于stress matrix，我们可以实现分布式控制律：

def affine_formation_control(positions, target_positions, Omega, k=0.1, dt=0.01): """ 仿射队形控制算法 :param positions: 当前实际位置(n,d) :param target_positions: 目标位置(n,d) :param Omega: stress matrix :param k: 控制增益 :param dt: 时间步长 :return: 更新后的位置 """ n, d = positions.shape error = positions - target_positions control_input = -k * (Omega @ error) new_positions = positions + control_input * dt return new_positions

参数调优技巧：

• 控制增益k过大可能导致震荡，建议从0.1开始逐步调整
• 时间步长dt应与系统实际响应时间匹配
• 可通过实时绘制误差范数监控收敛情况

4. 完整仿真系统搭建

将各模块整合为完整仿真系统：

import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation class AffineFormationSimulator: def __init__(self, initial_pos, target_pos, edges): self.positions = initial_pos.copy() self.target_pos = target_pos self.edges = edges self.Omega = compute_stress_matrix(initial_pos, edges) self.fig, self.ax = plt.subplots(figsize=(10,6)) def update(self, frame): self.positions = affine_formation_control( self.positions, self.target_pos, self.Omega) self.ax.clear() self.ax.scatter(self.positions[:,0], self.positions[:,1], c='b', label='Actual') self.ax.scatter(self.target_pos[:,0], self.target_pos[:,1], c='r', marker='x', label='Target') # 绘制连接线 for i,j in self.edges: self.ax.plot([self.positions[i,0], self.positions[j,0]], [self.positions[i,1], self.positions[j,1]], 'b--', alpha=0.3) self.ax.plot([self.target_pos[i,0], self.target_pos[j,0]], [self.target_pos[i,1], self.target_pos[j,1]], 'r--', alpha=0.3) self.ax.legend() self.ax.set_title(f'Formation Control Frame {frame}') return self.ax # 示例使用 initial_pos = np.array([[0,0], [1,0], [0.5,1], [1.5,1]]) target_pos = np.array([[1,1], [2,1], [1.5,2], [2.5,2]]) edges = [(0,1), (1,2), (2,0), (2,3), (3,1)] sim = AffineFormationSimulator(initial_pos, target_pos, edges) ani = FuncAnimation(sim.fig, sim.update, frames=100, interval=50) plt.show()

性能优化建议：

1. 对大规模系统，使用稀疏矩阵运算加速计算
2. 实时可视化会显著降低仿真速度，正式测试时可关闭
3. 考虑使用Numba加速核心计算部分

5. 典型问题解决方案

问题1：矩阵奇异导致计算失败

当配置点共线时，P_bar矩阵秩不足。解决方案：

def check_affine_independence(P_bar): rank = np.linalg.matrix_rank(P_bar) d = P_bar.shape[1] - 1 if rank < d + 1: raise ValueError("配置点仿射相关，请调整初始位置")

问题2：控制过程出现震荡

添加速度阻尼项：

def damped_affine_control(positions, velocities, target_pos, Omega, k=0.1, c=0.05, dt=0.01): error = positions - target_pos control = -k * (Omega @ error) - c * velocities new_pos = positions + velocities * dt new_vel = velocities + control * dt return new_pos, new_vel

问题3：部分智能体无法收敛

检查stress matrix的连通性：

def check_connectivity(Omega, threshold=1e-3): n = Omega.shape[0] L = np.diag(np.sum(np.abs(Omega), axis=1)) - Omega eigvals = np.linalg.eigvals(L) zero_eig = np.sum(np.abs(eigvals) < threshold) if zero_eig > 1: print("警告：图结构不连通")

在实际项目中，我们发现当leader节点数量不足时，系统会出现无法完全收敛的情况。根据经验，在二维空间中至少需要3个非共线的leader节点才能保证良好的控制效果。