Paper2022_多UGV系统的分布式最优编队控制 A Distributed Optimal Formation Control for Multi-UGV System

Zhao, J., Dai, F., Song, Y., Wang, F., Zhang, P., Li, F. (2022). A Distributed Optimal Formation Control for Multi-UGV System. In: Jia, Y., Zhang, W., Fu, Y., Zhao, S. (eds) Proceedings of 2022 Chinese Intelligent Systems Conference. CISC 2022. Lecture Notes in Electrical Engineering, vol 951. Springer, Singapore. https://doi.org/10.1007/978-981-19-6226-4_56

本文研究了UGV（无人驾驶地面车辆）的多智能体系统的分布式最优编队控制。从单个麦克纳姆轮（Mecanum）轮开始分析了UGV的模型，建立了PWM（脉宽调制）信号和状态信息之间的关系。基于一致性协议，设计了一个多UGV系统的编队控制协议。结合最优控制理论，研究了一个分布式最优编队控制协议。与非最优情况相比，最优控制可以有效降低控制成本和时间。最后，设计的协议被应用于一个工程平台，并验证了工程的价值。

1 Introduction

多智能体系统是指由许多单个智能体组成的系统，它们共同完成一项复杂的任务。多智能体的灵感最初来自于对自然界中集体行为的描述和观察。对自然行为的研究已经吸引了各领域大量研究人员的注意。

多智能体系统（multi-agent system，MAS）指由许多单个智能体（agent）组成，智能体之间通过相互协调而共同完成一个复杂任务的系统。多智能体协调是一个多学科、多领域融合的新兴工程，其概念最初是受到对自然界中集体行为描述和观察的启发。在多智能体系统协调控制的应用中，单个智能体设计为具有一定的传感、计算、存储与通信能力的个体，其结构较为简单，所完成的功能比较单一，动态系统的控制输入仅依赖于自身信息和其他有限个智能体的状态信息。生物界的这些行为在迁徙、躲避捕食者和找寻食物方面可能具有很大的优势，因此对这些行为的研究吸引了各领域大量研究人员的关注。

对于多智能体系统，大量的文献中提出了各种控制体系结构。它们中的大多数可以被描述为集中式和分布式方案。在集中式系统中，一个连接所有智能体的中央单元拥有全局的团队知识，并通过管理信息以保证任务的完成。对于分布式方案，所有的智能体都处于同一级别，拥有相同的设备。分布式系统中每个智能体的运动只依赖于邻居智能体的局部相对信息，降低了任务的难度。另外，局部智能体的失效不会影响整个系统的性能，因此分散式系统对恶劣环境的容错性更强。

现有的研究大多假设智能体为质点，这些质点满足一些控制要求。智能体之间的通信关系和控制协议都是基于质点模型来研究的。在实际生活中，智能体的运动比较复杂，不能直接简化为质点。这就限制了理论结果在实际工程中的应用。例如，在工程中，UGV（无人驾驶地面车辆）是由PWM（脉冲宽度调制）信号控制的。PWM是由主控制器产生的，这在大多数研究中都没有涉及。因此，我们通过分析UGV的模型，建立了PWM信号与UGV状态之间的关系。这些控制信息可以写入UGV的主控制器中，这大大提高了工程价值。

多智能体系统的一致性控制是编队控制的基础[1]。编队控制的问题指的是多个智能体向期望位置移动[2]。与一致性控制相比，编队控制下的最终状态值更加多样化。在本文中，我们设计了一个基于一致性控制的分布式编队控制协议。

由于各种实际问题的要求，我们总是要求系统的控制要受到各种约束[3]。这些约束条件通常被称为性能函数[4]。性能函数是衡量系统在任何允许的控制动作下的要求，其内容和形式取决于最优控制问题要完成的主要任务。本文结合设计的性能函数和分布式编队控制，构建了一个分布式最优编队控制协议。

本文的主要贡献包括三个方面。1）基于UGV的实际模型，将PWM信号与控制输入相连。2）基于该链接建立了分布式编队控制协议。3）根据分布式编队控制协议，结合最优控制理论，设计了一个分布式最优编队控制协议。本报告的其余部分由以下几节组成。在第2节中，介绍了代数图理论的一些基础知识。主要的结果是在第3节。第4节通过数值模拟验证了这些结果的可行性，并将这些结果应用于工程平台。最后，第5节给出了结论。

2 代数图论

