人工势场法改进领航跟随法的控制算法实现

Realization of Control Algorithm for Improved Pilot Following Method Based on Artificial Potential Field Method

摘要: 随着科技的发展，机器人发展日新月异，在当今时代，在工业、物流、医疗以及服务领域，都能看到机器人的身影，因为单个机器人功能的局限性，多机器人系统开始发展，多机器人编队避障问题成为研究热点。采用的研究方法主要是是实验法和文献研究法，主要通过查阅文献来获得思路，然后在MATLAB中仿真得到实验结果。研究的主要内容有人工势场法、领航跟随法、人工势场法改进领航跟随法实现过程中遇到的问题、在静态环境中运用人工势场法和领航跟随法使多个机器人协同合作完成编队和避障。实验结果在MATLAB仿真中实现，利用栅格法，对障碍物进行膨化处理，4个机器人从初始位置，初始队形，到达目标位置，在行进过程中遇到障碍物，遇到障碍物时队形发生变换，躲避障碍物后，队形再一次改变，整个过程中，领航者运动轨迹和跟随者不同，在领航者到达位置后，跟随者根据情况改变位置信息从而达到目标位置。

Abstract: With the development of science and technology, robots are developing rapidly. In today’s era, in the fields of industry, logistics, medical treatment and service, robots can be seen. Because of the limitations of single robot function, multi robot system begins to develop, and multi robot formation obstacle avoidance has become a research hotspot. The main research methods used are experi-ment method and literature research method, mainly through consulting literature to get ideas, and then in MATLAB simulation to get the experimental results. The main contents of the research include artificial potential field method, pilot following method, artificial potential field method to improve the problems encountered in the implementation process of pilot following method, and make multiple robots cooperate to complete formation and obstacle avoidance in static environ-ment by using artificial potential field method and pilot following method. The experimental results are realized in MATLAB simulation. The grid method is used to expand the obstacles. The four ro-bots move from the initial position, initial formation, to the target position. When they encounter obstacles, the formation changes when they encounter obstacles. After avoiding obstacles, the for-mation changes again. In the whole process, the movement track of the leader is different from that of the follower. After arriving at the location, the follower changes the location information to reach the target position.

1. 引言

1.1. 研究背景

自从人工智能的概念被提及，越来越多的人开始瞄向机器人领域，机器人领域的发展了让大众更能了解人工智能，上世纪七十年代，有人提出编队控制的内容，使多个机器人通过编队来实现各种工作，多机器人系统的提出更是推动了机器人领域的进步，其中，编队避障是多移动机器人所面临的最大的问题，常用的编队避障算法有领航跟随法 [1]、人工势场法 [2]、基于行为法 [3] 以及虚拟结构法 [4] 等。其中人工势场法因实时性强、突防突发威胁能力强、局部处理能力强等较为常用于避障处理，而领航跟随法因领航者作为控制整个智能体系统运动的主导，仅凭给定的运行轨迹便可以控制整个智能体群组的行为，从而大大简化了控制作用。

1.2. 国内外研究现状

经过二十几年的发展，多机器人系统的研究已在理论和实践方面取得很大进展，并建立了一些多机器人的仿真系统和实验系统。目前，国内的多机器人系统的研究已经有了进展，但国外的研究比较活跃,国外学者们对于多机器人的研究较早，技术也比较成熟，日本对于机器人编队的研究 [5] 主要是利用在大型足球比赛中，这一比赛项目的大大的促进了多机器人编队系统的研究，美国科研人员研究出来了呈分布式结构的SWARM系统，把原本具有自主能力的机器人降低自身的智能型，按照功能分配到整个编队中，使整个编队呈现出群体智能型。欧盟设立专门进行多机器人系统研究的做RTHA课题——“用于搬运的多自主机器人系统”，美国海军研究部和能源部也对多机器人系统的研究进行了资助。

国内在多机器人领域研究较晚，但我国对于机器人研究给予高度重视，投入了大量的人力、物力财力，加快了研究步伐，目前已经有了很大进展，刘瑞轩、张永林等人 [6] 将领航跟随与基于行为法结合，使用粒子群算法优化于各行为的适应度函数；郑新娟 [7] 结合改进的路径规划算法，对多机器人系统的避障和队形变换等问题进行研究，设计了由基于ARM嵌入式系统开发的履带移动机器人、VS2010设计的上位机监控软件及Zigbee无线通信模块组成实验平台系统。柴锐之 [8] 建立了多机器人队形的表征空间模型，设计了实现这个模型的分层结构，并分析了在这个模型下编队任务的约束；吴军成、肖宇峰 [9] 等人设计算法通过REYNOLDS类鸟群模拟方法设计了“分离”、“对齐”、“队形”三规则的控制器来实现多机器人任意队形保持控制，并增加“切换”规则来实现多机器人队形变换等等，还有更多学者正在研究阶段。

