近些年来，移动机器人的编队控制与避障能力引起了许多研究者的兴趣，是机器人领域中非常热门的研究方向之一。基于分布式一致性的编队控制获得了快速且持续的发展，在基于一致性的编队中，由于机器人之间的通信带宽受到限制及机器人之间的信息传输应具备高质量和无延时的高标准，因此考虑机器人只能与它的邻居接收和发送信息是十分有必要的。在领导者−跟随者机器人编队形式中^[1-3]，有一个及以上机器人为领导者，领导者可以是其中的某个机器人或者为虚拟机器人，它决定整个编队的运动轨迹和方向，其他移动机器人作为跟随者被领导者带领运动，并相互之间保持相对的距离与方位。文献[4]实现了一种模型预测控制(Model Predictive Control, MPC)方法来控制一个领导者−跟随者编队系统。通过在分离度−姿态−方位模型(Separation-Bearing-Orientation Scheme, SBOS)和避障模型之间进行切换，使编队避免碰撞，达到理想的编队状态，并对约束条件进行处理。但是SBOS模型难以与避障模型相结合起来。于是文献[5]提出了固定无向拓扑下基于距离与速度的人工势场方法的编队避障系统，但是没有考虑有向拓扑且拓扑可以变化下的避障，并且没考虑机器人相互避免碰撞的同时，能避免障碍物的问题。结合以上参考的工作，为在期望路径上规划机器人到达并与领导机器人保持期望距离，通过构造编队路径规划的控制输入与避障系统结合起来。

研究者们提出了许多的控制算法来控制机器人编队，如一致控制^[6-7]、自适应控制^[8]、滑模^[9-10]和人工势场^[11]等。对于群体编队，需要考虑系统约束问题。然而，模型预测控制(MPC)能够很好地考虑移动机器人的物理约束问题。另一方面，模型预测控制(MPC)方法已成功地应用于工业环境下的复杂系统的控制，是应用最广泛的最优控制技术之一。其中，文献[12-13]是一种模型预测控制(MPC)，具有轨迹优化能力，并充分考虑物理约束，适用于多机器人控制。一般来说，MPC可以解决包含约束的优化问题。在预测控制范围内，由最优解得到的输入序列总能考虑机器人的内在物理约束。为解决预测控制的优化问题，需要一种高效的优化算法。文献[14]将多机器人编队控制(General Projection Neural Network, GPNN)方法与多机器人一致编队相结合，来求解包含编队最优输入的约束(Quadratic Programming, QP)问题的最优解。与现有的领导者−跟随者方法相比，所提出的MPC方法能够考虑系统的输入约束和状态约束。本文利用投影神经网络优化算法来处理基于MPC方法的优化问题。

在实际机器人运动中，机器人处理避障的能力是十分重要的，研究者们对于这一问题进行了深入的研究^[15-18]，其中一种较简单且广泛使用的避障方式为基于距离的人工势场法。人工势场法的主要思想是，当一个机器人进入其他机器人的势场中时，机器人会产生一种很强的排斥力，迫使它远离其他机器人，从而避免发生碰撞。然而，文献[19-20]中的人工势场方法仅基于机器人之间的距离进行设计，在很多情况下具有相当大的局限性。例如：一机器人虽然进入势场中，但是两机器人的移动方向和速度不同，使得机器人之间不会产生碰撞，此时，机器人也会因靠近产生排斥力，从而会增加机器人的能量消耗，并且易致编队控制不稳定。机器人也需有避开障碍物的能力，运用人工势场法来进行避障处理。基于上述分析，本文利用一种基于距离和速度的人工势场方法来处理多移动机器人编队控制的机器人间的避碰问题和人工势场法来处理避免障碍物。

受以上文献启发，本文首次将文献[5]的基于距离与速度的人工势场方法引入有向拓扑且拓扑可以变化下考虑机器人相互避免碰撞避障。同时，运用人工势场法^[21]能避免障碍物的问题，且融合进一阶多移动机器人的一致性编队路径规划控制中。本文提出两层编队控制框架，在编队控制层面上，设计具有避碰的编队控制律控制。可以使机器人在相对平滑的轨迹上运动且具有避障能力，并与相邻机器人达到并保持预期的距离。在编队跟踪层，应用GPNN结合MPC方法重构并求解一个基于跟踪误差模型的约束QP问题。得到控制机器人的最优输入，同时考虑速度约束和系统状态约束。该两层群形成框架综合了轨迹生成方法和算法的优点。

1 预备知识及问题描述 1.1 代数图论

在大多数多机器人一致性编队控制中，成员只能在一定范围内接收或发送信息，为了描述在编队中交换的信息，运用有向图 $G\; =\; (V,E, {{\boldsymbol{A}}})$ ， $ V = 1,2, \cdots ,M $ 是节点集， $ E \subseteq V{{ \times }}V $ 表示有向边集合， $ (i,j)\in E $ 意味着机器人 $ {R_i} $ 可以接收到机器人 $ {R_j} $ 的信息，如果 $ (j,i) \in E $ ，则 $ {a_{ij}} = 1 $ ；如果 $ (j,i) \notin E $ 或者 $ i = j $ ，则 $ {a_{ij}} = 0 $ ； ${\boldsymbol{A}}$ 表示由 $ {a_{ij}} $ 构成的相关加权邻接矩阵。定义 ${d_i} =\displaystyle \sum\limits_{j = 1}^M {{a_{ij}}}$ 为机器人 $ i $ 的入度， ${\boldsymbol{D}} \in {R^{M \times M}}$ 是由 $ {d_i} $ 组成的对角矩阵。让 ${\boldsymbol{L}} = {\boldsymbol{D}}- {\boldsymbol{A}}$ 表示拉普拉斯矩阵 ${\boldsymbol{L}}\in {R^{M \times M}}$ 。

机器人在运行过程中，能够对机器人形成的拓扑通信关系进行变换是十分必要的，当拓扑发生变化时，在 ${\boldsymbol{A}}$ 中的加权邻接 $ {a_{ij}} $ 会根据新的信息传递方向而变化，从虚拟领导者获取的信息可以表示为一个权重矩阵 $ {\boldsymbol{B}} = {\rm{diag}}({b_1},{b_2}, \cdots ,{b_M}) $ 。若 $ t $ 时刻， $ {R_i} $ 可以访问 ${R_{\rm{L}}}$ 的信息，则 $ {b_i} = 1 $ ,否则， $ {b_i} = 0 $ 。矩阵 $ {\boldsymbol{F}} = {\boldsymbol{L}} + {\boldsymbol{B}} $ 。在本文中， ${R_{\rm{L}}}$ 被视为一个虚拟的领导者来领导编队。

1.2 问题描述