令 $G=(V,E,A)$ 表示一个加权有向图，节点集 V = { v 1 , v 2 , ⋯   , v n } V = \{ v_1, v_2, \cdots, v_n \} V={v1,v2,⋯,vn}。边 $E\subseteq (i,j)|i,j\in V$ 是 e i j e_{ij} eij 的集合，表示智能体 $i$ 和 $j$ 之间有信息交流，信息流从 $j$ 流向 $i$。 A = [ a i j ] A = [a_{ij}] A=[aij] 是图 $G$ 的邻接矩阵，描述 $i$ 和 $j$ 之间信息交流的权重。为了简化计算，假定 a i j ∈ { 0 , 1 } a_{ij}∈ \{ 0, 1 \} aij∈{0,1}。邻居集合 N i = { v j ∈ V : e i j ∈ E } N_i = \{ v_j ∈ V : e_{ij} ∈ E \} Ni={vj∈V:eij∈E}，表示由 $i$ 和 $j$ 组成的节点。表示由所有与智能体 $i$ 有信息交流的智能体组成的节点。输入的度 deg i n ( v i ) \text{deg}_{in} (v_i) degin(vi) 用于表示度矩阵 $D$。因此，该系统的图拉普拉斯矩阵被定义为公式1。

$$L=D-A\text{\hspace{1em}(1)}$$

其中 $D$ 是入度矩阵，$A$ 是邻接矩阵。

根据定义，拉普拉斯矩阵的每一行之和都是零。因此，拉普拉斯矩阵总是有一个零特征值，对应于一个右边的特征值 w r = ( 1 / n ) 1 n = ( 1 / n ) [ 1 1 ⋯ 1 ] T w_r = (1/ \sqrt{n}) \mathbf{1}_n = (1/ \sqrt{n}) [ \begin{matrix} 1 & 1 & \cdots & 1 \end{matrix}]^\text{T} wr=(1/n )1n=(1/n )[11⋯1]T。

3 主要结果

3.1 UGV 模型

在分析多智能体系统时，通常会将智能体模型转换为一阶积分器、二阶积分器或高阶积分器。然而，这并不便于将理论结果转化为工程。移动机器人的运动学模型决定了车轮速度如何映射到机器人的本体速度，而动力学模型则决定了车轮扭矩如何映射到机器人的加速度。

在工程上，UGV的主控制器产生PWM信号，驱动电机控制车轮的旋转。主控制器产生的PWM信号与电机速度有如下关系
ω M i = min ⁡ { K P 2 M ⋅ u i ,      ω max ⁡ } ,      i = 1 , 2 , 3 , 4 (2) \omega_{Mi} = \min \{ K_{P2M} \cdot u_i, ~~~~\omega_{\max}\}, ~~~~ i=1,2,3,4 \tag{2} ωMi=min{KP2M⋅ui,    ωmax},    i=1,2,3,4(2)

其中 ω M i , i = 1 , 2 , 3 , 4 \omega_{Mi}, i=1,2,3,4 ωMi,i=1,2,3,4 表示电机速度， u i , i = 1 , 2 , 3 , 4 u_i, i=1,2,3,4 ui,i=1,2,3,4 是PWM信号， K P 2 M K_{P2M} KP2M 是PWM到电机的缩放因子， ω max ⁡ \omega_{\max} ωmax 表示电机最大速度。

假设 1

电机速度不会超过最大值，即 ω M i ≤ ω max ⁡ , i = 1 , 2 , 3 , 4 \omega_{Mi} \le \omega_{\max}, i=1,2,3,4 ωMi≤ωmax,i=1,2,3,4。

本文使用的车轮与电机是Mecanum车轮，如图1（a）所示。地面坐标系 S g = { X g , Y g , Z g } S_g = \{ X_g , Y_g , Z_g \} Sg={Xg,Yg,Zg} 和车身坐标系 S b = { X b , Y b , Z b } S_b = \{ X_b , Y_b , Z_b \} Sb={Xb,Yb,Zb} 已经建立。Mecanum轮子确保UGV可以在平面内以任何角度移动而不需要车身旋转。Mecanum轮子有一排转子，可以以 4 5 ∘ 45^\circ 45∘ 的角度自由旋转。

以一号轮M1为例，如图2所示。请注意，图2中绘制的是轮子与地面接触一侧的俯视图，因此与图1（a）中观察到的转子方向有 18 0 ∘ 180^\circ 180∘ 的差异。

当电机驱动麦克纳姆轮以角速度 ω M 1 ω_{M1} ωM1 旋转时，会产生一个正的前进速度 V g 1 V_{g1} Vg1。

V g 1 = ω M 1 ⋅ r (3) V_{g1} = \omega_{M1} \cdot r \tag{3} Vg1=ωM1⋅r(3)

其中 $r$ 是车轮半径。

