一种机器人队形成形的目标点分配方法与流程

本发明属于机器人技术领域，具体涉及一种机器人队形成形的目标点分配方法。

背景技术：

目前，典型的机器人队形的目标点分配方法避障效果差，且队形形成效率低。中国专利《三维空间内多机器人编队的路径规划算法》(申请日：2016.11.23；申请号：20161104678.3；公开日：2017.03.15；专利号：cn106502250a)公开了基于拍卖机制的选择目标点法，该方法没有考虑障碍物环境下的目标点选择。中国专利《一种移动多目标群智感知任务分配方法》(申请日：2018-01-30；申请号：cn201810089310.3；公开日：2018-07-20；专利号：cn108304266a)公开了通过综合考虑自动感知任务和基于位置的查询任务两个目标来设计算法,从而使得工作者在任务中同时发挥主动感知能力和被动感知能力来实现目标点分配。该方法依赖于传感器，对传感器要求精度高，一般实验不能满足。

技术实现要素：

本发明的目的是提供一种机器人队形成形的目标点分配方法，在有障碍物的环境中，提高了机器人队形成形的目标点分配效率。

本发明所采用的技术方案是，一种机器人队形成形的目标点分配方法，具体包括以下步骤：

步骤1)建立平面坐标系，根据每个机器人和目标点的位置坐标，计算每个机器人的路径长度集合，每个机器人的路径长度集合由每个机器人到所有目标点的路径长度组成；

步骤2)根据排列组合随机抽取方法在机器人的路径长度集合中选取元素组成多个队形矩阵，每个队形矩阵由各机器人的路径集合中随机选取一个元素组成，且随机抽取的元素对应位置不重合；

步骤3)计算每个队形矩阵的范数，将所有队形矩阵的范数按照由小到大的顺序依次排列，最小的范数对应的队形矩阵为队形成形最短路径的矩阵；

步骤4)根据队形成形最短路径的矩阵列出各机器人队形成形过程中实时坐标的方程组，将所有实时坐标的方程组按照排列组合顺序抽取2个实时坐标的方程组组成新方程组，并通过求解实时时刻判断新方程组对应的两个机器人是否会发生碰撞，若所有新方程组均不存在实时时刻使新方程组成立，则所有机器人之间不会发生碰撞，队形成形最短路径的矩阵为最终目标点分配矩阵，机器人队形成形的目标点分配完成；若存在实时时刻使方程组成立，则该新方程组有解，舍去该队形成形最短路径的矩阵，按照范数的排列顺序选取下一范数对应的队形矩阵作为新的队形成形最短路径的矩阵，并列出相应方程组判断该队形成形最短路径的矩阵是否为最终目标点分配矩阵，按照此方式类推，直到选出最终目标点分配矩阵，机器人队形成形的目标点分配完成。

本发明的特点还在于，

步骤1)具体按照以下步骤实施：

步骤1.1)对机器人和目标点进行编号，机器人的编号i＝1,2,3,……,n,目标点的编号n＝1,2,3,……,n，n是不为0的自然数，建立平面直角坐标系，设机器人的坐标为(xi，yi)，目标点的坐标为(xn，yn)；

步骤1.2)计算各机器人到达每个目标点的路径长度lin，若机器人到达目标点不经过障碍物，则若机器人到达目标点经过障碍物，机器人绕过障碍物的路径为半圆弧，该半圆弧半径为r，则

步骤1.3)根据步骤1.2)的计算结果整理每个机器人的路径长度集合li＝{li1,li2,li3,......,lin}。

步骤2)具体为，在所有机器人的路径长度集合中，随机从每个机器人的路径集合中抽取一个元素lin组成矩阵，每个矩阵中元素对应的目标点不重复，矩阵中的元素lin下标表示矩阵中的第i行，矩阵中的元素lin下标n表示矩阵中第i列，共抽取个矩阵。

步骤4)具体按照以下步骤实施：

步骤4.1)设机器人均以速度v匀速运动，机器人在队形成形过程中t时刻的坐标为(xit，yit)，t为连续时间常数，则根据队形成形最短路径的矩阵列出各机器人队形成形过程中实时坐标的方程组如下：

多个方程组(1)中，θi是编号为i的机器人的初始坐标点到其对应目标点连线与坐标系x轴正方向的夹角，根据队形成形最短路径的矩阵第i行元素所在列的的位置，判断编号为i的机器人对应编号为n的目标点，则

