自主飞行机器人的蜂拥算法

本是古典何须时尚 2022-02-03 03:23 262阅读 0赞

### 文章目录 ###

*  2. Realistic model of a flying robot
 *  3. Self-propelled flocking model
 *   *  3.1. Short-range repulsion
     *  3.2. Velocity alignment of neighbours
 *  3.3. Boundaries and shill agents
 *  4. Collective target tracking
 *  5. Results and discussion
 *   *  5.1. Simulation of the flocking algorithm
     *  5.2. Simulation of the target tracking algorithm

--------------------

文章来自：《Flocking algorithm for autonomous flying robots》  
引用格式：Virágh, Csaba, Vásárhelyi, Gábor, Tarcai N , et al. Flocking algorithm for autonomous flying robots\[J\]. Bioinspiration & Biomimetics, 2014, 9(2):025012.

--------------------

# 2. Realistic model of a flying robot #

在本节中，我们提出了飞行机器人的模型，该模型被广泛的应用在许多真实的机器人系统中。 在这样的系统中，机器人的运动由低级算法控制，例如， 基于速度的PID控制器。 该低级控制算法通常将实际机器人的期望速度作为一个输入变量。 在群飞行期间，第 i i i飞行器寄予时间的期望速度可以由其他飞行器的位置 x i x\_i xi和速度 v i v\_i vi来定义：

v i d ( t ) = f i ( \{ x j ( t ) \} j = 1 N , \{ v j ( t ) \} j = 1 N ) v\_\{i\}^\{d\}(t)=\{ \{f\}\_\{i\}\}(\\\{ \{ \{x\}\_\{j\}\}(t)\\\}\_\{j=1\}^\{N\},\\\{ \{ \{v\}\_\{j\}\}(t)\\\}\_\{j=1\}^\{N\}) vid(t)=fi(\{ xj(t)\}j=1N,\{ vj(t)\}j=1N)

*Note： f i ( ) f\_i() fi()表示控制函数，即通过其他飞行器的位置和速度来改变第i个飞行器的位置。将一系列用来动态改变 t t t时刻第 i i i个飞行器速度的算法，统一用函数 f i ( ) f\_i() fi()表示。*

其中 N N N是集群总飞行器的数量， f i f\_i fi函数包含控制动态的任意特征。 在理想的情况下，第 i i i个机器人的速度可以在 t t t时刻迅速的变为 v i d ( t ) v\_\{i\}^\{d\}(t) vid(t)。然而，机器人系统永远不是理想的，在建模时应该考虑它的一些实际的缺陷：

*  **惯性**：机器人不能迅速的改变他们的姿态或者速度。通常情况下，渴望的速度是低级控制器的输入。我们假设在最优设置中，系统可以以指数收敛达到期望的速度，具有特征时间 τ C T R L \\tau\_\{ CTRL\} τCTRL。PID控制算法就是满足此行为的简单控制器算法。同时，加速度的大小也限制在最大值 a m a x a\_\{max\} amax。
 *  **内部噪声**：我们必须考虑提供相对位置和速度信息的传感器的不准确性。 例如，GPS设备测量的位置和速度的不确定性可以建模为随机函数 η i s \{\\eta^s\_i\} ηis。 该函数可以用标准偏差 σ s \\sigma\_s σs表征。 注意，“内部噪声”可以指实际机器人系统中使用的任何类型的传感器的不准确性。
 *  **传感器刷新频率**：传感器的输入的刷新率从根本上定义了机器人的反应时间和敏捷性。 我们考虑传感器的刷新率有限：每个单元更新传感器数据的频率为 t s − 1 t^\{-1\}\_s ts−1。 在我们当前的模型中， t s − 1 t^\{-1\}\_s ts−1是固定值。
 *  **通信局限**：机器人之间的通信具有有限范围 r c r\_c rc，因此如果两个机器人之间的距离大于 r c r\_c rc，则它们不能相互交互。 换句话说：只有满足 ∣ x j − x i ∣ < r c |x\_j-x\_i|<r\_c ∣xj−xi∣<rc时，函数 f i f\_i fi才依赖于参数 x j x\_j xj。
 *  **时间延迟**：当一个机器人接收或者处理来自其他机器人的位置、速度数据时，由于数据处理和传播会造成一些时延。最简单的解决方法是可以将时时间延迟定义为一个固定值 t d e l t\_\{del\} tdel。
 *  **常见噪音**：具有标准偏差 σ \\sigma σ的delta-correlated外部噪声项 η i ( t ) \\eta\_i(t) ηi(t)被加到单元的加速度上，该项是不可预测的环境影响的模型，例如低级控制算法中的风补偿的波动。