在本文中，目标是设计一个多移动机器人编队，来实现可变拓扑与避障的一致性编队控制。在笛卡尔全局坐标系下，机器人 $ {R_i}(i \in V) $ 与 ${R_{\rm{L}}}$ 的期望相对坐标可表示为： ${\boldsymbol{P}}_{_{i{\rm{L}}}}^*(t) = {\left[ {P_{ix}^*,P_{iy}^*} \right]^{\rm T}}$ 。则 $ M $ 个机器人与 ${R_{\rm{L}}}$ 的相对坐标可表示为： $ {\boldsymbol{P}}(t) = {\left[ {{{\boldsymbol{P}}_{1{\rm{L}}}}(t), \cdots ,{{\boldsymbol{P}}_{M{\rm{L}}}}(t)} \right]^{\rm T}} $ 。对于编队队形为正规则几何图案，则 $ {\boldsymbol{P}}(t) $ 满足于： $ \displaystyle \sum\limits_{i = 1}^M {{{\boldsymbol{P}}_{i{\rm{L}}}}(t)} = {{\boldsymbol{P}}_{0{\rm{L}}}} $ , $ {{\boldsymbol{P}}_{0{\rm{L}}}} = {\left[ {{P_{0x}},{P_{0y}}} \right]^{\rm T}} $ 为领导者 $ {R_{\rm{L}}} $ 的相对坐标，即为几何图案中心的相对坐标。编队形成或保持队形运动过程中，应满足于： $ \displaystyle \sum\limits_{i = 1}^M {{{\boldsymbol{P}}_i}(t)} = M{{\boldsymbol{P}}_{\rm{L}}}(t) $ 。 $ {{\boldsymbol{P}}_i}(t) = {\left[ {{x_i}(t),{y_i}(t)} \right]^{\rm T}} $ 为跟随机器人 $ {R_i} $ 的位置， $ {{\boldsymbol{P}}_{\rm{L}}}(t) = {\left[ {{x_{\rm{L}}}(t),{y_{\rm{L}}}(t)} \right]^{\rm T}} $ 为领导者的位置，即所需图案的几何中心。

以下定义提出领导者−跟随者时变一致性编队的控制目标。

定义1 　虚拟机器人 $ {R_{\rm{L}}} $ 领导一群移动机器人( $ {R_1} - {R_M} $ )形成规则几何图案运动时，需满足以下极限方程： $ \mathop {\lim }\limits_{t \to \infty } ({{\boldsymbol{P}}_{\rm{L}}}(t) - {{\boldsymbol{P}}_i}(t) - {\boldsymbol{P}}_{_{i{\rm{L}}}}^*(t)) = 0 $

考虑机器人之间的通信带宽限制问题，只有少数跟随者才能获得领导者的信号。因此，建立 $ {R_i} $ 的有向时变一致性误差：

$ {{\boldsymbol{e}}_i}(t) = \sum\limits_{j = 1}^M {{a_{ij}}} \left( {{\boldsymbol{\varDelta }}_i (t) - {\boldsymbol{ \varDelta }}_j (t)} \right) - {b_i}\left( {{\boldsymbol{\varDelta }}_i (t) - {{\boldsymbol{P}}_{\rm{L}}}(t)} \right) $

(1)

${{\boldsymbol{e}}_i}(t)$ = ${\left[ {\begin{array}{*{20}{c}} {{e_{ix}}(t),}&{{e_{iy}}(t)} \end{array}} \right]^{\rm T}} , {\boldsymbol{ \varDelta }}_i (t) = {{\boldsymbol{P}}_i}(t) - {{\boldsymbol{P}}_{i{\rm{L}}}}(t) , {\boldsymbol{ \varDelta }}_j (t) = {{\boldsymbol{P}}_j}(t) - {{\boldsymbol{P}}_{j{\rm{L}}}}(t)$ ， ${a_{ij}} $ 和 ${b_i} $ 为两个相关权值。本研究的目标是需要实现： $\mathop {\lim }\limits_{t \to \infty } {\boldsymbol{e}}_{i}(t) \rightarrow {{\rm{0}}}$ 。

2 一致性控制编队避障控制律

在编队系统中，跟随者需要与其他跟随者及领导者保持相对固定的期望位置， $ {R_{\rm{L}}} $ 为领导者， $ {R_i} $ 为跟随者。领导者 $ {R_{\rm{L}}} $ 与跟随者 $ {R_i} $ 相对一致性控制的位置误差定义为： $ {{\boldsymbol{e}}_{i{\rm{L}}}}(t) = {{\boldsymbol{P}}_{\rm{L}}}(t) - {\boldsymbol{P}}(t) - {\boldsymbol{P}}_{_{i{\rm{L}}}}^*(t) $ 。为实现位置跟踪误差为零的多机器人系统时变编队，即构造一致性编队的控制协议为

$ {\boldsymbol{u}}_i^{\text{f}}(t) = {\lambda _1}\sum\limits_{j \in {N_i}} {{a_{ij}}({{\boldsymbol{e}}_{i{\rm{L}}}}(t) - {{\boldsymbol{e}}_{j{\rm{L}}}}(t))} - c{{\boldsymbol{e}}_i}(t) + {\dot{\boldsymbol P}}_{_{i{\rm{L}}}}^*(t) $

(2)

式中： $ {\lambda _1} $ 和 $ c $ 为待定参数。

在实际工作过程中，机器人的避障能力是十分必要的，不仅仅是避免对障碍物的碰撞，而且机器人相互之间也需要有良好的避障能力。对于基于距离的人工势场法的避障系统，当某一机器人进入另一机器人的势场范围内或两个及以上机器人的势场范围发生接触时，机器人之间就会迅速产生排斥力。但是如图1所示，即使其进入势场的范围内时，机器人之间也不会发生碰撞。此时机器人之间的碰撞不仅仅取决于机器人之间的距离，也与机器人之间的速度的大小和方向相关。根据文献[5]，可用基于距离和速度的避障方式来建立机器人躲避障碍的模型。

为避免碰撞，考虑了以下条件

$ \parallel {p_{ij}}\parallel \in ({r_{{\text{safe}}}} + r,{r_{{\text{out}}}}] $

$ \left|{\theta }_{ij}\right| < {\varphi }_{\text{ca}}或\left|{\theta }_{ji}\right| < {\varphi }_{\text{ca}} $

(3)

$ \left\| {{v_i}} \right\|\cos {\theta _{ij}} + \left\| {{v_j}} \right\|\cos {\theta _{ji}} > 0 $

式中： $ {p_{ij}} $ 表示机器人 $ i $ 与机器人 $ j $ 的距离， $ r $ 为机器人的半径， $ {r_{{\text{out}}}} $ 为势场半径， $ {r_{{\text{safe}}}} $ 为安全半径， ${\varphi _{{\text{ca}}}} $ 为半视锥角。

