【论文笔记】仿射队形控制原理与stress matrix的构建

写在前面

原论文标题：Affine Formation Maneuver Control of Multiagent Systems.

本文为近期阅读的论文(Zhao 2018)¹的笔记。该论文研究基于仿射变换的编队控制，重点集中于如何通过控制leader实现maneuver。

预备基础

column stack(记作vec)的性质：
$$\begin{array}{rl}\mathrm{vec}(ABC)& =(C^T\otimes A)\mathrm{vec}(B)\\ x\otimes y& =\mathrm{vec}(y{x}^T)\end{array}$$
configuration matrix定义为:
$$P(p)=\left[\begin{array}{c}{p}_1^T\\ ⋮\\ p_n^T\end{array}\right]，\bar{P}(p)=\left[\begin{array}{cc}P(p)& 1_n\end{array}\right]，$$
其中$p_i\in {\mathbb{R}}^d$的column stack叫做configuration $p=[p_1^T,\cdots ,p_n^T{]}^T$。

结合上面两个，得到一个多智能体控制中常用的性质：
$$\mathrm{vec}((LP(p){)}^T)=(L\otimes I_d)\mathrm{vec}(P^T(p))。(1)$$
该性质在matlab编程中可以简化代码。

线性相关和仿射相关

线性相关(linearly dependant)要求${\sum }_{i=1}^n{a}_i{p}_i=0$，而仿射相关(affinely dependant)额外要求${\sum }_{i=1}^n{a}_i=0$。这个区别直接反映在configuration matrix上，故 { p i } i = 1 n \{p_i\}_{i=1}^n {pi​}i=1n​仿射相关当且仅当$\bar{P}(p)$行线性相关，即${\bar{P}}^T(p)a=0$。

仿射相关肯定线性相关，反之不一定。线性无关肯定仿射无关，反之不一定。

由于$\bar{P}(p)$有$d+1$列，所以${\mathbb{R}}^d$空间最多有$d+1$个点仿射无关(affinely independant)。相对应的，${\mathbb{R}}^d$空间最多有$d$个点线性无关(linearly dependant)。

当 { p i } i = 1 n \{p_i\}_{i=1}^n {pi​}i=1n​中存在$d+1$个点仿射无关时，${\mathbb{R}}^d$可以被 { p i } i = 1 n \{p_i\}_{i=1}^n {pi​}i=1n​仿射展开(affine span)，此时$\bar{P}(p)$行线性无关，$\mathrm{rank}(\bar{P}(p))=d+1$。

引理1： { p i } i = 1 n \{p_i\}_{i=1}^n {pi​}i=1n​仿射展开${\mathbb{R}}^d$，当且仅当$n\ge d+1$和$\mathrm{rank}(\bar{P}(p))=d+1$。

Stress Matrix

对基于图论定义的formation$(\mathcal{G},p)$，stress是每条边上的标量权重， { ω i j } ( i , j ) ∈ E \{\omega_{ij}\}_{(i,j)\in\mathcal E} {ωij​}(i,j)∈E​ ，其中${\omega }_{ij}={\omega }_{ji}\in \mathbb{R}$。

equilibrium stress满足
$$\sum _{j\in {\mathcal{N}}_i}{\omega }_{ij}(p_i-p_j)=0，i\in \mathcal{V}。(2)$$
可知，如果$\omega$是equilibrium stress，则$k\omega$也是($k\ne 0$)。

Distance Rigidity

对于一组formation，满足等价和全等的条件为：

• 等价(equivalent)：图相同的情况下，任意一条边对应的两个节点，在不同的formation中距离相等。

• 全等(congruent)：图相同的情况下，任意两个节点，在不同的formation中距离相等。

globally rigid：对于一个formation，等价和全等同时成立。

universally rigid：对于一个formation，在任意${\mathbb{R}}^{d_1}$空间满足globally rigid，其中$d_1\ge d$。

应用式(1)，定义对称的equilibrium stress matrix $\Omega$，我们将平衡条件式(2)重写为
$$(\Omega \otimes I_d)p=\mathrm{vec}((\Omega P(p){)}^T)=0。(3)$$
由于$\mathrm{rank}(P(p))=d+1$，故$\mathrm{rank}(\Omega )=n-d-1$²。

引理2：给定无向图$\mathcal{G}$和generic configuration $p$，formation $(\mathcal{G},p)$是universally rigid当且仅当存在半正定stress matrix $\Omega$满足$\mathrm{rank}(\Omega )=n-d-1$。