考虑上述问题，我们使用集合 \{ τ C T R L , a m a x , r c , t d e l , t s , \{ η j ( t ) , η j s ( t ) \} j = 1 N \} \\\{ \{\\tau\}\_\{CTRL\},a\_\{max\},r\_c,t\_\{del\},t\_s,\\\{ \{ \{\\eta\}\_\{j\}\}(t),\{ \{\\eta\}^s\_\{j\}\}(t)\\\}\_\{j=1\}^\{N\}\\\} \{ τCTRL,amax,rc,tdel,ts,\{ ηj(t),ηjs(t)\}j=1N\}表示真实的系统。时间延迟和通信范围难以测量并且可随机变化并，对稳定性的影响很大。 因此，所有的函数 f i f\_i fi都考虑 t d e l t\_\{del\} tdel和 r c r\_c rc。

综上，每个飞行器最终的模型是定义每个飞行器加速度的等式：  
 a i ( t ) = η i ( t ) + f i ( … ) − v i ( … ) − v i s ( … ) ∣ f i ( … ) − v i ( … ) − v i s ( … ) ∣ ⋅ min ⁡ \{ f i ( … ) − v i ( … ) − v i s ( … ) τ C T R L , a max ⁡ \} \{ \{a\}\_\{i\}\}(t)=\{ \{\\eta \}\_\{i\}\}(t)+\\frac\{ \{ \{f\}\_\{i\}\}(\\ldots )-\{ \{v\}\_\{i\}\}(\\ldots )-v\_\{i\}^\{s\}(\\ldots )\}\{|\{ \{f\}\_\{i\}\}(\\ldots )-\{ \{v\}\_\{i\}\}(\\ldots )-v\_\{i\}^\{s\}(\\ldots )|\}\\cdot \\min \\\{\\frac\{ \{ \{f\}\_\{i\}\}(\\ldots )-\{ \{v\}\_\{i\}\}(\\ldots )-v\_\{i\}^\{s\}(\\ldots )\}\{ \{ \{\\tau \}\_\{CTRL\}\}\},\{ \{a\}\_\{\\max \}\}\\\} ai(t)=ηi(t)\+∣fi(…)−vi(…)−vis(…)∣fi(…)−vi(…)−vis(…)⋅min\{ τCTRLfi(…)−vi(…)−vis(…),amax\}

f i ( … ) = f i ( \{ x j ( t − t d e l ) + x j s ( t − t d e l ) \} j ≠ 1 , x i ( t ) + x i s ( t ) , \{ v j ( t − t d e l ) + v j s ( t − t d e l ) \} j ≠ 1 , v i ( t ) + v i s ( t ) ) \{ \{f\}\_\{i\}\}(\\ldots )\\text\{=\}\{ \{f\}\_\{i\}\}(\{ \{\\\{ \{ \{x\}\_\{j\}\}(t-\{ \{t\}\_\{del\}\})+x\_\{j\}^\{s\}(t-\{ \{t\}\_\{del\}\})\\\}\}\_\{j\\ne 1\}\},\{ \{x\}\_\{i\}\}(t)+x\_\{i\}^\{s\}(t),\{ \{\\\{ \{ \{v\}\_\{j\}\}(t-\{ \{t\}\_\{del\}\})+v\_\{j\}^\{s\}(t-\{ \{t\}\_\{del\}\})\\\}\}\_\{j\\ne 1\}\},\{ \{v\}\_\{i\}\}(t)+v\_\{i\}^\{s\}(t)) fi(…)=fi(\{ xj(t−tdel)\+xjs(t−tdel)\}j̸=1,xi(t)\+xis(t),\{ vj(t−tdel)\+vjs(t−tdel)\}j̸=1,vi(t)\+vis(t))

其中 η i ( t ) \\eta\_i(t) ηi(t)是**常见噪声**， \{ … \} i ≠ j \{ \{\\\{\\ldots \\\}\}\_\{i \\ne j\}\} \{ …\}i̸=j表示迭代器的集合 i ≠ j i \\ne j i̸=j