进而，机器人 $ i $ 避障排斥力为

$ {\boldsymbol{u}}_i^{{\text{cas}}} = - \sum\limits_{j \in N_i^{{\text{ca}}}} {{\nabla _{i{\rm{L}}}}{\varOmega _{{\text{ca}}}}(\left\| {{p_{ij}}} \right\|)} $

(4)

式中： $ {\nabla _{i{\rm{L}}}} $ 为沿 $ i{\rm{L}} $ 的梯度， $ N_i^{{\text{ca}}} $ 为满足条件(3)的机器人 $ i $ 的邻居集合。

其中，势场函数为

$ \textit{Ω}_{\mathrm{ca}}\left(\left\|p_{i j}\right\|\right)=\left\{\begin{array}{l} \displaystyle\int_{r_{\mathrm{out}}}^{\left\|p_{p_{i}}\right\|} \phi_{\mathrm{ca}}(s) \mathrm{d} s ,\text { 式(3)被满足 } \\ 0, \;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\; \text { 其他 } \end{array}\right.$

$ {\phi _{{\text{ca}}}} = - (1 - \exp ( - (\left\| {{v_i}} \right\|\cos {\theta _{ij}} + \left\| {{v_j}} \right\|\cos {\theta _{ji}}))) \times \frac{{{\zeta _{{\text{ca}}}}}}{{{{(\left\| {{p_{ij}}} \right\| - r - {r_{{\text{safe}}}})}^2}}}, $

$ {\zeta _{{\text{ca}}}} $ 为正常数，

综上所述，基于距离和速度的人工势场方法能使机器人之间相互避免碰撞。

为了使机器人对障碍物进行避障，采用人工势场法对障碍物进行避障^[21]。在 $ t $ 时刻时，当第 $ l $ 个障碍物出现在机器人 $ i $ 的安全范围以内时，设置障碍物 $ l $ 上离机器人最近的点为 $ {{\boldsymbol{P}}_l} = {[{x_l},{y_l}]^{\rm T}} $ 。采用人工势场法对障碍物进行避障。对于机器人 $ i $ 受障碍物 $ l $ 影响的势函数为

$ {F_l}(t) = ||{\boldsymbol{P}}_i^*(t) - {{\boldsymbol{P}}_l}||({\exp {((||{\boldsymbol{P}}_i^*(t) - {{\boldsymbol{P}}_l}|| - {d_{{\text{allow}}}}) - 2)}}) $

式中： $ {d_{{\text{allow}}}} $ 为允许距离， ${d_{{\text{allow}}}} \leqslant {d_{\rm{s}}} $ ； $ {d_{\rm{s}}} $ 为机器人与障碍物安全距离； $ n $ 为障碍数。

人工势场法控制律可表示为

$ {\boldsymbol{u}}_i^{{\rm{0bs}}}(t) = - \sum\limits_{l = 1}^n {{\beta _l}{\nabla _{P_i^*}}} {F_l}(t) $

式中： $ {\beta _l} $ 为避障权重系数。当机器人与障碍物的距离小于安全距离时，势场力 $ {F_l}(t) $ 开始向机器人施加。当机器人与障碍物的距离小于允许距离时， $ {F_l}(t) \to \infty $ 。

避障控制律为 $ {\boldsymbol{u}}_i^{{\text{ca}}}(t) = {\boldsymbol{u}}_i^{{\text{cas}}}(t) + {\boldsymbol{u}}_i^{0{\text{bs}}}(t) $

具有避碰特性的期望一致性控制编队 $ i $ 控制律为

$ {\boldsymbol{u}}_i^{\text{r}}(t) = {\boldsymbol{u}}_i^{\text{f}}(t) + {\boldsymbol{u}}_i^{{\text{ca}}}(t) $

(5)

所述的群体控制问题在之后小节中表示为一个闭环离散时间最优控制问题。为了解决此问题，这两个系统方程可以用正向欧拉离散化描述为

$ {\boldsymbol{P}}(k + 1) = {\boldsymbol{P}}(k) + T \times {\dot{\boldsymbol P}} $

(6)

式中： $ T $ 为采样时间，离散时间形式的期望路径可以表示为

$ {\boldsymbol{P}}_i^{\text{r}}(k + 1) = {\boldsymbol{P}}_i^{\text{r}}(k) + T \times {\boldsymbol{u}}_i^{\text{r}}(k) $

(7)

为了使机器人的运动更具平滑性，引入参考轨迹。定义虚拟机器人 $ i $ 的移动过程中的参考轨迹为 $ {{\boldsymbol{x}}_{i{\rm r}}}(k) = {\left[ {\begin{array}{*{20}{c}} {{x_{i{\rm r}}}(k),}&{{y_{i{\rm r}}}(k),}&{{\theta _{i{\rm r}}}(k)} \end{array}} \right]^{\rm T}} $ ，实际轨迹为 ${{\boldsymbol{x}}_i}(k) = {\left[ {\begin{array}{*{20}{c}} {{x_i}(k),}& {{y_i}(k),}& {{\theta _i}(k)} \end{array}} \right]^{\rm T}}$ 。在时间 $ t $ 时，参考状态可以被定义为

$ {x_{i{\rm{r}}}}(k) = {x_i}(k) + ({x_{i{\rm{r}}}}(k) - {x_i}(t))(1 - \exp ( - \gamma (k - {T_C}(k))T)) $

$ {y_{i{\rm{r}}}}(k) = {y_i}(k) + ({y_{i{\rm{r}}}}(k) - {y_i}(t))(1 - \exp ( - \gamma (k - {T_C}(k))T)) $

$ {\theta _{i{\rm{r}}}}(k) = \arctan \frac{{{y_{i{\rm{r}}}}(k) - {y_{i{\rm{r}}}}(k - 1)}}{{{x_{i{\rm{r}}}}(k) - {x_{i{\rm{r}}}}(k - 1)}} $

式中： $ \gamma $ 为正常数， $ {T_C}(k) $ 为一个关于时间序列 $ k $ 的函数，即

$ T_{C}(k)=\left\{\begin{array}{cc} 0, & \text { 当开始运动时 } \\ k_{1}, & \text { 当避免障碍物后 } \end{array}\right. $

$ {k_1} $ 代表机器人避免障碍物结束时的时间序列。

输入的线速度和角速度可以表示为

$ {v_{i{\rm{r}}}}(k) = \frac{{||{x_{i{\rm{r}}}}(k) - {x_{i{\rm{r}}}}(k - 1)|| + ||{y_{i{\rm{r}}}}(k) - {y_{i{\rm{r}}}}(k - 1)||}}{T} $

$ {w_{i{\rm{r}}}}(k) = \frac{{{\theta _{i{\rm{r}}}}(k) - {\theta _{i{\rm{r}}}}(k - 1)}}{T} $

3 移动机器人编队跟踪系统 3.1 编队跟踪误差模型

本文研究的对象是移动机器人，机器人 $ {R_i}(i = 1, \cdots ,M) $ 一阶运动学方程可由下列微分方程描述：

$ {{\dot{\boldsymbol x}}_i}(t) = \left[ {\begin{array}{*{20}{c}} {{{\dot x}_i}(t)} \\ {{{\dot y}_i}(t)} \\ {{{\dot \theta }_i}(t)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos ({\theta _i}(t))}&0 \\ {\sin ({\theta _i}(t))}&0 \\ 0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\mathop \nu \nolimits_i (t)} \\ {\mathop \omega \nolimits_i (t)} \end{array}} \right] = {\boldsymbol{J}}{{\boldsymbol{x}}_i} $

(8)

在二维全局坐标系下，机器人的位姿可由向量 $ {{\boldsymbol{x}}_i}(t) = {[{x_i}(t),{y_i}(t),{\theta _i}(t)]^{\rm T}} $ 表示。 $ {{\boldsymbol{P}}_i}(t) = {\left[ {{x_i}(t),{y_i}(t)} \right]^{\rm T}} $ 表示机器人的位置。 $ {\theta _i}(t) $ 为机器人绕 $ x $ 轴逆时针转动的角度。 $ \mathop \nu \nolimits_i (t) $ 与 $ \mathop \omega \nolimits_i (t) $ 代表机器人的线速度和角速度。 $ {\boldsymbol{J}} $ 为雅可比矩阵。

由此，便可得到机器人 $ {R_i}(i = 1, \cdots ,M) $ 的参考轨迹：

$ {{\dot{\boldsymbol x}}_{i{\rm{r}}}}(t) = \left[ {\begin{array}{*{20}{c}} {{{\dot x}_{i{\rm{r}}}}(t)} \\ {{{\dot y}_{i{\rm{r}}}}(t)} \\ {{{\dot \theta }_{i{\rm{r}}}}(t)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos ({\theta _{i{\rm{r}}}}(t))}&0 \\ {\sin ({\theta _{i{\rm{r}}}}(t))}&0 \\ 0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\mathop \nu \nolimits_{i{\rm{r}}} (t)} \\ {\mathop \omega \nolimits_{i{\rm{r}}} (t)} \end{array}} \right] = {\boldsymbol{J}}{{\boldsymbol{x}}_{i{\rm{r}}}} $

