一种基于碰撞风险的多Agents系统最优编队防撞控制算法

一种基于碰撞风险的多agents系统最优编队防撞控制算法
技术领域
1.本发明涉及一种控制算法，尤其涉及一种基于碰撞风险的多agents系统最优编队防撞控制算法


背景技术：

2.近几十年来，移动多agents系统的航行与协同控制在控制领域和机器人领域越来越被广泛关注。作为一个典型的代表，多agents的一致性与编队控制吸引了越来越多的研究兴趣，大量的科研数据被报导。另外，多agents编队控制已经广泛应用到多个领域，比如水面和水下自动交通工具，无人机，传感器网络，机器人以及自动车辆。
3.对移动agent而言，通信网络是交换信息和实现分布式编队控制不可缺少的环节。为了节省实际带宽和网络资源，事件触发式多agents编队控制方案已经被研究，但大多采用固定或交换网络通信拓扑。即使在无编队碰撞风险时，多agents信息交换始终存在。因此对于网络资源而言是一种浪费，也会造成成本的增加。对防撞控制而言，可以通过超声波，红外线，或激光传感器去探测碰撞风险，且仅在多agents有碰撞风险时进行信息传输，从而节省网络资源及成本。
4.为了保证多agents编队无碰撞风险，有多种方法已经被研究，如最优算法，机器学习算法和人工势场算法，并取得了一些成果，但是前述方法计算复杂度高，输入信号变化跨度大。因此影响到了输入信号的平滑性和输出效果，并导致上述方法在复杂系统中的应用范围受限。此外，目前为止，避免潜在碰撞风险的关键要素：各移动agent的相对速度，运动方向角及碰撞风险方向角并没有在任何相关控制策略中被纳入考量。


技术实现要素：

