基于光学室内定位的多智能体协同控制算法验证系统

1.本发明为基于光学室内定位的多智能体协同控制算法验证系统，涉及多智能体控制领域。


背景技术：

2.近年来，随着计算机科学、网络通信等技术的不断发展，多智能体协同控制是控制领域的研究热点之一，在军事、交通、多机器人、传感器网络等领域具有广泛的应用前景。目前针对于对智能体系统的研究多集中在自主协同控制算法、集群分布式交互技术等方面，而对算法的评估验证研究较少。而已开展的多智能体协同控制演示验证，多采用室外环境，实验成本高，易受环境变化影响大，很难进行重复实验。相比之下，室内试验具有室外试验无法比拟的优势，比如成本低、实验数据便于观测和实时显示、迭代设计方便快捷、对周围及硬件环境依赖性低等，进而可以对多智能体系统协同控制相关算法进行反复验证，对算法进行优化调整。这对于加快多智能体系统的理论成果走向工程实际的速度、提高多智能体协同控制算法性能、评估算法研究的可行性等方面有着重大的意义。


技术实现要素：

3.基于上述背景技术中的问题，本发明设计了基于光学室内定位的多智能体协同控制算法验证系统。以便验证多智能体协同控制算法的可行性、实时性及稳定性，加快多智能体系统的理论成果走向工程实际应用的速度。本发明的多智能体协同控制问题主要包括一致性问题、编队问题、集群问题、蜂拥问题和聚集问题等等。其中一致性问题是系统协调控制的最基本也是最重要的问题，是智能体之间协调合作的基础。所谓的一致性是指随着时间的增加，多智能体系统中的所有智能体根据某种控制规则，信息相互传递，相互作用，使得每个智能体状态量趋于同一值。
4.本发明设计了基于光学室内定位的多智能体协同控制算法验证系统，其组成如图1所示，其主要包括光学室内定位子系统、通信子系统和控制子系统三部分组成。
5.各子系统的功能大致为：光学室内定位子系统获取空间中智能体的位置信息，并通过udp协议将上述信息广播在局域网中；通信子系统是系统中数据传输的桥梁，主要是将光学室内定位子系统与控制子系统接入到同一局域网中，实现数据的共享，同时在控制子系统中完成控制指令的发送；控制子系统主要接收智能体的位置信息，实现待验证多智能体协同控制算法的设计。其中光学室内定位子系统为待验证多智能体协同控制算法提供多智能体的位置信息，待验证多智能体协同控制算法的输出，也即控制指令通过通信子系统发送到被控对象中，完成控制执行的执行。
6.光学室内定位子系统包括数据获取模块、数据识别模块和数据发送模块。其硬件结构主要包括多个外光学摄像机、交换机以及服务器。其中数据获取模块是运行在红外光学摄像机的处理器中，而数据识别模块和数据发送模块则是运行在服务器当中。
7.数据获取模块功能主要是获取空间中目标点的位置信息，其包含图像获取单元和
图像处理单元，具体的实现步骤为：
8.s1：利用图像获取单元得到二维图像。二维图像的获取是确立目标点在空间中三维坐标信息的前提。通过针孔模型，红外光学摄像机捕捉到三维空间(世界坐标系)中的目标点投影到了相机图片的二维平面(像素坐标系)中，其中的对应关系为：
9.p＝k[r t]pw＝mpwꢀꢀ
(1)
[0010]
其中，pw(xw,yw,zw)为世界坐标系中的某一目标点的坐标，p(u,v)为目标点pw(xw,yw,zw)在像素坐标系中对应点的坐标。k为摄像机内参，[r t]为摄像机的外参；m称为投影矩阵，其描述了实际坐标系中的点与像素坐标系中的点的映射关系。
[0011]
特别地，在进行此步骤之前，需要对实验场地进行标定与摄像机标定工作。其中地场地标定的目的是为实验场地建立坐标原点，建立世界坐标系；摄像机标定实际上是求解摄像机的内外参数过程。本系统使用传统的摄像机标定方法，其需要标定的摄像机前放置一个标定参照物，利用标定参照物中特定点的图像坐标和世界坐标间的对应关系，通过数学理论计算方法得到摄像机的内部和外部参数。
[0012]
s2：对图像获取单元得到的二维图像信息，通过图像处理单元进行特征提取、完成图像处理的过程，具体操作流程如图2所示，包括读取图像各像素的rgb值、转化为灰度图像、灰度阈值化、高斯平滑、轮廓获取、特征点提取、输出特征点坐标。其中，输出的特征点坐标即为目标点的位置信息。至此，便完成了位置信息获取功能。
[0013]
数据识别模块目的是对数据获取模块获得的目标点的位置信息进行识别和封装，转化为智能体的位置信息。为了便于对多智能体系统进行控制，本系统需要对每个智能体进行标号。本发明在每个智能体主体顶部位置固定多个不同排列形式的反光标识点(marker点)，即为上述的目标点，如图3所示。数据识别模块能够根据标识点排列形式的不同，实现对智能体编号进行区分。
[0014]
数据发送模块通过作为udp服务端，udp广播协议，以固定的频率，将数据识别模块封装好的每个智能体的位置信息发送到局域网中。
[0015]
控制子系统由装有ros软件框架+matlab/simulink环境的上位机和多辆turtlebot3移动机器人硬件部分以及算法验证模块软件组成。通过网络配置将pc机设计控制主机，将移动机器人设置为从机，实现主从模式控制。特别的，在该控制主机上运行着ros master主节点，移动机器人是本系统中的智能体。控制子系统控制流程如图4所示，主要由data receive节点、simulink mode节点和agent gui节点组成。
[0016]
data receive节点接收由室内定位子系统传递的智能体位置信息，并转化话ros消息，通过话题发布在ros网络中。
[0017]
simulink mode节点和agent gui节点组成算法验证模块软件。其中在simulink mode节点设计待验证的多智能体协同控制算法，节点的输入为智能体位置信息，输出为智能体角的控制量，具体为角速度数据以及线速度数据。算法设计过程如下：
[0018]
考虑一个由n个智能体组成的多智能体系统，并且系统包含一个虚拟领导者，每个智能体的动态模型如下：
[0019]
[0020]
其中，分别为智能体i在t时刻表的位置、速度和加速度。ui(t)也称为系统的控制输入。i＝1,2,
…
n为系统中智能体的数量。表示n维实向量集
[0021]
系统虚拟领导者的模型为：
[0022][0023]
其中，x0(t)为虚拟领导者的位置，v0是虚拟领导者的速度值，为固定的常数，也称为该多智能体系统的期望速度。所谓的虚拟领导者是指其不一定为智能体，可以是一个标记，其作用只是体现在一致性协议当中，用于约束跟随智能体速度。
[0024]
对于多智能体系统(2)，本发明所采用的一致性协议，也即待验证的多智能体协同控制算法如下：
[0025][0026]
其中，α为控制增益，正常数；a
ij
表示该多智能体系统抽象的数学拓扑结构图对应的邻居矩阵元素；智能体j表示智能体i的邻居，即智能体i能够获得智能体j的信息；ni表示智能体i的邻居集合；si(t)表示在t时刻，智能体i与位于其前方智能体的距离，s表示多智能体系统的期望距离。
[0027]
上述一致性协议中，第一项(si(t)-s)的作用是保证所有智能体与其前方智能体的距离达到预设的期望值，第二项(v
j-vi)的作用是保证多智能体系统中所有跟随者速度的一致性，第三项(v
0-vi)的作用是保证多智能体系统中所有智能体的速度与虚拟领导者的速度，即期望速度一致。
[0028]
agent gui节点的功能，一是能够调用多个simulink mode节点，提高对算法验证的效率；二是能够以图形化的形式直观的展示待验证的多智能体协同控制算法在本系统中的实际效果，便于对算法进行调整，从而使待验证算法最优。
[0029]
agent gui节点的结构框架如图5所示，其中包括状态监控模块、实景显示模块、数据存储模块、参数管理模块、通信模块和启停控制模块。agent gui节点的主界面如图6所示。
[0030]
状态监控模块，用于对多智能体的状态数据进行检测和显示，其中包括速度信息、加速度信息和相邻智能体间距信息。本系统同时以图形化和表格形式对智能体的数据进行监控显示。
[0031]
实景显示模块，用于将多智能体的位置信息抽象为图形信息，并在二维场景中显示待验证的多智能体协同控制算法取得的效果。
[0032]
数据存储模块，用于存储从实验开始到结束此时间段内，多智能体的数据信息，包括加速度信息、速度信息以及智能体间距信息，便于实验后对数据进行分析。
[0033]
参数管理模块，用于管理本系统运行的所有参数，包括对控制算法的选择(当验证多个协同控制算法时用到)、对控制参数的设置以及通信参数的设置。
[0034]
通信模块，用于与ros master建立通信连接，以实现对智能体状态信息的获取以及控制命令的发送。
[0035]
启停控制模块，用于对本系统进行启动、停止和退出操作。
[0036]
通信子系统通过路由器建立本算法验证系统的局域网，光学室内定位子系统和控制子系统控制通过以太网数据线接入局域网，控制子系统从机，也即多智能体通过车载蓝牙模块接入局域网，从而利用局域网实现相互之间的数据发送和接收。
[0037]
通信子系统在功能上可以分为数据传输层、ros网络层和智能体底层，如图7所示，下面进行简要的说明：
[0038]
s1、数据传输层包括光学室内定位子系统中数据发送模块即udp服务端和控制子系统中的数据接收节点即udp客户端。
[0039]
s2、在光学室内定位子系统中，数据发送模块将获得智能体的位置信息通过udp协议发送到局域网中。
[0040]
s3、在控制子系统的控制主机当中创建数据接收节点，如图4所示，该数据接收节点作为udp客户端，通过udp协议接收智能体的位置信息，并把这些状态信息打包成ros消息，通过相应的话题(/robot id_pos，/robot id_ang)将消息发布到ros网络层中。
[0041]
s4、在simulink mode节点中，从上述话题中订阅智能体的位置信息，作为待验证算法的输入。同时将算法的输出量打包为ros消息，然后根据被控对象的不同封装成不同的ros话题(/robot1/cmd_vel,
…
/robot9/cmd_vel)，实现对智能体的控制。
[0042]
s5、在ros网络层，ros master主节点由控制子系统的控制主机维护。
[0043]
s6、智能体底层的通信。智能体结构上含有上下两层，其结构如图8所示。其中上层raspberry pi 3b智能版，其作为主控板，装有ubuntu mate系统，并搭载ros操作系统。上层raspberry pi 3b智能板通过订阅ros话题完成与控制主机的通信；下层动作控制板open cr，其作为驱动板直接控制智能体舵机的运动，实现动作的控制。下层动作控制板opencr驱动板通过串口与搭载ros操作系统的raspberry pi 3b主控板通信，负责将ros操作系统接收到的控制指令在硬件上执行，最终转化为驱动智能体运动的电信号。
[0044]
本发明提供了一种基于光学室内定位的多智能体协同控制算法验证系统，包括光学室内定位子系统、通信子系统和控制子系统三部分。与现有技术相比，本发明的有益效果在于：
[0045]
1、本发明在室内环境进行算法验证工作，相比于目前大多数室外环境的实验，具有成本低、实验环境可控、实验结果易得观测显示等优点，可以方便快捷的进行重复实验，提高对待验证算法优化调整的效率。
[0046]
2、本发明的光学室内定位子系统所使用的红外光学摄像机，具有亚毫米级的定位精度，能够以每秒100帧以上的拍摄速率捕捉智能体上marker点，能够实时精确的构建智能体的三维位置信息。
[0047]
3、本发明利用udp协议实现智能体的位置信息的发送与接收，具有较快的数据传递速度和和较高的实时性。
[0048]
4、本发明使用turtlebot3移动机器人作为智能体被控对象，是一款小型的、可编程的、基于ros的高性价比移动机器人，具有较高的灵活性，支持二次开发，非常的适合室内场景。
[0049]
5、本发明以模块化的设计思想，将待验证的多智能体协同控制算法单独的设计成一个simulink mode节点。特别地，位于便于用户对本系统的操作、简化对待验证算法的调试和对直观的对实验结果进行展示，本发明设计了agent gui节点。当有多个待验证算法，
即有多个simulink mode节点时，agent gui节点能够同时的对它们进行调用，极大的提高了算法的验证效率。
附图说明
[0050]
图1为基于光学室内定位的多智能体协同控制算法验证系统组成示意图。
[0051]
图2为图像处理单元工作流程图。
[0052]
图3为固定有不同排列形式反光标识点的智能体示意图。
[0053]
图4为控制子系统流程示意图。
[0054]
图5为agent gui节点的结构框架图。
[0055]
图6为agent gui节点主界面。
[0056]
图7为系统通信流程图。
[0057]
图8为本系统智能体结构示意图。
[0058]
图9为室内光学定位子系统硬件连接示意图。
[0059]
图10为本系统实验场地布局示意图。
[0060]
图11为本发明实例中所采用的通信拓扑结构图。
[0061]
图12为实验过程中智能体速度变化曲线示意图。
[0062]
图13为实验过程中智能体间距变化曲线示意图。
[0063]
图14为实验过程中智能体加速度变化曲线示意图。
具体实施方式
[0064]
下面结合附图和实施例，对本发明的基于光学室内定位的多智能体协同控制算法验证系统做进一步详细的描述。
[0065]
本发明实例是通过将一个具体的多智能体协同控制算法运用到本发明当中，通过实验结果对本发明的性能进行评估。在进行实验之前，需要对本系统进行设计。
[0066]
s1、光学室内定位子系统的搭建
[0067]
智能体位置信息的准确获取是本系统的基础。本发明定位功能的实现，采用的是红外光学三维动作捕捉系统，所选用的动作捕捉系统具有速度快、精度高和高分辨率等特点，能够精确的获取本发明中移动机器人的位置信息，实现对本发明移动机器人的实时定位跟踪。
[0068]
上述红外光学三维动作捕捉系统硬件上主要包括红外光学摄像机、交换机、服务器以及用于数据传输的以太网数据线，硬件的连接如图9所示。
[0069]
本实例选用实验场地的大小为长4米*宽4米*高3米，试验场地的布局如图10所示。首先将红外光学摄像机通过固定装置布置在场地上方，确保摄像机视野能够覆盖实验场地的捕捉区域，摄像机中的数据获取模块对智能体位置信息进行采集。将所有摄像机采集到的智能体的位置信息通过以太网数据线汇总到交换机。交换机同样地通过以太网数据线将位置信息传输到服务器中，由服务器中的数据识别模块对位置信息进行识别、封装成单个智能体位置信息。最后，服务器中的数据发送模块获取封装好的单个智能体的位置信息，并将其广播到局域网。
[0070]
具体地，上述数据识别模块为自定义udp服务端，能够以固定的频率，通过udp无线
网络通信将获取到的数据广播到局域网中。
[0071]
s2、控制子系统的搭建
[0072]
为了实现控制操作，需要对控制子系统的控制主机和从机(智能体)进行相应的环境配置。对于本系统，控制主机同时搭载ros/melodic和matlab/simulink环境，控制从机也同样装有ros环境，两者通过相应的网络配置，实现主从模式控制。
[0073]
在控制主机中，自定义了一个udp客户端节点，其主要有两个功能：第一，接收位于室内光学定位子系统服务器中的数据发送模块(udp服务端)传递的智能体位置信息；第二、将上述智能体位置信息打包成ros消息，通过话题将消息(位置信息)发布到ros网络中，供simulink mode节点调用。
[0074]
s3、通信子系统的搭建
[0075]
由路由器建立本系统的局域网，室内光学定位子系统与控制子系统主机通过以太网数据连入局域网，控制子系统从机通过车载蓝牙模块接入局域网，从而实现各系统相互之间的数据发送与接收。
[0076]
s4、算法测试
[0077]
为了测试本发明基于光学室内定位的多智能体协同控制算法验证系统的有效性，本小节将协同控制算法(4)运用本系统当中，通过观察实验实际的效果与理想结果的差异，来评估本发明的性能。
[0078]
s41、参数初始化
[0079]
本实例选用的通信拓扑结构为图11所示的结构，其中中心原点o表示实验场地的中心原地位置，编号1，2
…
9表示本系统选用的9辆多智能体。该拓扑结构所对应邻居矩阵为矩阵a：
[0080][0081]
所有智能体的初始速度值v均设为0，即v＝[1 0 0 0 0 0 0 1 0]
t
。由于场地大小限制，在初始时刻，将9辆智能体摆放在试验场地中的椭圆轨道上，在轨道上的位置是随机的，如图10所示，可以看到，每个智能体初始的车间距是各不相同。
[0082]
同时，将算法(4)期望速度v0设为0.1m/s,期望间距s设为0.53m，故在上述一致性协议的作用下，多智能体系统的理想结果为每个智能体的最终速度值应为0.1m/s,每个智能体的车间距为0.53m。
[0083]
s42、实验运行与结果分析
[0084]
s42-1：启动agent gui节点。
[0085]
s42-2：在通信模块的可编辑文本控件中输入控制子系统主机ip地址，本发明控制
子系统控制主机ip为10.1.1.100。然后单击connect to ros按钮控件，将控制子系统主机设置为ros master，与控制从机建立ros通信连接。
[0086]
s42-3：在参数管理模块中，选择二阶无时滞算法，与一致性算法(4)对应，同时将期望速度参数设置为0.1m/s，期望间距参数设置为0.53m，参数a对应算法(4)中的控制增益α，设为1。由于算法(4)中没有涉及参数b以及与通信相关的参数，将其设置为0。
[0087]
s42-4：参数配置完毕之后，启停控制模块中start按钮控件的enable属性变为on。单击该按钮，agent gui节点会自动的调用算法(4)对应的simulink mode节点，表示实验正式启动。仿真时间设置为无穷，表示手动停止，只需要单击启停控制模块中pause按钮控件即可。
[0088]
s42-5：单击实景显示模块中的display按钮控件，如图5所示，实景显示模块中的9个红色小球为抽象化的智能体，小球的圆心是智能体在实验场地中的位置坐标，图6所示的红色小球位置与图10所示的智能体的初始位置坐标相对应。
[0089]
智能体的速度、加速度和间距信息会在状态监控模块中同时以图形化和具体数值的形式进行显示。
[0090]
s42-6：图12、图13和图14为实验过程中智能体速度、间距和加速度变化曲线图。随着时间的增加，所有智能体的速度、间距与加速度最终趋于一致，并且达到期望速度值0.1m/s和期望间距值0.53m。
[0091]
上述实验结果说明本发明能够有效的对多智能体协同控制算法进行验证，具有操作简单、算法设计方便、参数可调节和效率高等优点。