基于方位信息的轮式机器人动态编队控制

李艳，贺彬彬，李明辉，戴庆瑜; LI Yan; HE Binbin; LI Minghui; DAI Qingyu

网刊加载中。。。

使用Chrome浏览器效果最佳，继续浏览，你可能不会看到最佳的展示效果，

确定继续浏览么?

复制成功，请在其他浏览器进行阅读

基于方位信息的轮式机器人动态编队控制 PDF

- ORCID：
李艳 ¹
- ORCID：
贺彬彬 ¹
✉
- ORCID：
李明辉 ²
- ORCID：
戴庆瑜 ¹

1. 陕西科技大学，电气与控制工程学院，西安710021； 2. 陕西科技大学，机电工程学院，西安710021

中图分类号： TP242

最近更新：2025-04-25

DOI：10.11835/j.issn.1000-582X.2025.04.007

摘要

针对非完整约束的多机器人动态编队中，各机器人只能获取相邻机器人方位信息的情况，提出一种纯方位角信息的分布式PID（proportion integration derivative）编队控制算法。考虑领航机器人易受风向或路面平整度等扰动影响，带来无法保持编队队形的问题，算法通过引入跟随机器人的相对位置和相对速度反馈，有效消除稳态误差，抑制干扰的影响，改善系统的动态性能，保证系统的全局稳定性。然后，利用Routh-Hurwitz稳定性判据进行分析，验证编队系统的全局稳定性。最后，通过仿真实验比较所提控制律与基于纯比例和比例积分的控制律在收敛速度、抗干扰能力等方面的性能。仿真结果表明：所提控制律使领航者受到干扰后依然形成期望编队，实现对领航者轨迹的快速跟踪，总方位角误差的相对最大偏差下降了5.4%。

关键词

方位信息; 轮式机器人; 编队控制; PID控制算法

随着机器人被应用于各个领域，多智能体协同控制引起了广泛关注。多智能体的编队控制是协同控制领域的重要研究课题之一。根据感知和控制变量的类型，现有的编队控制方法大致分为3类^[

1⁃5]：基于相对位置^{[参考文献 6⁃8}6⁃8]、基于距离^{[参考文献 9⁃11}9⁃11]和基于方位^{[参考文献 12⁃17}12⁃17]。与其他2种编队方法相比，基于方位的编队方法因具有缩放便利、对智能体的感知能力要求更低等优点被广泛应用。文献[18]基于相对方位信息提出一种有限时间的圆形编队分布式控制律，有效减少了方位角误差。文献[19]提出一种基于局部方位测量和相对角度约束的分布式三角形编队控制算法，编队控制效果良好，但只适用于3个智能体的情况。为了进一步扩展智能体个数，Bishop又在文献[20]中，提出一种具有纯方位约束的四边形编队控制的分布式控制律，将智能体的个数扩展到4个及以上，提高编队控制的稳定性。

上述文献研究的控制律仅适用于解决静止编队场景，当应用于跟踪运动目标编队时，这些控制律将无法消除动态编队中产生的较大跟踪误差。因此，如何使用方位信息实现对运动目标跟踪的动态编队控制是当前研究的难点。目前的动态编队控制根据智能体的动力学模型可分为单积分器、双积分器、非完整动力学模型。轮式机器人就属于其中的非完整动力学模型。文献[

21]将基于方位的控制方法应用于非完整轮式机器人模型，提出纯方位编队控制律，解决该模型下编队控制的队形缩放问题。文献[22]提出动态方位编队跟踪控制律，利用局部参考坐标系中的方位信息实现期望的运动编队，解决局部坐标系下非完整动力学模型的引导跟踪、编队跟踪问题。文献[23]提出新的自适应编队控制律，考虑在领航者速度恒定情况下机器人之间的避碰问题，实现6个非完整约束机器人系统的纯方位编队跟踪控制。以上文献都默认在理想环境下，只考虑领航者速度不变的情况。但实际应用中，领航机器人速度必然会因受到不同类型的干扰，发生不规律改变，无法保持编队队形。因此，为了使跟随机器人能快速响应并形成指定目标队形，必须考虑提高动态编队控制算法的抗干扰能力，保证领航机器人受到扰动速度不规律变化时，编队能稳定形成和缩放。

为解决以上问题，文献[

24]利用无迹卡尔曼滤波算法对领航机器人系统的状态进行估计，设计了输入-输出状态反馈控制规律，提高对非线性系统和不确定的适应性。但该方法需要对纯角度观测信息进行无迹卡尔曼滤波，增加了计算复杂度和延迟。文献[25]考虑领航者速度时变的情况，在控制律中引入积分项实现对领航者速度变化的补偿，从而实现期望编队，该方法的系统稳定性和抗干扰能力有待改进。因此，针对多移动机器人动态编队中领航者速度受扰动随机改变的情况，提出了一种基于方位的抗扰能力强的PID（proportion integration derivative）编队控制算法。该算法利用方位信息作为控制输入，结合PID控制器的比例、积分和微分作用，有效抑制领航者速度扰动对编队系统的影响，提高了编队系统的稳定性和鲁棒性。

1 准备工作

1.1 移动机器人模型