速度 V g 1 V_{g1} Vg1 被分解为与转子方向平行的速度 V p 1 V_{p1} Vp1 和与转子方向垂直的速度 V v 1 V_{v1} Vv1。 V v 1 V_{v1} Vv1 被转子的自由旋转所抵消，只剩下与转子方向平行的有效速度 V p 1 V_{p1} Vp1。
V p 1 = V g 1 ⋅ sin ⁡ ( 4 5 ∘ ) (4) V_{p1} = V_{g1} \cdot \sin(45^\circ) \tag{4} Vp1=Vg1⋅sin(45∘)(4)

通过将有效速度 V p 1 V_{p1} Vp1 分别沿车身坐标系的 X b X_b Xb 轴和 Y b Y_b Yb 轴分解，如图2所示，可以得到1号车轮对车身速度的影响。

V p 1 x = V p 1 ⋅ cos ⁡ ( 4 5 ∘ ) V p 1 y = V p 1 ⋅ sin ⁡ ( 4 5 ∘ ) (5) \begin{aligned} V_{p1x} &= V_{p1} \cdot \cos(45^\circ) \\ V_{p1y} &= V_{p1} \cdot \sin(45^\circ) \\ \tag{5} \end{aligned} Vp1xVp1y=Vp1⋅cos(45∘)=Vp1⋅sin(45∘)(5)

在UGV车身坐标系中建立四个车轮的分解，如图1（b）所示，我们可以得到，四个车轮对UGV的影响满足以下关系式
v b x = V p 1 x − V p 2 x V p 3 x − V p 4 x v b y = V p 1 y V p 2 y V p 3 y V p 4 y (6) \begin{aligned} v_b^x &= V_{p1x} - V_{p2x} V_{p3x} - V_{p4x} \\ v_b^y &= V_{p1y} V_{p2y} V_{p3y} V_{p4y} \\ \tag{6} \end{aligned} vbxvby=Vp1x−Vp2x Vp3x−Vp4x=Vp1y Vp2y Vp3y Vp4y(6)

关于UGV的旋转，假设有效速度 V p i , i = 1 , 2 , 3 , 4 V_{pi} , i = 1, 2, 3, 4 Vpi,i=1,2,3,4 垂直于连接车身中心和车轮中心的直线，那么就会有
ω b = ( V p 1 V p 2 − V p 3 − V p 4 ) / L G (7) \omega_b = (V_{p1} V_{p2} - V_{p3} - V_{p4}) / L_G \tag{7} ωb=(Vp1 Vp2−Vp3−Vp4)/LG(7)

其中 L G L_G LG 为车身的旋转半径。

通过四个轮子的配合，我们可以得到UGV的运动学模型为

[ v b x v b y ω b ] = [ 1 2 − 1 2 1 2 − 1 2 1 2 1 2 1 2 1 2 1 L G 1 L G − 1 L G − 1 L G ] [ V p 1 V p 2 V p 3 V p 4 ] (8) \begin{aligned} \left[\begin{matrix} v_b^x \\ v_b^y \\ \omega_b \\ \end{matrix}\right] = \left[\begin{matrix} \frac{1}{\sqrt{2}} & -\frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} & -\frac{1}{\sqrt{2}} \\ \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} & \frac{1}{\sqrt{2}} \\ \frac{1}{L_G} & \frac{1}{L_G} & -\frac{1}{L_G} & -\frac{1}{L_G} \\ \end{matrix}\right] \left[\begin{matrix} V_{p1} \\ V_{p2} \\ V_{p3} \\ V_{p4} \\ \end{matrix}\right] \end{aligned} \tag{8} ⎣ ⎡vbxvbyωb⎦ ⎤=⎣ ⎡2 12 1LG1−2 12 1LG12 12 1−LG1−2 12 1−LG1⎦ ⎤⎣ ⎡Vp1Vp2Vp3Vp4⎦ ⎤(8)

令地面坐标系中的位置为 p = [ p x p y ] p = \left[\begin{aligned}p^x & p^y \end{aligned}\right] p=[pxpy], 速度信息为 v = [ v x v y ] v = \left[\begin{aligned}v^x & v^y \end{aligned}\right] v=[vxvy]。

假设 2

假设UGV不发生旋转，即 ω b = 0 \omega_b = 0 ωb=0，同时地面坐标系与机体坐标系的原点重合。

那么机体坐标系中的速度 v b x , v b y v_b^x , v_b^y vbx,vby 等同于地面坐标系中的速度 v x , v y v^x , v^y vx,vy。同时，地面坐标系中的位置和速度也有如下关系。

p ˙ x = v x p ˙ y = v y (9) \begin{aligned} \dot{p}^x &= v^x \\ \dot{p}^y &= v^y \\ \end{aligned} \tag{9} p˙xp˙y=vx=vy(9)

