多机器人协同感知2——分布式优化理论学习

本文重点介绍了两种更具扩展性、更符合实际机器人协同决策的框架：约束耦合优化（Constraint-Coupled Optimization）和聚合优化（Aggregative Optimization） 。

定义：协同的目的是优化一个共同的性能指标，核心假设是每个智能体只负责优化问题的一小部分，并且只能与少数邻居agent进行通信.

1. 两大核心数学框架

约束耦合优化 (Constraint-Coupled Optimization):
- 问题模型： 每个机器人 $i$ 只关心并维护自己局部的决策变量 $x_i$ 和局部代价函数 $f_i(x_i)$，但所有机器人的变量被一个全局的约束条件（如共享资源限制）耦合在一起。公式表达为：$\min \sum_{i=1}^N f_i(x_i)$，受限于 $\sum_{i=1}^N g_i(x_i) \le 0$。
- 适用场景： 机器人之间存在资源竞争或任务分配时非常有效。例如多机器人任务分配、多机器人取送货 (PDVRP) 以及共享电源的电池充电调度。
聚合优化 (Aggregative Optimization):
- 问题模型： 每个机器人的代价函数不仅取决于其局部变量 $x_i$，还取决于所有机器人变量的一个聚合量 $\sigma(x)$（例如团队位置均值）。公式表达为：$\min \sum_{i=1}^N f_i(x_i, \sigma(x))$。
- 适用场景： 多机器人目标监视（既要靠近入侵者，又要兼顾团队整体重心不偏离保护目标）以及软约束下的资源分配。

2. Notation

2.1 闭凸集 (Closed Convex Set)

“闭凸集”是数学（特别是凸分析和优化理论）中的一个核心概念。它是一个同时满足“闭集”（Closed Set）和“凸集”（Convex Set）性质的集合。

2.1.1 核心概念拆解

凸集 (Convex Set)： 直观上说，如果在一个集合里任意挑两个点，连接这两个点的直线段上的所有点都仍然落在该集合内部，那么这个集合就是凸集。
- 数学定义： 设 $C$ 为实向量空间中的一个集合。若对于任意的 $x, y \in C$ 以及任意实数 $\theta \in [0, 1]$，都有 $\theta x + (1 - \theta) y \in C$，则称 $C$ 为凸集。
闭集 (Closed Set)： 直观上说，闭集包含了它自己所有的“边界点”（极限点）。
- 数学定义： 如果一个集合包含其所有的极限点（即集合内部存在一个收敛数列，该数列的极限点也必定属于这个集合本身），那么它就是闭集。

2.1.2. 闭凸集的结合与实例

将两者结合起来，闭凸集就是一个既没有凹陷或空洞，又包含了自身所有坚硬外边界的几何体。

典型例子： 包含表面的实心球体 $S = { x \in \mathbb{R}^n \mid |x| \le 1 }$ 是一个标准的闭凸集。
反例 1（是凸集但非闭集）： 不包含表面的实心球体 $S = { x \in \mathbb{R}^n \mid |x| < 1 }$。
反例 2（是闭集但非凸集）： 包含边界的实心“五角星”（连接星角的线段会跑到外面）。

2.1.3. 在优化问题中的核心地位

在许多现代优化算法（例如处理多机器人系统的约束耦合、ADMM，或位姿图优化时），通常要求可行域是一个闭凸集，因为它能保证以下完美性质：

投影的唯一性 (Projection Theorem)： 如果将空间外的一个点投影到一个闭凸集上，集合上必定存在唯一的一个距离该点最近的点。
局部最优即全局最优： 在一个闭凸集上最小化一个凸的代价函数，不会陷入局部最优的死胡同。只要算法找到了局部最优解，在数学上它必定是全局最优解。

2.2 克罗内克积 (Kronecker Product)

克罗内克积，通常用符号 $\otimes$ 表示，是线性代数中两个矩阵之间的一种特殊乘法操作。与普通的矩阵乘法不同，克罗内克积对两个矩阵的尺寸没有任何要求。

其核心思想是：用第一个矩阵中的每一个元素，去按比例缩放第二个矩阵，然后将所有结果拼接成一个更大的分块矩阵。

2.2.1. 数学定义

假设 $A$ 是一个 $m \times n$ 的矩阵，$B$ 是一个 $p \times q$ 的矩阵。那么它们的克罗内克积 $A \otimes B$ 就是一个尺寸为 $(mp) \times (nq)$ 的大矩阵：