如图1所示，假设车身的质心点与几何中心点重合，XOY为整体坐标系。其中： $θ_{i}$ 为移动机器人的偏航角； $v_{i}$ 和 $w_{i}$ 分别为移动机器人的线速度和角速度； $\dot{q} = {[\begin{matrix} \overset{•}{x} & \overset{•}{y} & \overset{•}{θ} \end{matrix}]}^{T}$ 表示移动机器人i的位姿向量； $p_{i} = {[\begin{matrix} x_{i} & y_{i} \end{matrix}]}^{T}$ 表示移动机器人的位置坐标。

图1 轮式机器人运动学模型

Fig. 1 Kinematics model of wheeled robot

移动机器人运动学模型如下

\dot{q} = [\begin{matrix} p_{i} \\ θ_{i} \end{matrix}] = [\begin{matrix} x_{i} \\ y_{i} \\ θ_{i} \end{matrix}] = [\begin{matrix} v_{i} c o s θ_{i} \\ v_{i} s i n θ_{i} \\ w_{i} \end{matrix}]

。

（1）

1.2 图论相关知识

在多智能体系统中，每个智能体可看作一个节点，多智能体编队中n个智能体之间通信关系可以用一个有向图 $G = \{V, E, A\}$ $表示$ ，图2为示意图。其中：顶点集 $V = \{1,2, 3,4, 5\}$ ，代表5个智能体； $E \subseteq V \times V$ 为边集，边的集合为 $E = \{(1,2), (2,3), (3,1), (4,2), (4,3), (5,1), (5,4)\} = \{e_{1}, e_{2}, e_{3}, e_{4}, e_{5}, e_{6}, e_{7}\}$ ； $A = [a_{i j}] \in R^{n * n}$ 是有向图 $G$ 的邻接矩阵。在有向图 $G$ 中，如果从节点 $ν_{j}$ 到节点 $ν_{i}$ 存在一条有向边，则代表智能体j可以将信息传给智能体i，同时用 $(j, i) \in E$ 表示。当 $(j, i) \in E$ 时， $a_{i j}$ 是一个正权值，取值为1；当 $(j, i) \notin E$ 时， $a_{i j} = 0$ ，一般来说，认为智能体不会和自身通信，即 $a_{i i} = 0$ 。

图2 有向图

Fig. 2 Directed graph

智能体i智能体j之间的方位信息定义为

g_{i j} ≜ \frac{p_{j} - p_{i}}{‖p_{j} - p_{i}‖} = \frac{e_{i j}}{‖e_{i j}‖}

，

（2）

其中， $e_{i j} = p_{j} - p_{i}$ 。 $p_{i}$ 和 $p_{j}$ 分别表示智能体i和j的位置坐标，方位的定义可以当作是相对位置信息的一个单位化。在具体实现中， $g_{i j}$ 不需要计算，可直接通过一些外部设备（如光学摄像头等）获得。在基于方位的编队控制中，主要利用智能体之间的相对方位信息实现编队形成和保持。只有2个方位信息向量彼此平行时，他们的方位才相同。因此，平行矢量的概念是判断智能体是否形成理想编队的重要依据。正交投影矩阵是一种用来判断2个向量是否平行的数学工具，它表示将任意向量投影到另一个向量的正交轴上，如果投影后的长度为零，则说明这2个向量是平行的。

1.3 正交投影矩阵

正交投影矩阵的定义为

P (x) ≜ I_{d} - \frac{x}{‖x‖} \frac{x^{T}}{‖x‖}

，

（3）

其中：令 $P (x) = P_{x}$ ； $I_{d} \in R^{d \times d}$ 为一个单位矩阵； $P_{x}$ 表示将任意向量投影到 $x$ 的正交轴上。它满足以下性质：①该矩阵是半正定的；② $P_{x}^{T} = P_{x}$ , $P_{x}^{2} = P_{x}$ 。如图2~3所示，向量 $P_{x} y$ 是指 $y$ 在 $x$ 的正交补上的正交投影。任意2个非零向量 $x$ ， $y$ ，如果 $P_{x} y = 0$ 或 $P_{y} x = 0$ 成立，则向量 $x$ 与 $y$ 平行。

图3 $P_{x} y = 0$

Fig. 3 $P_{x} y = 0$

将这个正交投影矩阵转换成具有方位信息的形式

P_{g_{i j}} ≜ I_{d} - \frac{e_{i j}}{‖e_{i j}‖} \frac{e_{i j}^{T}}{‖e_{i j}‖} = I_{d} - g_{i j} g_{i j}^{T}

，

（4）

显而易见， $P_{g_{i j}}$ 是半正定的，且对任意与 $g_{i j}$ 平行的向量 $x$ 都有 $P_{g_{i j}} x = 0$ 。

1.4 基于方位编队的唯一性

与传统基于位置的编队控制方法不同，基于方位的编队控制方法由于缺少距离信息，在同样的领导者位置与方位约束条件下，有不同的编队队形，如图4中（a）（b）所示。当只采用方位信息对期望队形进行描述时，对于同样的期望队形，不同的方位约束条件的选择会对队形的唯一性产生不同影响。因此，为了保证编队的唯一性，需要确保所构建的目标队形框架为无穷小方位刚性的。方位刚性矩阵被定义为方位函数的雅可比矩阵。为满足所构建目标队形的框架为无穷小方位刚性的，通过计算方位刚性矩阵的秩来检测，求方位刚性矩阵的秩首先要对方位函数求偏导，再求出雅可比矩阵的秩。因此，对于具有多节点的多智能体编队，工作量较大也不够直观。

