一种多机器人防碰撞方法及系统与流程

本发明涉及多机协作机器人领域，尤其涉及一种多机器人防碰撞方法及系统。

背景技术：

随着机器人行业的崛起，越来越多的行业已经引入了机器人，单个机器人的自动化改造是用生产线代替生产线，机器人作为整个生产线中的组成部分，很难单独拿出来，如果某个环节的机器人坏了，在没有设计备份的情况下，整个产线可能要停工在一些领域中存在效率低下的问题，这时机器人运动且不能发生碰撞是一个很重要的问题。在自主移动多机器人领域中，为了达到较大的工作效率，单个机器人移动速度都比较大，碰撞可能导致机器人损坏或造成其他人身财产损失。对于以相当大的速度移动的现代工业机器人，碰撞通常既对机器人又对它们所处理的工件引起严重的损坏，这将产生较大的损失而导致生产停工，与自动操作机器人进行交互的人员的安全也被给予最高的优先级。在具有移动功能的现代工业机器人或其它机器中，移动代表对于人员和其它对象的危险，因此必须防止在移动过程中，机器人与对象之间发生碰撞，为此，如何在发生碰撞之前控制机器人，使机器人停止移动或改变路径来防止碰撞的预测防碰撞方法是十分重要的。

在现有技术中，目前已有的防碰撞方法主要是在机器人的基座上安装测距仪器(如超声测距传感器、红外测距传感器)，根据测距仪器测量碰撞距离并改变机器人的轨迹实现在短距离内避免碰撞，但是在现有技术存在以下四个问题：

1、针对机器人与障碍物和机器人与机器人预测碰撞的判断方法相同，产生计算响应延迟，增加了计算时间；

2、机器人在复杂环境中的减速区间小，易堆积在一个地方；

3、不能够有效地防止机器人发生碰撞。

因而，如何解决现有技术中的问题，是目前本领域技术人员亟待解决的技术问题。

技术实现要素：

本发明的目的是提供一种多机器人防碰撞方法及系统，实现解决现有防碰撞方法中不能很好预测碰撞，提高机器人在复杂环境中的活动能力，增大机器人的活动范围的问题。

为了达到上述目的，本发明提供的技术方案如下：

一种多机器人防碰撞方法，包括步骤：

机器人获取运动速度和运动轨迹，得到在预设时间内进行仿真移动后的位置，创建环境地图；依次得到每个机器人几何外形顶点的坐标；依次判断每个机器人是否与障碍物和/或其他机器人发生碰撞，当是时标记；否则，不标记；采集所有碰撞标记信息、机器人位置相关信息，更新环境地图，并调整所述机器人的移动速度和移动轨迹。

本发明能够实现多机器人在同一工作区域内预测碰撞并防止碰撞的目的，通过仿真机器人一段时间的运动，并计算每个机器人的轨迹，以及在此过程中检测机器人是否会与机器人或障碍物发生碰撞，对于预测发生碰撞的机器人标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果，从而调整移动速度和移动方向，并调整即将发生碰撞的机器人的移动速度和移动方向，本发明对每个机器人的运行做一段时间的预测，避免即将发生碰撞的时候才被机器人检测到，可以做到提前预判，可以有效地防止机器人卡死或者摇摆不定的情况发生，从而避免发生碰撞，提高机器人在复杂环境中的活动能力，增大机器人的活动范围。

进一步，所述多机器人防碰撞方法包括：

获取所有机器人几何外形顶点在所述环境地图上的坐标信息；判断当前机器人是否有碰撞标记信息；当是时，切换下一机器人；否则判断所述碰撞标记信息是否为机器人；当碰撞标记信息是机器人时，预设每个机器人的中心点，按照标签先后顺序，依次判断所述当前机器人与任意一个所述下一机器人中心点之间的距离是否小于等于碰撞最小距离；当否时，输出不碰撞到其他机器人；当是时，判断所述当前机器人几何外形的任意一线段是否与所述下一机器人几何外形的任意一线段相交；当是时，输出碰撞到其他机器人，标记所述碰撞机器人；否则输出不碰撞到其他机器人；当碰撞标记信息不是机器人时，根据机器人几何外形顶点进行适应地图分辨率的插值运算，按照标签先后顺序，依次判断所述当前机器人几何外形上任意一顶点是否在所述移动环境地图上的障碍物标识区域内；当是时，输出碰撞到障碍物，标记所述碰撞机器人；否则，输出不碰撞到障碍物。