\[A \otimes B = \begin{bmatrix} a_{11}B & a_{12}B & \cdots & a_{1n}B \\ a_{21}B & a_{22}B & \cdots & a_{2n}B \\ \vdots & \vdots & \ddots & \vdots \\ a_{m1}B & a_{m2}B & \cdots & a_{mn}B \end{bmatrix}\]

2.2.2. 直观计算示例

假设 $A = \begin{bmatrix} 1 & 2 \ 3 & 4 \end{bmatrix}$，而 $B = \begin{bmatrix} 0 & 5 \ 6 & 0 \end{bmatrix}$。其计算过程如下：

\[A \otimes B = \begin{bmatrix} 1 \cdot \begin{bmatrix} 0 & 5 \\ 6 & 0 \end{bmatrix} & 2 \cdot \begin{bmatrix} 0 & 5 \\ 6 & 0 \end{bmatrix} \\ 3 \cdot \begin{bmatrix} 0 & 5 \\ 6 & 0 \end{bmatrix} & 4 \cdot \begin{bmatrix} 0 & 5 \\ 6 & 0 \end{bmatrix} \end{bmatrix} = \begin{bmatrix} 0 & 5 & 0 & 10 \\ 6 & 0 & 12 & 0 \\ 0 & 15 & 0 & 20 \\ 18 & 0 & 24 & 0 \end{bmatrix}\]

2.2.3. 核心数学性质

一般不满足交换律： $A \otimes B \neq B \otimes A$
满足结合律： $(A \otimes B) \otimes C = A \otimes (B \otimes C)$
转置性质： $(A \otimes B)^T = A^T \otimes B^T$
混合乘积性质（最关键）： $(A \otimes B)(C \otimes D) = (AC) \otimes (BD)$。该性质在解耦复杂系统时极其关键。

2.2.4. 工程与研究中的典型应用

在分布式优化、图论以及多智能体系统的数学建模中，克罗内克积常用于实现结构解耦。

例如在多机器人系统中： 假设有 $N$ 个机器人，每个机器人的状态是 $d$ 维向量 $x_i \in \mathbb{R}^d$。网络拓扑用 $N \times N$ 的拉普拉斯矩阵 $L$ 描述。整个系统的全局交互矩阵可以写成：

\[L \otimes I_d\]

左边的 $L$ 专门负责描述“谁和谁通信”（图的拓扑结构）。
右边的单位矩阵 $I_d$ 专门负责描述“每个机器人自身的物理空间维度”。这种表达方式极大地简化了全局位姿图优化或一致性控制协议的公式推导。

为什么克罗内克积能极大简化推导与代码实现？

为了直观理解克罗内克积的作用，我们以多机器人简化的位姿图优化为例，带入具体的数字进行推演。

假设我们有 $N=3$ 个机器人，在二维平面上移动，因此每个机器人的状态维度是 $d=2$（即 $x$ 坐标和 $y$ 坐标）。

机器人 1 的状态：$x_1 = [x_1, y_1]^T$
机器人 2 的状态：$x_2 = [x_2, y_2]^T$
机器人 3 的状态：$x_3 = [x_3, y_3]^T$

为了进行全局优化，我们将它们拼接成一个 $3 \times 2 = 6$ 维的全局状态向量 $X$：

\[X = [x_1, y_1, x_2, y_2, x_3, y_3]^T\]

假设它们的通信拓扑是一条线：机器人1 <–> 机器人2 <–> 机器人3。描述这种网络结构的拉普拉斯矩阵 $L$ 是一个 $3 \times 3$ 的矩阵（对角线为连接数，非对角线有连接则为 -1，无连接则为 0）：

\[L = \begin{bmatrix} 1 & -1 & 0 \\ -1 & 2 & -1 \\ 0 & -1 & 1 \end{bmatrix}\]

不用克罗内克积时:

在一致性控制或位姿图的后端优化中，我们经常要最小化机器人之间的状态差异。代价函数 $J$ 通常长这样：

\[J = \frac{1}{2} \sum_{i \sim j} \|x_i - x_j\|^2\]

如果不使用矩阵技巧，必须将这个公式按每个 $x$ 和 $y$ 坐标全部展开：

\[J = \frac{1}{2} \left[ (x_1 - x_2)^2 + (y_1 - y_2)^2 + (x_2 - x_3)^2 + (y_2 - y_3)^2 \right]\]

