多智能体集群协同控制笔记（1）:线性无领航多智能体系统的一致性

对于连续时间高阶线性多智能体系统的状态方程为：
$${\dot{\mathbf{x}}}_i(t)=A{\mathbf{x}}_i(t)+B{\mathbf{u}}_i(t),i=1,\mathrm{2..}N$$
下标$i$代表第$i$个智能体，${\mathbf{u}}_i(t)\in R^{q\times 1}$表示第$i$个智能体的控制输入变量，${\mathbf{x}}_i(t)\in R^{p\times 1}$表示第$i$个智能体的状态变量。系统的一致性即设计控制器${\mathbf{u}}_i(t)$保证如下等式成立：
$$\underset{t\to \infty }{\lim }{\mathbf{x}}_1(t)=\underset{t\to \infty }{\lim }{\mathbf{x}}_2(t)=...=\underset{t\to \infty }{\lim }{\mathbf{x}}_N(t)$$

• 定义Laplacian矩阵 L = { l i j } i , j = 1 , . . N L=\{l_{ij}\}_{i,j=1,..N} L={lij​}i,j=1,..N​，其中：
  $$l_{ij}=\{\begin{array}{ll}{\sum }_{j=1,j\ne i}^N{a}_{ij}& i=j\\ -a_{ij}& i\ne j\end{array}$$

• 定义变量：${\xi }_i(t)={\sum }_{j=1}^N{a}_{ij}({\mathbf{x}}_i(t)-{\mathbf{x}}_j(t))$，定义：
  $$\xi (t)=(L\otimes I_p)X(t)$$
  其中：$X(t)=[{\mathbf{x}}_1^T(t),{\mathbf{x}}_2^T(t),..{\mathbf{x}}_N^T(t){]}^T$，$\xi (t)=[{\xi }_1(t{)}^T,{\xi }_2(t{)}^T,..{\xi }_N(t{)}^T{]}^T$；

• 定义控制器:${\mathbf{u}}_i(t)=c{K}_1{\xi }_i(t)$，总的控制策略为：
  $$\mathbf{u}(t)=(I_N\otimes c{K}_1)\xi (t)=(I_N\otimes c{K}_1)(L\otimes I_p)X(t)$$
  其中$K_1$是待求的控制增益矩阵,$c$为加权参数；

• 定义$L$为该系统的Laplacian矩阵，存在一个酉矩阵$Y$，使得：
  $$Y^{T}LY=\mathbf{diag}({\lambda }_1,{\lambda }_2,...{\lambda }_N)$$
  其中： λ 1 = 0 ; λ i > 0 , i ∈ { 2 , 3 , . . N } \lambda_1=0;\lambda_i >0,i \in \{ 2,3,..N\} λ1​=0;λi​>0,i∈{2,3,..N}，由于$L$有右特征向量$1_N$,$Y$可以写成：
  $$Y=[\frac{1_N}{\sqrt{N}},M_1]$$

• 定义：$\epsilon (t)=[{\epsilon }_1(t{)}^T,{\epsilon }_2(t{)}^T,..{\epsilon }_N(t{)}^T{]}^T=(Y^T\otimes I_p)\xi (t)$，其中$p$为${\mathbf{x}}_i(t)$的维度。

  1.无向连通图定理

  引理1.1： 当多智能体系统网络为无向连通图时，系统实现状态一致的充要条件是:

$$\bar{\epsilon }(t)=[{\epsilon }_2(t{)}^T,{\epsilon }_3(t{)}^T,..{\epsilon }_N(t{)}^T{]}^T=0$$

​ 且$\bar{\epsilon }(t)$同时也满足:
$$\begin{gathered} \dot{\epsilon }(t)=(I_N\otimes A+c{\Lambda }_N\otimes B{K}_1)\epsilon (t) \\ \dot{\bar{\epsilon }}(t)=(I_{N-1}\otimes A+c{\Lambda }_{N-1}\otimes B{K}_1)\bar{\epsilon }(t)(\ast ) \end{gathered}$$
​ 其中：${\Lambda }_{N-1}=diag({\lambda }_2,...{\lambda }_N)$，${\Lambda }_N=diag({\lambda }_1,{\lambda }_2,...{\lambda }_N)$。

​ 定理1.1：给定矩阵$Q_1=Q_1^T>0$和$R_1=R_1^T>0$，若如下Riccati方程有正定解$P_1=P_1^T>0$：
$$P_{1}A+A{P}_1^T+Q_1-P_{1}B{R}_1^{-1}{B}^T{P}_1=0$$
​ 则系统$(\ast )$渐进稳定，由引理1.1可知，原系统能达到一致性。

​ 其中系统的控制增益矩阵$K_1=-R_1^{-1}{B}^T{P}_1$，且加权系数$c$满足$c\ge \frac{1}{2{\min }_{i=2,...N}{\lambda }_i(L)}$。

​ 证明：构造Lyapunov函数$V_1(t)=0.5{\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_1)\bar{\epsilon }(t)$,其满足：
$$\begin{gathered} \dot{V_1}(t)=0.5{\dot{\bar{\epsilon }}}^T(t)(I_{N-1}\otimes P_1)\bar{\epsilon }(t)+0.5{\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_1)\dot{\bar{\epsilon }}(t) \\ ={\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_1)\dot{\bar{\epsilon }}(t) \\ =\bar{\epsilon }(t{)}^T(I_{N-1}\otimes P_1)(I_{N-1}\otimes A+c{\Lambda }_{N-1}\otimes B{K}_1)\bar{\epsilon }(t) \\ ={\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_{1}A+c{\Lambda }_{N-1}\otimes P_{1}B{K}_1)\bar{\epsilon }(t) \\ ={\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_{1}A-c{\Lambda }_{N-1}\otimes P_{1}B{R}_1^{-1}{B}^T{P}_1)\bar{\epsilon }(t) \end{gathered}$$
​ 由于$c\ge \frac{1}{2{\min }_{i=2,...N}{\lambda }_i(L)}$，因此$c{\Lambda }_{N-1}\ge \frac{I_{N-1}}{2}$。带入上式有：
$$\begin{gathered} \dot{V_1}(t)={\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_{1}A-c{\Lambda }_{N-1}\otimes P_{1}B{R}_1^{-1}{B}^T{P}_1)\bar{\epsilon }(t) \\ \le {\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_{1}A-\frac{I_{N-1}}{2}\otimes P_{1}B{R}_1^{-1}{B}^T{P}_1)\bar{\epsilon }(t) \\ ={\bar{\epsilon }}^T(t)(\frac{I_{N-1}}{2}\otimes (P_{1}A+A^T{P}_1)-\frac{I_{N-1}}{2}\otimes P_{1}B{R}_1^{-1}{B}^T{P}_1)\bar{\epsilon }(t) \\ =-\frac{1}{2}{\bar{\epsilon }}^T(t)(I_{N-1}\otimes Q_1)\bar{\epsilon }(t) \end{gathered}$$
​ 由直积的性质有：
$$\begin{gathered} {\lambda }_{\min }(I_{N-1}\otimes Q_1)={\lambda }_{\min }(Q_1) \\ {\lambda }_{\max }(I_{N-1}\otimes P_1)={\lambda }_{\max }(P_1) \end{gathered}$$
​ 由矩阵的极大极小值原理：
$$\frac{\dot{V_1}(t)}{V_1(t)}=\frac{-\frac{1}{2}{\bar{\epsilon }}^T(t)(I_{N-1}\otimes Q_1)\bar{\epsilon }(t)}{\frac{1}{2}{\bar{\epsilon }}^T(t)(I_{N-1}\otimes P_1)\bar{\epsilon }(t)}\le -\frac{{\lambda }_{\min }(Q_1){\bar{\epsilon }}^T(t)\bar{\epsilon }(t)}{{\lambda }_{\max }(P_1){\bar{\epsilon }}^T(t)\bar{\epsilon }(t)}=-\frac{{\lambda }_{\min }(Q_1)}{{\lambda }_{\max }(P_1)}=-\delta$$
​ 解上述不等式有：
$$V_1(t)\le V_1(0)e^{-\delta t}$$
​ 带入$V_1(t)\ge 0.5{\lambda }_{\min }(P_1)||\bar{\epsilon }(t)|{|}^2$ 和$V_1(0)\le 0.5{\lambda }_{\max }(P_1)||\bar{\epsilon }(0)|{|}^2$有：
$$||\bar{\epsilon }(t)||\le \sqrt{\frac{{\lambda }_{\max }(P_1)}{{\lambda }_{\min }(P_1)}}||\bar{\epsilon }(0)||e^{\frac{-\delta t}{2}}$$
​ 因此上述系统（*）是一致指数稳定的，其必然是渐进稳定的。

2.仿真

2.1 问题

​ 设固定翼无人机集群中每个无人机的俯仰方向运动模型的线性化系统方程为:
$$\left(\begin{array}{c}\dot{\alpha }(t)\\ \dot{q}(t)\end{array}\right)=\left(\begin{array}{cc}-1.175& 0.9871\\ -8.458& -0.8776\end{array}\right)\left(\begin{array}{c}\alpha (t)\\ q(t)\end{array}\right)+\left(\begin{array}{cc}-0.194& -0.03593\\ -19.29& -3.803\end{array}\right)\left(\begin{array}{c}{\delta }_i^{\mathbf{ail}}(t)\\ {\delta }_i^{\mathbf{rud}}(t)\end{array}\right)$$
​ 其中：$\alpha (t)$：无人机俯仰角；$q_i(t)$:无人机俯仰角速度；

​ ${\delta }^{\mathbf{ail}}(t)$:副翼操作指令；${\delta }^{\mathbf{rud}}(t)$:升降舵操作指令；

​ 先有3架无人机，其通信拓扑图如下：

​ 3架无人机的初值为：
$$\left(\begin{array}{c}{\alpha }_1(0)\\ q_1(0)\end{array}\right)=\left(\begin{array}{c}10\\ -3\end{array}\right);\left(\begin{array}{c}{\alpha }_2(0)\\ q_2(0)\end{array}\right)=\left(\begin{array}{c}-7\\ 2\end{array}\right);\left(\begin{array}{c}{\alpha }_3(0)\\ q_3(0)\end{array}\right)=\left(\begin{array}{c}4\\ -1\end{array}\right)$$
​ 设计控制策略使3架无人机能完成俯仰角与俯仰角速度上的一致性。

2.2 求解

$A=\left(\begin{array}{cc}-1.175& 0.9871\\ -8.458& -0.8776\end{array}\right)$，$B=\left(\begin{array}{cc}-0.194& -0.03593\\ -19.29& -3.803\end{array}\right)$，设$R_1=50I_2$，$Q_1=10I_2$。

利用Matlab中的 care求解Riccati方程$P_{1}A+A{P}_1^T+Q_1-P_{1}B{R}_1^{-1}{B}^T{P}_1=0$得到：
$$P_1=\left(\begin{array}{cc}6.1743& -0.2904\\ -0.2904& 0.9991\end{array}\right)=P_1^T>0$$
代码如下：

>> A=[-1.175 0.9871;-8.458 -0.8776];
>> B = [-0.194 -0.03593;-19.29 -3.803];
>> R1 = 50*eye(2);
>> Q1 = 10*eye(2);
>> P1 = care(A,B,Q1,R1)

P1 =

    6.1743   -0.2904
   -0.2904    0.9991

故由定理1.1多智能体系统可以实现一致性。

可以知道在验证线性系统一致性的过程中并没有考虑个体间的通信Laplacian矩阵。这说明同构多智能体线性系统的一致性本质上与其Laplacian矩阵无关!!!

可以得到其Laplacian矩阵为：
$$L=\left(\begin{array}{ccc}1& -1& 0\\ -1& 2& -1\\ 0& -1& 1\end{array}\right)$$
其中矩阵$K_1=-R_1^{-1}{B}^T{P}_1=\left(\begin{array}{cc}-0.0881& 0.3843\\ -0.0177& 0.0758\end{array}\right)$，$c$要满足$c\ge \frac{1}{2{\min }_{i=2,...N}{\lambda }_i(L)}=0.5$，取$c=0.6$。得到控制策略：
$$\mathbf{u}(t)=c{K}_1\xi (t)=(I_2\otimes \left(\begin{array}{cc}-0.0529& 0.2306\\ -0.0106& 0.0455\end{array}\right))\xi (t)$$
下面用代码验证其可行性：

这里需要将连续系统零阶离散化，因此需要的采样时间很重要，不同采样时间会导致最后结果不收敛：

当$\Delta t=0.04s$时：

当$\Delta t=0.06s$时：

可以看到差距甚大，几乎不能用，因此说明了采样时间的重要性。同时也说明了该一致性控制策略有效!

3.代码

clc,clear;
A=[-1.175 0.9871;-8.458 -0.8776];
B = [-0.194 -0.03593;-19.29 -3.803];
c = 0.6;
K1 = [-0.0881 0.3843;...
      -0.0177 0.0758]; 
L = [1 -1 0;
    -1 2 -1;
     0 -1 1];
x1 = [10,-3]';x1_array = x1;
x2 = [-7,2]'; x2_array = x2;
x3 = [4,-1]'; x3_array = x3;
x_0 = [x1,x2,x3];
% 设置采样时间
delta_t = 0.04;
% num个时间步长内仿真
num = 100;
x = [x1;x2;x3];
%x_array = zeros(3*2,num);
for k = 1:num
   % 设计总的控制策略
   U = kron(L,c*K1*eye(2))*x;
   u1 = U(1:2);
   u2 = U(3:4);
   u3 = U(5:6);
for i = 1:3 
   x1 = (eye(2) +delta_t*A)*x1 + delta_t*B*u1; 
   x2 = (eye(2) +delta_t*A)*x2 + delta_t*B*u2;
   x3 = (eye(2) +delta_t*A)*x3 + delta_t*B*u3;
end
x = [x1;x2;x3];
x1_array = [x1_array,x1];
x2_array = [x2_array,x2];
x3_array = [x3_array,x3];
end
% 下面开始画图
subplot(121)
hold on;grid on;box on;
plot(delta_t*(1:num),x1_array(1,1:num),'r.-');
plot(delta_t*(1:num),x2_array(1,1:num),'g.-');
plot(delta_t*(1:num),x3_array(1,1:num),'b.-');
xlabel('时间/s');ylabel('俯仰角');
legend('无人机1','无人机2','无人机3');
subplot(122)
hold on;grid on;box on;
plot(delta_t*(1:num),x1_array(2,1:num),'r.-');
plot(delta_t*(1:num),x2_array(2,1:num),'g.-');
plot(delta_t*(1:num),x3_array(2,1:num),'b.-');
xlabel('时间/s');ylabel('俯仰角速度');
legend('无人机1','无人机2','无人机3');