5.本发明提出了一种新型算法，将多agents编队的各移动agent的运动方向角，碰撞风险角和碰撞风险等级纳入考量，建立一个由开关函数，控制力方向函数和控制强度函数构成的最优编队防撞控制算法。可以仅在有碰撞风险时实现防撞控制算法和编队控制算法的实时切换，并在保证精确度情况下，提高运算效率，有效降低控制成本。
6.为了实现上述目的，本发明算法的具体步骤如下：
7.s1.建立一个多agents控制系统模型，所述多agents系统包含一个领航agent和n个跟随agents；
8.s2.确定编队防撞算法
9.s3.确定最优编队控制算法
10.s4.建立一个由防撞算法和最优编队控制算法构成的最优编队防撞控制算法：i表示第i个跟随agent；
11.s5.根据实际编队控制要求，确定系统模型，和最优编队防撞控制算法中的系数；
12.s6.结合跟随agents的动态数据，判断碰撞风险水平。若满足ui(t)的触发标准，开关函数自动激活最优编队防撞控制器以避免碰撞，随后恢复编队正常运行路线；仅在有碰
撞风险时实现防撞控制器的激活。图1为编队最优防撞控制算法步骤框图。
13.作为优选，s1中的一个由一个领航agent和n个跟随agents组成的多agents系统(n≥1)的动力学模型可定义为：
14.y
si
(t)＝y0(t)-si(t)，i∈n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0015][0016]
其中，y
si
(t)为理想情况下的第i个跟随agent的位置信号，y0(t)∈rq为领航agent的位置信号(通常为已知量，由编队队形需求确定。q代表维度)，si(t)是理想情况下领航agent和第i个跟随agent之间的位置偏移量(通常为已知量，由编队队形需求确定)。为第i个跟随agent的状态矢量。ui(t)∈rq，为第i个跟随agent的输入控制矢量，yi(t)∈rq，为第i个跟随agent的输出矢量，代表了该agent在编队的实际位置信号。ai，bi和ci是合适维度的矩阵(由编队队形需求决定)，ni为向量空间维度；t是终止时间，n＝{1，2，...，n}。
[0017]
本发明所有涉及物理量均采用标准国际单位。
[0018]
作为优选，q为2(2维)，如移动机器人，自动水面航行设备等；或3(3维)，如无人机，自动水下航行器等。
[0019]
s2中的编队防撞算法与开关函数p
ij
(t)，控制强度函数r
ij
(t)和控制力方向函数q
ij
(t)与运动方向角α
ij
和碰撞风险角β
ij
和β
ji
有关，因此，首先对运动方向角和碰撞风险角进行定义。
[0020]
作为优选，运动方向角α
ij
定义为第i个agent的运动方向与第i个agent和第j个agent连线的夹角(如图2所示)，α
ji
同理。yi和yj分别代表第i个agent和第j个agent的实时位置，vi第i个agent的速度。
[0021]
作为优选，碰撞风险角β
ij
和β
ji
可由公式(3)确定，碰撞风险角为当第i个agent和第j个agent在风险区域运行，描述agents碰撞风险的物理量，由运动方向角α
ij
，α
ji
和第i个agent和第j个agent的实时速度vi(t)和vj(t)共同确定。
[0022][0023]
作为优选，s2中的p
ij
(t)是一个开关函数，可定义为公式(4)。其中，e
ij
(t)(公式(5))为第i个跟随agent的位置(信号yi(t))和第j个跟随agent的位置(信号yj(t))之间的实时距离。ω
ij
为第i个agent和第j个agent之间的最低安全距离。
[0024][0025]
其中，
[0026]eij
(t)＝yi(t)-yj(t)，i，j n，j i
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0027]
作为优选，若||e
ij
(t)||＜ω
ij
，此时第i个agent和第j个agent可能会发生碰撞，此时防撞控制器激活。
[0028]
作为优选，防撞控制器仅在有碰撞风险时激活，以节约网络资源及成本。
[0029]
作为优选，s2中的r
ij
(t)∈r定义为控制强度函数(公式(6))，该函数与第i个跟随agent和第j个跟随agent的位置之间的最低安全距离ω
ij
，第i个跟随agent的位置和第j个跟随agent的位置之间的实时距离e
ij
(t)，可调节参数θ(通常为正整数)，以及碰撞风险角β
ij
相关。该控制强度函数通常被存储于函数库中，根据不同碰撞风险水平随时调用。
[0030]
当碰撞风险角β
ij
在[0，βh)区间时为高碰撞风险，在[βh，βm)区间时为中碰撞风险，在[βm，β
l
)区间时为低碰撞风险，在[β
l
，βn)区间时为无碰撞风险。
[0031][0032]
作为优选，βn＝π。
[0033]
作为优选，s2中q
ij
(t)定义为控制力方向函数(公式(7))，其确保如果第i个agent和第j个agent之间存在碰撞风险，那么第i个agent将远离第j个agent。其中，h∈(0，1)为控制系数，其与最低安全距离ω
ij
的值成反比。
[0034]
g为q
×
q反对称矩阵(见公式(8))。e
ij
(t)(公式(5))为第i个跟随agent的位置(信号yi(t))和第j个跟随agent的位置(信号yj(t))之间的实时距离。
[0035][0036][0037]
图3展示了控制力方向函数q
ij
(t)应用的两个实例，yi和yj分别代表第i个agent和第j个agent的实时位置。yi和yj的轨迹分别用实线和虚线表示。
[0038]
(1)第i个agent和第j个agent分别以相同的速度vi和vj面对面运行(图3(a)所示)；
[0039]
(2)第i个agent和第j个agent以相同速度vi和vj和相同方向角α
ij
和α
ji
相向运行(图3(c)所示)
[0040]
从图3(b)和图3(d)可以看出，当第i个agent和第j个agent进入ω＝ω
ij
＝ω
ji
直径范围之内(有碰撞风险)，开关函数p
ij
(t)(公式(4))激活防撞控制器，碰撞风险水平将被确认(公式(6))，此时控制力方向函数能保证agents沿弧形轨道运行以避免潜在的碰撞可能，并在之后保持原有队形，然后防撞控制器待命，直到再次被开关函数激活。
[0041]
作为优选，s2中定义为编队防撞算法(公式(9))，
[0042]
[0043]
作为优选，s3中的最优编队控制算法的反馈分量：(公式(10))，是为了保证闭环控制系统的渐近稳定性。
[0044][0045]
作为优选，s3中的最优编队控制算法的回馈分量(公式(11))，是为了保证系统输出信号yi(t)能够尽可能接近理想输出信号y
si
(t)。
[0046][0047]
作为优选，s3中的最优编队控制算法为最优编队控制算法(公式(12))。
[0048][0049]
作为优选，pi是公式(13)riccati代数方程的唯一解，gi(t)是公式(14)微分方程组的唯一解。为第i个跟随agent的状态矢量。ai，bi，ci，ri，d
i1
和d
i2
是合适维度的矩阵(由编队队形需求决定，为已知量)，ni为向量空间维度。si(t)是理想情况下领航agent和第i个跟随agent之间的位置偏移量(通常为已知量，由编队队形需求确定)。y
si
(t)为理想情况下的第i个跟随agent的位置信号。(通常为已知量，由编队队形需求确定)
[0050][0051][0052]
作为优选，s4中的最优编队防撞控制算法如下(公式(15))：
[0053][0054]
编队防撞算法和为最优编队控制算法的叠加构成了本发明所描述最优编队防撞控制算法ui(t)。
[0055]
作为优选，s5根据实际编队控制要求，确定系统模型和最优编队防撞控制算法中的系数，包括：
[0056]
系统方程模型系数ai，bi，ci；
[0057]
向量空间维度ni；
[0058]
系统维度q；
[0059]
控制强度函数的可调节参数θ；
[0060]
控制强度函数中确定各碰撞风险水平区间临界角βh，βm，β
l
和βn；
[0061]
第i个跟随agent和第j个跟随agent的位置之间的最低安全距离ω
ij；
[0062]
最优编队控制算法微分方程组的系数d
i1
，d
i2
和ri。
[0063]
作为优选，s5中分别计算出最优控制算法参数中公式(13)和公式(14)的唯一解pi和gi(t)。
[0064]
作为优选，s6中结合跟随agents的动态数据，判断碰撞风险水平。若满足ui(t)的触发标准，开关函数自动激活最优编队防撞控制器以避免碰撞，随后恢复编队正常运行路
线。
[0065]
作为优选，仅在有碰撞风险时实现防撞控制器的激活。
[0066]
本发明的具体实施步骤s5，和s6将通过具体实施例进行详细阐述。
[0067]
与现有技术相比较，本发明具有如下的优点：
[0068]
(1)本发明将各移动agent的运行速度vi和vj，运动方向角α
ij
和α
ji
，碰撞风险角β
ij
，控制强度函数r
ij
(t)和控制力方向函数q
ij
(t)纳入防撞算法，能够提高控制器的精确度，并简化算法的复杂程度。
[0069]
(2)本发明通过开关函数p
ij
(t)，仅在有碰撞风险时实现编队防撞控制器的实时切换，能有效节约网络资源，降低控制成本。
[0070]
(3)本发明在编队防撞算法基础上结合了最优控制算法，能使系统输出信号yi(t)更加接近理想输出信号y
si
(t)，并提高系统稳定性。
附图说明
[0071]
图1是编队最优防撞控制算法步骤框图；
[0072]
图2是运动方向角α
ij
示意图；
[0073]
图3是控制力方向函数q
ij
(t)应用的两个示例；
[0074]
图4是固定编队模式下(以飞行编队为例)无碰撞风险时各agents的轨迹图；
[0075]
图5是固定编队模式下(以飞行编队为例)中等碰撞风险时各agents的轨迹图(未引入最优编队防撞控制算法)；
[0076]
图6是固定编队模式下(以飞行编队为例)中等碰撞风险时各agents的轨迹图(引入最优编队防撞控制算法)；
[0077]
图7是在非固定编队模式下(以飞行编队为例)低碰撞风险时各agents的轨迹图(未引入最优编队防撞控制算法)；
[0078]
图8是在非固定编队模式下(以飞行编队为例)低碰撞风险时各agents的轨迹图(引入最优编队防撞控制算法)；
[0079]
图9是在非固定编队模式下(以飞行编队为例)高碰撞风险时各agents的轨迹图(未引入最优编队防撞控制算法)；
[0080]
图10是在非固定编队模式下(以飞行编队为例)高碰撞风险时各agents的轨迹图(引入最优编队防撞控制算法)。
[0081]
具体实施方式及实施例
[0082]
下面结合附图对本发明的具体实施方式及实施例做详细说明。实施例以飞行器多agents系统为例。通过飞行器agents的轨迹说明最优编队防撞算法的控制效果。其中各轨迹图的坐标横轴y
i1
(m)和纵轴y
i2
(m)分别代表第i个agent的横向和纵向位移，单位：m(米)。
[0083]
首先对于多agents系统模型以及最优编队防撞控制算法中的部分物理量进行假设(s5)：
[0084]
作为优选，ni＝4；
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0085]
作为优选，q＝2；
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0086]
作为优选，
[0087]
作为优选，
[0088]
作为优选，
[0089]
作为优选，
[0090]
作为优选，
[0091]
作为优选，ω
ij
＝2m；
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)
[0092]
作为优选，
[0093]
在s6中，pi和gi(t)分别是公式(13)和公式(14)的唯一解，根据s5设定的参数，它们可分别经由求解riccati代数方程(13)和下面式子得到：
[0094][0095]
其中，m为正整数，t是终止时间，
[0096]
具体实施例1：固定飞行编队模式
[0097]
(1)固定飞行编队模式在最优编队控制下无碰撞风险时，
[0098]
图4是固定飞行编队模式下无碰撞风险时各agents的轨迹图；领航agent的路线假设为y0(t)(公式(27))，第i个跟随者的理想路径偏移函数假设为si(t)(公式(29))，起始位置为xi(0)(公式(28))。虚线是领航agent的轨迹(y)，实线是8名跟随agents的轨迹(y1-y8)，以类似“∧”的轨迹运行。由s6，此时对于第i个和第j个agents，并无碰撞风险，也就是||e
ij
(t)||＞ω，防撞控制器并未被激活。图4中，点(1-9)表示t＝0s时各agents的位置，即起始位置(用
○
表示)；点(10-18)表示，t＝10s时，各agents的位置(用
●
表示)；点(19-27)表示t＝20s时各agents的位置，即终端位置(用
○
表示)。从仿真轨迹可以看出，多agents系统能达到并保持预定队形。
[0099][0100][0101][0102]
(2)固定飞行编队模式在最优编队防撞控制下有中等碰撞风险时，
[0103]
假设领航agent的路线假设为y0(t)(公式(27))，第i个跟随者的理想路径偏移函数假设为si(t)(公式(29))，起始位置为xi(0)(公式(30))。虚线是领航agent的轨迹(y)，实线是8名跟随agents的轨迹(y1-y8)，以类似“∧”的轨迹运行。
[0104]
在时间t＝0.5s时，||e
ij
(t)||＜ω，第1和第2agent的碰撞风险角β
ij
在[βh，βm)区间，为中等碰撞风险。同理，第3和第4agent，第5和第6agent，第7和第8agent也分别有中等碰撞的风险。
[0105]
图5中，点(1-9)表示t＝0s时各agents的位置，即起始位置(用
○
表示)；点(10-14)表示，t＝0.6s时，各agents的位置(用
●
表示)；点(15-23)表示t＝20s时各agents的位置，即终端位置(用
○
表示)。明显可以看出：未引入最优防撞控制算法时，第1和第2agents，第3和第4agents，第5和第6agents，第7和第8agents在t＝0.6s时有相撞的风险。因此由s6引入防撞控制算法。图6是在引入最优编队防撞控制算法后的各agents轨迹图，起始位置点和终端位置点如图5，没有发生任何变化。但明显可以看出：引入最优防撞控制算法后，在0.6s附近，各agent在可能发生碰撞的时间点前变更了运行轨迹，包括在第10点和11点，第12点和13点，第15点和16点，第17点和第18点(均用
●
表示)，碰撞风险被提前预判并成功避免，之后编队仍按照预定路线完成运行。
[0106][0107]
具体实施例2：非固定飞行编队模式
[0108]
(1)非固定飞行编队模式在最优编队控制下有低碰撞风险时，
[0109]
假设一个多agents系统，包括一个领航agent和四个跟随agents。虚线是领航agent的轨迹(y)，实线是4名跟随agents的轨迹，以类似“∧”的路径移动，分别用y1，y2y3和y4表示。领航agent的路线假设为y0(t)(公式(31))，第i个跟随者的理想路径偏移函数假设为si(t)(公式(32))，起始位置为xi(0)(公式(33))。
[0110]
y0(t)＝[t 3 0]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(31)
[0111]
[0112][0113]
由于||e
ij
(t)||＜ω，各agents的碰撞风险角β
ij
在[βm，β
l
)区间，因此为低碰撞风险。图7是低碰撞风险时各agents的轨迹图(未引入最优编队防撞控制算法)。点(1-5)表示t＝0s时各agents的轨迹，即起始位置；点(20-24)表示t＝25s时各agents的轨迹，即终端位置。点(6-9)，点(10-13)，点(14-16)和点(17-19)分别表示时间t＝2.44s，t＝10.44s，t＝14.44s和t＝22.44s时各agents的运行轨迹。可以看出，1和3agents可能在t＝2.44s和t＝10.44s发生碰撞，1和2agents可能在t＝14.44s发生碰撞，3和4agents可能在t＝14.44s和t＝22.44s发生碰撞。
[0114]
图8是在非固定飞行编队模式下低碰撞风险时各agents的轨迹图(引入最优编队防撞控制算法)；初始位置和终端位置同图7。可以看出，根据步骤s6，在各agent在可能发生碰撞的时间点前，最优防撞控制器能使各agent沿弧形轨迹相互避让(如第7点和第8点，第12点和第13点，第16点和第17点，第19点和第20点，第21点和第22点，第24点和第25点)，并可以恢复既定轨迹运行。
[0115]
(2)非固定飞行编队模式在最优编队控制下有高碰撞风险时，
[0116]
假设一个多agents系统，包括一个领航agent和四个跟随agents。虚线是领航agent的轨迹(y)，实线是4名跟随agents的轨迹，以类似“∧”的路径移动，分别用y1，y2y3和y4表示。领航agent的路线假设为y0(t)(公式(34))，第i个跟随者的理想路径偏移函数假设为si(t)(公式(35))，起始位置为xi(0)(公式(36))。
[0117]
y0(t)＝[t 0]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(34)
[0118][0119][0120]
由于||e
ij
(t)||＜ω，各agents的碰撞风险角β
ij
在[0，βh)区间，图9是在非固定飞行编队模式下高碰撞风险时各agents的轨迹图(未引入最优编队防撞控制算法)。点(1-5)表示t＝0s时各agents的轨迹，即起始位置；点(23-26)表示t＝22s时各agents的轨迹，即终端位置。点(6-8)，点(9-11)，点(12-15)，点(16-19)和点(20-22)分别表示t＝1.78s，t＝7.1s，t＝9.78s，t＝15.1s和t＝17.78s时各agents的轨迹。可以看出，1和2agents可能在t＝1.78s，t＝7.1s和t＝17.78s发生碰撞，3和4agents可能在t＝1.78s，t＝7.1s和t＝17.78s发生碰撞，1和3agents可能在t＝9.78s和t＝15.1s发生碰撞。
[0121]
图10是在非固定飞行编队模式下高碰撞风险时各agents的轨迹图(引入最优编队防撞控制算法)。可以看出，根据步骤s6，在各agent在可能发生碰撞的时间点前，最优防撞控制器能使各agent沿弧形轨迹相互避让，(如第6点和第7点，第9点和第10点，第11点和第12点，第14点和第15点，第17点和第19点，第23点和第24点，第26点和第27点，第29点和第30点)，并可以恢复既定轨迹运行。
[0122]
本发明的上述实施例仅是为清楚说明本发明所做的举例，而并非是对本发明实施方式的限定。其他2维控制系统，如移动机器人，自动水面航行设备等；或3维控制系统，如无人机，自动水下航行器等，均适用本发明所述控制算法。任何本领域技术人员，在本发明的精神和原则之内所做的任何修改，等同替换等，均应包含在本发明要求的保护范围之内。