(9)

机器人进行轨迹跟踪的目的是为了沿着已设定的路径运动，路径可视为参考机器人的运动轨迹。由此，定义 ${{\boldsymbol{x}}_{i{\rm{e}}}}(t)$ 为实际机器人与参考机器人之间的误差向量。根据运动学模型，误差可定义为

$\begin{aligned} & {{\boldsymbol{x}}_{i{\rm{e}}}}(t) = \left[ {\begin{array}{*{20}{c}} {{x_{i{\rm{e}}}}(t)} \\ {{y_{i{\rm{e}}}}(t)} \\ {{\theta _{i{\rm{e}}}}(t)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos {\theta _i}(t)}&{\sin {\theta _i}(t)}&0 \\ { - \sin {\theta _i}(t)}&{\cos {\theta _i}(t)}&0 \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_{i{\rm{r}}}}(t) - {x_i}(t)} \\ {{y_{i{\rm{r}}}}(t) - {y_i}(t)} \\ {{\theta _{i{\rm{r}}}}(t) - {\theta _i}(t)} \end{array}} \right] \hfill \\& {} \end{aligned}$

(10)

机器人位姿误差微分方程为

$\begin{aligned}& {{\dot{\boldsymbol x}}_{i{\rm{e}}}}(t) = \left[ {\begin{array}{*{20}{c}} {{{\dot x}_{i{\rm{e}}}}(t)} \\ {{{\dot y}_{i{\rm{e}}}}(t)} \\ {{{\dot \theta }_{i{\rm{e}}}}(t)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\omega _i}(t){y_{i{\rm{e}}}}(t) + {v_{i{\rm{r}}}}(t)\cos {\theta _{i{\rm{e}}}}(t) - {v_i}(t)} \\ { - {\omega _i}(t){x_{i{\rm{e}}}}(t) + {v_{i{\rm{r}}}}(t)\sin {\theta _{i{\rm{e}}}}(t)} \\ {{\omega _{i{\rm{r}}}}(t) - {\omega _i}(t)} \end{array}} \right] \hfill \\& {} \end{aligned}$

(11)

为了更好地提高移动机器人的跟踪控制，参考文献[20]对式(11)中误差模型进行变换，定义一个与 $ {\theta _{ie}}(t) $ 相关的新的状态变量为

$ {\hat \theta _{i{\rm{e}}}}(t) = {\theta _{i{\rm{e}}}}(t) + \arctan \left( {\frac{{{k_0}{y_{i{\rm{e}}}}(t)}}{{\sqrt {1 + x_{i{\rm{e}}}^2(t) + y_{i{\rm{e}}}^2(t)} }}} \right) $

使

$ {\psi _{i1}}(t) = \sqrt {1 + x_{i{\rm{e}}}^2(t) + y_{i{\rm{e}}}^2(t)} , $

$ {\psi _{i2}}(t) = \sqrt {1 + x_{i{\rm{e}}}^2(t) + (1 + k_0^2)y_{i{\rm{e}}}^2(t)} $

$ {\psi _{i3}}(t) = \frac{{{k_0}{\nu _{i{\rm{r}}}}(t)\sin {\theta _{i{\rm{e}}}}(t)(1 + x_{i{\rm{e}}}^2(t)) - {k_0}{x_{i{\rm{e}}}}(t){y_{i{\rm{e}}}}(t){\nu _i}(t)\cos {\theta _{i{\rm{e}}}}(t)}}{{{\psi _{i1}}(t)\psi _{i2}^2(t)}} $

则新的跟踪误差状态向量为

$ {{\hat{\boldsymbol x}}_{i{\rm{e}}}}(t) = \left[ {{x_{i{\rm{e}}}}(t),{y_{i{\rm{e}}}}(t),{{\hat \theta }_{i{\rm{e}}}}(t)} \right] $

新的 $ {R_i} $ 跟踪误差运动学表示为