综上所述，我们可以得到UGV在世界坐标系中的位置与控制信号的关系为

[ u 1 u 2 u 3 u 4 ] = 1 2 ⋅ K P 2 M ⋅ r [ 1 1 − 1 1 1 1 − 1 1 ] [ p ˙ x p ˙ y ] (10) \begin{aligned} \left[\begin{matrix} u_1 \\ u_2 \\ u_3 \\ u_4 \\ \end{matrix}\right]= \frac{1}{2 \cdot K_{P2M} \cdot r} \left[\begin{matrix} 1 & 1 \\ -1 & 1 \\ 1 & 1 \\ -1 & 1 \\ \end{matrix}\right] \left[\begin{matrix} \dot{p}^x \\ \dot{p}^y \\ \end{matrix}\right] \end{aligned} \tag{10} ⎣ ⎡u1u2u3u4⎦ ⎤=2⋅KP2M⋅r1⎣ ⎡1−11−11111⎦ ⎤[p˙xp˙y](10)

因此，UGV的模型可以描述为一阶积分器系统。

3.2 分布式编队控制

对于一阶积分器系统，它有如下表示。

p ˙ x = u x p ˙ y = u y (11) \begin{aligned} \dot{p}^x &= u^x \\ \dot{p}^y &= u^y \\ \end{aligned} \tag{11} p˙xp˙y=ux=uy(11)

定义期望编队向量为 d p ( t ) = [ d p x ( t ) d p y ( t ) ] T d^p(t) = [\begin{matrix} d^{px}(t) & d^{py}(t) \end{matrix}]^\text{T} dp(t)=[dpx(t)dpy(t)]T。令编队误差向量为 δ ( t ) = [ δ p x ( t ) δ p y ( t ) ] T = [ p x ( t ) p y ( t ) ] T − [ d p x ( t ) d p y ( t ) ] T \delta(t) = [\begin{matrix} \delta^{px}(t) & \delta^{py}(t) \end{matrix}]^\text{T} = \red{ [\begin{matrix} p^{x}(t) & p^{y}(t) \end{matrix}]^\text{T} - [\begin{matrix} d^{px}(t) & d^{py}(t) \end{matrix}]^\text{T} } δ(t)=[δpx(t)δpy(t)]T=[px(t)py(t)]T−[dpx(t)dpy(t)]T。关于编队控制任务，可以这样描述。

lim ⁡ t → ∞ ∥ δ i p x ( t ) − δ j p x ( t ) ∥ = 0 , lim ⁡ t → ∞ ∥ δ i p y ( t ) − δ j p y ( t ) ∥ = 0 , i = 1 , 2 , ⋯   , N (12) \begin{aligned} & \lim_{t\rightarrow \infty} \| \delta^{px}_i(t) - \delta^{px}_j(t) \| = 0, \\ & \lim_{t\rightarrow \infty} \| \delta^{py}_i(t) - \delta^{py}_j(t) \| = 0, \quad i=1,2,\cdots, N \\ \end{aligned} \tag{12} t→∞lim∥δipx(t)−δjpx(t)∥=0,t→∞lim∥δipy(t)−δjpy(t)∥=0,i=1,2,⋯,N(12)

其中 $N$ 是智能体的数量。

那么编队控制问题就可以转化为关于编队误差向量的一致性问题。结合之前的工作[1]，我们提出了以下编队控制协议

u ˙ i x = α ∑ j ∈ N i ( δ j p x ( t ) − δ i p x ( t ) ) u ˙ i y = α ∑ j ∈ N i ( δ j p y ( t ) − δ i p y ( t ) ) , i = 1 , 2 , ⋯   , N (13) \begin{aligned} \dot{u}_{i}^x &= \alpha \sum_{j\in N_i} (\delta^{px}_{j}(t) - \delta^{px}_{i}(t)) \\ \dot{u}_{i}^y &= \alpha \sum_{j\in N_i} (\delta^{py}_{j}(t) - \delta^{py}_{i}(t)), \quad i=1,2,\cdots,N \end{aligned} \tag{13} u˙ixu˙iy=αj∈Ni∑(δjpx(t)−δipx(t))=αj∈Ni∑(δjpy(t)−δipy(t)),i=1,2,⋯,N(13)

其中 $\alpha >0$ 是正向增益。

定理1

假设 $G$ 是一个无向连接图或一个有向图有生成树。系统（11）可以通过使用协议（13）实现（12）中定义的编队。

证明 请参考文献[4]。

3.3 分布式最优编队控制