注意：线性代数里正定性的前提是实对称矩阵。

Consider all of the coordinates of $p=[p_1^T,\cdots ,p_n^T{]}^T$ as a set of numbers $x_1,x_2,\cdots ,x_{dn}$, and consider any non-zero polynomial equation $f(x_1,x_2,\cdots ,x_{dn})$ with integer coeffcients and the numbers (the coordinates) substituted for the variables $x_1,x_2,\cdots ,x_{dn}$. If $f(p)\ne 0$ for every such $f$, we say that the configuration is generic.³

论文(Zhao 2018)中对generic的定义是：the coordinates of all the nodes do not satisfy any nontrivial equations with rational coefficients. 由于有理数可以通过乘以最小公倍数得到整数，该定义与上述定义一致。

Affine Formation Control的实现

给定nominal configuration $r=[r_1^T,\cdots ,r_n^T{]}^T$和nominal formation $(\mathcal{G},r)$，target formation中的configuration为
$$p^{\ast }(t)=[I_n\otimes A(t)]r+1_n\otimes b(t)，A(t)\in {\mathbb{R}}^{d\times d}，b(t)\in {\mathbb{R}}^d，$$
即$p^{\ast }(t)\in \mathcal{A}(r)$，其中$\mathcal{A}(r)$为$r$的affine image。

引理3：$\mathcal{A}(r)$的维度为$d^2+d$当且仅当 { r i } i = 1 n \{r_i\}_{i=1}^n {ri​}i=1n​仿射展开${\mathbb{R}}^d$。

假设1： { r i } i = 1 n \{r_i\}_{i=1}^n {ri​}i=1n​仿射展开${\mathbb{R}}^d$。我们不讨论控制算法的设计和稳定性证明，主要研究下面几个问题。

Affine Image和Stress Matrix的关系

引理4：对任意nominal configuration $r$，以下条件始终成立：
$$\begin{array}{rl}\mathcal{A}(r)& \subseteq \mathrm{Null}(\Omega \otimes I_d)，\\ \mathrm{Col}(\bar{P}(r))& \subseteq \mathrm{Null}(\Omega )。\end{array}$$

因为 { r i } i = 1 n \{r_i\}_{i=1}^n {ri​}i=1n​满足(2)，所以 { A r i + b } i = 1 n \{Ar_i+b\}_{i=1}^n {Ari​+b}i=1n​满足(2)，第一个条件成立。由式(3)知，第二个条件成立。

假设2：nominal formation $(\mathcal{G},r)$存在满足引理2的$\Omega$。

引理5：满足假设2的条件下，下列条件等价。

1. { r i } i = 1 n \{r_i\}_{i=1}^n {ri​}i=1n​仿射展开${\mathbb{R}}^d$。
2. $\mathrm{Null}(\Omega \otimes I_d)=\mathcal{A}(r)$。
3. $\mathrm{Null}(\Omega )=\mathrm{Col}(\bar{P}(r))$。

在引理4的基础上，证明引理5只需要证明维度相等。由假设2和引理2，知$\dim (\mathrm{Null}(\Omega \times I_d))=d(d+1)$，再由引理3，知两者维度相等。同理可证第3条。

什么条件下Stress Matrix可行

定理1：满足假设1的条件下，nominal formation $(\mathcal{G},r)$仿射可定位(affinely localizable)当且仅当leader节点，即 { r i } i ∈ V l \{r_i\}_{i\in\mathcal V_l} {ri​}i∈Vl​​，仿射展开${\mathbb{R}}^d$。

仿射可定位：已知$(\mathcal{G},r)$，给定leader节点$p_l$可以唯一确定target configuration $p=[p_l^T,p_f^T{]}^T$。也就是说，$A$和$b$被唯一确定。

推论1：如果 { r i } i ∈ V l \{r_i\}_{i\in\mathcal V_l} {ri​}i∈Vl​​仿射展开${\mathbb{R}}^d$，那么对任意$p\in \mathcal{A}(r)$，相对应的$A$和$b$被唯一确定为
$$\begin{array}{rl}A& =\left(\sum _{i\in {\mathcal{V}}_l}{p}_i{\tilde{r}}_i^T\right){\left(\sum _{i\in {\mathcal{V}}_l}{\tilde{r}}_i{\tilde{r}}_i^T\right)}^{-1}，\\ b& =\frac{1}{n_l}\sum _{i\in {\mathcal{V}}_l}{p}_i-A\bar{r}，\end{array}$$
其中$\bar{r}={\sum }_{i\in {\mathcal{V}}_l}{r}_i/n_l$和${\tilde{r}}_i=r_i-\bar{r}$。