如果此时要对全局状态 $X$ 求一阶导（雅可比矩阵）或二阶导（海森矩阵 Hessian），需要对着这 6 个变量挨个求偏导。当机器人数量增至上百个，或者维度变成 3D（甚至 6D 位姿 SE(3)）时，这种底层标量展开会导致极其恐怖的求和符号嵌套，推导论文或编写代码时极易出错。

用克罗内克积时:

现在我们来看看 $L \otimes I_2$ 到底生成了什么。其中单位矩阵 $I_2 = \begin{bmatrix} 1 & 0 \ 0 & 1 \end{bmatrix}$。

将 $L$ 和 $I_2$ 进行克罗内克积计算：

\[L \otimes I_2 = \begin{bmatrix} 1 \cdot I_2 & -1 \cdot I_2 & 0 \cdot I_2 \\ -1 \cdot I_2 & 2 \cdot I_2 & -1 \cdot I_2 \\ 0 \cdot I_2 & -1 \cdot I_2 & 1 \cdot I_2 \end{bmatrix} = \begin{bmatrix} 1 & 0 & -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -1 & 0 & 0 \\ -1 & 0 & 2 & 0 & -1 & 0 \\ 0 & -1 & 0 & 2 & 0 & -1 \\ 0 & 0 & -1 & 0 & 1 & 0 \\ 0 & 0 & 0 & -1 & 0 & 1 \end{bmatrix}\]

可以看出，这个 $6 \times 6$ 的矩阵，完美地把 $L$ 的拓扑结构“膨胀”到了每一个物理坐标维度上。

在数学推导上（极度优雅）： 有了 $L \otimes I_2$，前面冗长的代价函数 $J$，可以直接写成完美的二次型矩阵形式：

\[J(X) = \frac{1}{2} X^T (L \otimes I_d) X\]

一旦写成这个形式，矩阵微积分就变得极其简单：

一阶导（梯度）： $\nabla J = (L \otimes I_d) X$
二阶导（海森矩阵 Hessian / 信息矩阵）： $\nabla^2 J = L \otimes I_d$

在位姿图优化中极其关键的全局海森矩阵，直接就等于 $L \otimes I_d$。研究者不再需要去写复杂的求和公式或推导分块矩阵。

在代码实现上（极度高效）： 如果不使用这个技巧，要在代码里构建这个稀疏海森矩阵，通常需要写两层嵌套循环：一层遍历机器人节点，一层遍历坐标维度，然后小心翼翼地计算索引并填入数值。这不仅容易产生越界或错位 Bug，运行效率也低下。利用克罗内克积，只需调用线性代数库中的单行内置函数（将拓扑矩阵与维度单位矩阵相乘），即可瞬间生成全局稀疏矩阵，彻底消除了手动计算索引的烦恼。

3. 动态图论基础与双随机矩阵的物理意义（多机器人系统应用）

在多机器人系统（尤其是水下等存在恶劣通信环境的场景）的分布式优化与协同控制中，为了用严谨的数学语言描述时断时续、单向广播的通信网络，通常会引入动态图论的一系列核心概念。

3.1 动态图论基础符号解析

首先明确几个最基础的数学符号：

$\mathbb{I}$：代表节点集合。例如系统中有 3 个机器人，则 $\mathbb{I} = {1, 2, 3}$。
$t \in \mathbb{N}$：代表时间步。$\mathbb{N}$ 为自然数，说明系统是离散时间的（$t=0, 1, 2…$）。
$\mathcal{E}^t$：代表在时间 $t$ 时的边集合（Edge Set）。即在这一时刻，哪些机器人之间建立了通信链路。
$\mathcal{G}^t$：代表在时间 $t$ 时的图，由节点和边共同组成：$\mathcal{G}^t = (\mathbb{I}, \mathcal{E}^t)$。

3.1.1. 静态图与时变图 (Static vs. Time-varying Graph)

静态图： 边集 $\mathcal{E}^t$ 不依赖于时间 $t$。机器人的通信网络拓扑固定不变。
时变图： 通信网络随时间动态变化。受环境干扰（如洋流或障碍物），某一时刻存在连接，下一时刻可能断开。上标 $t$ 正是为了体现这种动态特性。

3.1.2. 无向图与有向图 (Undirected vs. Directed)

