CN113341956A - 基于改进人工势场法的多智能体主从式编队控制方法 - Google Patents

Abstract

本发明公开一种基于改进人工势场法的多智能体主从式编队控制方法，包括以下步骤：首先基于实际应用场景建立障碍物及机器人物理模型，建立机器人动力学模型；其次，建立目标点对领航机器人的引力势场、障碍物对领航机器人的斥力势场、领航机器人对跟随机器人的引力势场、障碍物对跟随机器人的斥力势场以及跟随机器人之间的斥力势场，基于上述势场对领航机器人和跟随机器人进行受力分析；最后，设置机器人作为领航机器人的优先级，建立跟随机器人队形保持控制器，通过使跟踪误差趋向于0来控制期望队形。采用本方法可实现多机器人编队的智能追踪控制，保证稳定的协作性，在面对复杂应用场景时具有更高的工作效率和更优的工作质量。

Description

基于改进人工势场法的多智能体主从式编队控制方法

技术领域

本申请涉及机器人控制领域，尤其涉及一种基于改进人工势场法的多智能体主从式编队控制方法。

背景技术

近年来，随着传感器网络、机器人理论、控制科学和网络通信技术的不断发展，机器人技术也日趋成熟。由于机器人的运行不受时间空间的限制，且能够代替人类完成各种复杂任务，机器人逐渐被广泛应用于各行各业。从流水线工业机械臂到无人送货小车、从家庭服务机器人到无人驾驶技术，机器人的应用领域正在以极高的速度不断扩大。

应用场景的广泛化与任务的复杂化对移动机器人的性能提出了更高的要求，单个机器人越来越难以满足复杂多变的环境与应用需求。多移动机器人相较于单个机器人而言，能够完成更为复杂的任务，对环境的适应性更高。在执行复杂的任务时，多机器人可以通过相互协作的方式提高系统的工作效率。然而，多机器人控制方法并不是单个机器人控制方法的简单叠加，为避免多机器人系统之间的相互干扰，需对多机器人系统制定合适的控制准则，使多个机器人协同工作。编队作为多机器人协作控制技术中的一种基本控制方法，是指多机器人在满足应用场景环境约束的条件下，保持期望的空间队形，共同完成指定任务。对多移动机器人进行适当的编队，能够在完成某些复杂危险任务(如目标搜寻、环境探查等)时具有更强的信息获取能力与更高的效率。

目前，多机器人编队跟踪控制研究中主要采用主从式编队方法，基于行为的编队方法以及虚拟结构的编队方法等。基于行为的控制方法核心是将多机器人的运动分解为一系列的行为，从而实现类人的决策控制。其缺点在于难以对群体行为进行定量的数学分析，从而无法确定控制系统的稳定性。虚拟结构法是将多个机器人的编队方式看作一个虚拟的刚体，每个个体在虚拟刚体中的位置固定。其控制简单，信息传输量少，但是由于其结构固定，缺乏对环境的适应性，在动态环境中无法进行队形调整，无法保障系统的安全避障。主从式方法是确定一个领航者和多个跟随者，领航者按照传统的单机器人路径规划方法进行控制，跟随着根据与领航者之间的相对位置进行编队控制，可实现灵活避障并根据外部环境改变队形，但是其对系统的领航者依赖性较强，当领航者出现故障时，系统的稳定性无法得到保证，系统无法正常运转。

因此，在上述三种编队跟踪控制方法的基础上，提出一种系统稳定性高、环境适应性强且避障性能良好的编队控制算法具有非常重要的意义。

发明内容

针对目前技术存在的不足，本发明提出一种基于改进人工势场法的多智能体主从式编队控制方法。该方法从单机器人人工势场路径规划方法出发，结合多机器人跟踪控制的主从式编队方法，能够快速、高效地完成多机器人协同追踪控制，降低了系统对领航者的依赖性，提高了机器人的避障性能以及系统的稳定性，为多移动机器人在复杂环境下对目标进行编队跟踪奠定了基础。

实现本发明目的的技术方案为：