由推论1可知， { r i } i ∈ V l \{r_i\}_{i\in\mathcal V_l} {ri​}i∈Vl​​仿射展开${\mathbb{R}}^d$当且仅当${\sum }_{i\in {\mathcal{V}}_l}{\tilde{r}}_i{\tilde{r}}_i^T$非奇异。

下面研究什么样的$\Omega$满足仿射可定位。定义$\bar{\Omega }=\Omega \otimes I_d=\left[\begin{array}{cc}{\bar{\Omega }}_{ll}& {\bar{\Omega }}_{lf}\\ {\bar{\Omega }}_{fl}& {\bar{\Omega }}_{ff}\end{array}\right]$。

定理2：满足假设1、2的情况下，nominal formation $(\mathcal{G},r)$仿射可定位当且仅当${\bar{\Omega }}_{ff}$非奇异。此时，$p_f$被唯一确定，即$p_f=-{\bar{\Omega }}_{ff}^{-1}{\bar{\Omega }}_{fl}{p}_l$。

假设3：nominal formation $(\mathcal{G},r)$仿射可定位。

假设1、2保证$\Omega$的零空间正好是affine image。假设3保证${\bar{\Omega }}_{ff}$是正定的(因为$\bar{\Omega }$半正定，且${\bar{\Omega }}_{ff}$非奇异)。

参考schur complement condition⁴，对于$X=[A,B;B^T,C]$，有：

1. $X>0$ $\iff A>0,C-B^T{A}^{-1}B>0$；
2. $X>0$ $\iff C>0,A-B{C}^{-1}{B}^T>0$；
3. 如果$A>0$，那么 $X\ge 0$ $\iff$ $C-B^T{A}^{-1}B\ge 0$；
4. 如果$C>0$，那么 $X\ge 0$ $\iff$ $A-B{C}^{-1}{B}^T\ge 0$。

如何构建Stress Matrix

令$H\in {\mathbb{R}}^{|\mathcal{E}|\times n}$为incidence matrix。已知formation $(\mathcal{G},r)$如何求取$\Omega$满足平衡条件？

首先，由$\Omega =H^T\mathrm{diag}(\omega )H$和$\Omega \bar{P}(r)=0$，知${\bar{P}}^T(r)H^T\mathrm{diag}(\omega )H={\bar{P}}^T(r)H^T\mathrm{diag}(\omega )[h_1,\cdots ,h_n]=0$。

由$\mathrm{diag}(\omega )h_i=\mathrm{diag}(h_i)\omega$，两者位置互换，得到${\bar{P}}^T(r)H^T\mathrm{diag}(h_i)w=0$。

定义$E=\left[\begin{array}{c}{\bar{P}}^T(r)H^T\mathrm{diag}(h_1)\\ ⋮\\ {\bar{P}}^T(r)H^T\mathrm{diag}(h_n)\end{array}\right]$。则$E\omega =0$，因此$\omega$在$E$的零空间里。

定义$z_1,\cdots ,z_q\in {\mathbb{R}}^{|\mathcal{E}|}$为$E$的零空间上的一组基。则$\omega ={\sum }_{i=1}^q{c}_i{z}_i$，其中$c_1,\cdots ,c_n\in \mathbb{R}$是一组待定系数。

对于宽(fat)矩阵$A\in {\mathbb{R}}^{m\times n}$，其零空间可以写为$I_n-A^{†}A$，然而$E\in {\mathbb{R}}^{n(d+1)\times |\mathcal{E}|}$也可能是长(slim)矩阵。

对于长矩阵$A\in {\mathbb{R}}^{m\times n}$，可以通过奇异值分解求$A$的零空间。

然后，我们继续确定系数$c_1,\cdots ,c_n\in \mathbb{R}$。令$\bar{P}(r)=U\Sigma V^T$。令$U=[U_1,U_2]$，其中$U_1$包含$U$的前$d+1$列。因为$\bar{P}(r)$的秩为$d+1$，所以$U_1$是非零奇异值对应的左奇异向量，即$\bar{P}(r)$的列空间。同理$U_2$是${\bar{P}}^T(r)$的零空间。

根据假设2和引理2，我们要求$\Omega$是半正定，且秩为$n-d-1$。