无向图： 如果存在边 $(i, j)$，则必定存在反向边 $(j, i)$。通信是双向的（对讲机模式）。
有向图： 通信是单向的（广播模式）。例如 1 号发射功率大，2 号能收到（存在边 $(1, 2)$），但 2 号回复 1 号收不到（不存在边 $(2, 1)$）。

3.1.3. 强连通 (Strongly Connected)

在一个静态有向图中，如果任意一对节点 $(i, j)$ 之间都存在一条有向路径，则该图是强连通的。

物理意义： 网络中任意挑两个机器人，信息都能通过中间节点的一步步转发互相送达。

3.1.4. 联合强连通与 L-强连通 (Jointly & L-strongly Connected)

这是处理时变网络（如 CADMM 分布式优化）时最核心的假设条件。

定义： 存在一个整数 $L \ge 1$，使得在任何长度为 $L$ 的时间窗口内，将所有时间步出现的边取“并集”构建的图 $(\mathbb{I}, \cup_{\tau=t}^{t+L-1} \mathcal{E}^\tau)$ 是强连通的。
物理意义： 在水下等带宽极小的场景中，每一秒网络可能都是支离破碎的。但只要考察足够长的时间窗口 $L$，将这期间出现过的所有通信链路“叠加”在一起，构成的总网络是全连通的。这保证了信息最终能在整个团队中完全流通，是分布式算法收敛的基石。

3.2 为什么需要“双随机矩阵” (Doubly Stochastic Matrix)？

给定有向图 $\mathcal{G}^t$，可以用权重邻接矩阵 $\mathcal{A}^t \in \mathbb{R}^{N \times N}$ 来描述。矩阵中的元素 $a_{ij}$ 代表：在状态更新时，节点 $i$ 采纳节点 $j$ 信息的权重。

如果一个矩阵满足 $\mathcal{A}1_N = 1_N$ 并且 $1_N^\top\mathcal{A} = 1_N^\top$（即每一行的和为 1，每一列的和也为 1），则称为双随机矩阵。在平均一致性 (Average Consensus) 算法中，这不仅是数学上的要求，更是极其严谨的物理守恒定律。

3.2.1. 行和为 1：保证系统稳定不发散（凸组合）

核心逻辑：我（节点 $i$）的新状态，是怎么由大家的老状态拼凑出来的？

物理意义： 行和为 1 保证了每个机器人在更新状态时，新数值稳稳地落在所有参考值的极值区间内。在数学上，这称为凸组合（Convex Combination）。
反例： 如果行和大于 1，意味着系统在不断放大输入数值，经过几次迭代后，状态变量会呈指数级爆炸，算法彻底发散。

3.2.2. 列和为 1：保证系统信息总量守恒

核心逻辑：我（节点 $j$）的信息总量（权重），被分发给了网络里的哪些人？分发出去的总量是多少？

物理意义： 这是 Average Consensus 能收敛到“真实平均值”的绝对前提。列和为 1 说明每个节点刚好把自己的 $100\%$ 影响力，不增不减地分配给了整个系统。
反例（行和为 1，但列和不为 1）： 假设矩阵 $A = \begin{bmatrix} 0.8 & 0.2 & 0 \ 0 & 1 & 0 \ 0 & 0.8 & 0.2 \end{bmatrix}$。在这个矩阵中，节点 2（第 2 列）的列和为 $2.0$。这意味着节点 2 保留了自己 $100\%$ 的权重，还额外凭空向节点 1 和节点 3 输出了 $100\%$ 的权重。如果初始状态为 $x = [10, 20, 30]^T$（总和为 60，真实平均值为 20）。经过一次迭代：
- $x_1 = 0.8(10) + 0.2(20) = 12$
- $x_2 = 1(20) = 20$
- $x_3 = 0.8(20) + 0.2(30) = 22$ 系统的新总和变成了 $12 + 20 + 22 = 54$。原本总量是 60，迭代一次蒸发了 6。由于总信息量不守恒，系统即使最终达成一致，也绝对不可能收敛到正确的平均值 20。

# 3.2.3 总结

在多机器人的局部通信与状态更新中：

行和为 1 让算法能收敛（保证数值稳定）。
列和为 1 让算法收敛到正确的值（保证全局信息总量守恒）。这也是分布式算法中常利用 Metropolis-Hastings 等规则来强行构造双随机矩阵的根本原因。