一种基于改进人工势场法的多智能体主从式编队控制方法，包括以下步骤：

步骤1：建立障碍物以及机器人的物理模型：应用场景中障碍物的形状通常并非规则形状，在保障系统安全运行的前提下对障碍物进行外包圆建模；将障碍物表面所有点任意两两连接，取其中最长的线作为外包圆的直径，中点为圆心画圆，即能覆盖障碍物的最小圆；障碍物的数量为m，则第i个障碍物圆心为X_bi，i＝1,2,3,…，m,半径为R_bi，i＝1,2,3,…,m；移动机器人的体积同样也不能忽略，按照上述方法同理得到机器人的外包圆，圆心为X_ri，i＝1,2,3,…，n,半径为R_ri，i＝1,2,3,…，n,其中n表示机器人的数量；

步骤2：获取第i个机器人的当前位置表示为X_i＝(x_i,y_i),当前速度表示为V_i＝(v_ix,v_iy),控制量的输入为T_i＝(t_i1,t_i2)；基于此构建单个移动机器人的动力学模型，

步骤3：建立目标点对领航机器人的引力势场以及障碍物对领航机器人的斥力势场，基于上述势场对领航机器人进行受力分析；领航机器人i受到目标点的引力，其引力势场为

X_d为目标点的矢量坐标，k₁为目标点对领航者的引力增益系数，k₁>0；引力为F_1i(X)＝-grad(U_1i(x))＝-k₁(X_i-X_d)；引力的大小和目标点与机器人的距离相关，势能随着距离的增大而增高，引力随之增大；领航机器人i受到障碍物j的斥力，其斥力势场为:

p₁为障碍物对领航机器人的斥力增益系数,f₁(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与领航机器人之间的影响距离,S为设定的斥力影响范围临界值；对应的斥力为

因此领航机器人i的势能函数为

所受到的合力为

其中：U_1i表示领航机器人受目标点的引力势能，U_2ij表示领航机器人i受到障碍物j的斥力势能，F_1i表示领航机器人i受目标点的引力，F_2ij表示领航机器人i受到障碍物j的斥力；

步骤4：建立领航者对跟随机器人的引力势场、障碍物对跟随机器人的斥力势场以及跟随机器人之间的斥力势场，基于上述势场对跟随机器人进行受力分析；跟随机器人i受到领航机器人j的引力，其引力势场为

k₂为领航机器人对跟随机器人的引力增益系数，k₂>0；引力为F_3i(X)＝-grad(U_3i(x))＝-k₂(X_i-X_j)；跟随机器人i受到障碍物j的斥力，其斥力势场为:

p₂为障碍物对跟随机器人的斥力增益系数,f₂(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与跟随机器人之间的影响距离；对应的斥力为

跟随机器人i同样受到来自其余同伴j的斥力，其斥力势场为:

p₃为跟随机器人之间的斥力增益系数，f₃(x)＝||X_i-X_j||²-R_ri-R_rj；对应的斥力为

因此跟随机器人i的势能函数为

所受到的合力为

U_3i表示跟随机器人i受到领航机器人j的引力势能，U_4ij表示跟随机器人i受到障碍物j的斥力势能，U_5ij表示跟随机器人i受到来自其余同伴j的斥力势能；F_3i表示跟随机器人i受到领航机器人j的引力，F_4ij表示跟随机器人i受到障碍物j的斥力，F_5ij表示跟随机器人i受到来自其余同伴j的斥力；

步骤5：设置机器人作为领航机器人的优先级：人为地将所有机器人进行编号，编号数字越小，则作为领航机器人的优先级越高；当前领航机器人出现故障后，将故障信息发送至下一个领航继承机器人，当领航继承机器人收到消息后，正式成为领航机器人；因此每一个机器人都有可能成为领航机器人；根据步骤3中领航机器人的受力分析以及步骤4中跟随机器人的受力分析对机器人总体受力分析进行推广；定义

因此第i个机器人的势能函数为

受到的合力为

步骤6：建立跟随机器人队形保持控制器：跟随机器人i与领航机器人j之间的期望位置差为

两者之间实际位置差为Δx＝X_i-X_j,因此队形误差为E_i＝Δx-Δx^*；通过使跟踪误差E_i趋向于0，可使队形趋向于期望的队形，选择如下控制器对编队进行控制

根据目标点的运动规律建立相应坐标系，得到其位置坐标，将其输入至控制器中，即可实现多机器人的编队追踪控制。

和现有技术相比较，本发明具备如下优点：

本发明综合利用主从式编队控制和人工势场算法实现移动机器人在复杂环境下跟踪特定目标，降低了传统主从式控制方法对领航机器人的依赖性，增强了控制系统的鲁棒性与稳定性，从而为移动机器人在复杂环境下对目标进行跟踪奠定基础。

本发明所述的多机器人路径规划方法可应用在以Nvidia Jetson TK1处理器为核心芯片的移动机器人上，结合加速度传感器、位姿传感器等传感器，实现多机器人路径规划的编队跟踪控制。

附图说明

图1为本发明提出的特征选择方法流程图。

图2为本发明中所选实例实际的特征选择过程。

图3为本发明中所选实例实际的多机器人编队跟踪误差变化。

图4为本发明中所选实例实际的领航机器人发生故障时的多机器人编队跟踪误差变化。

具体实施方式

为了更加清晰的阐述本发明的目的、技术方案及其优点，以下结合多个移动机器人在某处任务场景的编队跟踪控制的实例对本发明进行更详细的说明。

步骤1：建立障碍物、移动机器人的物理模型，如图1所示。采用外包圆建模方法对障碍物、机器人进行建模。本实例中，障碍物的数量为1，则第i个障碍物圆心为X_bi(i＝1),半径为R_bi(i＝1)；移动机器人的数量为4，圆心为X_ri(i＝1,2,3,4),半径为R_ri(i＝1,2,3,4)。本实例中，R_bi＝35cm(i＝1),R_ri＝25cm(i＝1,2,3,4)，所采用移动机器人平台参数信息如下表所示。

表1：移动机器人平台参数

参数名称	字符表示	标称值
			机器人质量	m	3.4kg
轮子半径	R<sub>w</sub>	8.8cm
			外包圆半径	R<sub>ri</sub>	25cm
电机等效电阻	R	2.312Ω
			电机电感	L	0.0075H

步骤2：通过在移动机器人上安装位姿传感器、速度传感器和加速度传感器，获取其运动数据。第i个机器人的当前位置表示为X_i＝(x_i,y_i),当前速度表示为V_i＝(v_ix,v_iy),控制量的输入为T_i＝(t_i1,t_i2)。基于此构建出单个移动机器人的动力学模型，

步骤3：基于人工势场对领航机器人进行受力分析，受力分析图如图2所示，依据矢量合成法则对各个受力进行合成。领航机器人i仅受到目标点的引力和障碍物的斥力，为此建立目标点对领航机器人的引力势场以及障碍物对领航机器人的斥力势场。目标点对领航机器人的引力势场为

X_d为目标点的矢量坐标，k₁为目标点对领航机器人的引力增益系数，k₁>0，对应的引力为F_1i(X)＝-grad(U_1i(x))＝-k₁(X_i-X_d)。障碍物j(j＝1)对领航机器人的斥力势场为:

p₁为障碍物对领航机器人的斥力增益系数,f₁(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与领航机器人之间的影响距离,S为设定的斥力影响范围临界值。对应的斥力为

因此领航机器人i的势能函数为

所受到的合力为

本实例中，引力增益系数k₁＝2，斥力增益系数p₁＝1.5,障碍物与领航机器人之间的影响距离f₁(x)＝‖X_i-X_j‖²-R_ri-R_bj＝||X_i-X_j||²-60，斥力影响范围临界值S＝35cm。

步骤4：基于人工势场对跟随机器人进行受力分析，受力分析图如图2所示，依据矢量合成法则对各个受力进行合成。跟随机器人受到领航机器人的引力以及障碍物和其他跟随机器人的斥力，为此建立领航机器人对跟随机器人的引力势场、障碍物对跟随机器人的斥力势场以及跟随机器人之间的斥力势场。跟随机器人i受到领航机器人j的引力，其引力势场为

k₂为领航机器人对跟随机器人的引力增益系数，k₂>0。引力为F_3i(X)＝-grad(U_3i(x))＝-k₂(X_i-X_j)。跟随机器人i受到障碍物j(j＝1,2,3,…,m)的斥力，其斥力势场为:

p₂为障碍物对跟随机器人的斥力增益系数,f₂(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与跟随机器人之间的影响距离。对应的斥力为

跟随机器人i同样受到来自其余同伴j(j＝1,2,3,…,n-1)的斥力，其斥力势场为:

p₃为跟随机器人之间的斥力增益系数，f₃(x)＝||X_i-X_j||²-R_ri-R_rj表示跟随机器人之间的影响距离。对应的斥力为

因此跟随机器人i的势能函数为

所受到的合力为

本实例中，引力增益系数k₂＝2，障碍物对跟随机器人的斥力增益系数p₂＝1.5,跟随机器人之间的斥力增益系数p₃＝1.5，障碍物与跟随机器人之间的影响距离为f₂(x)＝||X_i-X_j||²-R_ri-R_bj＝||X_i-j_j||²-60，跟随机器人之间的影响距离f₃(x)＝||X_i-X_j||²-R_ri-R_rj＝||X_i-X_j||²-50，斥力影响范围临界值S＝35cm。

步骤5：设置机器人作为领航机器人的优先级。将所有机器人进行编号，编号数字越小，则作为领航机器人的优先级越高，本实例设置编号如表1所示。

表2：机器人优先级编号

当前领航机器人出现故障后，将故障信息发送至下一个领航继承机器人，当领航继承机器人收到消息后，正式成为领航机器人。在此基础上，根据步骤3中领航机器人的受力分析以及步骤4中跟随机器人的受力分析对机器人总体受力分析进行推广。定义

因此第i个机器人的势能函数为

受到的合力为

步骤6：建立跟随机器人队形保持控制器。跟随机器人i与领航机器人j之间的期望位置差为

两者之间实际位置差为Δx＝X_i-X_j,因此队形误差为E_i＝Δx-Δx^*。通过使跟踪误差E_i趋向于0，可使队形趋向于期望的队形，选择如下控制器对编队进行控制

本实例中a_i＝7，b_i＝2。以目标点所在的初始位置作为坐标原点，构建坐标系，本实例中目标点的运动规律为

领航机器人与跟随机器人实时获取目标点的位置，并将其输入控制器之中，通过使跟踪误差逐渐趋于0，即可实现多机器人编队跟踪控制。当未发生故障时，跟踪误差如图3所示，开始运行后，跟踪误差迅速下降，随后机器人编队遇到障碍物，为了避开障碍物，跟踪误差有所增加，避障后，跟踪误差逐渐趋于0。特别的当领航机器人发生故障时，编号最低的跟随机器人会迅速继承领航机器人的职务与功能，并再次向着目标点运动，跟踪误差如图4所示，由于领航机器人的从机器人1切换至机器人2，机器人2的跟踪目标从原领航机器人变为目标点，跟踪误差突然增大，在避障后，带领剩余机器人向目标点移动，跟踪误差逐渐趋于0。

Claims (1)

1.基于改进人工势场法的多智能体主从式编队控制方法，其特征在于：包括如下步骤：

步骤1：建立障碍物以及机器人的物理模型：应用场景中障碍物的形状通常并非规则形状，在保障系统安全运行的前提下对障碍物进行外包圆建模；将障碍物表面所有点任意两两连接，取其中最长的线作为外包圆的直径，中点为圆心画圆，即能覆盖障碍物的最小圆；障碍物的数量为m，则第i个障碍物圆心为X_bi，i＝1，2，3，…，m，半径为R_bi，i＝1，2，3，…，m；移动机器人的体积同样也不能忽略，按照上述方法同理得到机器人的外包圆，圆心为X_ri，i＝1，2，3，…，n，半径为R_ri，i＝1，2，3，…，n，其中n表示机器人的数量；

步骤2：获取第i个机器人的当前位置表示为X_i＝(x_i，y_i)，当前速度表示为V_i＝(v_ix，v_iy)，控制量的输入为T_i＝(t_i1，t_i2)；基于此构建单个移动机器人的动力学模型，

步骤3：建立目标点对领航机器人的引力势场以及障碍物对领航机器人的斥力势场，基于上述势场对领航机器人进行受力分析；领航机器人i受到目标点的引力，其引力势场为

X_d为目标点的矢量坐标，k₁为目标点对领航机器人的引力增益系数，k₁＞0；引力为F_1i(X)＝-grad(U_1i(x))＝-k₁(X_i-X_d)；引力的大小和目标点与机器人的距离相关，势能随着距离的增大而增高，引力随之增大；领航机器人i受到障碍物j的斥力，其斥力势场为：

p₁为障碍物对领航机器人的斥力增益系数，f₁(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与领航机器人之间的影响距离，S为设定的斥力影响范围临界值；对应的斥力为

因此领航机器人i的势能函数为

所受到的合力为

其中：U_1i表示领航机器人受目标点的引力势能，U_2ij表示领航机器人i受到障碍物j的斥力势能，F_1i表示领航机器人i受目标点的引力，F_2ij表示领航机器人i受到障碍物j的斥力；

步骤4：建立领航机器人对跟随机器人的引力势场、障碍物对跟随机器人的斥力势场以及跟随机器人之间的斥力势场，基于上述势场对跟随机器人进行受力分析；跟随机器人i受到领航机器人j的引力，其引力势场为

k₂为领航机器人对跟随机器人的引力增益系数，k₂＞0；引力为F_3i(X)＝-grad(U_3i(x))＝-k₂(X_i-X_j)；跟随机器人i受到障碍物j的斥力，其斥力势场为：

p₂为障碍物对跟随机器人的斥力增益系数，f₂(x)＝||X_i-X_j||²-R_ri-R_bj表示障碍物与跟随机器人之间的影响距离；对应的斥力为

跟随机器人i同样受到来自其余同伴j的斥力，其斥力势场为：

p₃为跟随机器人之间的斥力增益系数，f₃(x)＝||X_i-X_j||²-R_ri-R_rj；对应的斥力为

因此跟随机器人i的势能函数为

所受到的合力为

U_3i表示跟随机器人i受到领航机器人j的引力势能，U_4ij表示跟随机器人i受到障碍物j的斥力势能，U_5ij表示跟随机器人i受到来自其余同伴j的斥力势能；F_3i表示跟随机器人i受到领航机器人j的引力，F_4ij表示跟随机器人i受到障碍物j的斥力，F_5ij表示跟随机器人i受到来自其余同伴j的斥力；

步骤5：设置机器人作为领航机器人的优先级：人为地将所有机器人进行编号，编号数字越小，则作为领航机器人的优先级越高；当前领航机器人出现故障后，将故障信息发送至下一个领航继承机器人，当领航继承机器人收到消息后，正式成为领航机器人；因此每一个机器人都有可能成为领航机器人；根据步骤3中领航机器人的受力分析以及步骤4中跟随机器人的受力分析对机器人总体受力分析进行推广；定义

因此第i个机器人的势能函数为

受到的合力为

步骤6：建立跟随机器人队形保持控制器：跟随机器人i与领航机器人j之间的期望位置差为

两者之间实际位置差为Δx＝X_i-X_j，因此队形误差为E_i＝Δx-Δx^*；通过使跟踪误差E_i趋向于0，可使队形趋向于期望的队形，选择如下控制器对编队进行控制

根据目标点的运动规律建立相应坐标系，得到其位置坐标，将其输入至控制器中，即实现多机器人的编队追踪控制。

Images