本发明通过判断当前机器人是否有碰撞信息，能够有效地减少判断时间，只需要得知若有碰撞信息，则切换至下一个机器人进行判断，若没有碰撞信息，再进行下一步骤，大大地减少了判断时间。本发明针对机器人与障碍物和机器人与机器人预测碰撞做不同的判断方法，可以极大地减少计算时间，保证计算实时性。

进一步，所述多机器人防碰撞方法包括：所述碰撞最小距离为两个机器人之间的安全距离的和值，所述机器人的安全距离为所述机器人的中心点到所述机器人几何外形顶点的最远距离。

进一步，所述多机器人防碰撞方法应用下列公式(1)进行运算：

C＝A+k₁·U＝B+k₂·V (3)

其中，C表示原点到碰撞点的向量，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例、k₂表示C-B与V的长度比例。

进一步，所述多机器人防碰撞方法应用下列公式(2)进行运算：

k₂·(U×V)＝(B-A)×U (2)

如果U×V＝0并且(B-A)×U＝0，说明两条直线共线；

如果U×V＝0并且(B-A)×U≠0，说明两条直线平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，说明两条线段相交；

其他情况，说明两条线段不平行也不相交；

其中，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例，k₂表示C-B与V的长度比例。

进一步，所述多机器人防碰撞方法包括：

获取所有机器人的位置相关信息和障碍物的位置相关信息；所述机器人进行垂直投影得到所述机器人的几何外形，并得到所述机器人几何外形顶点的坐标；根据所述位置相关信息和机器人的几何外形，设置不同灰度值来标定位置区域，第一灰度值表示宽敞区域，第二灰度值表示障碍物区域，第三灰度值表示机器人垂直投影几何外形区域，第四灰度值表示未知区域；根据所述位置相关信息和所述灰度值的映射关系创建环境地图。

本发明判断机器人是否发生碰撞只需要判断在地图中该机器人几何外形下是否只存在当前机器人信息和地图空闲信息，如果该机器人几何外形下存在其他信息(如障碍物信息、其他机器人几何外形信息)则说明机器人发生碰撞。

进一步，所述多机器人防碰撞方法包括：

获取所有机器人的初始信息，包括机器人位置相关信息和环境信息；得到每个机器人的运动速度，构建每个机器人的运动轨迹。

进一步，所述多机器人防碰撞方法包括：

采集所述碰撞标记信息、机器人位置相关信息；储存所述采集的信息，更新环境地图；规划所述机器人的防碰撞路径，并广播发送控制指令给各个机器人；判断所述机器人是否在所述预设响应时间内接收到所述控制指令；若是则所述机器人根据所述控制指令调整所述移动速度和移动轨迹；否则，标记所述机器人发生故障，获取所述发生故障的机器人的位置相关信息，并返回重新开始。

进一步，所述多机器人防碰撞方法包括：所述运动轨迹包括运动方向、运动角度和运动路径；所述位置相关信息包括所述位置、速度、方位、形状和高度。

本发明还提供一种多机器人防碰撞系统，包括：

碰撞预测模块、主控模块；

所述碰撞预测模块与所述主控模块通讯连接，获取所述主控模块发送的运动速度和运动轨迹；所述机器人在预设时间内根据所述运动速度和运动轨迹仿真移动；获取仿真移动后的位置，创建环境地图；依次得到每个机器人几何外形顶点的坐标；依次判断每个机器人是否与障碍物和/或其他机器人发生碰撞，当是时标记；否则，不标记；

所述主控模块采集所有碰撞标记信息、机器人位置相关信息；根据所述采集的信息更新环境地图，并发送移动速度控制指令和移动轨迹控制指令至所述机器人。

本发明能够实现多机器人在同一工作区域内预测碰撞并防止碰撞的目的，通过仿真机器人一段时间的运动，并计算每个机器人的轨迹，以及在此过程中检测机器人是否会与机器人或障碍物发生碰撞，对于预测发生碰撞的机器人标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果，从而调整移动速度和移动方向，并调整即将发生碰撞的机器人的移动速度和移动方向，可以有效地防止机器人卡死或者摇摆不定的情况发生，从而避免发生碰撞，提高机器人在复杂环境中的活动能力，增大机器人的活动范围。

进一步，所述多机器人防碰撞系统的所述碰撞预测模块还包括：

获取创建子模块和判断标记子模块；