通过国内外多机器人发展状况的分析，我们可以得出，我国在多机器人研究方面还有很多不足，无论是编队队形的研究还是机器人系统都存在很大差距，在路径规划方面，实施环境监测以及多机器人协调合作的问题较为突出。

1.3. 研究的目的和意义

近来关于移动机器人发展有了很大的进步，移动机器人以环境感知、动态决策与规划、行为控制等的功能受到很多领域研究人员的青睐，在工业中常用多个机器人来搬运货物，在物流中用来运送货物，在生产车间做一些简单机械化的工作，提升生产力等。

单个机器人的性能和工作效率在近年来有很大的提升，但多个机器人协同工作要比单个机器人独立工作更强大，单个机器人所执行任务和能力有限，多机器人之间相互合作能够提升系统的冗余度和容错性，提高执行任务的鲁棒性，如今的多机器人主要在无人机和移动机器人、水下机器人方面发展较为迅速。在执行任务时，多机器人可以根据情况变换队形，提升了执行任务的效率。研究多移动机器人编队和避障问题早已成为热点，本课题所研究的人工势场法改进领航跟随法的控制算法为多移动机器人编队和避障的研究奠定基础，为实现多移动机器人协同合作完成任务方向发展。

2. 人工势场法改进的领航跟随算法

2.1. 人工势场法

人工势场法路径规划的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。

常用引力势场函数：在混合势场中，目标点对机器人产生引力，人工势场法所用到引力势场函数为：

$U_{att}\left(q\right)=\frac{1}{2}\times k_{att}\times {\left(q-q_g\right)}^2$ (1)

其中 $U_{att}\left(q\right)$ 为引力场，q为当前机器人， $k_{att}$ 为引力增量函数，其值大于零； $q_g$ 为机器人与目标点的相对距离，下标g表示目标点；引力 $F_{att}\left(q\right)$ 是由引力势场函数的负梯度所得，其大小计算如式：

$F_{att}\left(q\right)=-\nabla U_{att}\left(q\right)=-k_{att}\left|q-q_g\right|$ (2)

常用斥力函数：在混合势场中，障碍物对机器人产生斥力，人工势场法中斥力势场函数公式为：

