基于距离-角度优先级的多移动机器人编队避障方法与流程

本发明属于机器人技术领域，具体涉及一种基于距离-角度优先级的多移动机器人编队避障方法略。

背景技术：

目前，多移动机器人广泛应用在生产生活中，如变电站巡检、无人值守仓库、自动化工厂货物运输、搬运等，通过机器人之间的协作，有效节约投资成本，提高工作效率。在多机协调完成任务过程中，通常会遇到静态或动态障碍物，所以有效避障是必要的，否则会造成任务失败。目前常用的避障方法有两种，一种为编队拆分避障，如栅格法、可视图法、模糊逻辑法、rtt算法、遗传算法等，另一种为编队整体避障，如人工势场法、模型预测控制法、固定队形变换法和基于行为法等。编队整体避障能使编队内部维持原有控制，通过动态调节队形进行在线避障，相比于拆分避障，更为具有灵活性、整体性和可靠性，可以有效避免拆分避障出现的跟随者掉队、机器人之间的碰撞问题，但多机之间若没有进行有效的协调和沟通，将不能发挥出多机协同避障的优势，造成严重的经济损失。为此，中国专利《一种面向多移动机器人系统的模糊编队及避障控制方法》(申请日：2013.09.06；申请号：201310402941.3；公开日：2013.12.18；专利号：cn103455033a)公开一种基于模糊控制的以领航机器人广播的形式告知所有跟随机器人避障，跟随者根据存储的不同队形信息以及领航机器人广播的信息切换队形的方法，但模糊规则繁杂且属人为规定，控制精度难以保证，且没有考虑避障后队形恢复和机器人之间在避障过程中碰撞问题。中国专利《一种多无人机编队飞行避障控制策略制定方法》(申请日：2018.10.29；申请号：201811272488.8；公开日：2019.03.29；专利号：cn109542115a)公开一种基于斥力场函数的编队避障方法以及避障后路径重规划策略，保证在多机编队执行任务中，实现机内实时避障，但斥力场越大，无人机离开触发区的响应速度和运动速度越快，队形变换超调量较大，控制系统稳定性降低。

针对机器人避障过程中机器人之间碰撞、队形变换超调量大的问题，本发明提出一种距离-角度优先级避障策略。充分考虑机器人的自身安全距离和通讯范围，设计基于距离和角度优先级避障策略，并设计运动控制器，使多个机器人安全稳定地完成避障的同时提高避障效率，使避障路径最短。

技术实现要素：

本发明的目的是针对二维平面提供一种基于距离-角度优先级的多移动机器人编队避障方法，使多个机器人在复杂环境中能够在线有效避障，同时提高工作效率，使机器人编队整体避障总的运动路径最短。

为减小跟随者对领航者的过度依赖，同时避免链式结构造成的跟踪误差累积问题，采用虚拟领航-跟随结构。通过建立队形数据库，由机器人安全距离设计距离-角度优先级策略，对避障避碰进行规划，并根据领航者与跟随者相对位姿误差状态方程设计运动控制器，使机器人群完成避障后恢复初始队形继续稳定运动。

基于距离-角度优先级的多移动机器人编队避障方法，包括以下步骤：

(1)根据机器人数量建立队形数据库；

(2)形成距离-角度优先级避障策略；对单双侧障碍物与队形的距离进行分析，判断是否需要避障，若需要则根据距离-角度优先级判断以何种队形避障，并调用步骤(1)中的队形数据库进行避障准备；

(3)设计运动控制器，根据步骤(2)中决策的避障队形开始避障；

(4)避障结束后即最后一个机器人通过障碍物，快速恢复队形。

步骤(1)所述的建立队形数据库，设有i个机器人参与编队，虚拟领航者表示为l，其位置坐标表示为[xlylθl]^t,跟随者表示为fi,根据机器人数量设定相应的队形数据库。

步骤(2)中形成距离-角度优先级避障策略的方法为：设机器人自身安全距离为半径为rs，则两个机器人并排运动的最小安全距离为4rs，设有3个机器人初始状态为三角形队形沿水平方向前进；

a单侧障碍物避障策略

当虚拟领航机器人检测到前方为单侧障碍物时：

(a1)判断是否需要避障，若d0≤asinα+rs，则需要避障，若d0＞asinα+rs，则不需要避障；