所述获取创建子模块与所述判断标记子模块通讯连接，获取所述在预设时间内进行仿真移动后的位置，创建环境地图，依次得到每个机器人几何外形顶点的坐标；所述获取创建子模块还获取所有机器人的位置相关信息和障碍物的位置相关信息，进行垂直投影得到所述机器人的几何外形，并得到所述机器人几何外形顶点的坐标，根据所述位置相关信息和机器人的几何外形，设置不同灰度值来标定位置区域，第一灰度值表示宽敞区域，第二灰度值表示障碍物区域，第三灰度值表示机器人垂直投影几何外形区域，第四灰度值表示未知区域；根据所述位置相关信息和所述灰度值的映射关系创建环境地图，并将所述位置相关信息和所述环境地图发送至所述判断标记子模块；

所述判断标记子模块与所述主控模块通讯连接，根据所述机器人几何外形顶点在所述环境地图上的坐标信息，依次判断每个机器人是否有碰撞标记信息，当是时切换下一机器人进行判断；否则判断所述碰撞标记信息是否为机器人；当碰撞标记信息是机器人时，预设每个机器人的中心点，按照标签先后顺序，依次判断所述当前机器人与任意一个所述下一机器人中心点之间的距离是否小于等于碰撞最小距离；当中心点之间的距离大于碰撞最小距离时，输出不碰撞到其他机器人；当中心点之间的距离小于等于碰撞最小距离时，判断所述当前机器人几何外形的任意一线段是否与所述下一机器人几何外形的任意一线段相交；当是时，输出碰撞到其他机器人，标记所述碰撞机器人；否则，输出不碰撞到其他机器人；当碰撞标记信息不是机器人时，按照标签先后顺序，依次判断所述当前机器人几何外形上任意一顶点是否在所述移动环境地图上的障碍物标识区域内；当是时，输出碰撞到障碍物，标记所述碰撞机器人；否则，输出不碰撞到障碍物；

其中，所述运动轨迹包括运动方向、运动角度和运动路径，所述位置相关信息包括所述位置、速度、方位、形状和高度，所述碰撞最小距离为两个机器人之间的安全距离的和值，所述机器人的安全距离为所述机器人的中心点到所述机器人几何外形顶点的最远距离。

本发明通过判断当前机器人是否有碰撞信息，能够有效地减少判断时间，只需要得知若有碰撞信息，则切换至下一个机器人进行判断，若没有碰撞信息，再进行下一步骤，大大地减少了判断时间。本发明针对机器人与障碍物和机器人与机器人预测碰撞做不同的判断方法，可以极大地减少计算时间，保证计算实时性。

进一步，所述多机器人防碰撞系统包括，

所述判断标记子模块根据下列公式(1)进行运算：

C＝A+k₁·U＝B+k₂·V (4)

其中，C表示原点到碰撞点的向量，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例、k₂表示C-B与V的长度比例。

进一步，所述多机器人防碰撞系统包括，

所述判断标记子模块根据下列公式(2)进行判断：

k₂·(U×V)＝(B-A)×U (2)

如果U×V＝0并且(B-A)×U＝0，说明两条直线共线；

如果U×V＝0并且(B-A)×U≠0，说明两条直线平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，说明两条线段相交；

其他情况，说明两条线段不平行也不相交；

其中，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例，k₂表示C-B与V的长度比例。

进一步，所述多机器人防碰撞系统还包括：所述主控模块还获取所有机器人的初始信息，包括机器人位置相关信息和环境信息，得到每个机器人的所述运动速度，构建每个机器人的所述运动轨迹，并发送所述运动速度和运动轨迹至所述碰撞预测模块，所述主控模块储存所述采集的碰撞标记信息和机器人位置相关信息，根据所述储存的信息更新环境地图，规划所述机器人的防碰撞路径，并广播发送控制指令给各个机器人；判断所述机器人是否在所述预设响应时间内接收到所述控制指令；若是则根据所述控制指令调整所述移动速度和移动轨迹；否则，标记所述机器人发生故障，获取所述发生故障的机器人的位置相关信息，并发送至所述碰撞预测模块。

本发明中对于预测发生碰撞的机器人标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果供主控模块做调整。能极大地避免机器人与障碍物或其他机器人发生碰撞，提高机器人在复杂环境中的活动能力，增大机器人的活动范围。

与现有技术相比，本发明提供一种多机器人防碰撞方法及系统，至少带来以下一种技术效果：

1、本发明能极大地避免机器人与障碍物或其他机器人发生碰撞，可以有效地防止机器人卡死或者摇摆不定的情况发生，提高机器人在复杂环境中的活动能力，增大机器人的活动范围。

2、本发明尽量保证大多数机器人能正常运行。

3、本发明中对于预测发生碰撞的机器人标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果供主控模块做调整。