$ {{\hat{\boldsymbol x}}_{i{\rm{e}}}}( t ) = \left[ {\begin{array}{*{20}{c}} {{\omega _i}(t){y_{i{\rm{e}}}}(t) + \cos {\theta _{i{\rm{e}}}}(t){\nu _{i{\rm{r}}}}(t) - {\nu _i}(t)}\\ {\sin {\theta _{i{\rm{e}}}}(t){\nu _{i{\rm{r}}}}(t) - {\omega _i}(t){x_{i{\rm{e}}}}(t)}\\ {{\omega _{i{\rm{r}}}}(t) - {\omega _i}(t) \left( {\dfrac{{{k_0}{x_{i{\rm{e}}}}( t ){\psi _{i1}}( t )}}{{\psi _{i2}^2(t)}} + 1} \right) + {\nu _i}( t ) \dfrac{{{k_0}{y_{i{\rm{e}}}}(t){x_{i{\rm{e}}}}(t)}}{{{\psi _{i1}}(t)\psi _{i2}^2(t)}} + {\psi _{i3}}( t )} \end{array}} \right] $

(12)

式中： ${k_0}$ 为一个正常数。

于是，编队跟踪误差系统离散时间非线性仿射模型可表示为

$ {{\boldsymbol{x}}_{i{\rm{e}}}}(k + 1) = {\boldsymbol{f}}_i^{{\text{tr}}}(k) + {\boldsymbol{g}}_i^{{\text{tr}}}(k){{\boldsymbol{u}}_i}(k) $

(13)

$ {\boldsymbol{f}}_i^{{\rm{tr}}}(k) = \left[ {\begin{array}{*{20}{c}} {T\cos ({\theta _{i{\rm{e}}}}(k)){\nu _{i{\rm{r}}}}(k) + {x_{i{\rm{e}}}}(k)} \\ {T\sin ({\theta _{i{\rm{e}}}}(k)){\nu _{i{\rm{r}}}}(k) + {y_{i{\rm{e}}}}(k)} \\ {T{\omega _{i{\rm{r}}}}(k) + {\psi _{i3}}(k) + {{\hat \theta }_{i{\rm{e}}}}(k)} \end{array}} \right] $

$ {\boldsymbol{g}}_i^{tr}(k) = \left[ {\begin{array}{*{20}{l}} { - T}&{T{y_{i{\rm{e}}}}(k)} \\ 0&{T{x_{i{\rm{e}}}}(k)} \\ {T\dfrac{{{k_0}{x_{i{\rm{e}}}}(k){y_{i{\rm{e}}}}(k)}}{{{\psi _{i1}}(k)\psi _{i2}^2(k)}}}&{ - T \left(\dfrac{{{k_0}{x_{i{\rm{e}}}}(k){\psi _{i1}}(k)}}{{\psi _{i2}^2(k)}} + 1\right)} \end{array}} \right] $

$ {{\boldsymbol{u}}_i}(k) = {\left[ {{\nu _i}(k),{\omega _i}(k)} \right]^{\rm T}} $ 为离散输入信号向量。

3.2 模型预测控制策略

为了控制编队跟踪良好，采用MPC方法。在有限时间范围之类，经过欧拉离散化的系统，可转化为具有输入和状态饱和约束以及离散非线性模型方程的闭环离散时间最优控制问题。因此，将系统的控制输入和状态方法转化成离散时间状态空间方程。编队控制目标、避障实现和轨迹跟踪可以通过MPC方法转化为离散时间闭环优化控制问题。不失一般性，可得到广义非线性离散仿射系统：

$ {{\boldsymbol{x}}_i}(k) = {{\boldsymbol{F}}_i}(k) + {{\boldsymbol{G}}_i}(k){{\boldsymbol{u}}_i}(k),i \in V $

(14)

约束条件为

$ \Delta {{\boldsymbol{u}}_{\min }} \leqslant \Delta {{\boldsymbol{u}}_i}(k) \leqslant \Delta {{\boldsymbol{u}}_{\max }}$

$ {{\boldsymbol{u}}_{\min }} \leqslant {{\boldsymbol{u}}_i}(k) \leqslant {{\boldsymbol{u}}_{\max }} $

$ {{\boldsymbol{x}}_{\min }} \leqslant {{\boldsymbol{x}}_i}(k) \leqslant {{\boldsymbol{x}}_{\max }} $

其中， $ {{\boldsymbol{F}}_i}(k) = {\boldsymbol{f}}_i^{{\text{tr}}}(k) \in {R^{3 \times 1}} $ ， ${{\boldsymbol{G}}_i}(k) = {\boldsymbol{g}}_i^{{\text{tr}}}(k) \in {R^{3 \times 2}} $ ， $ {{\boldsymbol{u}}_i}(k) \in {R^2} $ 表示系统的控制输入向量， $ {{\boldsymbol{x}}_i}(k) ={\boldsymbol{x}}_{i{\rm{e}}}^{\rm T}(k) \in {R^{3 \times 1}} $ 表示系统的状态向量。

模型预测控制是一种基于模型的闭环优化控制策略，其算法的核心是:可预测未来的动态模型，在线反复优化计算并滚动实施的控制作用和模型误差的反馈校正。因此，对比于传统或经典的控制器，模型预测控制具有控制效果好、鲁棒性强等优点，可有效地克服过程的不确定性、非线性和并联性，并能方便地处理被控变量和操纵变量中的各种约束。因此，代价函数可表示为

$ J(k) = \sum\limits_{j = 1}^N {\left\| {{\boldsymbol{x}}(k + j|k)} \right\|_{\boldsymbol{Q}}^2} + \sum\limits_{j = 0}^{{N_u} - 1} {\left\| {\Delta {\boldsymbol{u}}(k + j|k)} \right\|_{\boldsymbol{R}}^2} $

(15)

式中： $ {N_u} $ 是控制时域， $ N $ 是预测时域，且满足 $ 0 \leqslant {N_u} \leqslant N $ 。 $ {\boldsymbol{R}} $ 和 $ {\boldsymbol{Q}} $ 为合适的权重矩阵； $ \left\| \cdot \right\| $ 表示对应向量的欧几里德范数。而且，定义 $ {a^j} $ 为向量 $ {\boldsymbol{a}} \in {R^n} $ 的第 $ j $ 个元素，输入向量和状态向量的容许集可以描述为

$ {\boldsymbol{\chi}}=\left\{{\boldsymbol{x}}^{j} \in R^{N}: \underline{{\boldsymbol{x}}}^{j} \leqslant {\boldsymbol{x}}^{j} \leqslant \bar{{\boldsymbol{x}}}^{j}, \forall j=1,2, \cdots, N\right\} $

$ \boldsymbol{U}=\left\{\boldsymbol{u} \in R^{N_{u}}: \underline{\boldsymbol{u}}^{j} \leqslant \boldsymbol{u}^{j} \leqslant \overline{\boldsymbol{u}}^{j}, \forall j=1,2, \cdots, N_{u}\right\} , $