# 3. Self-propelled flocking model #

在本节中，我们提出了一种最小算法，它能够驱动集体机器人系统走向稳定，无碰撞，相关的f蜂拥状态。 该算法基于动物群的早期模型\[2\] \[3\]。 我们将Agents定义为具有优选速度 V f l o c k V\_\{flock\} Vflock的自推进粒子：

v i S P P = v f l o c k v i ∣ v i ∣ v\_\{i\}^\{SPP\}=\{ \{v\}\_\{flock\}\}\\frac\{ \{ \{v\}\_\{i\}\}\}\{|\{ \{v\}\_\{i\}\}|\} viSPP=vflock∣vi∣vi

## 3.1. Short-range repulsion ##

为了避免碰撞，我们定义了一个局部线性排斥速度：  
 v i j r e p = D ( ∣ d i j ∣ − r s ) ∣ d i j ∣ d i j θ ( r s − ∣ d i j ∣ ) v\_\{ij\}^\{rep\}=\\frac\{D(|\{ \{d\}\_\{ij\}\}|-\{ \{r\}\_\{s\}\})\}\{|\{ \{d\}\_\{ij\}\}|\}\{ \{d\}\_\{ij\}\}\\theta (\{ \{r\}\_\{s\}\}-|\{ \{d\}\_\{ij\}\}|) vijrep=∣dij∣D(∣dij∣−rs)dijθ(rs−∣dij∣)  
其中 d i j = x j − x i d\_\{ij\}=x\_j-x\_i dij=xj−xi， D D D是排斥力的力度， r s r\_s rs相邻飞行器之间的平衡距离。我们认为由内部噪声引起的测量位置的波动幅度可以与 r s r\_s rs在相同的范围内。 在这种噪声系统中，简单的线性排斥优于高阶项，因为测量位置的误差不会导致输出中的突然变化或奇点。 如果机器人能够更准确地测量它们的位置，可以使用更高阶项，如Lennard-Jones势能\[13\]。

## 3.2. Velocity alignment of neighbours ##

现实控制算法中任何一种速度对齐项都应满足三个假设：它应该  
i）减小彼此接近的单位的速度差;  
ii）是局部的  
iii）即使当飞行器之间的距离接近于零（具有排斥项）时也具有上阈值。  
鉴于这些，我们实现了具有类似粘性摩擦的相互作用项的对齐规则，类似于\[14\]和\[15\]：  
 v i j f r i c t = C f r i c t v j − v i ( m a x \{ r min ⁡ , ∣ d i j ∣ \} ) 2 v\_\{ij\}^\{frict\}=\{ \{C\}\_\{frict\}\\frac\{ \{ \{v\}\_\{j\}\}-\{ \{v\}\_\{i\}\}\}\{ \{ \{(max\\\{ \{ \{r\}\_\{\\min \}\},|\{ \{d\}\_\{ij\}\}|\}\\\})\}^\{2\}\}\} vijfrict=Cfrict(max\{ rmin,∣dij∣\})2vj−vi  
其中 C f r i c t C\_\{frict\} Cfrict是对齐的强度， r m i n r\_\{min\} rmin定义了一个阈值，以避免除以近距离。

这一项是考虑到粒子/机器人对准其运动方向的趋势的特定实用选择。 在某种意义上，它是粘性摩擦项的离散对应物，它在粒子/机器人运动的过程中持续存在。

在上述公式中，如果 ∣ C f r i c t ∣ ≪ v f l o c k ∣ d i j ∣ 2 / ( ∣ v j − v i ∣ ) |C\_\{frict\}| \\ll v\_\{flock\}|d\_\{ij\}|^2/(|v\_j-v\_i|) ∣Cfrict∣≪vflock∣dij∣2/(∣vj−vi∣)，则 v i j f r i c t v^\{frict\}\_\{ij\} vijfrict的大小相对于 ∣ v i S P P ∣ |v^\{SPP\}\_\{i\}| ∣viSPP∣而言是微不足道的(即)。速度 v i v\_i vi和 v j v\_j vj的大小是有限制的，因此通过选取最佳 v f l o c k v\_\{flock\} vflock和 C f r i c k C\_\{frick\} Cfrick来保证局部粘性摩擦项的效果。

# 3.3. Boundaries and shill agents #