4、本发明针对机器人与障碍物和机器人与机器人预测碰撞做不同的判断方法，可以极大地减少计算时间，保证计算实时性。

5、本发明对每个机器人的运行做一段时间的预测，避免即将发生碰撞的时候才被机器人检测到，可以做到提前预判。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对一种多机器人防碰撞方法及系统特性、技术特征、优点及其实现方式予以进一步说明。

图1是本发明一种多机器人防碰撞方法一个实施例的流程图；

图2是本发明一种多机器人防碰撞方法另一个实施例的流程图；

图3是本发明一种多机器人防碰撞方法又一实施例的流程图；

图4是本发明一种多机器人防碰撞系统一个实施例的结构示意图；

图5是本发明一种多机器人防碰撞系统另一个实施例的结构示意图；

图6是本发明一种多机器人防碰撞系统再一实施例的环境地图的示意图；

图7是本发明机器人与机器人碰撞的示意图；

图8是本发明机器人与障碍物碰撞的示意图；

图9是本发明机器人与机器人之间发生碰撞的碰撞最小距离示意图；

图10是本发明机器人与机器人之间发生碰撞的判断线段相交的示意图。

附图标号说明：

1000机器人；

1001万向轮；

1002基座；

1003滚轮；

1100碰撞预测模块；

1110获取创建子模块；

1120判断标记子模块；

1200主控模块；

2000障碍物；

1宽敞区域；

2障碍物区域；

3机器人垂直投影几何外形区域；

4未知区域。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

本发明提供一种多机器人防碰撞方法的一个实施例，参照图1，包括：

S300机器人获取运动速度和运动轨迹，得到在预设时间内进行仿真移动后的位置，创建环境地图；S400依次得到每个机器人几何外形顶点的坐标；S500依次判断每个机器人是否与障碍物和/或其他机器人发生碰撞，当是时标记；否则，不标记；S600采集所有碰撞标记信息、机器人位置相关信息，更新环境地图，并调整所述机器人的移动速度和移动轨迹。

本发明实施例中，机器人1000在预设时间内进行仿真移动，本发明通过仿真机器人1000一段时间的运动，并计算每个机器人1000的轨迹，判断机器人1000是否与障碍物2000和/或其他机器人1000发生碰撞，是否发生碰撞只需要判断在环境地图中该机器人1000几何外形下是否只存在当前机器人1000信息和地图空闲信息，如果该机器人1000几何外形下存在其他信息(如障碍物2000信息、其他机器人1000几何外形信息)则说明机器人1000与障碍物2000和/或其他机器人1000发生碰撞，对于预测发生碰撞的机器人1000标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果，从而调整移动速度和移动轨迹，避免即将发生碰撞的时候才被机器人1000检测到，可以做到提前预判，能极大地避免机器人1000与障碍物2000或其他机器人1000发生碰撞，可以有效地防止机器人1000卡死或者摇摆不定的情况发生，提高机器人1000在复杂环境中的活动能力，增大机器人1000的活动范围。

本发明提供一种多机器人防碰撞方法的另一个实施例，参照图2。相对于第一个实施例，相同的步骤在此不再赘述，步骤S500进一步包括：S510获取所有机器人几何外形顶点在所述环境地图上的坐标信息；S520判断当前机器人是否有碰撞标记信息；当是时，执行步骤S590；否则，执行步骤S530；S530判断所述碰撞标记信息是否为机器人；当是时，执行步骤S560；否则，执行步骤S540；S540根据机器人几何外形顶点进行适应地图分辨率的插值运算；S550按照标签先后顺序，依次判断所述当前机器人几何外形上任意一顶点是否在所述移动环境地图上的障碍物标识区域内；当是时，执行步骤S551；否则，执行步骤S552；S551输出碰撞到障碍物，标记所述碰撞机器人；S552输出不碰撞到障碍物；S560预设每个机器人的中心点；S570按照标签先后顺序，依次判断所述当前机器人与任意一个所述下一机器人中心点之间的距离是否小于等于碰撞最小距离；当是时，执行步骤S580；否则，执行步骤S582；S580判断所述当前机器人几何外形的任意一线段是否与所述下一机器人几何外形的任意一线段相交；当是时，执行步骤S581；否则，执行步骤S582；S581输出碰撞到其他机器人，标记所述碰撞机器人；S582输出不碰撞到其他机器人；S590切换下一机器人，继续执行步骤S520。

