一种应用于三维空间的同时定位和巡航系统的制作方法

transaction on automatic control，vol.59，no.8，pp.2182-2188，2014.)提出了一种适用于二维空间的单个智能体同时定位和圆周巡航单个目标体的算法。该算法在工作时，首先通过基于方位角测量的位置估计器来对目标体的位置进行估计，然后通过一个非线性的巡航控制器来驱使智能体以期望的圆周轨道巡航目标体。在此基础上，deghat等(m.deghat，l.xia，b.d.o.anderson，etal，“multitarget localization and circumnavigation by a single agent using bearing measurements，”international journal of robust and nonlinear control，vol.25，pp.2362-2374，2015.)进一步提出了一种适用于二维空间的单个智能体同时定位和圆周巡航多个目标体的算法。该算法通过一个集中式的几何中心估计器来估计所有目标体的几何中心，然后以几何中心为圆心圆周巡航所有的目标体。shao等(j.shao and y.p.tian，“multitarget localisation and circumnavigation by a multiagent system with bearing measurements in 2d space，”international journal of systems science，vol.49，no.1，pp.15-26，2018.)在二维空间中考虑了一种更为一般的情况，即多个智能体同时定位和圆周巡航多个目标体。该算法通过分布式协作的方式来估计所有目标体的几何中心，然后每个智能体均以几何中心为圆心巡航所有的目标体。但由于该算法需要获取某些变量的全局信息，从而大幅度提升了整个系统的通信繁忙程度。yu等(x.yu，l.liu and g.feng，“distributed circular formation control of nonholonomic vehicles without direct distance measurements，”ieee transactions on automatic control，vol.63，no.8，pp.2730-2737，2018.)基于方位角和相对朝向角测量提出了一种适用于二维空间的单个静止目标体的分布式圆周编队巡航算法。这个算法不需要智能体之间进行数据通信，但需要假设目标体的位置至少能被一个智能体获取。wang等(l.wang，y.zou and z.meng，“stationary target localization and circumnavigation by a nonholonomic differentially driven mobile robot：algorithms and experiments，”international journal of robust and nonlinear control，vol.3，pp.2061-2081，2021.)基于方位角和速度的信息，研究了二维空间中单个差分驱动的智能体同时定位和圆周巡航单个静止目标体的问题。yang等(z.yang，s.zhu，c.chen，etal，“entrapping a target in an arbitrarily shaped orbit by a single robot using bearing measurements，”automatica，vol.1，pp.1-8，2020.)在二维空间中研究了单个智能体在任意的轨道上同时定位和巡航单个静止目标体的问题。dou等(l.dou，c.songe，x.wang，etal，“target localization and enclosing control for networked mobile agents with bearing measurements，”automatica，vol.118，pp.1-12，2020.)在二维空间中研究了多个智能体同时定位和圆周编队巡航一个运动目标体的问题。cao等(s.cao，r.li，y.shi，etal，“a low cost estimator for target localization and circumnavigation using bearing measurements，”journal of the franklin institute engineering and applied mathematics，vol.357，no.14，pp.9654-9672，2020.)在二维空间中针对单个智能体定位和圆周巡航单个目标体这一问题，提出了一种更加经济的标量位置估计器。
5.通过分析可知，上述的算法仍存在很大的局限性。首先是算法的适用空间。上述算法基本都是在二维空间中考虑和设计的，而对适用于三维空间的算法未做深入探讨。在实际应用中，完全的二维空间的应用场景是十分稀少的，基本都需要能适用于三维空间的算
法。再者是智能体和目标体的数量，目标体的运动状态以及一些参数的获取。上述算法很多都要求智能体的数量是单个或者目标体的数量是单个，并且很多都只考虑了静止的目标体这一情形，而对于运动的多目标体并未做过多研究。即使有算法考虑过多智能体同时定位和巡航多目标体这一问题，但该算法需要获取某些参数的全局信息，这增加了网络的通信繁忙程度，降低了算法的实用性。在一些领域中，尤其是军事侦察领域，敌对目标体往往在三维空间中活动，并且是多个的和运动的。为提高巡航效率和成功的可能性，我方也会派出多个智能体执行侦察任务。这时上述算法要么直接不能工作，要么即使能工作也存在巡航效率低下和能量消耗大等问题。此外，上述研究都还处于理论研究和数值仿真阶段，而对于算法的实际实施未做深入探讨。因此，如何设计一个新的同时定位和巡航控制系统来有效解决上述问题是一个值得深入研究的课题，并且也更具有实际意义。