蜂拥行为的一个重要原则是使用某种全局位置约束来帮助群体实现的完整性。 在模拟中，位置约束的这个特征可以通过使用周期性边界条件地替代。 这是检查系统的大规模统计特性的有效方法。 在实际实验中，可以通过将飞行器约束在低纬度空间中来模仿周期性边界条件，例如， 进入一个环形的舞台\[17\] \[18\]，但在三维场景中这些限制根本不实用。

为了通过模拟研究蜂拥模型，我们将单元放置在具有排斥壁的方形的场地中（后面称为竞技场）。 我们将墙的排斥定义为虚拟的“shill”\[19\]Agents。 如果这些飞行器在墙外，那些shill agent会尝试协调这些飞行器，将其速度对准竞技场的中心，使其逐渐进入竞技场中心：

v i f o r m = C f r o m ⋅ s ( ∣ x a − x i ∣ , R ~ ( x i , x a , R ) , d ) ( v f l o c k x a − x i ∣ x a − x i ∣ − v i ) v\_\{i\}^\{form\}=\{ \{C\}\_\{from\}\}\\cdot s(|\{ \{x\}\_\{a\}\}-\{ \{x\}\_\{i\}\}|,\\tilde\{R\}(\{ \{x\}\_\{i\}\},\{ \{x\}\_\{a\}\},R),d)\\left( \{ \{v\}\_\{flock\}\}\\frac\{ \{ \{x\}\_\{a\}\}-\{ \{x\}\_\{i\}\}\}\{\\left| \{ \{x\}\_\{a\}\}-\{ \{x\}\_\{i\}\} \\right|\}-\{ \{v\}\_\{i\}\} \\right) viform=Cfrom⋅s(∣xa−xi∣,R~(xi,xa,R),d)(vflock∣xa−xi∣xa−xi−vi)