步骤4.2)在多个方程组(1)中按照排列组合顺序依次将抽取的两个实时坐标的方程组组成新方程组，如下，

新方程组(2)中，j为机器人的编号，j是不为0的自然数且j≠i；

步骤4.3)判断所有的新方程组是否有解，若均无解，则所有机器人之间不会发生碰撞，该队形成形最短路径的矩阵为最终目标点分配矩阵，机器人队形成形的目标点分配完成；若存在新方程组有解，则机器人在t时刻发生碰撞，舍去该队形成形最短路径的矩阵，按照范数的排列顺序选取下一范数对应的队形矩阵作为新的队形成形最短路径的矩阵，并依照步骤4.1)～步骤4.2)实施，判断新的队形成形最短路径的矩阵对应的所有新方程组是否有解，按照上述方式类推，直到选出最终目标点分配矩阵，机器人队形成形的目标点分配完成。

步骤4.3)中计算所有的新方程组t是否有解具体为，由式(2)中的第一个横坐标等式求得将t带入第二个纵坐标等式即vt|sinθi|+yi＝vt|sinθj|+yj中判断该等式是否成立，若成立，则新方程组有解；若不成立，则新方程组无解。

本发明的有益效果是：

本发明一种机器人队形成形的目标点分配方法，简单清晰，可实施性强；在有障碍物的环境中，能够实现在避障的同时实现时间和路径消耗最少的高效率队形形成即队形初始化，以达到最优的路径规划；机器人在移动过程中能够有效避免与障碍物碰撞，同时避免机器人之间的碰撞，提高移动机器人的使用寿命的同时为高效率的完成整个任务奠定良好基础。

附图说明

图1是本发明一种机器人队形成形的目标点分配方法的流程图。

具体实施方式

下面结合具体实施方式对本发明进行详细说明。

本发明一种机器人队形成形的目标点分配方法，如图1所示，一种机器人队形成形的目标点分配方法，具体包括以下步骤：

步骤1)建立平面坐标系，根据每个机器人和目标点的位置坐标，计算每个机器人的路径长度集合，每个机器人的路径长度集合由每个机器人到所有目标点的路径长度组成；

步骤1.1)对机器人和目标点进行编号，机器人的编号i＝1,2,3,……,n,目标点的编号n＝1,2,3,……,n，n是不为0的自然数，建立平面直角坐标系，设机器人的坐标为(xi，yi)，目标点的坐标为(xn，yn)；

步骤1.2)计算各机器人到达每个目标点的路径长度lin，若机器人到达目标点不经过障碍物，则若机器人到达目标点经过障碍物，机器人绕过障碍物的路径为半圆弧，该半圆弧半径为r，则

步骤1.3)根据步骤1.2)的计算结果整理每个机器人的路径长度集合li＝{li1,li2,li3,......,lin}。

步骤2)根据排列组合随机抽取方法在机器人的路径长度集合中选取元素组成多个队形矩阵，每个队形矩阵由各机器人的路径集合中随机选取一个元素组成，且随机抽取的元素对应位置不重合；

具体为，在所有机器人的路径长度集合中，随机从每个机器人的路径集合中抽取一个元素lin组成矩阵，每个矩阵中元素对应的目标点不重复，矩阵中的元素lin下标表示矩阵中的第i行，矩阵中的元素lin下标n表示矩阵中第i列，共抽取个矩阵。

步骤3)计算每个队形矩阵的范数，将所有队形矩阵的范数按照由小到大的顺序依次排列，最小的范数对应的队形矩阵为队形成形最短路径的矩阵；

步骤4)根据队形成形最短路径的矩阵列出各机器人队形成形过程中实时坐标的方程组，将所有实时坐标的方程组按照排列组合顺序抽取2个实时坐标的方程组组成新方程组，并通过求解实时时刻判断新方程组对应的两个机器人是否会发生碰撞，若所有新方程组均不存在实时时刻使新方程组成立，则所有机器人之间不会发生碰撞，该队形成形最短路径的矩阵为最终目标点分配矩阵，机器人队形成形的目标点分配完成；若存在实时时刻使方程组成立，则该新方程组有解，舍去该目标点对应的队形成形最短路径的矩阵，按照范数的排列顺序选取下一范数对应的队形矩阵作为新的队形成形最短路径的矩阵，并列出相应方程组判断新的队形成形最短路径的矩阵是否为最终目标点分配矩阵，按照此方式类推，直到选出最终目标点分配矩阵，机器人队形成形的目标点分配完成。