$U_{rep}\left(q\right)=\{\begin{array}{l}\frac{1}{2}{k}_{rep}{\left(\frac{1}{q-q_0}-\frac{1}{{\rho }_0}\right)}^2,q-q_0\le {\rho }_0\\ 0,q-q_0>{\rho }_0\end{array}$ (3)

其中 $q-q_0$ 表示机器人与障碍物的距离， ${\rho }_0$ 为障碍物的影响距离， $k_{rep}$ 斥力增量系数。斥力 $F_{att}\left(q\right)$ 是由斥力势场函数的负梯度所得，其大小的计算公式为：

$F_{att}\left(q\right)=-\nabla U_{att}\left(q\right)=\{\begin{array}{l}{k}_{rep}\left(\frac{1}{q-q_0}-\frac{1}{{\rho }_0}\right)\frac{1}{q-q_0}\frac{\partial \left(q-q_0\right)}{\partial q},q-q_0\le {\rho }_0\\ 0,q-q_0>{\rho }_0\end{array}$ (4)

则在混合势场中，机器人所受合力公式为：

$F\left(q\right)=F_{att}\left(q\right)+{\displaystyle \sum F_{att}\left(q\right)}$ (5)

其中 ${\displaystyle \sum F_{att}\left(q\right)}$ 表示所有障碍物对机器人q产生的斥力和，领航者与跟随者模型如图1所示。

Figure 1. Motion model of leader and follower

图1. 领航者与跟随者模型

2.2. 领航跟随法

Leader-Follower法其基本思想是：在由多智能体组成的群组中，其中某个智能体被指定为领航者，其余的智能体被指定为跟踪领航者运动的跟随者，跟随者以设定的距离或速度等参量跟踪领航智能体的位置和方向。进一步来讲，对同一个多智能体系统，领航者可以仅仅指定一个，也可以存在多个，但控制群组编队形状的领航者只能有一个。通过设定领航智能体与跟随智能体间不同的位置关系，便可得到不同的网络拓扑结构，即不同的编队队形。该方法的突出特点在于，智能体群组成员间的协作作用是通过对领航智能体状态信息的共享来实现的。

2.3. 算法实现

移动：初始机器人坐标：(r_x, r_y)；目标坐标：(r_px, r_py)；设置4个角度范围：(−Π/4，Π/4)；(Π/4, 3Π/4)；(3Π/4, 5Π/4)；(5Π/4, 7Π/4)；判断角度值范围，改变x、y轴坐标，到达目标位置；机器人运动方式为先确定方向向量，判断角度值范围，然后通过改变坐标位置，到达指定位置点。

编队：确定领航者以及领航者位置：(leader_x, leader_y)；领航者到达目标位置：(r_px1, r_py1)；定义在领队到达指定位置时，队形如何变换，确定每个机器人应该移动到队形中的位置，在遇到障碍物时，更新坐标，判断机器人是否到达指定位置。

路径规划：初始化机器人的参数：起点位置、引力增益函数、斥力增益函数、障碍物影响距离、障碍物个数、障碍物影响距离以及步长；给出障碍物信息以及目标位置；设置数组，数组中包含障碍物位置信息以及目标位置；运用角度函数计算出机器人和障碍物和目标之间与x轴的夹角，运用引力模块计算出目标对车的引力在x，y轴中的分量，运用斥力模块计算出斥力在x，y轴中的分量计算车的下一步位置，判断是否到达目标位置。

2.4. 建立编队队形

在实际生活中，经常见到线性队形(如图2所示)，给人一种整洁的的视觉需求，运用在编队中更易于控制，0号为领航者，1、2、3为跟随者；排成一排呈现“一”字，各个机器人之间更加密切，在变换队形时更加方便，快捷。

Figure 2. Linear formation

图2. 线性队形

在面对变换队形使用菱形队形(如图3所示)有更高的自由空间，各个机器人之间灵活度较高，队员之间的妨碍更小化，0号作为领航者，1、2、3号作为跟随者，在遇到障碍物时，0号按照轨迹行驶，队伍中的队员变换队形躲避障碍物，躲避障碍物后回归线性队形。

Figure 3. Diamond formation

图3. 菱形队形

2.5. 领航者与跟随者人工势场设计

机器人编队中，领航者在势场环境中会受到引力和斥力的作用，在两种里的共同作用下，领航者沿着合力的方向运动。编队中跟随者机器人所受到的力主要来自于三个方向：领航者对跟随者的引力作用，障碍物对跟随者机器人斥力的作用，跟随者受到其他跟随者势力场的作用。

2.6. 情况概述

1) 无障碍物

在编队行进过程中，要保持队伍之间不发生碰撞，必须保持各个机器人之间间隔一定的距离，即各个机器人的位置坐标不能重合，在行进过程中，一直保持线性队形按照轨迹运动，用L1来表示各个机器人之间的间隔距离，情况如下：当机器人间隔大于等于L1时，无碰撞；当机器人间隔小于L1时，发生碰撞；

2) 有障碍物时

在队伍行进过程中，遇到障碍物时，领航者更新坐标，根据轨迹继续行驶，跟随者更新位置坐标，进行队形变换，到达目标位置，在经过障碍物后，回归原始队形，再次保持线性队形行进。在进行队形变换时，各个队员之间间隔发生变化，各个机器人与障碍物之间也存在一定间隔，用L2来表示各个机器人与障碍物的间隔距离，情况如下：当机器人与障碍物之间距离大于等于L2，无碰撞；当机器人与障碍物之间距离小于L2时，发生碰撞。

2.7. 流程图

流程图如图4所示。

Figure 4. Flow chart

图4. 流程图

2.8. 多机器人控制结构

分散式控制(如图5所示)指的是编队中只需要保持自己在编队中的相对位置关系，不需要与别机进行传递与交互，这种控制方法大大解决了大量计算问题与数据交互。

Figure 5. Decentralized structure

图5. 分散式结构

3. 仿真

利用栅格图来搭建模拟环境 [10]，设置仿真模拟的参数，设置栅格图长宽以及每一小格的大小，确定机器人初始位置左边及目的地的坐标，模拟环境矩阵，设置元素值，障碍物以及机器人个数，确定机器人坐标，确定领航者，在不同环境下队形设置，如果障碍物在一定范围内，机器人呈菱形队形，如果探测距离内无障碍物，机器人处于线性队形，在变换队形时领航者到达目的地，其他队员排好队依次移动到达目的地，最后创建图像。