C s h i l l C\_\{shill\} Cshill是墙壁排斥力的强度， x a x\_a xa是竞技场中心的坐标。定位 s ( x , R , d ) s(x,R,d) s(x,R,d)为S形曲线，它可以平滑地降低竞技场内的排斥强度：  
 s ( x , R , d ) = \{ 0 x ∈ \[ 0 , R \] sin ⁡ ( π d ( x − R ) − π 2 ) + 1 x ∈ ( R , R + d \] 1 x > R + d s(x,R,d)=\\left\\\{ \\begin\{matrix\} 0 & x\\in \[0,R\] \\\\ \\sin \\left( \\frac\{\\pi \}\{d\}(x-R)-\\frac\{\\pi \}\{2\} \\right)+1 & x\\in (R,R+d\] \\\\ 1 & x>R+d \\\\ \\end\{matrix\} \\right. s(x,R,d)=⎩⎨⎧0sin(dπ(x−R)−2π)\+11x∈\[0,R\]x∈(R,R\+d\]x>R\+d

R ~ \\widetilde\{R\} R是关于竞技场形状的函数（在本文中指的是一个边长为 R R R的方形场地）。

注意，竞技场的墙壁在模拟中是全局预定义的，但是排斥项仅取决于相对坐标 x i − x a x\_i-x\_a xi−xa，因此在真实的机器人系统中，竞技场可以在本地被感知，与感知相邻飞行器的方式相同。

上面定义的三个术语的总和是蜂拥行为的最小先决条件。  
 v i d = v i S P P + ∑ j ≠ i ( v i j r e p + v i j a l i g ) θ ( r c − ∣ d i j ∣ ) v\_\{i\}^\{d\}=v\_\{i\}^\{SPP\}+\\sum\\limits\_\{j\\ne i\}\{(v\_\{ij\}^\{rep\}+v\_\{ij\}^\{alig\})\}\\theta (\{ \{r\}\_\{c\}\}-|\{ \{d\}\_\{ij\}\}|) vid=viSPP\+j̸=i∑(vijrep\+vijalig)θ(rc−∣dij∣)

--------------------

# 4. Collective target tracking #

在本节中，我们论证交互项 v i j r e p v\_\{ij\}^\{rep\} vijrep和$v\_\{ij\}^\{frict\}可以包含在其他任务特定的控制算法中。 我们使用先验定义的固定目标点创建了集体目标跟踪算法。 该算法允许单元在两个稳定状态之间执行平滑过渡：蜂拥状态（飞行器远离目标时）和集体悬停状态（飞行器在目标周围时）。 在目标点附近的这种过渡期间，速度的优选幅度必须平滑地接近零，并且应该保持群体的一致性和稳健性而没有干扰或振荡的迹象。

想象一个群体朝着目标位置（质量中心,meta-agent）移动，速度为 v − 0 v-0 v−0。 每个单元必须完成两个任务而不会发生冲突：  
i）接近这个meta-agent足够接近以实现蜂拥  
ii）与meta-agent平行移动以到达集体运动的目标。

根据我们的定义，机器人之间的通信是局部的， 因此计算全局质心在物理上是不可能的。 然而，机器人可以根据其通信范围内的可用信息（在半径为 r c r\_c rc的球形环境中）计算局部质心（CoM）。 因此，朝向目标点的吸引力定义为：

![在这里插入图片描述][20190509160120387.png]  
其中 v 0 v\_0 v0是优选速度的大小， x t r g x^\{trg\} xtrg是目标的位置， x i C o M x^\{CoM\}\_i xiCoM是从第i个飞行器的角度看的局部质心的位置， r t r g r\_\{trg\} rtrg是目标区域的半径，  r C o M r\_\{CoM\} rCoM是球形meta-agent的半径。 s ( x , R , d ) s(x,R,d) s(x,R,d)是前一节中定义的S形函数。注意，（4）中定义的粘性摩擦项的局部性取决于该算法中的 v − 0 v-0 v−0和 C f r i c t C\_\{frict\} Cfrict的值。

目标跟踪项的大小饱和于 v 0 v\_0 v0：  
![在这里插入图片描述][20190509161306778.png]  
算法计算出的最终所需速度为：  
![在这里插入图片描述][20190509161327576.png]

--------------------

# 5. Results and discussion #

在本节中，我们提出了真实的模拟和机器人实验结果。

## 5.1. Simulation of the flocking algorithm ##

首先，我们证明了即使在通信中存在大的延迟以及存在内部和外部噪声的情况下，也可能出现典型的蜂拥模式。 可以用顺序参数表示蜂拥状态的稳定性  
![在这里插入图片描述][20190509162456167.png]  
其中 v i ( t ) ⋅ v j ( t ) v\_i(t)\\cdot v\_j(t) vi(t)⋅vj(t)是两个速度矢量的标量积。在理想的蜂拥状态下， Ψ s c a l ≈ 1 \\Psi\_\{scal\}\\approx1 Ψscal≈1，在无序（不稳定）状态， Ψ s c a l ≈ 0 \\Psi\_\{scal\}\\approx0 Ψscal≈0。 根据图1，在较低的C值下，不能观察到具有高标度ψ的相关植绒行为。 较高的 C f r i c t C\_\{frict\} Cfrict保证即使存在噪声和大的时间延迟，出现的蜂拥状态也是稳定的。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]

## 5.2. Simulation of the target tracking algorithm ##

本小节的目的是表明通过我们选择的植绒算法中使用的交互项，可以保证目标跟踪算法的稳定性。 为了研究稳定性，我们分析了系统的两种可能的准稳态：植绒状态（大速度，远离目标位置）和悬停状态（零速度，靠近目标位置）。 请注意，我们的目标是显示交互项对稳定性的影响，因此其他参数 ( v 0 , r C o M , R t r g , d ) (v\_0,r\_\{CoM\},R\_\{trg\},d) (v0,rCoM,Rtrg,d)设置为固定的默认值。 优化参数选择以保证目标跟踪任务的稳定完成，在理想情况下在植绒和悬停状态之间平滑过渡。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]

[20190509160120387.png]: /images/20220203/1cc053d7e380447ea7f65ba05917999d.png
[20190509161306778.png]: /images/20220203/ac44dbe11a1f4b1f930a3fcb3f4bc7af.png
[20190509161327576.png]: /images/20220203/1eeb683f45ff4dcb8e5efc080042617c.png
[20190509162456167.png]: /images/20220203/85d7e25e04eb4050ba47369e933df0dc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70]: /images/20220203/691cca9097c74aaeb53c6e5b0b0ae2cc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE2Nzc1Mjkz_size_16_color_FFFFFF_t_70 1]: /images/20220203/b003e0cd71a64603b89bb4d8c7870ecc.png