由于各种工程问题的要求，我们总是要求系统的控制要受到各种限制，这些限制通常被称为性能函数。根据控制问题的不同，应采取不同形式的性能函数。线性二次性能函数的最优解有一个统一的分析表达式，并生成一个简单的状态线性反馈控制法，便于闭环反馈控制的计算和实施。

本节构建了一个由误差向量和控制输入组成的积分型性能函数（14）。

J = ∫ 0 ∞ [ ∑ j ∈ N i ( δ j p − δ i p ) T Q ( δ j p − δ i p ) u i T R u i ] d t J = \int_0^\infty [\sum_{j \in N_i} (\delta^{p}_j-\delta^{p}_i)^\text{T} Q (\delta^{p}_j - \delta^{p}_i) u_i^\text{T} R u_i] dt J=∫0∞[j∈Ni∑(δjp−δip)TQ(δjp−δip) uiTRui]dt

其中 $Q$ 是一个具有适当维度的对称非负定矩阵，$R$ 是一个具有适当维度的对称正定矩阵。为了便于工程应用，性能指标中的权重矩阵 $Q,R$ 往往取为对角线型矩阵，则其对称性自然满足。

通过最优控制理论，多UGV系统的最优控制法是
u i ∗ = − R − 1 B T P ( X − d X ) (15) u_i^* = - R^{-1} B^\text{T} P (X - d^X) \tag{15} ui∗=−R−1BTP(X−dX)(15)

其中 $P$ 是下述黎卡提方程 （16）的解。

A T P P A − P B R − 1 B T P Q = 0 (16) A^\text{T} P P A - P B R^{-1} B^\text{T} P Q = 0 \tag{16} ATP PA−PBR−1BTP Q=0(16)

令 K = − R − 1 B T P \red{ K = -R^{-1} B^\text{T} P } K=−R−1BTP，矩阵 $K$ 的维度一定是 $2\times 2$，同时它有如下形式
K = [ k 1 0 0 k 1 ] (17) K = \left[\begin{matrix} k_1 & 0 \\ 0 & k_1 \\ \end{matrix}\right] \tag{17} K=[k100k1](17)

UGV的多智能体系统是同构的，即所有智能体的动态性能是相同的，所以最优控制法可以直接应用于UGV系统。因此，最优编队控制法得到如下结果

u ˙ i x = k 1 ∑ j ∈ N i ( δ j p x ( t ) − δ i p x ( t ) ) u ˙ i y = k 1 ∑ j ∈ N i ( δ j p y ( t ) − δ i p y ( t ) ) , i = 1 , 2 , ⋯   , N (18) \begin{aligned} \dot{u}_{i}^x &= k_1 \sum_{j\in N_i} (\delta^{px}_{j}(t) - \delta^{px}_{i}(t)) \\ \dot{u}_{i}^y &= k_1 \sum_{j\in N_i} (\delta^{py}_{j}(t) - \delta^{py}_{i}(t)), \quad i=1,2,\cdots,N \end{aligned} \tag{18} u˙ixu˙iy=k1j∈Ni∑(δjpx(t)−δipx(t))=k1j∈Ni∑(δjpy(t)−δipy(t)),i=1,2,⋯,N(18)

定理2

假设 $G$ 是一个无向连接图或有向图的生成树。系统（11）不仅实现了（12）中定义的编队，而且还通过使用协议（18）优化了性能函数。

证明。请参考文献[4]。

4 Simulations

本节使用三个UGV来验证协议在理论和工程方面的有效性。系统的通信拓扑结构如图3所示。关于硬件平台的更多信息请参考[5]。

初始位置设置为 p 1 x ( 0 ) = 10 , p 1 y ( 0 ) = 20 , p 2 x ( 0 ) = 10 , p 2 y ( 0 ) = 30 , p 3 x ( 0 ) = 10 , p 3 y ( 0 ) = 10 p_1^x (0) = 10, p_1^y (0) = 20, p_2^x (0) = 10, p_2^y (0) = 30, p_3^x (0) = 10, p_3^y (0) = 10 p1x(0)=10,p1y(0)=20,p2x(0)=10,p2y(0)=30,p3x(0)=10,p3y(0)=10。编队任务被设定为三角形编队。以UGV1为参考，假设其期望的编队位置 d 1 p x ( t ) = 0 , d 1 p y ( t ) = 0 d_1^{px} (t) = 0, d_1^{py} (t) = 0 d1px(t)=0,d1py(t)=0，则其他期望的编队位置为 d 2 p x ( t ) = − 10 , d 2 p y ( t ) = 10 , d 3 p x ( t ) = − 10 , d 3 p y ( t ) = − 10 d_2^{px} (t) = -10, d_2^{py} (t) = 10, d_3^{px} (t) = -10, d_3^{py} (t) = -10 d2px(t)=−10,d2py(t)=10,d3px(t)=−10,d3py(t)=−10。设置增益参数 $\alpha =0.4$。 K P 2 M = 2 ， r = 0.25 K_{P2M} = 2，r = 0.25 KP2M=2，r=0.25。为了便于观察，假定三个UGV在 $X$ 轴上的基本速度恒定为6。基于分布式编队控制协议（13）的位置如图4（a）所示，三角形编队大约在第3秒时完成。