$ \Delta \boldsymbol{U}=\left\{\Delta \boldsymbol{u} \in R^{N_{u}}: \Delta \underline{\boldsymbol{u}}^{j} \leqslant \Delta \boldsymbol{u}^{j} \leqslant \Delta \overline{\boldsymbol{u}}^{j}, \forall j=1,2, \cdots, N_{u}\right\} $

式中： $ \Delta {\boldsymbol{u}} $ 为输入增量， ${\underline a ^j}$ 和 $ {\overline a^j} $ 分别为 $ {a^j} $ 的上限和下限。

对于系统(14)，利用代价函数 $ J(k) $ 可以得到一个二次优化问题，且能高效可靠地得到其最优解。

定义向量:

$ {\bar{\boldsymbol x}} = {\left[ {{\boldsymbol{x}}(k + 1|k), \cdots ,{\boldsymbol{x}}(k + N|k)} \right]^{\text{T}}} \in {R^{3N}} $

$ {\bar{\boldsymbol u}} = {\left[ {{\boldsymbol{u}}(k|k), \cdots ,{\boldsymbol{u}}(k + {N_u} - 1|k)} \right]^{\text{T}}} \in {R^{2{N_u}}} $

$ \Delta {\bar{\boldsymbol u}} = {\left[ {\Delta {\boldsymbol{u}}(k|k), \cdots ,\Delta {\boldsymbol{u}}(k + {N_u} - 1|k)} \right]^{\text{T}}} \in {R^{2{N_u}}} $

其中， $ \Delta {\boldsymbol{u}}(k + j|k) $ 为输入的增量，即 $\Delta {\boldsymbol{u}}(k + j|k) = {\boldsymbol{u}}(k + j|k) - {\boldsymbol{u}}(k + j - 1|k)$ 代表未来预测范围状态。

系统的预测输出可表示为

$ {\bar{\boldsymbol x}}(k) = {{\boldsymbol{G}}_i}\Delta {\bar{\boldsymbol u}}(k) + {{\tilde{\boldsymbol F}}_i} + {{\tilde{\boldsymbol G}}_i} $

(16)

$ {{\boldsymbol{G}}_i} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k|k - 1}})}& \cdots &{\rm{0}} \\ {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k + 1|k - 1}})}& \cdots &{\rm{0}} \\ \vdots & & \vdots \\ {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k + N - 1|k - 1}})}& \cdots &{{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k + N - 1|k - 1}})} \end{array}} \right] , $

$ {{\tilde{\boldsymbol F}}_i} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{F}}_i}({{\boldsymbol{x}}_{k|k - 1}})} \\ {{{\boldsymbol{F}}_i}({{\boldsymbol{x}}_{k + 1|k - 1}})} \\ \vdots \\ {{{\boldsymbol{F}}_i}({{\boldsymbol{x}}_{k + N - 1|k - 1}})} \end{array}} \right] , {{\tilde{\boldsymbol G}}_i} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k|k - 1}}){\boldsymbol{u}}(k - 1)} \\ {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k + 1|k - 1}}){\boldsymbol{u}}(k - 1)} \\ \vdots \\ {{{\boldsymbol{G}}_i}({{\boldsymbol{x}}_{k + N - 1|k - 1}}){\boldsymbol{u}}(k - 1)} \end{array}} \right] $

$ {{\boldsymbol{G}}_i} \in {R^{3N \times 2{N_u}}} $ , $ {{\boldsymbol{F}}_i} $ 和 $ {{\tilde{\boldsymbol G}}_i} \in {R^{3N}} $ 。因此，在约束下基于代价函数(15)的最优化问题可以表示为

$ \min J(k) = \left\| {{{\boldsymbol{G}}_i}\Delta {\bar{\boldsymbol u}}(k) + {{{\tilde{\boldsymbol F}}}_i} + {{{\tilde{\boldsymbol G}}}_i}} \right\|_{\boldsymbol{Q}}^2 + \big\| {\Delta {\bar{\boldsymbol u}}(k)} \big\|_{\boldsymbol{R}}^2 $

(17)

约束为 $ \Delta {{\bar{\boldsymbol u}}_{\min }} \leqslant \Delta {\bar{\boldsymbol u}}(k) \leqslant \Delta {{\bar{\boldsymbol u}}_{\max }} $

$ {{\bar{\boldsymbol u}}_{\min }} \leqslant {\bar{\boldsymbol u}}(k - 1) + {\tilde{\boldsymbol I}}\Delta {\bar{\boldsymbol u}}(k) \leqslant \Delta {{\bar{\boldsymbol u}}_{\max }} $

(18)

$ {{\bar{\boldsymbol x}}_{\min }} \leqslant {{\tilde{\boldsymbol F}}_i} + {{\tilde{\boldsymbol G}}_i} + {{\boldsymbol{G}}_i}\Delta {\bar{\boldsymbol u}}(k) \leqslant {{\bar{\boldsymbol x}}_{\max }} $

式中： $\tilde{\boldsymbol{I}}=\left[\begin{array}{cccc}\boldsymbol{I} & 0 & \cdots & 0 \\\boldsymbol{I} & \boldsymbol{I} & \cdots & 0 \\\vdots & \vdots & & \vdots \\\boldsymbol{I} & \boldsymbol{I} & \cdots & \boldsymbol{I}\end{array}\right]\in {R^{2{N_u} \times 2{N_u}}}$

为了解决问题(17)，可以规划一个QP问题(19)来表示最优化：

$ \min \dfrac{1}{2}\Delta {{\bar{\boldsymbol u}}^{\text{T}}}{{\boldsymbol{W}}_i}\Delta {\bar{\boldsymbol u}} + {\boldsymbol{c}}_i^{\text{T}}\Delta {\bar{\boldsymbol u}} $

(19)

约束条件为 $ {\boldsymbol{l}} \leqslant {{\boldsymbol{E}}_i}\Delta {{\bar{\boldsymbol u}}^{\rm T}} \leqslant {\boldsymbol{f}} $

$ \Delta {{\bar{\boldsymbol u}}_{\min }} \leqslant \Delta {\bar{\boldsymbol u}}(k) \leqslant \Delta {{\bar{\boldsymbol u}}_{\max }} $

式中： ${{\boldsymbol{W}}_i} \;=\; 2({\boldsymbol{G}}_i^{\text{T}}{\boldsymbol{Q}}{{\boldsymbol{G}}_i} \;+\; {\boldsymbol{R}}) \in {R^{2{N_u} \times 2{N_u}}}$ ， ${{\boldsymbol{c}}_i}\; =\; 2{\boldsymbol{G}}_i^{\text{T}}{\boldsymbol{Q}}({{\tilde{\boldsymbol F}}_i} \;+\; {{\tilde{\boldsymbol G}}_i}) \in {R^{2{N_u}}}$ ，