本发明实施例中，上述步骤效果不再赘述，步骤S510到S590中，步骤S510通过判断当前机器人1000是否有碰撞信息，能够有效地减少判断时间，只需要得知若有碰撞信息，则切换至下一个机器人1000进行判断，若没有碰撞信息，再进行下一步骤，大大地减少了判断时间；步骤S520到步骤590通过判断碰撞信息是否为机器人1000，当碰撞标记信息为机器人1000时，预设每个机器人1000的中心点；如图7，是机器人1000与机器人1000发生碰撞的示意图，若所述机器人1000为轮式机器人1000，以万向轮1001所在的位置作为轮式机器人1000的头部，则所述轮式机器人1000的车体1002和车轮1003的垂直投影几何外形区域与其余的轮式机器人1000的车体1002和车轮1003的垂直投影几何外形区域在所述环境地图上相交则表示所述两个轮式机器人1000之间发生碰撞；如图9是机器人1000与机器人1000之间发生碰撞的碰撞最小距离示意图，依次判断所述当前机器人1000与任意一个所述下一机器人1000中心点之间的距离是否小于等于碰撞最小距离；如果当前机器人1000与任意一个所述下一机器人1000中心点之间的距离大于碰撞最小距离，输出当前机器人1000不碰撞到任一其他机器人1000；如果当前机器人1000与任意一个所述下一机器人1000中心点之间的距离小于等于碰撞最小距离，如图10是机器人1000与机器人1000之间发生碰撞的判断线段相交的示意图，再继续判断所述当前机器人1000几何外形的任意一线段是否与所述下一机器人1000几何外形的任意一线段相交；如果所述当前机器人1000几何外形的任意一线段与所述下一机器人1000几何外形的任意一线段相交时，输出当前机器人1000碰撞到至少一个机器人1000的碰撞信息，标记所述相互之间发生碰撞的两个机器人1000；否则，如果所述当前机器人1000几何外形的任意一线段与所述下一机器人1000几何外形的任意一线段不相交时，输出当前机器人1000不碰撞到任何机器人1000，切换下一机器人1000，继续判断当前机器人1000是否有碰撞标记信息。

其中所述碰撞最小距离为两个机器人1000之间的安全距离的和值，所述机器人1000的安全距离为所述机器人1000的中心点到所述机器人1000几何外形顶点的最远距离。

所述步骤S580应用下列公式(1)进行运算：

C＝A+k₁·U＝B+k₂·V (5)

其中，C表示原点到碰撞点的向量，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例、k₂表示C-B与V的长度比例。

对于式(1)左右两边分别叉乘向量V，即

(A+k₁·U)×V＝(B+k₂·V)×V (6)

又因为V×V＝0，可以求出k₁：

k₁·(U×V)＝(B-A)×V (7)

同理对于式(4)左右两边分别叉乘向量U，即因为U×U＝0，可以求出k₂：

(A+k₁·U)×U＝(B+k₂·V)×U (8)

k₂·(V×U)＝(A-B)×U (9)

其中U×V＝-V×U，则能够得到式(6)

k₂·(U×V)＝(B-A)×U (10)

如果U×V＝0并且(B-A)×U＝0，说明两条直线共线；

如果U×V＝0并且(B-A)×U≠0，说明两条直线平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，说明两条线段相交；

其他情况，说明两条线段不平行也不相交。

其中，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例，k₂表示C-B与V的长度比例。

本发明是为了解决多机器人1000在同一工作区域内预测碰撞的方法，该方法通过仿真机器人1000一段时间的运动，并计算每个机器人1000的轨迹，以及在此过程中检测机器人1000是否会与障碍物2000和机器人1000发生碰撞，将得到的预测结果输出。该方法可以有效地指出哪个机器人1000在什么时候、什么位置、与什么物体发生碰撞，可以有效地防止机器人1000卡死或者摇摆不定的情况发生，提高机器人1000在复杂环境中的活动能力，增大机器人1000的活动范围。每次对机器人1000预测很小的时间的动作，因此机器人1000移动的距离很短，判断机器人1000是否发生碰撞只需要判断在地图中该机器人1000几何外形下是否只存在当前机器人1000信息和地图空闲信息，如果该机器人1000几何外形下存在其他信息(如障碍物2000信息、其他机器人1000几何外形信息)则说明机器人1000发生碰撞。首先判断两个机器人1000的距离是否满足两个机器人1000碰撞的最小距离，若不满足这个最小距离则说明两个机器人1000还足够远以至于不会发生碰撞。最小距离为两个机器人1000的安全距离之和，机器人1000的安全距离为机器人1000中心点到机器人1000几何外形的最远距离，考虑到机器人1000的几何外形可以看作一个多边形，机器人1000的安全距离为机器人1000中心点到机器人1000几何外形顶点的最远距离。其次机器人1000的几何外形可以看作一个多边形，机器人1000的碰撞与机器人1000的碰撞可以看作多边形与多边形的重叠，进一步可以看作是线段与线段的相交，即如果机器人1000的几何外形的一条线段与另一个机器人1000的几何外形的一条线段相交，说明两个机器人1000发生碰撞。