下一步是分析分布式最优编队控制协议（18）。假设权重矩阵 Q = 3 ⋅ I 6 \red{ Q=3 \cdot I_6 } Q=3⋅I6， R = 1 ⋅ I 6 \red{ R=1 \cdot I_6 } R=1⋅I6，那么我们可以求出最优参数 k 1 = 1.7321 k_1=1.7321 k1=1.7321。图4(b)显示了基于分布式最优编队控制协议的位置，它大约在第1秒完成了编队任务。

图5显示了UGV1的PWM信号在非最优情况和最优情况下的比较。很明显，在最佳情况下，系统控制输入较低，同时UGV完成任务的时间也较短。

工程实验场景图见图6。工程实验的结果如图7所示，显示了三个UGVs在不同时间段的位置。实验开始时，三辆UGV排成一条直线（$t=0s$），然后由于控制协议，开始逐渐远离对方（$t=15s$）。之后，编队任务完成（$t=30s$），运行停止（$t=35s$）。

5 结论

本文通过对单个麦克纳姆轮的动态分析，构建了UGV的运动学模型。在分布式一致性协议的基础上，设计了一个多UGV系统的分布式编队控制协议。为了提高协议的工程应用价值，结合最优控制理论设计了分布式最优编队控制协议。通过调整权重矩阵，可以满足不同的任务要求。

参考文献

1. Zhao, J., Dai, F., Song, Y.: Consensus of heterogeneous mixed-order multi-agent systems including UGV and UAV. In: Jia, Y., Zhang, W., Fu, Y., Yu, Z., Zheng,S. (eds.) Proceedings of 2021 Chinese Intelligent Systems Conference. LNEE, vol. 805, pp. 202–210. Springer, Singapore (2022). https://doi.org/10.1007/978-981-166320-8 21
2. Liu, Z., Li, Y., Wu, Y., et al.: Formation control of nonholonomic unmanned ground vehicles via unscented Kalman filter-based sensor fusion approach. ISA Trans. 125, 60–71 (2022)
3. Zhang, J., Yue, X., Zhang, H., et al.: Optimal unmanned ground vehicle-unmanned aerial vehicle formation-maintenance control for air-ground cooperation. Appl. Sci. 12(7), 3598 (2022)
4. Zhao, J., Dai, F., Song, Y.: A distributed optimal formation control for multi-agent system of UAVs. In: 2022 Proceedings of International Conference on Artificial Life and Robotics (ICAROB), pp. 803–807. IEEE (2022)
5. Zhao, J., Dai, F.: A design of multi-agent system simulation platform for UGV and research on its formation control protocol. J. Adv. Artif. Life Robot. 3(1), 13–17 (2022)

附录

% Paper: A Distributed Optimal Formation Control for Multi-UGV System
% Author: Z-JC
% Data: 2022-03-27
clear
clc

%%
L = [1  0 -1
    -1  2 -1
     0 -1  1];

% UGV system states
UGV1(:,1) = [10 20]';
UGV2(:,1) = [10 30]';
UGV3(:,1) = [10 10]';

p_x = [UGV1(1,1)  UGV2(1,1)  UGV3(1,1)]';
p_y = [UGV1(2,1)  UGV2(2,1)  UGV3(2,1)]';

% Control inputs
u_x(:,1) = [0  0  0]';
u_y(:,1) = [0  0  0]';

u_PWM1(:,1) = [0  0  0  0]';
u_PWM2(:,1) = [0  0  0  0]';
u_PWM3(:,1) = [0  0  0  0]';

% Desired formation states
d_px_1 = 00; d_py_1 = 00;
d_px_2 =-10; d_py_2 = 10;
d_px_3 =-10; d_py_3 =-10;

% Positive gain
K_P2M = 2;
r = 0.25;
alpha = 0.4;

% Optimal control
% System matrix
A = zeros(2,2);
B = eye(2);
Q = eye(2) * 3;
R = eye(2) * 1;
P = care(A,B,Q,R);
K = pinv(R)*B'*P;
alpha = K(1,1)

% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 5;
dT = 0.1;
times = (tFinal-tBegin)/dT;

M_M2P = [1  1
        -1  1
         1  1
        -1  1];