$ {{\boldsymbol{E}}_i} = \left[ {\begin{array}{*{20}{c}} { - {\tilde{\boldsymbol {I}}}} \\ {{\tilde{\boldsymbol I}}} \\ { - {{\boldsymbol{G}}_i}} \\ {{{\boldsymbol{G}}_i}} \end{array}} \right] , {{\boldsymbol{b}}_i} = \left[ {\begin{array}{*{20}{c}} {{\bar{\boldsymbol u}}(k - 1) - {{{\bar{\boldsymbol u}}}_{\min }}} \\ {{\bar{\boldsymbol u}}(k - 1) + {{{\bar{\boldsymbol u}}}_{\max }}} \\ {{{{\tilde{\boldsymbol F}}}_i} + {{{\tilde{\boldsymbol G}}}_i} - {{{\bar{\boldsymbol x}}}_{i\min }}} \\ { - ({{{\tilde{\boldsymbol F}}}_i} + {{{\tilde{\boldsymbol G}}}_i} - {{{\bar{\boldsymbol x}}}_{i\max }})} \end{array}} \right] , $

${\boldsymbol{f}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{b}}_i}} \\ {{{{\bar{\boldsymbol u}}}_{\max }}} \end{array}} \right] ,{\boldsymbol{l}} = \left[ {\begin{array}{*{20}{c}} { - \infty } \\ {{{{\bar{\boldsymbol u}}}_{\min }}} \end{array}} \right] $

3.3 广义投影神经网络优化

为了在线模型预测控制中获得已规划的优化问题的最优解，需要寻求一种高效的最优化方法来求解。因此，一种广义投影神经网络优化的方法被提出。为了解决二次规划问题(19)，开发了一种单层结构一般投影神经网络(GPNN)，它是一种递归神经网络(Recurrent Neural Network, RNN)。广义投影神经网络是一类求解更广泛的单调变分不等式和相关的优化问题。本文所提出的神经网络具有低模型复杂度的单层结构，包含了现有的用于约束优化的神经网络，如原始对偶神经网络、对偶神经网络和投影神经网络，均为广义投影神经网络的特殊情况。广义投影神经网络的渐进神经网络的递归神经网络(RNN)用于解 MPC 控制中产生的二次规划问题，并表现出了较低的计算复杂度。通过GPNN方法，可得到具有全局指数收敛性和李雅普诺夫稳定性的二次规划问题最优解。因此，有以下定理^[20]：

定理1 　QP优化(19)的目标可等价得到向量 $ \Delta {\bar{\boldsymbol u}} \in {R^{2{N_u}}} $ 满足线性变分不等式问题，

$ {\boldsymbol{\rho }} + {\boldsymbol{\varphi }}{{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}} - {\boldsymbol{\beta }}) = {{\boldsymbol{H}}_\Lambda }({\boldsymbol{\rho }} - {{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}} - {\boldsymbol{\beta }})) + {\boldsymbol{\varphi }}{{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}} - {\boldsymbol{\beta }}) $

式中： $ {\boldsymbol{\beta }} = - {\boldsymbol{W}}_i^{ - 1}{{\boldsymbol{c}}_i} $ ， $ {\boldsymbol{\psi }} = - {\boldsymbol{W}}_i^{ - 1} $ ， $ {\boldsymbol{\rho }} = - {{\boldsymbol{E}}_i}{\boldsymbol{W}}_i^{ - 1}{{\boldsymbol{c}}_i} $ ， ${\boldsymbol{\varphi }} = {{\boldsymbol{E}}_i}{\boldsymbol{W}}_i^{ - 1}{\boldsymbol{E}}_i^{\rm T}$ 。

改进的GPNN动力学系统为

$ \begin{split} & \qquad \gamma \dfrac{{{\text{d}}\Delta {\bar{\boldsymbol u}}}}{{{\text{d}}t}}\; =\; {\boldsymbol{\lambda }}\;[{\boldsymbol{\varphi }}{{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}} \;- \;{\boldsymbol{\beta }}) -{{\boldsymbol{H}}_\Lambda }({\boldsymbol{\varphi }}{{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}}\; -\; {\boldsymbol{\beta }}) \;- \\& {{\boldsymbol{\psi }}^ + }(\Delta {\bar{\boldsymbol u}} - {\boldsymbol{\beta }}) + {\boldsymbol{\rho }}) + {\boldsymbol{\rho }}] \end{split} $

(20)

式中： $ {\boldsymbol{\lambda }} $ 为一个缩放矩阵， $ {\boldsymbol{\lambda }} = {\boldsymbol{\psi }} $ ； $ \gamma $ 为一个正常数， $ {{\boldsymbol{\psi }}^ + } $ 为 $ {\boldsymbol{\psi }} $ 的伪逆；投影算子的表达式为

$ H_{\Lambda}\left(\varepsilon_{i}\right)= \begin{cases}\boldsymbol{\varepsilon}^{-}, & \text {如果} \varepsilon_{i} < \boldsymbol{\varepsilon}^{-} \\ \varepsilon_{i}, & \text {如果} {\boldsymbol{\varepsilon}}^{-} \leqslant \varepsilon_{i} \leqslant {\boldsymbol{\varepsilon}}^{+}, \forall i \in R^{6 N_{u}+6 N} \\ \boldsymbol{\varepsilon}^{+}, & \text {如果} \varepsilon_{i} > \boldsymbol{\varepsilon}^{+}\end{cases} $

(21)

式中： $ {{\boldsymbol{\varepsilon }}^ - } = {\boldsymbol{l}} $ ， $ {{\boldsymbol{\varepsilon }}^ + } = {\boldsymbol{f}} $ 。

4 仿真

在本节中，利用Matlab对一组编队的非完整移动机器人进行轨迹跟踪和避障仿真，以验证本文提出的避障与轨迹跟踪的控制性能和有效性。本文以3个机器人和1个虚拟机器人为例进行数值计算。机器人被标记为 $ {R_1} \sim {R_3} $ ，虚拟领导者被标记为 $ {R_{\rm{L}}} $ 。

在本文中，图2为机器人的通信拓扑结构，在机器人运动过程中可选用两种通信拓扑。

根据图2两个通信拓扑图可得，图2(a)的邻接矩阵 $ {{\boldsymbol{A}}_1} = \left[ {\begin{array}{*{20}{c}} 0&0&0 \\ 1&0&0 \\ 0&1&0 \end{array}} \right] $ 。图2(b)的邻接矩阵 $ {{\boldsymbol{A}}_2} = \left[ {\begin{array}{*{20}{c}} 0&0&0 \\ 0&0&1 \\ 1&0&0 \end{array}} \right] $ 。权重矩阵均为 ${\boldsymbol{B}} = {\rm{diag}}\left(\left[ {\begin{array}{*{20}{c}} 1&0&0 \end{array}} \right]\right)$ 。