技术实现要素：

6.发明目的：给多智能体系统围捕和监控多个目标体这类问题提供一种适用于三维空间的同时定位和巡航系统，以克服现有的同时定位和巡航系统只能适用于二维空间，智能体的数量受限，目标体的数量和运动状态受限，算法工作时需要获取某些参数的全局信息，实用性差等缺陷。
7.技术方案：为实现上述目的，本发明提供一种应用于三维空间的同时定位和巡航系统，其特征在于，包括同时定位和巡航算法、多个非合作目标体以及多个智能体设备，所述智能体设备为所述同时定位和巡航算法运行的载体，所述同时定位和巡航算法运行于所述智能体设备中，所述非合作目标体为所述智能体设备需要同时定位和巡航的对象；
8.所述同时定位和巡航算法包括：
9.平面法向量估计器，用于根据邻居的所述智能体设备的法向量和自身的法向量计算出一个统一的法向量；
10.位置估计器，用于计算所述多个非合作目标体在三维空间中的位置；
11.几何中心估计器，用于计算所述多个非合作目标体位置的几何中心；
12.巡航控制器，用于控制所述智能体设备运行到预设圆周轨道上，并同时驱使所述智能体设备在预设的轨道上做圆周运动；
13.所述智能体设备包括：
14.方位角测量模块，所述方位角测量模块为测量经度角和纬度角的执行机构，所述智能体设备通过所述方位角测量模块来测量自身到所述非合作目标体的经度角和纬度角；
15.算法计算模块，所述算法计算模块与所述同时定位和巡航算法相对应，用于在所述智能体设备的硬件芯片中编程计算所述同时定位和巡航算法的各中间变量；
16.牵引控制模块，用于根据算法计算模块所得信息，控制所述智能体设备的移动方向和移动速度；
17.定位模块，用于获取所述智能体设备的绝对位置数据和相对位置数据；
18.内核模块，用于控制所述方位角测量模块的启动和停止；还用于读取所述智能体设备的自身位置数据和所述算法计算模块计算的所述非合作目标体的位置数据；还用于进行所述智能体设备的自身位置数据和所述算法计算模块计算的所述非合作目标体的位置数据的编号及上传；还用于接收和解析邻居智能体设备和控制中心发送的数据包；还用于
完成数据的分发、丢包和存储处理；
19.无线通信模块，用于控制发送自身数据包到邻居智能体设备和接收邻居智能体设备的数据包；还用于建立所述多个智能体设备之间通信连接；还用于控制发送所述智能体设备的标号数据；还用于将自身位置数据和所述非合作目标体的位置数据发送至控制中心。
20.进一步地，所述同时定位和巡航算法中的平面法向量估计器具体为：以智能体设备i和邻居智能体设备j为例，当所述智能体设备i接收到所述邻居智能体设备j的数据包后，按照如下公式计算所述智能体设备i的平面单位法向量ni(t)，
[0021][0022]
其中∈为正常数增益，为智能体设备i的所有邻居智能体设备的集合，对任意||ni(0)||＝1。
[0023]
进一步地，所述同时定位和巡航算法中的位置估计器为：以智能体设备i和目标体k为例，当所述智能体设备i测得自身到所述目标体k的经度角和纬度角后，按照如下公式计算所述智能体设备i对所述目标体k位置的估计
[0024][0025]
其中ki为正常数增益，i为单位矩阵，xi(t)为一个已知量，即所述智能体设备i的位置，位所述智能体设备i到所述目标体k的单位方向向量，即位所述智能体设备i到所述目标体k的单位方向向量，即为所述智能体设备i对所述目标体k位置的估计。
[0026]
进一步地，所述同时定位和巡航算法中的几何中心估计器具体为：所述智能体设备以广播的形式发送自己的数据包，以智能体设备i和邻居智能体设备j为例，当所述智能体设备i接收到所述邻居智能体设备j发送的数据包后，按照如下公式计算对所有目标体几何中心ξ
*
(t)的估计ri(t)，
[0027][0028][0029][0030]
其中εi(t)是正常数增益，sgn