(a2)判断障碍物方向，位于前进方向的上侧或下侧，若为上侧，则机器人系统保持初始队形右拐前进进行避障，若为下侧，则机器人系统保持初始队形左拐前进进行避障；

(a3)当编队整体通过障碍物后队形收敛到原轨迹水平线继续前行；

b双侧障碍物避障策略

当虚拟领航机器人检测到前方为双侧障碍物时，

(b1)首先判断是否需要避障，若d0≤dl，则需要避障，若d0＞dl，则不需要避障，继续保持原队形运动，其中dl＝2asinα+2rs；

(b2)若需要避障，则判断以哪种队形避障；

若d0＜2rs，则按单侧避障方案绕行避障；

若2rs＜d0＜4rs，则需变换一字队形通过障碍物；

若d0＞4rs时，则无需改变队形，只改变角度α，成树杈状三角形队形通过；即a值不变，减小α.若障碍物沿机器人运动方向的沿面障碍距离较小，为缩短避障时间，则队形成比例缩小通过障碍物；

通过对一般环境中可能出现的单双侧障碍物，通过机器人自身安全距离及队形整体与障碍物之间距离计算，得出距离-角度优先级，即先判断是否需要避障，其次优先考虑距离判断是否需要改变队形，最后决定是否需要改变角度以何种队形通过；根据以上策略分析，得出最短路径即消耗最少的方案；

步骤(3)运动控制器设计，包括：

设虚拟领航者的位姿为qd＝[xdydθd]，ud＝[vdwd]，跟随机器人的位姿为qi＝[xiyiθi]，ui＝[viwi]，跟随机器人i与虚拟领航者的相对位姿方程：

其中，δx＝xd-xi，δy＝yd-yi；

机器人的运动学方程：

其中x，y，θ分别为二维坐标系位置坐标及航向角，分别为x，y，θ的导数。

由反步法对线速度v和角速度w设计控制率得：

v＝vdcos(pθid)+k1pθid(4)

w＝wd+k2vdpyid+k3vrsin(pθid)(5)

其中k1，k2，k3为大于0的常数，vd，wd分别为理想线速度和角速度；

通过对跟随者与虚拟领航者的相对位姿分析，结合运动学方程，得到线速度和角速度控制率，通过控制率调节机器人实际位姿，使其无限逼近理想位姿，实现机器人高精度编队。

步骤(4)队形快速恢复算法，包括：

为提高多机器人的工作效率，当机器人编队整体通过障碍物后需及时恢复队形，沿原轨迹继续前行，设避障后虚拟领航者位姿为p(t)＝[xlylθl]^t，设队形快速恢复函数为：

p(t)＝(p0-p∞)e^-μl+p∞(1)

其中,m>0,p0为的初始值即避障后虚拟领航者位置状态量，p∞为p(t)的稳态值即恢复队形后的位置状态量,p(t)按指数快速递减到p∞的值，可实现队形以指数形式快速恢复队形。

本发明针对机器人编队避障效率低、队形超调大，控制精度低的问题，通过建立队形数据库，对单侧障碍物和双侧障碍物与机器人队形之间的距离进行分析，得到最短路径避障策略，最后根据跟随者与虚拟领航者的相对位姿及运动学方程，利用反步法设计运动控制率，使机器人完成稳定编队的同时安全高效的通过障碍物并迅速恢复队形。

附图说明

图1为实施例一字型编队位置示意图；

图2为实施例三角形编队位置示意图；

图3为实施例躲避上侧障碍物示意图；

图4为实施例躲避下侧障碍物示意图；

图5为实施例树杈状三角形队形避障示意图；

图6是本发明的流程示意图。

具体实施方式

结合实施例说明本发明的具体技术方案。

如图6所示，基于距离-角度优先级的多移动机器人编队避障方法，包括以下步骤：

(1)根据机器人数量建立队形数据库；

(2)对单双侧障碍物与队形的距离进行分析，判断是否需要避障，若需要则根据距离-角度优先级判断以何种队形避障，并调用步骤(1)中的队形数据库进行避障准备；

(3)设计运动控制器，根据步骤(2)中决策的避障队形开始避障；

(4)避障结束后即最后一个机器人通过障碍物，快速恢复队形。

