一种移动机器人编队与避障控制方法
技术领域
本发明属于机器人技术领域，具体涉及一种移动机器人编队与避障控制方法。
背景技术
随着移动机器人技术的不断突破，单个机器人进行避障的技术方面已经日趋成熟，但面对复杂的任务不能高效的完成，而多移动机器人协作可以高效的完成任务而且节约成本，在完成任务的过程中难免会遇到障碍物，编队避障一直以来都是机器人研究领域的难点，也是需要一直深入研究的问题。
目前对于移动机器人躲避静态避障物的方法有如下两种方案，一种是编队整体进行避障，采用的控制方法有：人工势场法、领航-跟随编队控制方法、基于行为法等，另一种是编队拆分避障，采用的控制方法有：栅格法、可视图法、模糊逻辑法，遗传算法等。编队整体避障相对于拆分避障，更具有稳定性、整体性和实用性，可以有效避免拆分避障过程出现机器人发生碰撞，更高效的通过障碍物。中国专利《一种适用于全向轮移动机器人的人工势场避障方法》(申请日：2018.05.31；申请号：CN201810550180.9；公开日：2018.10.02；专利号：G05D1/02)》人工势场法存在局部最小值的问题，会导致编队避障失败。中国专利《一种多移动机器人系统半自主编队及避障控制方法》(申请日：2019.10.14；申请号：CN110703751A；公开日：2020.01.17》基于虚拟结构的整体编队结构难以改变，导致编队避障轨迹不平滑且稳定性低。
发明内容
针对编队整体避障中的跟踪曲线不平滑和稳定性低的问题，本发明提出了一种移动机器人编队与避障控制方法。在多机器人编队系统中引入旋转势场和虚拟刚体的概念，建立基于分散虚拟结构的移动机器人编队模型，设计旋转势场避障方法。使机器人群完成编队通过障碍物的同时，面对机器人增加或减少的情况仍然能够完成任务，提高控制系统的容错性。
本发明的目的是提出一种基于分散虚拟结构的移动机器人编队控制技术和基于旋转势场的编队整体避障策略，各移动机器人都由一个相同电荷来进行建模，所有移动机器人被安置在一个半径为预定半径的圆上，形成一个正多边形编队，每个移动机器人都能在编队中自主找到自己的位置。假设圆中心有一个虚拟机器人，以虚拟机器人为编队中心，实现正多边形编队控制。假设编队整体处于旋转势场中，通过引入旋转势场，合成矢量的方向被调整到接近移动机器人的方向，使移动机器人不会停留在局部最小值，从而顺利避开障碍物。
一种移动机器人编队与避障控制方法，包括以下步骤：
(1)建立每个移动机器人的动力学方程；
(2)定义一个圆形的期望轨迹，用步骤(1)动力学方程表示的移动机器人的轨迹都收敛在圆上，形成一个圆形编队；
(3)每个移动机器人都用相同电荷来建模，通过步骤(2)形成的圆形编队，输入相应的控制力与圆相切的斥力的合力为零，可以得到圆上移动机器人的正多边形编队；
(4)在步骤(3)形成正多边形编队的基础上，在正多边形中心建立一个虚拟机器人，圆上的移动机器人会跟随虚拟移动机器人轨迹进行编队运动；
(5)假设基于虚拟结构控制的编队整体处于一个旋转势场，移动机器人编队在旋转势场中不会停留在局部最小位置，能够顺利避开障碍物到达目标点。
具体的各个步骤技术方案为：
步骤(1)建立每个移动机器人的动力学方程，包括以下过程：
假设有n个无运动约束的全驱动移动机器人，第i个机器人运动学方程为：
其中(xi,yi)是移动机器人的位置坐标，M是机器人质量，B为阻力系数，fi＝(fxi,fyi)是控制力，id是控制移动机器人瞬态相应系数。
步骤(2)中，引入期望轨迹，形成一个圆形编队，定义在(1)控制下的移动机器人强制向半径为α和中心(xc,yc)的圆移动，当移动机器人到达圆上时，移动机器人将停止移动。
期望轨迹方程为：
把r2＝(x-xc)2+(y-yc)2,
由(3)可知为了让轨迹收敛到圆上，定义e＝r-α，让r收敛到α。定义一个Lyapanov函数
V(e)＝0的解是e＝0，并且θ＝0，当移动机器人向圆移动时，轨迹会收敛到圆上，不会发生旋转运动，形成圆形编队。
步骤(3)，将移动机器人形成圆形编队的情况下，排列成正多边形编队。移动机器人都由一个相同电荷来建模，定义一个力(5)将移动机器人固定在半径为α和中心(xc,yc)的圆上，当定义的力大于圆半径方向上的斥力的合力时，因为电荷是相同的，如果移动机器人之间的距离是相同的，移动机器人会达到平衡点，这时控制力与圆相切的斥力的合力为零，可以得到圆上移动机器人的正多边形编队。
定义外力为：
定义两个电荷之间的斥力为：
Fki是第k和第i个电荷之间的力，kr是正常数系数，rki是两电荷之间距离。因此，N-1电荷对第k电荷施加的力如下：
其中fi为机器人受到的吸引力，Fi为机器人受到的排斥力。
步骤(4)，在正多边形编队中心建立虚拟移动机器人，在圆上的移动机器人会跟踪虚拟机器人轨迹来进行调整。虚拟移动机器人的动力学方程为：
其中fxv,fyv为虚拟力。
在虚拟结构中第k个机器人的虚拟力：
基于排斥效应可以得到正多边形编队，不需要直接控制移动机器人之间的距离，传统的虚拟结构需要一直保持同一个刚性结构，缺乏灵活性和适应性，尤其在躲避障碍物过程中存在一定的局限性。该方法下的虚拟结构可以根据移动机器人的数量进行队形变化。
步骤(5)，移动机器人编队整体处于旋转势场，势场矢量围绕障碍物旋转，障碍物的方向取决于移动机器人的方向。移动机器人从左侧接近障碍物的情况下，障碍物周围的旋转电势场是逆时针的，因此势能场不会与机器人的运动方向相反，移动机器人的合力也不会为零，所以，移动机器人不会停留在局部最小位置。视障碍物为一个矩形，机器人可以通过传感器来获得障碍物的中心和尺寸。考虑障碍物周围的椭圆，椭圆至少具有通过矩形顶点的最小面积，定义旋转势场矢量位于该椭圆平行的椭圆上。势场的方向视根据第k个接近移动机器人的方向来确定的，当移动机器人接近障碍物时，应将方向
定义最小面积椭圆方程：
其中(x0,y0)为障碍物的中心点。考虑一个椭圆方程：A2(x-x0)2+B2(y-y0)2＝1，椭圆面积与
本发明主要用电荷代表移动机器人进行建模，通过排斥效应使移动机器人形成正多边形编队队形，引入旋转势场，使机器人的合成矢量不会与移动机器人的运动方向相反，确保编队整体不会停留在局部最小值，能够顺利完成避障。
附图说明
图1为本发明第i个移动机器人的受力图；
图2为本发明机器人移动的旋转势场示意图；
图3为本发明的流程示意图。
图4为实施例正三角形编队示意图。
具体实施方式
结合实施例说明本发明的具体技术方案。
用三个移动机器人来验证此方法：
如图3所示，一种移动机器人编队与避障控制方法，包括以下步骤：
步骤(1)建立每个移动机器人的动力学方程，包括以下过程：
有3个无运动约束的全驱动移动机器人，第i个机器人运动学方程为：
其中(xi,yi)是移动机器人的位置坐标，M是机器人质量，B为阻力系数，fi＝(fxi,fyi)是控制力，id是控制移动机器人瞬态相应系数。
步骤(2)中，引入期望轨迹，形成一个圆形编队，定义在(1)控制下的移动机器人强制向半径为α和中心(xc,yc)的圆移动，当3个移动机器人到达圆上时，移动机器人将停止移动。期望轨迹方程为：
把r2＝(x-xc)2+(y-yc)2,
由(3)可知为了让轨迹收敛到圆上，定义e＝r-α，让r收敛到α。定义一个Lyapanov函数
V(e)＝0的解是e＝0，并且θ＝0，三个移动机器人轨迹会收敛到圆上，形成圆形编队。
步骤(3)，将3个移动机器人形成圆形编队的情况下，排列成正三角形编队。移动机器人都由一个相同电荷来建模，定义一个力(5)将3个移动机器人固定在半径为α和中心(xc,yc)的圆上，当定义的力大于圆半径方向上的斥力的合力时，因为电荷是相同的，如果移动机器人之间的距离是相同的，移动机器人会达到平衡点，这时控制力与圆相切的斥力的合力为零，可以得到圆上移动机器人的正三角形编队。如图4所示
定义外力为：
定义两个电荷之间的斥力为：
Fki是第k和第i个电荷之间的力，kr是正常数系数，rki是两电荷之间距离。因此，N-1电荷对第k电荷施加的力如下：
圆上移动机器人的受力示意图1。
其中fi为机器人受到的吸引力，Fi为机器人受到的排斥力。
步骤(4)，在正三角形编队中心建立虚拟移动机器人，在圆上的移动机器人会跟踪虚拟机器人轨迹来进行调整。虚拟移动机器人的动力学方程为：
其中fxv,fyv为虚拟力。
在虚拟结构中第k个机器人的虚拟力：
基于排斥效应可以得到正三角形编队，此时为一个刚性结构，根据三个移动机器人之间的距离保持队形稳定。
步骤(5)，正三角形移动机器人编队整体处于旋转势场，如图2，移动机器人编队从左侧接近障碍物的情况下，障碍物周围的旋转电势场是逆时针的，因此势能场不会与机器人的运动方向相反，移动机器人的合力也不会为零，所以，移动机器人不会停留在局部最小位置。由三个移动机器人组成的正三角形编队通过旋转势场的排斥力和虚拟力的共同作用下可以顺利避开障碍物。势场的方向视根据第k个接近移动机器人的方向来确定的，当移动机器人接近障碍物时，应将方向
定义最小面积椭圆方程：
其中(x0,y0)为障碍物的中心点。考虑一个椭圆方程：A2(x-x0)2+B2(y-y0)2＝1，椭圆面积与