(*)被定义为当x＞0时，sgn

(x)＝1，否则则为0，ωi(t)是一个中间状态变量，sgn(*)是标准的符号函数，n是所有智能体设备的总数，m是所有目标体的总数，是能被所述智能体设备i测量经度角和纬度角的所有目标体的集合，是能测量目标体k的所有智能体设备的集合，表示集合中元素的个数。
[0031]
进一步地，所述智能体设备i发送的数据包包括平面法向量ni(t)、增益εi(t)、目标体的几何中心估计ri(t)、自身位置数据xi(t)和计算的目标体的位置数据
[0032]
进一步地，所述同时定位和巡航算法中的巡航控制器具体为：所述智能体设备i按照如下公式计算自身运动的速度ui(t)，
[0033][0034]
其中表示智能体设备i到ri(t)的距离，即η
i1
，η
i2
和η
i3
是正常数增益，di是智能体i期望的圆周巡航的半径，φ
i1
(t)是智能体设备i到ri(t)的单位方向向量，φ
i2
(t)是智能体设备i切线速度的单位方向向量，φ
i3
(t)是φ
i1
(t)和φ
i2
(t)的正交向量，即φ
i3
(t)＝φ
i2
(t)
×
φ
i1
(t)，sgn(*)是标准的符号函数。
[0035]
进一步地，所述智能体设备i按照如下公式计算切线速度的单位方向向量φ
i2
(t)，
[0036]
φ
i2
(t)＝φ
i1
(t)
×
ni(t)。
[0037]
进一步地，所述智能体设备根据自身运动速度的大小来自适应设置发送数据包的频率；
[0038]
所述算法计算模块采用四阶龙格库塔算法来对连续的同时定位和巡航算法进行离散化处理；
[0039]
所述四阶龙格库塔算法采用定时间步长的方式进行计算，步长的选取根据智能体设备的移动速度来选取，选取范围为0.01s～1s。
[0040]
进一步地，所述定位模块通过gps或北斗芯片获取智能体设备的绝对位置信息，通过空间中安装的已知锚节点设备计算智能体设备的相对位置信息。
[0041]
进一步地，所述四阶龙格库塔算法采用定时间步长的方式进行计算，步长的选取根据智能体设备的移动速度来选取，选取范围为0.01s～1s。
[0042]
进一步地，所述控制中心根据接收的智能体设备的标号数据，所述智能体设备的位置数据和目标体的位置数据对所述定位和巡航系统的现场进行重构。
[0043]
有益效果：本发明与现有技术相比，提供的一种应用于三维空间的同时定位和巡航系统，通过设计一个分布式的平面法向量估计器，保证了每个智能体设备只需与邻居智能体设备进行数据交换就能计算出统一的平面法向量，避免了通过频繁交换数据来获取统一的平面法向量；通过引入经度角和纬度角的概念，保证了设计的同时定位和巡航系统能适用于三维空间，大幅度提升了系统的实用性；通过设计一个增益估计器来计算几何中心估计器中的增益，保证了几何中心估计器稳定收敛的同时，也降低了稳定后算法的误差；通过设计一个巡航控制器，保证了多智能体设备在能对目标体进行巡航的同时，也能跟随目标体一起运动，从而实现围捕和监控目标体的目的；此外，通过设计一种硬件方案进一步提升了本发明的实用性；由于提出的同时定位和巡航是基于切换网络拓扑结构，这使得该系统能有效应对数据丢包等突发情况，进一步提高了整个系统的稳定性和适用性。
附图说明
[0044]
图1示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的结构示意图；
[0045]
图2示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的时钟同步算法的计算流程图；
[0046]
图3示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的智能体设备的工作流程图；
[0047]
图4示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的智能体设备的硬件结构示意图；
具体实施方式
[0048]
为使本领域的技术人员能更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步的详细说明。
[0049]
图1示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的结构示意图，该系统包括同时定位和巡航算法，多个非合作目标体和多个智能体设备。智能体设备是同时定位和巡航算法运行的载体，每个智能体设备中都运行相同的同时定位和巡航算法，智能体设备之间通过无线网络进行通信，非合作目标体是智能体设备需要同时定位和巡航的对象。
[0050]
本发明实施例中，多智能体设备的数量为4个，通信拓扑为链状无向图(实线连接)，多目标体的数量为3个，但并不局限于固定数量的多智能体和多目标体，以及无向链状图这一种拓扑，可以是任意规模的多智能体和多目标体，多智能体的通信拓扑可以是任意的有向平衡图这类拓扑。
[0051]
首先介绍非合作目标体，这类目标体是三维空间中某些事物的抽象，它包含的范围很广，可以是敌对飞行器，敌对士兵，被困于某处的受伤人员，太空中漂浮的碎片，因遭受自然灾害而失去通信能力的局部区域等。智能体设备不能获取这类目标体的位置和速度等信息，但能测量自身到目标体的经度角和纬度角。
[0052]
然后介绍同时定位和巡航算法，该算法一共包括平面法向量估计器，位置估计器，几何中心估计器和巡航控制器。在同时定位和巡航的过程中，各智能体设备以广播的形式发送自己的数据包，该数据包包括平面法向量，增益的估计和多目标体几何中心的估计。当邻居智能体设备接收到这个数据包后，就结合测量的经度角和纬度角，按照同时定位和巡航算法计算控制智能体设备的速度矢量。
[0053]
计算统一的平面法向量：智能体设备以广播的形式发送自己的数据包，以智能体设备i和邻居智能体设备j为例，当智能体设备i接收到邻居智能体设备j的数据包后，按照如下公式计算智能体设备i期望的平面单位法向量ni(t)，
[0054][0055]
其中∈是正常数增益，是智能体设备i的所有邻居智能体设备的集合，对任意||ni(0)||＝1。
[0056]
计算目标体的位置：当智能体设备i测得自身到目标体k的经度角和纬度角后，按照如下公式计算智能体设备i对目标体k位置的估计
[0057][0058]
其中ki是正常数增益，i是单位矩阵，xi(t)是一个已知量，即智能体i的位置，
是智能体i到目标体k的单位方向向量，即是智能体i到目标体k的单位方向向量，即是智能体设备i对目标体k位置的估计。
[0059]
计算所有目标体位置的几何中心：当智能体设备i接收到邻居智能体设备j发送的数据包后，按照如下公式计算对所有目标体几何中心ξ
*
(t)的估计ri(t)，
[0060][0061][0062][0063]
其中εi(t)是正常数增益，sgn