% Iterations
for k=1:times
    % Record time
    t(:,k 1) = t(:,k)   dT;
    
    % Calculate control input
    u_x(1,k 1) = alpha * (-L(1,1)*(p_x(1,k)-d_px_1-p_x(1,k) d_px_1) - L(1,2)*(p_x(2,k)-d_px_2-p_x(1,k) d_px_1) - L(1,3)*(p_x(3,k)-d_px_3-p_x(1,k) d_px_1) );
    u_x(2,k 1) = alpha * (-L(2,1)*(p_x(1,k)-d_px_1-p_x(2,k) d_px_2) - L(2,2)*(p_x(2,k)-d_px_2-p_x(2,k) d_px_2) - L(2,3)*(p_x(3,k)-d_px_3-p_x(2,k) d_px_2) );
    u_x(3,k 1) = alpha * (-L(3,1)*(p_x(1,k)-d_px_1-p_x(3,k) d_px_3) - L(3,2)*(p_x(2,k)-d_px_2-p_x(3,k) d_px_3) - L(3,3)*(p_x(3,k)-d_px_3-p_x(3,k) d_px_3) );
    
    u_y(1,k 1) = alpha * (-L(1,1)*(p_y(1,k)-d_py_1-p_y(1,k) d_py_1) - L(1,2)*(p_y(2,k)-d_py_2-p_y(1,k) d_py_1) - L(1,3)*(p_y(3,k)-d_py_3-p_y(1,k) d_py_1) );
    u_y(2,k 1) = alpha * (-L(2,1)*(p_y(1,k)-d_py_1-p_y(2,k) d_py_2) - L(2,2)*(p_y(2,k)-d_py_2-p_y(2,k) d_py_2) - L(2,3)*(p_y(3,k)-d_py_3-p_y(2,k) d_py_2) );
    u_y(3,k 1) = alpha * (-L(3,1)*(p_y(1,k)-d_py_1-p_y(3,k) d_py_3) - L(3,2)*(p_y(2,k)-d_py_2-p_y(3,k) d_py_3) - L(3,3)*(p_y(3,k)-d_py_3-p_y(3,k) d_py_3) );
    
    % Calculate PWM
    u_PWM1(:,k 1) = 1/(2*K_P2M*r) * M_M2P * [u_x(1,k 1); u_y(1,k 1)];
    u_PWM2(:,k 1) = 1/(2*K_P2M*r) * M_M2P * [u_x(2,k 1); u_y(2,k 1)];
    u_PWM3(:,k 1) = 1/(2*K_P2M*r) * M_M2P * [u_x(3,k 1); u_y(3,k 1)];
    
    % Update states
    p_x(1,k 1) = p_x(1,k)   dT * u_x(1,k 1)   0.5;
    p_x(2,k 1) = p_x(2,k)   dT * u_x(2,k 1)   0.5;
    p_x(3,k 1) = p_x(3,k)   dT * u_x(3,k 1)   0.5;
    p_y(1,k 1) = p_y(1,k)   dT * u_y(1,k 1);
    p_y(2,k 1) = p_y(2,k)   dT * u_y(2,k 1);
    p_y(3,k 1) = p_y(3,k)   dT * u_y(3,k 1);
    
end


%% Plot results
figure(1)
subplot(2,1,1)
plot(t,p_x, 'linewidth',1.5); hold on
grid on

subplot(2,1,2)
plot(t,p_y, 'linewidth',1.5); hold on
grid on

figure(2)
plot(p_x(1,:),p_y(1,:), '--r', 'linewidth',1.5); hold on
plot(p_x(2,:),p_y(2,:), '--g', 'linewidth',1.5); hold on
plot(p_x(3,:),p_y(3,:), '--b', 'linewidth',1.5); hold on
xlabel("x (m)",'Interpreter','latex'); ylabel("y (m)",'Interpreter','latex');
grid on;
xlim([0,50])
ylim([0,50])

tt1 = 1;
scatter(p_x(1,tt1),p_y(1,tt1),'r'); hold on
scatter(p_x(2,tt1),p_y(2,tt1),'g'); hold on
scatter(p_x(3,tt1),p_y(3,tt1),'b'); hold on
line([p_x(1,tt1),p_x(2,tt1)],[p_y(1,tt1),p_y(2,tt1)])
line([p_x(2,tt1),p_x(3,tt1)],[p_y(1,tt1),p_y(3,tt1)])
line([p_x(3,tt1),p_x(1,tt1)],[p_y(3,tt1),p_y(1,tt1)])
text(p_x(3,tt1)-1,p_y(3,tt1)-1,'t = 0s','Interpreter','latex')