图4 目标队形框架

Fig. 4 Target formation frame

研究采用以下判断条件：只要一个队形框架的边数m和顶点数n满足 $m \geq$ 2n-3关系，该框架是无穷小距离刚性的^[

25]，由于在二维空间中，无穷小距离刚性图等价于无穷小方位刚性图。因此，借助以上判断条件，可以简洁直观地构建出唯一的目标队形。如图4所示，图4中（a）（b）不满足无穷小方位刚性条件，故几何形状不唯一；图4中（c）（d）所示的框架，满足无穷小方位刚性条件，因此，几何形状唯一，只是平移和缩放的条件不同。

2 基于方位信息的移动机器人编队控制算法

2.1 控制目标

研究将多智能体中双积分器模型的基于方位编队控制方法应用于非完整约束多机器人模型编队控制中。对满足方位约束 $\{g_{i j}^{*}\}$ 的编队 $G (p)$ ，根据相对位置测量值 $(p_{i} - p_{j})$ 设计跟随机器人的控制律 $\{v_{i}, w_{i}\}$ ，使实际的方位收敛到期望值，多机器人形成目标队形且以期望速度整体移动，完成多机器人系统编队的形成、保持和跟踪。

2.2 编队控制算法设计

为实现期望的编队机动控制，指定了时变速度领导者。假设领导者速度恒定方向变化， $p_{i} = {[\begin{matrix} x_{i} & y_{i} \end{matrix}]}^{T}$ 表示领航机器人在平面中的位置。动态模型如下

\{\begin{matrix} {\dot{p}}_{i} = h_{i} v_{i}^{*} ， \\ \dot{θ} = w_{i}^{*}, \end{matrix}

（5）

其中： $h_{i} ≜ [\begin{matrix} c o s θ_{i} \\ s i n θ_{i} \end{matrix}]$ ， $h_{i}^{⊥} ≜ [\begin{matrix} c o s θ_{i} \\ s i n θ_{i} \end{matrix}]$ 则 ${\dot{h}}_{i} = w_{i} h_{i}^{⊥}$ ； $v_{i}^{*}$ 和 $ω_{i}^{*}$ 分别是第i个领航者的线速度和角速度。研究对跟随者提出一种基于正交投影矩阵的PID控制算法，具体方程如下：

\{\begin{array}{l} v_{i} (t) = - k_{P} h_{i}^{⊥} \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (t) - p_{j} (t)) - k_{I} h_{i}^{T} \int_{0}^{t} \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (s) - p_{j} (s)) d s - k_{D} h_{i}^{T} \sum_{j \in N_{i}} P_{g_{i j}^{*}} ({\dot{p}}_{i} (t) - {\dot{p}}_{j} (t)) ， \\ w_{i} = k_{w} {(h_{i}^{^})}^{T} \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (t) - p_{j} (t)) ， \end{array}

（6）

其中： $k_{p}$ ， $k_{I}$ ， $k_{D}$ ， $k_{w}$ 都为正实数； $P_{g_{i j}^{*}} = I_{d} - g_{i j}^{*} (g_{i j}^{*})^{T}$ 。

研究采用领导者——第一跟随者结构的编队方式，通过跟随机器人与其邻居间相对位置测量 $(p_{i} - p_{j})$ ，对跟随机器人提出一种基于正交投影矩阵的PID控制算法。其中，正交投影矩阵 $P_{g_{i j}^{*}} (p_{i} - p_{j})$ 是控制算法的重要组成部分，用来约束跟随者间的方位。 $g_{i j}^{*}$ 代表跟随者i和跟随者j之间的期望方位角， $(p_{i} - p_{j})$ 代表跟随者i和跟随者j之间的实际方位角，可由光学摄像头获得。如图5所示， $X_{i} = P_{g_{i j}} (p_{i} - p_{j})$ 被定义为第i个机器人的期望运动矢量。 $X_{i}$ 在 $h_{i}$ ， $h_{i}^{⊥}$ 这2个方向的正交投影大小，为机器人的线速度和角速度。Zhao和Zelazo^[

21]以及谭瑶等^{[参考文献 26

百度学术}26]也使用了类似的几何控制设计方法。这种方位角控制律具有方向性，且可通过调整航向矢量参数实现控制。相较于其他方位控制律，它不仅能解决基于方位的编队控制问题，还可以用于避障等其他任务。

图5 式中控制律的几何意义

Fig. 5 The geometric meaning of the law of control in the formula

$v_{i}$ 和 $ω_{i}$ 分别是第i个跟随者的线速度和角速度。当 $v_{i}^{*} = 0$ ， $ω_{i}^{*} = 0$ ，动态编队控制问题则转化为静态编队控制问题。利用比例部分可以形成期望编队，考虑到一般领导者的速度是时变的，加入积分部分消除稳态误差。最后，实际应用中会出现各种动态干扰，为了提高系统的动态性能，加入微分环节，与其他编队控制方法相比，提出的控制律采用分布式控制具有更好的稳定性，易于实现。