(*)被定义为当x＞0时，sgn

(x)＝1，否则则为0，ωi(t)是一个中间状态变量，sgn(*)是标准的符号函数，n是所有智能体设备的总数，m是所有目标体的总数，是能被智能体设备i测量经度角和纬度角的所有目标体的集合，是能测量目标体k的所有智能体设备的集合，表示集合中元素的个数。
[0064]
计算智能体设备的控制速度矢量：智能体设备i按照如下公式计算自身运动的速度ui(t)(该速度为矢量，即包含速度的方向和大小)，
[0065][0066]
其中表示智能体设备i到ri(t)的距离，即η
i1
，η
i2
和η
i3
是正常数增益，di是智能体i期望的圆周巡航的半径，φ
i1
(t)是智能体设备i到ri(t)的单位方向向量，φ
i2
(t)是智能体设备i切线速度的单位方向向量，计算公式为φ
i2
(t)＝φ
i1
(t)
×
ni(t)，φ
i3
(t)是φ
i1
(t)和φ
i2
(t)的正交向量，即φ
i3
(t)＝φ
i2
(t)
×
φ
i1
(t)，sgn(*)是标准的符号函数。
[0067]
接下来介绍智能体设备，智能体设备主要包括方位角测量模块，算法计算模块，牵引控制模块，定位模块，内核模块和无线通信模块。
[0068]
方位角测量模块：该模块是测量经度角和纬度角的执行机构，智能体设备通过这一模块来测量自身到非合作目标体的经度角和纬度角。
[0069]
算法计算模块：该模块就是在智能体设备的硬件芯片中实现同时定位和巡航算法，它的组成结构与同时定位和巡航算法一一对应。当算法计算模块接收到方位角测量模块传输的数据后，就开始在硬件中按照同时定位和巡航算法，计算平面法向量，目标体的位置估计，所有目标体位置的几何中心以及控制速度矢量。当得到新的控制速度矢量后，就把控制速度矢量信息传输给牵引控制模块。同时定位和巡航算法都是连续的，算法计算模块采用四阶龙格库塔法来对连续的算法进行离散化处理。四阶龙格库塔算法采用定时间步长的方式进行计算，步长的选取根据智能体设备的移动速度来选取，选取范围为0.01s～1s。
[0070]
牵引控制模块：根据算法计算模块输出的速度矢量信息，来驱动智能体设备的电
机和舵机单元以控制智能体设备下一时刻的移动方向和移动速度，从而完成对目标体的定位和巡航。
[0071]
定位模块：该模块包含gps或北斗控制接收单元以及质心定位单元两部分。gps或北斗控制接收单元用来获取智能体设备的绝对位置数据，质心定位单元通过射频芯片与空间中安装的锚节点进行通信和计算，从而获取智能体设备在空间中的相对位置数据。
[0072]
内核模块：该模块的主要功能有控制方位角测量模块的启动和停止；读取定位模块输出的智能体设备的自身位置数据和算法计算模块输出的目标体的位置数据，完成这两种不同位置数据的编号和上传；接收和解析其他设备包括邻居智能体设备和控制中心发送的数据包，完成数据的分发、丢包和存储等处理。
[0073]
无线通信模块：该模块受内核模块控制，是发送和接收数据的执行机构，智能体设备之间，以及智能体与控制中心之间通过该模块建立网络连接。该模块按功能可分为两个部分，分别是完成智能体与邻居智能体设备之间的通信，以及完成智能体设备与控制中心之间的通信。
[0074]
图2示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的时钟同步算法的计算流程图，当智能体设备完成数据包的接收后，就按照图2的同时定位和巡航算法流程计算控制速度矢量，参考图2，该算法包括：
[0075]
平面法向量估计器，用于根据邻居的所述智能体设备的法向量和自身的法向量计算出一个统一的法向量；
[0076]
位置估计器，用于计算所述多个非合作目标体在三维空间中的位置；
[0077]
几何中心估计器，用于计算所述多个非合作目标体位置的几何中心；
[0078]
巡航控制器，用于控制所述智能体设备运行到预设圆周轨道上，并同时驱使所述智能体设备在预设的轨道上做圆周运动。
[0079]
当平面法向量估计器，位置估计器和几何中心估计器计算完成后，就覆盖原来保存的数据。当完成数据的覆盖后，就触发启动巡航控制器读取保存的所有目标体位置的几何中心以及单位方向向量等信息，紧接着完成控制速度矢量的计算。计算完成后，就把得到的控制速度矢量输出到牵引控制模块，从而完成对目标体的同时定位和巡航。
[0080]
图3示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的智能体设备的工作流程图，图3包含了发送和接收两种进程，左边模块(步骤4对应的程序)为发送进程，右边模块(步骤7对应的程序)为接收进程。智能体设备的工作流程具体如下：
[0081]
步骤1，智能体设备上电启动。
[0082]
步骤2，初始化智能体设备。初始化包括对一些变量设置初始值，复位寄存器，复位收发数据的缓冲区等。
[0083]
步骤3，返回空闲状态。当智能体设备完成初始化后，或不收发数据和不计算后，智能体设备就进入空闲状态。
[0084]
步骤4，判断是否为发送数据包命令。智能体设备是以伪周期的形式广播自己的数据包(数据包包括发送给邻居智能体设备的数据包和发送给控制中心的数据包这两个种类。发送给邻居智能体设备的数据包是用来进行算法计算的一些变量信息，发送给控制中心的数据包是目标体和智能体设备的位置数据，这些位置数据被控制中心用来重构同时定
位和巡航的运动现场。)，这一步用来判断触发信号是否为发送数据包的命令。如果是发送数据包命令，则接管无线通信模块的，进入步骤5。如果不是，则返回到空闲状态。需要说明的是，步骤4与步骤7不是选择运行的关系，而是并列运行的。
[0085]
步骤5，向发送缓冲区中装载发送数据包。把发送数据包写入到发送缓冲区中，写入完毕后进入步骤6。
[0086]
步骤6，启动发送。根据射频芯片的控制时序，启动发送缓冲区的数据包。发送完毕后进入步骤25。
[0087]
步骤7，判断是否为接收数据包命令。如果为接收数据包命令，进入步骤8。如果不是，则返回到空闲状态
[0088]
步骤8，等待数据包接收完成。智能体设备接收的数据包除包括邻居智能体设备发送数据包(包括平面法向量，增益数据，目标体的几何中心估计)外，还包括控制中心发送的数据包(包括启动或停止命令)。当智能体设备完成数据包的接收后，就进入步骤9。
[0089]
步骤9，判断接收的数据包是否为启停工作命令。智能体设备根据帧头信息判断数据包是否为启停工作命令，如果是启停工作命令，则表明是控制中心发送的数据包，进入步骤10。如果不是，则进入步骤13。
[0090]
步骤10，判断数据包内容是否为启动工作命令。智能体设备对数据包的内容进行解析，判断命令是否启动工作命令还是停止工作命令。如果是启动工作命令，则进入步骤11。如果不是，则进入步骤12。
[0091]
步骤11，把标识位start_flag赋值为1。标识位start_flag等于1表明控制中心使能智能体设备工作。使能完毕后，智能体设备就进入空闲状态，以等待下一次触发或数据包接收。
[0092]
步骤12，把标识位start_flag赋值为0。标识位start_flag等于0表明控制中心关闭智能体设备。关闭完成后，智能体设备同样进入空闲状态，以等待下一次触发或数据包接收。
[0093]
步骤13，判断接收的数据包是否为邻居智能体设备发送的。当接收的数据包不是启停工作命令后，就继续根据帧头信息判断是否为邻居智能体设备发送的数据包。如果是，则进入步骤14。如果不是，则表明接收的是未知的数据包。这时就做丢包处理，并进入空闲状态。
[0094]
步骤14，判断start_flag是否为1。这一步是为了判断从接收数据包到解析数据包这段时间内，控制中心是否发送了停止工作命令。如果start_flag＝1，表明没有发送停止工作命令，就进入步骤16。如果start_flag＝0，表明这段时间发送了停止工作命令，就进入步骤15。
[0095]
步骤15，清空接收数据包。当检测到控制中心发送了停止工作命令后，各智能体设备就会立刻停止工作，清空所有的缓冲区，然后进入空闲状态。
[0096]
步骤16，测量方位角信息。智能体设备触发方位角测量模块执行三维空间中经度角和纬度角的测量。当完成方位角信息测量后，并行进入步骤17和步骤19。
[0097]
步骤17，获取智能体设备自身位置数据。智能体设备根据定位模块来获取自身的绝对位置数据或相对位置数据。当位置数据获取完成后，进入步骤18。
[0098]
步骤18，计算目标体的位置。智能体设备根据自身位置数据和测量的方位角信息，
计算目标体的位置。当完成目标体位置的计算后，进入步骤20。
[0099]
步骤19，计算平面法向量。智能体设备根据接收的邻居智能体设备发送的数据包，计算期望巡航的平面的法向量。当完成平面法向量的计算后，该法向量等待被步骤22调用。
[0100]
步骤20，计算增益参数。智能体设备根据接收的邻居智能体设备发送的数据包，计算集合中心估计器中的增益参数。当完成增益参数的计算后，进入步骤21。
[0101]
步骤21，计算所有目标体位置的几何中心。智能体设备根据接收的邻居智能体设备发送的数据包和已完成计算的变量，计算所有目标体位置的几何中心。当完成计算后，进入步骤22。
[0102]
步骤22，计算智能体设备的速度矢量。智能体设备根据计算的平面法向量和目标体位置的几何中心等信息，计算智能体设备的速度矢量。当完成计算后，进入步骤23。
[0103]
步骤23，同步控制电机和舵机产生匹配的运动速度大小和方向。智能体设备根据计算得到的速度矢量信息，同时控制电机和舵机来调节运动速度的大小和方向。当完成控制后，进入步骤24，等待下一次接收和触发。
[0104]
步骤24，返回。就是返回到步骤3，等待下一次的触发。
[0105]
具体而言，图4示出了本技术一个示例性实施例提供的一种应用于三维空间的同时定位和巡航系统的智能体设备的硬件结构示意图，智能体设备采用fpga芯片来作为控制芯片。
[0106]
当上电开机后，方位角测量模块在内核模块的控制下被周期性启动。方位角测量模块通过spi总线与角度测量仪进行连接，并通过发送旋转和俯仰等命令控制角度测量仪完成经度角和纬度角的测量。当得到这些角度信息后，方位角测量模块再通过spi总线读取这些角度，并输出给算法计算模块。
[0107]
算法计算模块就是在fpga芯片中实现同时定位和巡航算法，它的功能结构与同时定位和巡航算法一一对应。当该模块接收到方位角测量模块传输的数据后，就开始在硬件中按照同时定位和巡航算法，计算平面法向量，目标体的位置估计，所有目标体位置的几何中心以及控制速度矢量。当得到新的控制速度矢量后，就把控制速度矢量信息传输给牵引控制模块。同时定位和巡航算法都是连续的，为使在fpga芯片中能完成该算法的设计，该模块采用四阶龙格库塔法来对连续的算法进行离散化处理。四阶龙格库塔算法采用定时间步长的方式进行计算，步长的选取根据智能体设备的移动速度来选取，选取范围为0.01s～1s。
[0108]
牵引控制模块根据算法计算模块输出的速度矢量信息，以驱动智能体设备的电机和舵机单元以控制智能体设备下一时刻的移动方向和移动速度，从而完成对目标体的定位和巡航。
[0109]
算法计算模块在进行同时定位和巡航算法的计算时，需要知道自身的位置数据。定位模块就是用来提供智能体设备的位置数据。该模块包含gps或北斗接收控制单元以及质心定位单元两部分。gps接收控制单元通过uart接口与gps或或北斗芯片连接，用来获取智能体设备的绝对位置数据。质心定位单元通过spi总线与外界射频芯片进行连接，然后完成与空间中安装的锚节点进行通信和计算，从而获取智能体设备在空间中的相对位置数据。当定位模块获取到自身位置数据后会同时传输给算法计算模块和内核模块。
[0110]
内核模块的主要功能有控制方位角测量模块的启动和停止；读取定位模块输出的
智能体设备的自身位置数据和算法计算模块输出的目标体的位置数据，完成这两种不同位置数据的编号和上传；接收和解析其他设备包括邻居智能体设备和控制中心发送的数据包，完成数据的分发、丢包和缓存等处理。
[0111]
当内核模块读取到智能体位置数据和目标体位置数据后，会通过无线通信模块把这些数据发送给控制中心，控制中心根据这些数据可以重构出多智能体同时定位和巡航多目标体的现场情况，从而可以更好地完成对整个系统的状态跟踪和监控。内核模块会对接收数据进行解析。解析是通过数据的帧头和帧尾等信息来判断接受的数据是否为一个完整的数据，以及接收数据是来自邻居智能体设备还是控制中心，从而做好数据的分发、丢包和缓存等处理。
[0112]
无线通信模块受内核模块控制，是发送和接收数据的执行机构。智能体设备之间，以及智能体与控制中心之间通过该模块建立网络连接。该模块按功能可分为两个部分，分别是完成智能体与邻居智能体设备之间的通信，完成智能体设备与控制中心之间的通信，完成。
[0113]
综上所述，本技术提供的一种应用于三维空间的同时定位和巡航系统，通过设计一个分布式的平面法向量估计器，保证了每个智能体设备只需与邻居智能体设备进行数据交换就能计算出统一的平面法向量，避免了通过频繁交换数据来获取统一的平面法向量；通过引入经度角和纬度角的概念，保证了设计的同时定位和巡航系统能适用于三维空间，大幅度提升了系统的实用性；通过设计一个增益估计器来计算几何中心估计器中的增益，保证了几何中心估计器稳定收敛的同时，也降低了稳定后算法的误差；通过设计一个巡航控制器，保证了多智能体设备在能对目标体进行巡航的同时，也能跟随目标体一起运动，从而实现围捕和监控目标体的目的；此外，通过设计一种硬件方案进一步提升了本发明的实用性；由于提出的同时定位和巡航是基于切换网络拓扑结构，这使得该系统能有效应对数据丢包等突发情况，进一步提高了整个系统的稳定性和适用性。
[0114]
值得说明的是，本实施例提供的智能体设备的硬件结构，仅仅是一种示例的实现形式。在不背离本发明的精神和原理的前提下，智能体设备的硬件结构可以有多种实现形式。