tt1 = 1/dT;
scatter(p_x(1,tt1),p_y(1,tt1),'r'); hold on
scatter(p_x(2,tt1),p_y(2,tt1),'g'); hold on
scatter(p_x(3,tt1),p_y(3,tt1),'b'); hold on
line([p_x(1,tt1),p_x(2,tt1)],[p_y(1,tt1),p_y(2,tt1)])
line([p_x(2,tt1),p_x(3,tt1)],[p_y(2,tt1),p_y(3,tt1)])
line([p_x(3,tt1),p_x(1,tt1)],[p_y(3,tt1),p_y(1,tt1)])
text(p_x(3,tt1)-1,p_y(3,tt1)-1,'t = 1s','Interpreter','latex')

tt1 = 3/dT;
scatter(p_x(1,tt1),p_y(1,tt1),'r'); hold on
scatter(p_x(2,tt1),p_y(2,tt1),'g'); hold on
scatter(p_x(3,tt1),p_y(3,tt1),'b'); hold on
line([p_x(1,tt1),p_x(2,tt1)],[p_y(1,tt1),p_y(2,tt1)])
line([p_x(2,tt1),p_x(3,tt1)],[p_y(2,tt1),p_y(3,tt1)])
line([p_x(3,tt1),p_x(1,tt1)],[p_y(3,tt1),p_y(1,tt1)])
text(p_x(3,tt1)-1,p_y(3,tt1)-1,'t = 3s','Interpreter','latex')

tt1 = 5/dT;
scatter(p_x(1,tt1),p_y(1,tt1),'r'); hold on
scatter(p_x(2,tt1),p_y(2,tt1),'g'); hold on
scatter(p_x(3,tt1),p_y(3,tt1),'b'); hold on
line([p_x(1,tt1),p_x(2,tt1)],[p_y(1,tt1),p_y(2,tt1)])
line([p_x(2,tt1),p_x(3,tt1)],[p_y(2,tt1),p_y(3,tt1)])
line([p_x(3,tt1),p_x(1,tt1)],[p_y(3,tt1),p_y(1,tt1)])
text(p_x(3,tt1)-1,p_y(3,tt1)-1,'t = 5s','Interpreter','latex')
grid on

legend("UGV1", "UGV2", "UGV3",'Interpreter','latex');

figure(3)
subplot(3,1,1)
plot(t(1,1:k),u_PWM1(1,2:k 1), t(1,1:k),u_PWM1(2,2:k 1), t(1,1:k),u_PWM1(3,2:k 1), t(1,1:k),u_PWM1(4,2:k 1), 'linewidth',1.5)
legend("$u_{11}$", "$u_{12}$", "$u_{13}$", "$u_{14}$", 'Interpreter','latex'); grid on
xlabel("t (s)",'Interpreter','latex'); ylabel("PWM $u_1$",'Interpreter','latex');

subplot(3,1,2)
plot(t(1,1:k),u_PWM2(1,2:k 1), t(1,1:k),u_PWM2(2,2:k 1), t(1,1:k),u_PWM2(3,2:k 1), t(1,1:k),u_PWM2(4,2:k 1), 'linewidth',1.5)
legend("$u_{21}$", "$u_{22}$", "$u_{23}$", "$u_{24}$", 'Interpreter','latex'); grid on
xlabel("t (s)",'Interpreter','latex'); ylabel("PWM $u_2$",'Interpreter','latex');

subplot(3,1,3)
plot(t(1,1:k),u_PWM3(1,2:k 1), t(1,1:k),u_PWM3(2,2:k 1), t(1,1:k),u_PWM3(3,2:k 1), t(1,1:k),u_PWM3(4,2:k 1), 'linewidth',1.5)
legend("$u_{31}$", "$u_{32}$", "$u_{33}$", "$u_{34}$", 'Interpreter','latex'); grid on
xlabel("t (s)",'Interpreter','latex'); ylabel("PWM $u_3$",'Interpreter','latex');

figure(4)
subplot(2,1,1)
plot(t(1,1:k),u_x(1,2:k 1), t(1,1:k),u_x(2,2:k 1), t(1,1:k),u_x(3,2:k 1), 'linewidth',1.5); hold on; grid on;
xlabel("t (s)",'Interpreter','latex'); ylabel("PWM $u^{x}$",'Interpreter','latex');

subplot(2,1,2)
plot(t(1,1:k),u_y(1,2:k 1), t(1,1:k),u_y(2,2:k 1), t(1,1:k),u_y(3,2:k 1), 'linewidth',1.5); hold on; grid on;
xlabel("t (s)",'Interpreter','latex'); ylabel("PWM $u^{y}$",'Interpreter','latex');

这篇好文章是转载于：学新通技术网