步骤4)具体按照以下步骤实施：

步骤4.1)设机器人均以速度v匀速运动，机器人在队形成形过程中t时刻的坐标为(xit，yit)，t为连续时间常数，则根据队形成形最短路径的矩阵列出各机器人队形成形过程中实时坐标的方程组如下：

多个方程组(1)中，θi是编号为i的机器人的初始坐标点到其对应目标点连线与坐标系x轴正方向的夹角，根据队形成形最短路径的矩阵第i行元素所在列的的位置，判断编号为i的机器人对应编号为n的目标点，则

步骤4.2)在多个方程组(1)中按照排列组合顺序依次将抽取的两个实时坐标的方程组组成新方程组，如下，

新方程组(2)中，j为机器人的编号，j是不为0的自然数且j≠i；

步骤4.3)判断所有的新方程组是否有解，若均无解，则所有机器人之间不会发生碰撞，该队形成形最短路径的矩阵为最终目标点分配矩阵，机器人队形成形的目标点分配完成；若存在新方程组有解，则机器人在t时刻发生碰撞，舍去该队形成形最短路径的矩阵，按照范数的排列顺序选取下一范数对应的队形矩阵作为新的队形成形最短路径的矩阵，并依照步骤4.1)～步骤4.2)实施，判断新的队形成形最短路径的矩阵对应的所有新方程组是否有解，按照上述方式类推，直到选出最终目标点分配矩阵，机器人队形成形的目标点分配完成。

判断新方程组是否有解具体为，由式(2)中的第一个横坐标等式求得将t带入第二个纵坐标等式即vt|sinθi|+yi＝vt|sinθj|+yj中判断该等式是否成立，若成立，则新方程组有解；若不成立，则新方程组无解。

实施例

本实施例提供一种机器人队形成形的目标点分配方法，具体包括以下步骤：

步骤1)建立平面坐标系，根据每个机器人和目标点的位置坐标，计算每个机器人的路径长度集合，每个机器人的路径长度集合由每个机器人到所有目标点的路径长度组成；

步骤1.1)将三个机器人按照1,2,3进行编号，将三个目标点按照1,2,3，进行编号，建立平面直角坐标系，三个机器人的初始位置分别为(0，1，)、(1，2)、(1，0)，三个目标点的坐标为(3，2)、(2.5，1.5)、(3.5，1.5)，两个障碍物坐标分别为(2，2)、(2，1)，机器人绕过障碍物的路径均为半圆弧，半圆弧半径均为r＝2；

步骤1.2)计算各机器人到达每个目标点的路径长度

l11＝3.1623,l12＝2.5495,l13＝3.5355；

l21＝4.2832,l22＝1.5811,l23＝2.5495；

l31＝2.8284,l32＝2.1213,l33＝2.9155.

步骤1.3)根据步骤1.2)的计算结果整理每个机器人的路径长度集合

l1＝{l11,l12,l13}＝{3.1623,2.5495,3.5355}

l2＝{l21,l22,l23}＝{4.2832,1.5811,2.5495}。

l1＝{l31,l32,l33}＝{2.8284,2.1213,2.9155}

步骤2)根据排列组合随机抽取方法在机器人的路径长度集合中选取元素组成多个队形矩阵，每个队形矩阵由各机器人的路径集合中随机选取一个元素组成，且随机抽取的元素对应位置不重合；

具体为，在所有机器人的路径长度集合中，随机从每个机器人的路径集合中抽取一个元素组成矩阵，每个矩阵中元素对应的目标点不重复，

步骤3)计算每个队形矩阵的f范数，将所有队形矩阵的f范数按照由小到大的顺序依次排列，最小的范数对应的队形矩阵为队形成形最短路径的矩阵，

按从小到大排列：||a1||f＝||a2||f＝||a5||f＜||a6||f＜||a3||f＜||a4||f

当前选||a1||f对应矩阵为队形成形最短路径的矩阵，即(表示机器人1选位置1，机器人2选位置2，机器人3选位置3)

步骤4)根据队形成形最短路径的矩阵列出各机器人队形成形过程中实时坐标的方程组，将所有实时坐标的方程组按照排列组合顺序抽取2个实时坐标的方程组组成新方程组，

设v＝1,由(2)式得：

由t|cosθ1|+0＝vt|cosθ2|+1得：t无解

等式不成立，即方程组无解。

同理，求得均无解，即选定该队形成形最短路径的矩阵为目标点分配矩阵，机器人队形成形的目标点分配完成。