接下来，为便于分析该控制律的稳定性，定义

{\dot{ε}}_{i} (t) = \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (t) - p_{j} (t))

，

（7）

将式（6）（7）代入机器人模型式（1）可得

\{\begin{array}{l} {\dot{p}}_{i} (t) = - k_{P} h_{i} h_{i}^{T} \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (t) - p_{j} (t)) - k_{I} h_{i} h_{i}^{T} ε_{i} (t) - k_{D} h_{i} h_{i}^{T} \sum_{j \in N_{i}} P_{g_{i j}^{*}} ({\dot{p}}_{i} (t) - {\dot{p}}_{j} (t)) ， \\ {\overset{•}{θ}}_{i} (t) = - k_{w} {(h_{i}^{^})}^{T} \sum_{j \in N_{i}} P_{g_{i j}^{*}} (p_{i} (t) - p_{j} (t)) 。 \end{array}

（8）

为便于收敛性分析，需推导式（8）控制律的矩阵表达式。引入方位拉普拉斯矩阵，刻画基于方位控制的编队中机器人间的拓扑结构。定义方位拉普拉斯 Laplacian矩阵如下

{[B (G^{'} (p^{*}))]}_{i j} = \{\begin{array}{l} 0_{d \times d} ， & i \neq j, (i, j) \notin E ， \\ - P_{g_{i j}^{*}} ， & i \neq j, (i, j) \in E ， \\ \sum_{k \in N_{i}} P_{g_{i j}^{*}} ， & i = j, i \in V ， \end{array}

（9）

即 $B = B (G^{'} (p^{*}))$ ，根据领航者和跟随者位置，将方位拉普拉斯矩阵 $B (G^{'} (p^{*}))$ 进行如下分解

B (G' (p^{*})) = [\begin{matrix} B_{l l} & B_{l f} \\ B_{f l} & B_{f f} \end{matrix}]

，

（10）

其中， $B_{l l} \in R^{d 2 \times d 2}$ ， $B_{l f} \in R^{d 2 \times d n_{_{f}}}$ ， $B_{f l} \in R^{d n_{_{f}} \times d 2^{}}$ ， $B_{f f} \in R^{d n_{f} \times d n_{f}}$ ，式（8）可写为

\{\begin{array}{l} {\dot{p}}_{f} (t) = - k_{P} h_{i} h_{i}^{T} (B_{f f} p_{f} (t) + B_{f l} p_{l} (t)) - k_{I} h_{i} h_{i}^{T} ε (t) - k_{D} h_{i} h_{i}^{T} (B_{f f} v_{f} (t) + B_{f l} v_{l} (t)) ， \\ \overset{•}{ε} (t) = B_{f f} p_{f} (t) + B_{f l} p_{l} (t) ， \end{array}

（11）

其中， $ε (t) = {[\begin{matrix} ε_{n_{l} + 1} {(t)}^{T} & L & ε_{n} {(t)}^{T} \end{matrix}]}^{T}$ 。

2.3 收敛性分析

定义跟随者跟踪误差为

δ_{p} (t) = p_{f} (t) - p_{f}^{*} (t)

，

（12）

其中： $p_{f} (t)$ 为跟随机器人真实的位置； $p_{f}^{*} (t)$ 为跟随机器人随时间变化的预期位置。收敛性分析的目的是证明 $δ_{p} (t)$ 收敛于零。 $p_{f}^{*} (t)$ ，对式两边求导

{\dot{δ}}_{p} (t) = {\dot{p}}_{f} (t) - {\dot{p}}_{f}^{*} (t)

。

（13）

定理1：任何满足方位约束 $\{g_{i j}^{*}\}$ 的编队 $G (p)$ ，满足 $B_{p} = 0$ 和 $B_{f f} p_{f}^{*} (t) + B_{f l} p_{l} (t) = 0$ 。证明：

B_{p} = [\begin{matrix} B_{l l} & B_{l f} \\ B_{f l} & B_{f f} \end{matrix}] [\begin{matrix} p_{l} \\ p_{f} \end{matrix}] = [\begin{matrix} ⋮ \\ \sum_{j \in N_{i}}^{} P_{g_{i j}^{*}} (p_{i} - p_{j}) \\ ⋮ \end{matrix}]

，

（14）

根据上式，如果 $G (p)$ 满足方位约束 $g_{i j}^{*} = \frac{p_{i} - p_{j}}{‖p_{i} - p_{j}‖}$ ，则 $B_{p} = 0$ 。通过 $B_{p}$ 的分块矩阵表达式，看出 $B_{p} = 0$ 意味着 $B_{f f} p_{f}^{*} (t) + B_{f l} p_{l} (t) = 0$ 。

定理2：如果理想编队 $G (p^{*} (t))$ 是方位刚性的，则矩阵 $B_{f f}$ 是正定的。

由于目标编队 $p^{*} (t)$ 满足方位约束，从定理1可以得出 $B_{f f} p_{f}^{*} (t) + B_{f l} p_{l} (t) = 0$ 。此外，由定理2可知 $B_{f f}$ 是由定理2正定的，有

p_{f}^{*} (t) = - B_{f f}^{- 1} B_{f l} p_{l} (t)

，

（15）

对上式求导得

{\dot{p}}_{f}^{*} (t) = - B_{f f}^{- 1} B_{f l} v_{l} (t) h_{i}

，

（16）

则

δ_{p} (t) = p_{f} (t) + B_{f f}^{- 1} B_{f l} p_{l} (t)

，

（17）

{\overset{•}{δ}}_{p} (t) = {\dot{p}}_{f} (t) + B_{f f}^{- 1} B_{f l} v_{l} (t) h_{i}

，

（18）

将式（12）（13）（15）（16）代入式（11）得

\{\begin{array}{l} {\overset{•}{δ}}_{p} (t) = h_{i} h_{i}^{T} (- k_{P} B_{f f} δ_{p} (t) - k_{I} ε (t) - k_{D} {\overset{•}{δ}}_{p} (t)) + B_{f f}^{- 1} B_{f l} v_{l} (t) h_{i} ， \\ \overset{•}{ε} (t) = B_{f f} δ_{p} (t) ， \end{array}

（19）

化简得

{\overset{•}{δ}}_{p} (t) = - \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} B_{f f} δ_{p} (t) + - \frac{k_{I} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} ε (t) + B_{f f}^{- 1} B_{f l} v_{l} (t) h_{i}

，

（20）

将上式改为矩阵形式

[\begin{matrix} {\overset{•}{δ}}_{p} (t) \\ \overset{•}{ε} (t) \end{matrix}] = \underset{A}{\underset{︸}{[\begin{matrix} - \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} B_{f f} & - \frac{k_{I} h_{i} {h_{i}}^{T}}{1 + k_{D} h_{i} {h_{i}}^{T}} \\ B_{f f} & 0 \end{matrix}]}} [\begin{matrix} δ_{p} (t) \\ ε (t) \end{matrix}] + [\begin{matrix} B_{f f}^{- 1} B_{f l} h_{i} \\ 0 \end{matrix}] v_{l} (t) 。

（21）

定理3：当 $k_{P}$ ， $k_{I}$ ， $k_{D}$ 都大于零时，式（21）中的状态矩阵 $A$ 是Hurwitz。

证明：

d e t (λ I - A) = d e t ([\begin{matrix} λ I + \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} B_{f f} & \frac{k_{I} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} \\ - B_{f f} & λ I \end{matrix}]) = d e t (λ^{2} I + \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} λ B_{f f} + \frac{k_{I} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} B_{f f})

（22）

令 $d e t (λ I - A) = 0$ ，则

d e t (λ^{2} I + \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} λ B_{f f} + \frac{k_{I} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} B_{f f}) = 0

，

（23）

假定 $μ$ 为矩阵 $B_{f f}$ 的特征值，由定理2可知：设计的主从编队具有唯一性，相应方位拉普拉斯矩阵的子矩阵 $B_{f f}$ 的特征值 $μ > 0$ 。对应的跟随机器人特征值特征多项式表示为

d e t (λ^{2} I + \frac{k_{P} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} λ μ + \frac{k_{I} h_{i} h_{i}^{T}}{1 + k_{D} h_{i} h_{i}^{T}} μ) = 0

，

（24）

根据 Routh-Hurwitz 稳定性判据可知：为了主从编队的跟随者的位置、速度、加速度都能全局和指数收敛到理想值，即保证智能间的相对方位收敛到设定理想方位，对应特征方程式（24）中特征值 $λ$ 的实部必须小于0，由于 $k_{P}$ ， $k_{I}$ ， $k_{D}$ 均大于零，则易得 $R e (λ) < 0$ 。

定理4：当式（21）中的状态矩阵 $A$ 是Hurwitz，则系统是稳定的。

根据跟随机器人的状态矩阵特征值 $R e (λ) < 0$ ，结合跟随机器人误差输出方程，得到当领航者的位置给定以及其速度大小为恒定值时，设计的跟随机器人控制律，能够使得主从编队中所有跟随机器人位置误差 $δ_{p} (t)$ 速度误差 ${\dot{δ}}_{p} (t)$ 全局收敛到零。

假设领航机器人在t时刻的理想位置为 $p_{l}^{*} (t) = p_{l} (t)$ ，此时所有跟随机器人位置误差 $δ_{p} (t)$ 全局收敛到零，即 $\{p_{f} (t) \to p_{f}^{*} (t) = - B_{f f}^{- 1} B_{f l} p_{l} (t)\} \to 0$ ；则每个跟随机器人与其相邻机器人之间方位全都收敛到理想方位约束，即在所设计的控制律下多机器人系统在方位约束下可以形成理想目标编队。

3 仿真

为了验证所提控制律的有效性，使用MATLAB软件对多机器人动态编队控制问题进行模拟仿真研究。研究采用4个轮式移动机器人所组成的系统，所有轮式移动机器人的动力学均为相同的非完整约束动力模型。

3.1 无干扰的目标编队控制

首先选取期望队形，根据编队唯一性设置合适的方位约束与领航者。本次仿真实验分别采用长方形和菱形的期望队形进行验证，编队期望队形的平面示意图如图6所示。其中，机器人 $p_{1}$ 是第1领航者，机器人 $p_{2}$ 是第2领航者，机器人 $p_{3}$ 和 $p_{4}$ 均为编队中的跟随者。根据编队的信息拓扑结构：领航机器人 $p_{1}$ 和 $p_{2}$ 之间是单向信息交互，跟随机器人 $p_{3}$ 和 $p_{4}$ 与领航机器人 $p_{1}$ 和 $p_{2}$ 也是单向信息交互。而跟随机器人 $p_{3}$ 和 $p_{4}$ 之间的信息交互是双向。

图6 期望队形

Fig. 6 Expect formation

1）长方形编队

设定各移动机器人的初始位置为

p_{1} = {[\begin{matrix} 0 & - 5 \end{matrix}]}^{T}

，

p_{2} = {[\begin{matrix} 0 & 5 \end{matrix}]}^{T}

，

p_{3} = {[\begin{matrix} - 8 & 7 \end{matrix}]}^{T}

，

p_{4} = {[\begin{matrix} - 9 & - 13 \end{matrix}]}^{T}

。

长方形编队中，如图7机器人间理想方位约束如下

g_{31}^{*} = {[\begin{matrix} - 0.7071 & - 0.7071 \end{matrix}]}^{T}

，

g_{32}^{*} = {[\begin{matrix} 0.7071 & - 0.7071 \end{matrix}]}^{T}

，

g_{34}^{*} = {[\begin{matrix} 0.7071 & 0.7071 \end{matrix}]}^{T}

，

g_{41}^{*} = {[\begin{matrix} - 0.7071 & 0.7071 \end{matrix}]}^{T}

，

g_{43}^{*} = {[\begin{matrix} 0.7071 & 0.7071 \end{matrix}]}^{T} 。

图7 长方形目标队形保持和队形变换

Fig. 7 Rectangular target formation hold and formation transformation

2）菱形编队

如图8所示菱形目标编队，机器人理想方位约束如下

g_{31}^{*} = {[\begin{matrix} - 0.7071 & 0.7071 \end{matrix}]}^{T}

，

g_{32}^{*} = {[\begin{matrix} 0.7071 & 0.7071 \end{matrix}]}^{T}

，

g_{34}^{*} = {[\begin{matrix} 0.7071 & - 0.7071 \end{matrix}]}^{T}

，

g_{41}^{*} = {[\begin{matrix} - 0.7071 & - 0.7071 \end{matrix}]}^{T},

g_{43}^{*} = {[\begin{matrix} - 0.7071 & 0.7071 \end{matrix}]}^{T} 。

图8 菱形目标队形保持和队形变换

Fig. 8 Diamond target formation retention and formation transformation

本次仿真主要采用基于Ziegler-Nlichols方法进行PID的参数整定。Ziegler-Nichols方法是一种经验性的PID控制器整定方法，相较于传统的辨识法整定方法，它的优势是不需要知道系统的数学模型，只需要观察系统的输出变化。并且可以适用于比例、比例-积分、比例-积分-微分等多种类型的控制器。

在此次PID整定中，首先只调节 $k_{P}$ ，从一个较小的值开始逐渐增大，直到 $k_{P} = 10$ 时，系统响应速度和稳态误差达到满意的程度。然后固定 $k_{P}$ ，分别依次调节 $T_{I}$ ， $T_{D}$ ，直至系统到达最佳状态。经过多次调整PID控制器参数，最后得到无干扰编队情况下Z-N方法整定后的PID参数值为： $k_{P} = 10$ ， $k_{I} = 2$ ， $k_{D} = 0.15$ 。

如图7和图8所示，初始位置不满足期望队形。实验首先令各机器人到达初始期望位置点后，跟随机器人在所提出的控制律式（6）中，利用方位信息完成编队的形成与跟踪任务，对2个领航机器人则采用方位信息进行跟踪控制。此外，在10 s加入放缩信号，验证整个编队通过操纵2个领导者实现队伍平移和缩放，同时保持所需的编队队形，这表明所提出的动态编队控制解决方案可用于避免碰撞或通过狭窄通道。

不同的初始位置会影响编队控制的难度和效果。因此，为了保证编队控制算法的可靠性和安全性，笔者选择了3种不同的初始位置，考虑领航者轨迹不同的情况。如图9和图10所示，尽管3次初始位置不同，但并不影响不同编队队形的形成，且形成过程中无碰撞情况，仿真证明了编队算法可靠性和安全性。但是当跟随者初始位置与期望编队位置相距过远时，形成编队的时间会更长。

图9 不同初始位置下的长方形编队控制

Fig. 9 Rectangular formation control in different initial positions

图10 不同初始位置下菱形编队控制

Fig. 10 Control of diamond-shaped formations in different initial positions

3）不同编队控制方法的对比实验

为进一步验证所提出编队控制方法的可靠性，在形成长方形编队过程中，设计如下2组对比仿真实验：

A1：领航者速度不变情况下，采用PI控制的方位角误差；

A2：领航者速度不变情况下，采用PD控制的方位角误差；

A3：领航者速度不变情况下，采用PID控制的方位角误差；

B1：领航者速度随时间变化情况下，采用PI 控制的方位角误差；

B2：领航者速度随时间变化情况下，采用PD 控制的方位角误差；

B3：领航者速度随时间变化情况下，采用PID 控制的方位角误差。

图11和图12中在领航者速度保持不变和随时间变化2种情况下，对比了PI控制算法、PD控制算法和PID控制算法的编队总方位误差 $\sum_{(i, j) \in ε} ‖g_{i j} - g_{i j}^{*}‖$ 。从图11中看出，在领航者速度不变时，加入微分环节，尤其是A组第3 s时PI控制算法的相对方位角误差为35%，PID控制算法的方位角误差为30%，下降5%。从图12中看出，在领航者速度随时间变化时，较于PD算法，PID控制的响应曲线没有明显的尖峰和波动，更平滑。同时控制更快地收敛，没有明显的延迟和震荡。而在图7和图9轨迹图中，紫色线为PID算法，黄色线为PI算法的。由图也可看出，采用PID的控制方法比以往只用PI控制器的跟随者轨迹更平滑。

图11 A组对比实验

Fig. 11 Group A comparative experiment

图12 B组对比实验

Fig. 12 Group B comparative experiment

3.2 加入干扰的编队控制

在实际情况中，领航者易受其他动态障碍物、风向或路面平整度等扰动。因此，在领航者速度规律变化情况下，10~15 s加入干扰，设计如下2组对比仿真实验：

C1：加入随机干扰，采用PI控制的方位角误差；

C2：加入随机干扰，采用PID控制的方位角误差；

D1：加入干扰h₁=0.2cos t的情况下，采用PI控制的方位角误差

D2：加入干扰h₁=0.2cos t的情况下，采用PID控制的方位角误差。

采用长方形编队跟踪进行验证，图13展示了领航机器人速度受到随机干扰时的队形保持与变换，图14~15展示了C，D 2组实验在编队过程中总方位角误差变化情况。对图14与图15中的数据的稳态过程进行定量对比分析，选取第10~40 s的数据，分别对图中各量求取最大偏差和均方根误差，计算结果如表1所示。对比图14与图15，发现在受到未知扰动，系统产生振荡时，设计的非线性控制律可以在更短的时间内用更合理的控制量进行校正。对比表1,可以发现2种环境下，采用PID控制律的方位角偏差都有所下降，加入随机干扰情况下最为明显，相对最大偏差降低了5.4%。这说明在所设计控制律的作用下，多机器人系统形成的编队更趋近于期望编队，且波动更小，较为稳定。

图13 加入随机干扰的队形保持、变换

Fig. 13 Adds random interference to the formation holding and transformation

图14 C组对比实验

Fig. 14 Group C comparative experiment

图15 D组对比实验

Fig. 15 Group D comparative experiment

表1 定量对比表

Table 1 Quantitative comparison table

条件	变量	PI		PID
条件	变量	最大偏差	均方根误差	最大偏差	均方根误差
理想情况	领航者速度不变	0.185 29	0.120 36	0.183 10	0.118 59
理想情况	领航者速度随时间规律变化	0.248 76	0.167 40	0.248 15	0.165 90
干扰	加入随机干扰	0.298 78	0.194 70	0.282 73	0.193 79
情况	加入干扰h₁=0.2cos t	0.776 35	0.429 60	0.761 96	0.429 38

选取第10~40 s的数据，分别对图中各量求取最大偏差和均方根误差

4 结束语

笔者主要研究了多机器人的动态编队控制，提出一种适用于二维非完整多移动机器人系统的分布式纯方位PID编队控制律，通过Routh-Hurwitz稳定性判据证明了所提方法的稳定性。仿真和实验结果验证了所设计方法的有效性。基于方位的方法因其易于实现队形缩放在机器人避障中发挥着重要作用，在未来的研究中，计划进一步深入探讨多移动机器人的避障问题。

参考文献

刘银萍, 杨宜民.多机器人编队控制的研究综述[J].控制工程, 2010,17(S3): 182-186. [百度学术]

Liu Y P, Yang Y M. Formation control of multi-robot research[J]. Control Engineering of China, 2010, 17(S3): 182-186. (in Chinese) [百度学术]

王帅,周乐来李贻斌, 等.多移动机器人编队领航跟随方法研究进展[J].无人系统技术, 2019,2(5): 1-8. [百度学术]

Wang S, Zhou L L, Li Y B, et al. Advances in the leader-following method of multi-mobile robots formation[J]. Unmanned Systems Technology, 2019, 2(5): 1-8. (in Chinese) [百度学术]

Yang Z Q,Pan X F,Zhang Q,et al.Formation control of first-order multi-agents with region constraint[J]. Automatika, 2020, 61(4): 651-656. [百度学术]

Oh K K,Park M C,Ahn H S.A survey of multi-agent formation control[J].Automatica,2015,53:424-440. [百度学术]

Zhao S Y,Zelazo D.Bearing rigidity theory and its applications for control and estimation of network systems: life beyond distance rigidity[J]. IEEE Control Systems Magazine, 2019, 39(2): 66-83. [百度学术]

郭鹏军,张睿,高关根,等.基于相对速度和位置辅助的无人机编队协同导航[J].上海交通大学学报,2022,56(11): 1438-1446. [百度学术]

Guo P J, Zhang R, Gao G G, et al. Cooperative navigation of UAV formation based on relative velocity and position assistance[J]. Journal of Shanghai Jiaotong University, 2022,56(11): 1438-1446. (in Chinese) [百度学术]

郑重,刘帅,钱默抒,等.航天器编队系统相对位置自适应分布式控制[J].中国惯性技术学报, 2019,27(1): 129-135. [百度学术]

Zheng Z, Liu S, Qian M S, et al. Adaptive distributed control for relative position of spacecraft formation system[J]. Journal of Chinese Inertial Technology, 2019, 27(1): 129-135. (in Chinese) [百度学术]

Ogawa T, Sakurama K, Nakatani S,et al.Relative position estimation for formation control with the fusion of predicted future information and measurement data[J].SICE Journal of Control, Measurement, and System Integration, 2020, 13(5): 225-232. [百度学术]

刘树光,王欢,刘荣华.基于距离的无人机编队路径跟踪控制[J].飞行力学,2023,41(1): 27-33. [百度学术]

Liu S G, Wang H, Liu R H. Distance-based control for UAVs formation path tracking[J]. Flight Dynamics, 2023, 41(1): 27-33. (in Chinese) [百度学术]

Chen Z, Jiang C, Guo Y. Distance-based formation control of a three-robot system[C]//2019 Chinese Control and Decision Conference (CCDC). Nanchang, China: IEEE, 2019: 5501-5507. [百度学术]

孟蕾.基于距离约束的单领航者多智能体编队控制[J].电光与控制, 2021,28(7): 48. [百度学术]

Meng L.Multi-agent leader-follower formation control based on distance constraints[J].Electronics Optics & Control, 2021,28(7): 48.(in Chinese) [百度学术]

叶结松, 龚柏春,李爽,等.基于相对方位信息和单间距测量的多智能体编队协同控制[J].航空学报,2021,42(7): 324610. [百度学术]

Ye J S, Gong B C, Li S, et al.Multi-agent formation cooperative control using relative bearing information and single-spacing measurement[J].Acta Aeronautica et Astronautica Sinica, 2021,42(7): 324610.(in Chinese) [百度学术]

李宪珞.基于方位信息的非完整性多智能体编队控制[D].秦皇岛:燕山大学,2020. [百度学术]

Li X L.Formation control of nonholonomic multi-agent systems with bearing-only information[D].Qinhuangdao: Yanshan University, 2020. (in Chinese) [百度学术]

Han Z M,Guo K X,Xie L H,et al. Integrated relative localization and leader-follower formation control[J].IEEE Transactions on Automatic Control, 2019, 64(1): 20-34. [百度学术]

Choi Y H, Kim D. Distance-based formation control with goal assignment for global asymptotic stability of multi-robot systems[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 2020-2027. [百度学术]

Dong W J,Farrell J A. Cooperative control of multiple nonholonomic mobile agents[J].IEEE Transactions on Automatic Control, 2008, 53(6): 1434-1448. [百度学术]

李刚,周尚波,郭尚志等.通信延迟下自适应容错控制及其在航天器编队中的应用[J].重庆大学学报,2024,47(4): 104-113. [百度学术]

Li G, Zhou S B, Guo S Z, et al. Adaptive fault-tolerant control with communication delays and its application in spacecraft formation[J]. Journal of Chongqing University, 2024, 47(4): 104-113. (in Chinese) [百度学术]

Zhao S Y, Lin F, Peng K M, et al. Distributed control of angle-constrained cyclic formations using bearing-only measurements[J]. Systems & Control Letters, 2014, 63: 12-24. [百度学术]

Basiri M, Bishop A N, Jensfelt P. Distributed control of triangular formations with angle-only constraints[J]. Systems & Control Letters, 2010, 59(2): 147-154. [百度学术]

Bishop A N. Distributed bearing-only quadrilateral formation control[J].IFAC Proceedings Volumes, 2011, 44(1): 4507-4512. [百度学术]

Zhao S Y,Li Z H, Ding Z T. Bearing-only formation tracking control of multiagent systems[J].IEEE Transactions on Automatic Control, 2019, 64(11): 4541-4554. [百度学术]

Zhao J N, Yu X, Li X W,et al.Bearing-only formation tracking control of multi-agent systems with local reference frames and constant-velocity leaders[J]. IEEE Control Systems Letters, 2021, 5(1): 1-6. [百度学术]

Van Tran Q, Kim J.Bearing-constrained formation tracking control of nonholonomic agents without inter-agent communication[J]. IEEE Control Systems Letters, 2022, 6: 2401-2406. [百度学术]

韩青. 基于纯角度观测信息的多机器人编队控制方法研究[D]. 西安: 西北工业大学, 2018. [百度学术]

Han Q. Bearing-only observations multi-robot formation control[D]. Xi’an: Northwestern Polytechnical University, 2018. (in Chinese) [百度学术]

Li X L, Luo X Y, Wang J G, et al. Distributed bearing-based formation control of networked thrust-propelled vehicles[J]. Journal of the Franklin Institute, 2019, 356(9): 4907-4927. [百度学术]

谭瑶, 梅杰. 利用方位角信息的移动机器人编队控制[J]. 控制理论与应用, 2021,38(7): 1043-1050. [百度学术]

Tan Y, Mei J.Formation control of mobile robots using bearing-only measurements[J].Control Theory & Applications, 2021, 38(7): 1043-1050. (in Chinese) [百度学术]

期刊社主页

编辑部首页

期刊介绍

编委会

数据库收录

过刊浏览

联系我们