步骤(1)所述的建立队形数据库：

设有i个机器人参与编队，虚拟领航者表示为l，其位置坐标表示为[xlylθl]^t,跟随者表示为fi,根据机器人数量设定相应的队形数据库,常用的队形有：一字型如图1、三角形如图2、人字形、矩形、正多边形、圆形等。

以三个机器人为例，将队形表示为矩阵形式如表1，表2所示：

表1一字型队形各跟随者位置坐标

表2三角形队形各跟随者位置坐标

其中且α＜β,a＞2rs且a＜cl，cl为最大通讯距离，β为最大通讯角度，rs为机器人自身安全距离半径。

为了方便理解，具体表述以三个机器人为例，其他数量机器人同样适用，只需根据当前队形相应计算距离、角度即可。

步骤(2)中形成距离-角度优先级避障策略的方法为：

设机器人自身安全距离为半径为rs，则两个机器人并排运动的最小安全距离为4rs，设有3个机器人初始状态为三角形队形沿水平方向前进，如图2。

a.单侧障碍物避障策略

当虚拟领航机器人检测到前方为单侧障碍物时：

(a1)判断是否需要避障，若d0≤asinα+rs，则需要避障，若d0＞asinα+rs，则不需要避障；

(a2)判断障碍物方向，位于前进方向的上侧或下侧，如图3，若为上侧，则机器人系统保持初始队形右拐前进进行避障，如图4，若为下侧，则机器人系统保持初始队形左拐前进进行避障；

(a3)当编队整体通过障碍物后队形收敛到原轨迹水平线继续前行；

b.双侧障碍物避障策略

当虚拟领航机器人检测到前方为双侧侧障碍物时，

(b1)首先判断是否需要避障，若d0≤dl，则需要避障，若d0＞dl，则不需要避障，继续保持原队形运动，其中dl＝2asinα+2rs；

(b2)若需要避障，则判断以哪种队形避障；

若d0＜2rs，则按单侧避障方案绕行避障；

若2rs＜d0＜4rs，则需变换一字队形通过障碍物；

若d0＞4rs时，则无需改变队形，只改变角度α，成树杈状三角形队形通过，如图5所示；即a值不变，减小α.若障碍物沿机器人运动方向的沿面障碍距离较小，为缩短避障时间，则队形成比例缩小通过障碍物。

通过对一般环境中可能出现的单双侧障碍物，通过机器人自身安全距离及队形整体与障碍物之间距离计算，得出距离-角度优先级，即先判断是否需要避障，其次优先考虑距离判断是否需要改变队形，最后决定是否需要改变角度以何种队形通过。根据以上策略分析，得出最短路径即消耗最少的方案。

步骤(3)运动控制器设计：

设虚拟领航者的位姿为qd＝[xdydθd]，ud＝[vdwd]，跟随机器人的位姿为qi＝[xiyiθi]，ui＝[viwi]，得跟随机器人i与虚拟领航者的相对位姿方程：

其中，δx＝xd-xi，δy＝yd-yi。

机器人的运动学方程：

其中x，y，θ分别为二维坐标系位置坐标及航向角，分别为x，y，θ的导数。

由反步法对线速度v和角速度w设计控制率得：

v＝vdcos(pθid)+k1pθid(4)

w＝wd+k2vdpyid+k3vrsin(pθid)(5)

其中k1，k2，k3为大于0的常数，vd，wd分别为理想线速度和角速度。

通过对跟随者与虚拟领航者的相对位姿分析，结合运动学方程，得到线速度和角速度控制率，通过控制率调节机器人实际位姿，使其无限逼近理想位姿，实现机器人高精度编队。

步骤(4)队形快速恢复算法：

为提高多机器人的工作效率，当机器人编队整体通过障碍物后需及时恢复队形，沿原轨迹继续前行，设避障后虚拟领航者位姿为p(t)＝[xlylθl]^t，设队形快速恢复函数为：

p(t)＝(p0-pゥ)e^-mt+p(1)

其中，m＞0，p0为的初始值即避障后虚拟领航者位置状态量，p∞为p(t)的稳态值即恢复队形后的位置状态量，p(t)按指数快速递减到p∞的值，可实现队形以指数形式快速恢复队形。