本发明插值运算的方法有Lagrange插值(拉格朗日插值)、Newton插值(牛顿插值)、Hermite插值(艾米插值)、分段线性插值、样条插值等插值方法。通过判断当前机器人1000是否有碰撞信息，能够有效地减少判断时间，只需要得知若有碰撞信息，则切换至下一个机器人1000进行判断，若没有碰撞信息，再进行下一步骤，大大地减少了判断时间。本发明针对机器人1000与障碍物2000和机器人1000与机器人1000预测碰撞做不同的判断方法，可以极大地减少计算时间，保证计算实时性。

本发明提供一种多机器人防碰撞方法的另一个实施例，参照图3。相对于第一个实施例，相同的步骤在此不再赘述，进一步包括：

S100获取所有机器人的初始信息，包括机器人位置相关信息和环境信息；

S200得到每个机器人的所述运动速度，构建每个机器人的所述运动轨迹；

S300机器人获取运动速度和运动轨迹，得到在预设时间内进行仿真移动后的位置，创建环境地图；

S410获取所有机器人的位置相关信息和障碍物的位置相关信息；

S420所述机器人进行垂直投影得到所述机器人的几何外形，并得到所述机器人几何外形顶点的坐标；

S430根据所述位置相关信息和机器人的几何外形，设置不同灰度值来标定位置区域，第一灰度值表示宽敞区域，第二灰度值表示障碍物区域，第三灰度值表示机器人垂直投影几何外形区域，第四灰度值表示未知区域；

S440根据所述位置相关信息和所述灰度值的映射关系创建环境地图；

S500依次判断每个机器人是否与障碍物和/或其他机器人发生碰撞，当是时标记；否则，不标记；

S610采集所述碰撞标记信息、机器人位置相关信息；

S620储存所述采集的信息，更新环境地图；

S630规划所述机器人的防碰撞路径，并广播发送控制指令给各个机器人；

S640判断所述机器人是否在所述预设响应时间内接收到所述控制指令；若是则执行步骤S650；否则，执行步骤S660；

S650所述机器人根据所述控制指令调整所述移动速度和移动轨迹；

S660标记所述机器人发生故障，获取所述发生故障的机器人的位置相关信息，并返回所述步骤S100。

本发明实施例中，上述步骤效果不再赘述，步骤S100到S200中，获取所有机器人1000的初始信息，包括机器人1000位置相关信息和环境信息；根据所述位置相关信息和环境信息可以计算得到每个机器人1000的在短时间内进行仿真移动的运动速度和运动轨迹，如图7本发明通过不同的灰度值来标定该位置区域的内所包含的信息，第一灰度值表示宽敞区域1，第二灰度值表示障碍物2000区域2，第三灰度值表示机器人垂直投影几何外形区域3，第四灰度值表示未知区域4，根据所述位置相关信息和所述灰度值的映射关系创建环境地图；本发明判断机器人1000是否发生碰撞只需要判断在地图中该机器人1000几何外形下是否只存在当前机器人1000信息和地图空闲信息，如果该机器人1000几何外形下存在其他信息(如障碍物2000信息、其他机器人1000几何外形信息)则说明机器人1000发生碰撞。根据所述环境地图和判断方法依次判断每个机器人1000是否发生碰撞，当发生碰撞时进行标记；否则，不进行标记；标记结束后，采集所述碰撞标记信息、机器人1000位置相关信息，将所述采集的信息进行储存，当根据标签顺序依次储存所有机器人1000的位置相关信息和碰撞标记信息后，更新环境地图，根据更新后的环境地图，规划所述机器人1000的防碰撞路径，并广播发送控制指令给各个机器人1000；判断所述机器人1000是否在所述预设响应时间内接收到所述控制指令；如果在预设时间内所述机器人1000接收到控制指令，则机器人1000根据所述控制指令调整移动速度和移动轨迹，如果在预设时间内所述机器人1000没有接收到控制指令，则标记所述机器人1000发生故障，获取所述发生故障的机器人1000的位置相关信息，则所述发生故障的机器人1000成为环境障碍物2000，变成环境信息的一部分，因此需要重新获取所有机器人1000的初始信息，包括机器人1000位置相关信息和环境信息，这样能够避免机器人1000因为延迟导致在预设时间内所述机器人1000没有接收到控制指令，或者机器人1000因为发生突发性的故障导致在预设时间内所述机器人1000没有接收到控制指令，从而能极大地避免机器人1000与障碍物2000或其他机器人1000发生碰撞，可以有效地防止机器人1000卡死或者摇摆不定的情况发生，提高机器人1000在复杂环境中的活动能力，增大机器人1000的活动范围。