因为$\Omega \bar{P}(r)=0$，故$\Omega U_1=0$，所以$\Omega$在前$d+1$维秩为0，于是剩下的$n-d-1$维需要满秩，即$\Omega$在${\bar{P}}^T(r)$的零空间上正定，对任意$x\in \mathrm{Null}({\bar{P}}^T(r))$，有$x^T\Omega x>0$。换句话说，对任意$y\in {\mathbb{R}}^n$，$y^T(U_2^T\Omega U_2)y>0$。

命题：$\mathrm{rank}(\Omega )=n-d-1$当且仅当$U_2^T\Omega U_2=U_2^T{H}^T\mathrm{diag}(\omega )H{U}_2>0$。

将$\omega ={\sum }_{i=1}^q{c}_i{z}_i$代入$U_2^T{H}^T\mathrm{diag}(\omega )H{U}_2$，得到${\sum }_{i=1}^n{c}_i{U}_2^T{H}^T\mathrm{diag}(z_i)H{U}_2>0$。

定义$M_i=U_2^T{H}^T\mathrm{diag}(z_i)H{U}_2$。可知系数$c_1,\cdots ,c_n\in \mathbb{R}$需要满足条件${\sum }_{i=1}^n{c}_i{M}_i>0$。

奇异值分解的原理和应用

我们回顾一下奇异值分解(singular value decomposition)。

特征值和特征向量

如果$A\in {\mathbb{R}}^{n\times n}$是实矩阵，其特征值和特征向量定义为$Aq=\lambda q$，那么$A=QD{Q}^T$是它的特征分解(谱分解)，其中$Q=[q_1,\cdots ,q_n]$是它的特征向量， D = diag ⁡ { λ i } i = 1 n D=\operatorname{diag}\{\lambda_i\}_{i=1}^n D=diag{λi​}i=1n​是它的特征根。

一般我们会将特征向量转化为标准正交基，使得$Q$是标准正交矩阵。

# Eigenvalues and eigenvectors in MATLAB
# such that e are eigenvalues and D = diag(e)
e = eig(A)
# such that A*V=V*D (right) and W'*A=D*W' (left)
[V,D,W] = eig(A)

奇异值分解

如果$A\in {\mathbb{R}}^{m\times n}$不是方阵，可以进行奇异值分解，即$A=U\Sigma V^T$，其中$U\in {\mathbb{R}}^{m\times m}$和$V\in {\mathbb{R}}^{n\times n}$是正交矩阵，$\Sigma \in {\mathbb{R}}^{m\times n}$是(矩形)对角阵。

$U$和$V$分别是对$A$正交输出和输入的基向量，也是$A{A}^T$和$A^{T}A$的特征向量，即
$$\begin{gathered} (A{A}^T)U=U{D}_u， \\ (A^{T}A)V=V{D}_v。 \end{gathered}$$
证明：$A=U\Sigma V^T\Rightarrow A^T=V{\Sigma }^T{U}^T\Rightarrow A^{T}A=V{\Sigma }^T{U}^{T}U\Sigma V^T=V({\Sigma }^T\Sigma )V^T$。

# Singular value decomposition
# such that s are non-zero sigular values
s = svd(A)
# such that A = U*S*V'
[U,S,V] = svd(A)

常见应用

• 求伪逆

如果$A=U\Sigma V^T$，那么$A^{†}=V{\Sigma }^{†}{U}^T$，其中${\Sigma }^{†}$是将$\Sigma$非零主对角元求倒数，再转置得到。

• 列空间、零空间和秩

对角矩阵$\Sigma$的非零对角元素的个数对应于矩阵$A$的秩。

与零奇异值对应的右奇异向量生成矩阵$A$的零空间，与非零奇异值对应的左奇异向量则生成矩阵$A$的列空间。

---

1. Zhao, S. (2018). Affine Formation Maneuver Control of Multiagent Systems. IEEE Transactions on Automatic Control, 63(12), 4140–4155. https://doi.org/10.1109/TAC.2018.2798805 ↩︎

2. Wikipedia contributors. (2020, September 5). Rank–nullity theorem. In Wikipedia, The Free Encyclopedia. Retrieved 13:55, September 24, 2020, from https://en.wikipedia.org/w/index.php?title=Rank%E2%80%93nullity_theorem&oldid=976828687 ↩︎

3. Connelly, R., & Guest, S. D. (2015). Frameworks, Tensegrities and Symmetry: Understanding Stable Structures. Section 7.2. ↩︎

4. Wikipedia contributors. (2020, August 12). Schur complement. In Wikipedia, The Free Encyclopedia. Retrieved 13:06, September 25, 2020, from https://en.wikipedia.org/w/index.php?title=Schur_complement&oldid=972571695 ↩︎