根据上述参数设置和编队建模，利用MATLAB工具搭建50 × 50的正方形的仿真环境，目的地坐标(45, 30)，领航者初始位置坐标(11, 25)，跟随者1号坐标为(5, 25)，跟随者2号坐标为(8, 27)，跟随者3号坐标为(8, 23)。整个编队初始呈现为菱形结构(如图6所示)。

Figure 6. Initial formation

图6. 初始队形

在势场力的作用下，使机器人在期望角度方向行进，在障碍物斥力和领航者引力的作用下，遇到障碍物时通过下移、上移等的动作躲避障碍物，当障碍物较少时，其运动轨迹为(如图7所示)。

在障碍物较少时，机器人对障碍物躲避情况不明显，增加障碍物个数，由图8可以看出，遇到障碍物时，机器人通过下移以及右移等的一系列轨迹改变较为平滑的躲过障碍物，其运动轨迹为(如图8所示)。

Figure 7. Motion trajectory with few obstacles

图7. 障碍物较少时运动轨迹

Figure 8. Trajectory of obstacle increase

图8. 障碍物增加运动轨迹

在遇到到障碍物时，队形改变，由菱形变为线性(如图9所示)。避过障碍物后，回归菱形(如图10所示)。在即将到达目标位置时，队形变为线性(如图11所示)。

Figure 9. Change formation in case of obstacles

图9. 遇障碍物改变队形

Figure 10. Formation after obstacle avoidance

图10. 避过障碍物后队形

Figure 11. Formation before reaching target position

图11. 到达目标位置前队形

在整个过程中，多移动机器人编队队形进行多次改变，采用分布式控制系统，对遇到变换队形时更新信息有较大的帮助，每次改变队形更新一次坐标信息，在模拟情况中，对障碍物膨化处理，利用网格可以更清楚的看出队伍行进过程中的变化，运动轨迹看起开更加清晰。

4. 总结

本文对人工势场法改进的领航跟随法内容展开，文章简述了人工势场法和领航跟随法的内容，结合两种算法，对机器人编队及避障的内容加以深入，从整合材料理论知识到实践，初步建立编队队形，确定队形呈线性到躲避障碍物变换队形，到达目标位置，两种算法的结合使得机器人在编队方面可以更加灵活，执行任务时有更高的效率。本文工作总结如下：

1) 基于领航跟随法控制策略，加入路径规划的编队总体架构，实现分散式编队的整体队形，领航者和跟随者间通过建立期望角度和距离，实现队形变换及躲避障碍物；

2) 建立编队队形，初始位置队形保持线性，行进过程遇障碍时变换菱形，不同情况下队形不同，势场力的作用下各个机器人到达队形中指定位置；

3) 不同情况下多机器人队形情况差异，多传感器分布式的控制结构中，机器人和传感器一对一可以更好地更新数据从而达到机器人到达目标位置更精确的目的；

4) 搭建仿真环境，通过对编队算法的仿真验证了算法在不同情况下的执行力；

本文建立的编队策略是在障碍物处于静态环境中的一种方式，还存在一些不足之处，对未来发展中还有很大的空间，在编队避障算法的研究由更大的展望，本文对未来可规划发展如下：

1) 多移动机器人面对不同环境。本文只对障碍物为静态环境下避障编队进行研究，对于动态环境下机器人编队及避障问题，需要考虑队伍行进过程中遇到障碍时躲避角度以及方向问题；在动态环境中突发情况时，机器人采用立刻停止命令或者固定方向移动躲避；

2) 多移动机器人队员间问题。在躲避障碍物时，队伍中的成员是否作为障碍物来看待，在队员突然遇到突发情况时，空余的位置补缺问题；

3) 增加机器人时整体结构不同。在增加更多机器人时，编队结构，控制队形以及多个领航者共同控制，在大型多个机器人控制时与少量机器人控制的不同；

4) 多个算法综合实现。本文结合了两种算法实现，为减小误差以及机器人到达位置更精确，可以考虑多种算法混合结合使用，提出一种更加高效率、更加实用性的策略方法。

基金项目

2020年度河南省高等学校重点科研项目20B120003，2020年度郑州科技学院大学生创新创业训练计划项目DC202021。