参照图4，本发明一种多机器人防碰撞系统的一个实施例，包括：机器人1000，所述机器人1000包括碰撞预测模块1200、主控模块1100；

所述碰撞预测模块与所述主控模块通讯连接，获取所述主控模块发送的运动速度和运动轨迹；所述机器人在预设时间内根据所述运动速度和运动轨迹仿真移动；获取仿真移动后的位置，创建环境地图；依次得到每个机器人几何外形顶点的坐标；依次判断每个机器人是否与障碍物和/或其他机器人发生碰撞，当是时标记；否则，不标记；

所述主控模块与所述碰撞预测模块通讯连接，采集所有碰撞标记信息、机器人位置相关信息；根据所述采集的信息更新环境地图，并发送移动速度控制指令和移动轨迹控制指令至所述机器人。

本发明实施例中，所述机器人1000获取所述主控模块1200发送的运动速度和运动轨迹，根据所述获取的运动速度和运动轨迹，在预设时间内移动；通过仿真机器人1000一段时间的运动，所述主控模块1200获取所述在预设时间内进行仿真移动后的位置，创建环境地图；依次得到每个机器人1000几何外形顶点的坐标；碰撞预测模块1100依次判断每个机器人1000是否发生碰撞，当是时标记碰撞；否则，不标记碰撞；采集所有碰撞标记信息、机器人1000位置相关信息；根据所述采集的信息更新环境地图，并发送移动速度控制指令和移动轨迹控制指令至所述机器人1000；并计算每个机器人1000的轨迹，以及在此过程中检测机器人1000是否会与机器人1000或障碍物2000发生碰撞，将得到的预测结果输出采集作参考，对于预测发生碰撞的机器人1000标记碰撞信息，将碰撞时间、地点、相关物体信息输出预测结果，从而调整移动速度和移动轨迹，避免即将发生碰撞的时候才被机器人1000检测到，可以做到提前预判，能极大地避免机器人1000与障碍物2000或其他机器人1000发生碰撞，并调整即将发生碰撞的机器人1000的移动速度和移动轨迹，可以有效地防止机器人1000卡死或者摇摆不定的情况发生，从而避免发生碰撞，提高机器人1000在复杂环境中的活动能力，增大机器人1000的活动范围。

本发明提供一种多机器人防碰撞系统的另一个实施例，参照图5。相对于上一个实施例，相同的部分在此不再赘述，所述碰撞预测模块1100还包括：获取创建子模块1110和判断标记子模块1120；

所述获取创建子模块1110与所述主控模块1200通讯连接，获取所述在预设时间内进行仿真移动后的位置，创建环境地图，依次得到每个机器人1000几何外形顶点的坐标；所述获取创建子模块1110还获取所有机器人的位置相关信息和障碍物2000的位置相关信息，进行垂直投影得到所述机器人1000的几何外形，并得到所述机器人1000几何外形顶点的坐标，根据所述位置相关信息和机器人1000的几何外形，设置不同灰度值来标定位置区域，第一灰度值表示宽敞区域1，第二灰度值表示障碍物区域2，第三灰度值表示机器人垂直投影几何外形区域3，第四灰度值表示未知区域4；根据所述位置相关信息和所述灰度值的映射关系创建环境地图，并将所述位置相关信息和所述环境地图发送至所述判断标记子模块1120；

所述判断标记子模块1120与所述判断标记子模块1120通讯连接，根据所述机器人几何外形顶点在所述环境地图上的坐标信息，依次判断每个机器人1000是否有碰撞标记信息，当是时切换下一机器人1000进行判断；否则判断所述碰撞标记信息是否为机器人1000；当碰撞标记信息是机器人1000时，预设每个机器人1000的中心点，按照标签先后顺序，依次判断所述当前机器人1000与任意一个所述下一机器人1000中心点之间的距离是否小于等于碰撞最小距离；当中心点之间的距离大于碰撞最小距离时，输出不碰撞到其他机器人1000；当中心点之间的距离小于等于碰撞最小距离时，判断所述当前机器人1000几何外形的任意一线段是否与所述下一机器人1000几何外形的任意一线段相交；当是时，输出碰撞到其他机器人1000，标记所述碰撞机器人1000；否则，输出不碰撞到其他机器人1000；当碰撞标记信息不是机器人1000时，按照标签先后顺序，依次判断所述当前机器人1000几何外形上任意一顶点是否在所述移动环境地图上的障碍物标识区域2内；当是时，输出碰撞到障碍物2000，标记所述碰撞机器人1000；否则，输出不碰撞到障碍物2000；