采样时间 $T = 0.1\;{\text{s}}$ ，时间为 $t \in [0\;{\text{s}},20\;{\text{s}}]$ ，避障的参数设置为： ${d_{{\text{allow}}}} = 0.2\;{\text{m}}$ ， ${d_{\text{s}}} = 0.3 \;{\text{m}}$ ， $r + {r_{{\text{safe}}}} = 0.25\;{\text{m}}$ ， ${r_{{\text{out}}}} = 0.35\;{\text{m}}$ ， $ {\zeta _{{\text{ca}}}} = 120 $ ， $d_{i{\rm{L}}}^* = 0.52\;{\text{m}}$ 。MPC的参数设置为： $ {N_u} = 2 $ ， $ N = 3 $ ， ${\boldsymbol{Q}} = 100\;{\boldsymbol{I}}$ ， ${\boldsymbol{R}} = 10\;{\boldsymbol{I}}$ 。 $\Delta {v_{\max }} = 2\;{\text{m}}/{\text{s}}$ ， $\Delta {\omega _{\max }} \;= \;6\;{\text{rad}}/{\text{s}}$ ， $- 2\;{\text{m}}/{\text{s}} \leqslant {v_i} \leqslant 2\;{\text{m}}/{\text{s}}$ ， $- 3\;{\text{rad}}/{\text{s}} \leqslant {\omega _i} \leqslant 3\;{\text{rad}}/{\text{s}}$ 。 $ \Delta {{\boldsymbol{u}}_{\max }} = {[\Delta {v_{\max }},\Delta {\omega _{\max }}, \cdots ,\Delta {v_{\max }},\Delta {\omega _{\max }}]^{\rm T}} \in {R^{2{N_u}}} $ 。跟随机器人 $ {R_i} $ 与领导者 $ {R_{\rm{L}}} $ 的期望位置为

$ \left[ {\begin{array}{*{20}{c}} {{P_{ix}}(k)} \\\\ {{P_{iy}}(k)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {0.3\cos \left(\dfrac{{2{\text{π}} (k - 1)}}{3} - \dfrac{{\text{π}} }{{30}}\right)} \\ {0.3\sin \left(\dfrac{{2{\text{π}} (k - 1)}}{3} - \dfrac{{\text{π}} }{{30}}\right)} \end{array}} \right] $

领导者的的期望线速度和角速度为 ${v_r}(k) = 0.2\;{\text{m}}/{\text{s}},{\omega _r}(k) = 0\;{\text{rad}}/{\text{s}}$ 。

在仿真过程中，领导者的初始位姿为

$ {{\boldsymbol{x}}_{\rm{L}}}(0) = {({x_{\rm{L}}}(0),{y_{\rm{L}}}(0),{\theta _{\rm{L}}}(0))^{\rm T}} = {(0.3\;{\text{m}},0.6\;{\text{m}},0\;{\text{rad}})^{\rm T}} $

跟随者 $ {R_1} \sim {R_3} $ 的初始位姿分别为

$ {{\boldsymbol{x}}_1}(0) = {({x_1}(0),{y_1}(0),{\theta _1}(0))^{\rm T}} = {(0.5\;{\text{m}},0.6\;{\text{m}},0\;{\text{rad}})^{\rm T}} $

$ {{\boldsymbol{x}}_2}(0) = {({x_2}(0),{y_2}(0),{\theta _2}(0))^{\rm T}} = {(0\;{\text{m}},0.3\;{\text{m}},0\;{\text{rad}})^{\rm T}} $

$ {{\boldsymbol{x}}_3}(0) = {({x_3}(0),{y_3}(0),{\theta _3}(0))^{\rm T}} = {(0\;{\text{m}},0.8\;{\text{m}},0\;{\text{rad}})^{\rm T}} $

实验1：领导者 ${R_{\rm{L}}}$ 的运动轨迹设为直线。

机器人的期望位姿为

$ \left[ {\begin{array}{*{20}{c}} {{x_{\rm{L}}}(k)} \\ {{y_{\rm{L}}}(k)} \\ {{\theta _{\rm{L}}}(k)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{x_{\rm{L}}}(k - 1) + 0.2\;T\cos {\theta _{\rm{L}}}(k - 1)} \\ {{y_{\rm{L}}}(k - 1)} \\ {{\theta _{\rm{L}}}(k - 1)} \end{array}} \right] $

实验1仿真结果如图3~图7所示。在图3中，跟随者可以实现并保持所期望的直线运动的规则三角形，跟随者的几何中心可以跟踪领导者的轨迹。并且跟随者即实际机器人可以实现良好的避障效果。与文献[5]相比，本文不仅实现了良好的避障而且可以进行编队控制与跟踪。在图4~图5中显示了机器人的线速度和角速度的变化，可以看到线速度和角速度受到约束。图6~图7中，通过使用的一致性控制方法，所有的一致性误差都可以稳定到原点。

实验2：领导者 $ {R_{\rm{L}}} $ 的运动轨迹设置为正弦曲线。

机器人的期望位姿为

$ \left[ {\begin{array}{*{20}{c}} {{x_{\rm{L}}}(k)} \\ {{y_{\rm{L}}}(k)} \\ {{\theta _{\rm{L}}}(k)} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{x_{\rm{L}}}(k - 1) + 0.2\;T} \\ {{y_{\rm{L}}}(k - 1) + 0.2\;T\sin (0.6\;kT)} \\ {{\text{a}} \tan 2(0.2\;cT\sin (0.6\;kT),0.2\;T)} \end{array}} \right] $

实验2仿真结果如图8~图12所示。在图8中，跟随者可以实现并保持所期望的正弦曲线的规则三角形运动，跟随者的几何中心可以跟踪领导者的轨迹。并且跟随者即实际机器人可以实现良好的避障效果。在图9~图10中显示了机器人的线速度和角速度的变化，表示线速度和角速度受到约束。图11~图12中，通过使用的一致性控制方法，所有的一致性误差都可以稳定到零或趋于零。综合上述仿真结果表明:采用所提出的控制策略，本文系统能够在充分考虑输入约束的前提下实现编队跟踪目标与避障处理。通过上述实例，验证了该控制方法系统的有效性。

5 结论

本文研究了在虚拟领导者下的具有一阶多移动机器人系统在基于位移与速度下的一致性编队跟踪与避障控制问题。首先设计多机器人的通信拓扑，再设计具有避障功能的一致性编队控制律，然后设计移动机器人编队跟踪系统，运用模型预测控制(MPC)方法，将编队误差运动问题按代价函数转化为最小优化问题。利用广义投影神经网络优化的方法求解最优值。本文给出的理论分析和仿真结果验证了该方法的有效性。