其中，所述运动轨迹包括运动方向、运动角度和运动路径，所述位置相关信息包括所述位置、速度、方位、形状和高度，所述碰撞最小距离为两个机器人1000之间的安全距离的和值，所述机器人1000的安全距离为所述机器人1000的中心点到所述机器人1000)几何外形顶点的最远距离。

所述判断标记子模块1120根据下列公式(1)进行运算：

C＝A+k₁·U＝B+k₂·V (11)

其中，C表示原点到碰撞点的向量，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例、k₂表示C-B与V的长度比例。

所述判断标记子模块1120对于式(1)左右两边分别叉乘向量V进行判断，即

(A+k₁·U)×V＝(B+k₂·V)×V (12)

又因为V×V＝0，可以求出k₁：

k₁·(U×V)＝(B-A)×V (13)

同理对于式(4)左右两边分别叉乘向量U，即因为U×U＝0，可以求出k₂：

(A+k₁·U)×U＝(B+k₂·V)×U (14)

k₂·(V×U)＝(A-B)×U (15)

其中U×V＝-V×U，则能够得到式(6)

k₂·(U×V)＝(B-A)×U (16)

如果U×V＝0并且(B-A)×U＝0，说明两条直线共线；

如果U×V＝0并且(B-A)×U≠0，说明两条直线平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，说明两条线段相交；

其他情况，说明两条线段不平行也不相交。

其中，A和B分别表示原点到两条线段起点的向量、U和V分别表示两条线段起点到终点的向量、k₁表示C-A与U的长度比例，k₂表示C-B与V的长度比例。

本发明实施例中，上述相同模块的效果不再赘述，本发明能够实现多机器人1000在同一工作区域内预测碰撞并防止碰撞的目的，本发明通过仿真机器人1000一段时间的运动，并计算每个机器人1000的轨迹，以及在此过程中检测机器人1000是否会与其余的机器人1000发生碰撞，通过判断当前机器人1000是否有碰撞信息，能够有效地减少判断时间，只需要得知若有碰撞信息，则切换至下一个机器人1000进行判断，若没有碰撞信息，再进行下一步骤，大大地减少了判断时间；当有碰撞信息时，判断当前机器人1000与其余机器人1000是否发生碰撞的方法与判断当前机器人1000与障碍物2000是否发生碰撞的方法是不同的，本发明针对机器人1000与障碍物2000和机器人1000与机器人1000预测碰撞做不同的判断方法，可以极大地减少计算时间，保证计算实时性。根据所述环境地图和判断方法依次判断每个机器人1000是否发生碰撞，当发生碰撞时进行标记；否则，不进行标记；标记结束后，采集所述碰撞标记信息、机器人1000位置相关信息，将所述采集的信息进行储存，当根据标签顺序依次储存所有机器人1000的位置相关信息和碰撞标记信息后，更新环境地图，根据更新后的环境地图，规划所述机器人1000的防碰撞路径，并广播发送控制指令给各个机器人1000；判断所述机器人1000是否在所述预设响应时间内接收到所述控制指令；如果在预设时间内所述机器人1000接收到控制指令，则机器人1000根据所述控制指令调整移动速度和移动轨迹，如果在预设时间内所述机器人1000没有接收到控制指令，则标记所述机器人1000发生故障，获取所述发生故障的机器人1000的位置相关信息，则所述发生故障的机器人1000成为环境障碍物2000，变成环境信息的一部分，因此需要重新获取所有机器人1000的初始信息，包括机器人1000位置相关信息和环境信息，这样能够避免机器人1000因为延迟导致在预设时间内所述机器人1000没有接收到控制指令，或者机器人1000因为发生突发性的故障导致在预设时间内所述机器人1000没有接收到控制指令，从而能极大地避免机器人1000与障碍物2000或其他机器人1000发生碰撞，可以有效地防止机器人1000卡死或者摇摆不定的情况发生，提高机器人1000在复杂环境中的活动能力，增大机器人1000的活动范围。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。