CN111399509B - 一种多移动机器人协同转运控制方法及系统 - Google Patents

Abstract

本发明属于智能控制相关技术领域，其公开了一种多移动机器人协同转运控制方法及系统，包括以下步骤：(1)在多个机器人子系统中任选两个机器人子系统构成协商领航层，剩余的机器人子系统构成跟随层；(2)协商领航层的机器人子系统同时接收指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划；(3)分别计算得到速度控制量；(4)协商确定沿工件方向的共同速度；(5)分别对各自原速度控制量进行修正以达到协同运行；同时，跟随层的机器人子系统以保证自身与协商领航层的两个机器人子系统的相对距离不变而进行动作控制。本发明提高了利用效率，节约了大量成本，且具有较好的通用性。

Description

一种多移动机器人协同转运控制方法及系统

技术领域

本发明属于智能控制相关技术领域，更具体地，涉及一种多移动机器人协同控制方法及系统。

背景技术

随着机器人技术的不断发展，越来越多的机器人被使用于智能工厂物料转运和智能仓储物料货物转运工作中。其中，智能移动机器人种类繁多、功能完备，它们在物料自动配送、自动转运中起到了巨大作用，为智能工厂和仓储系统提供了坚实的技术基础。但是在一些大工件转运的应用环境中，现有的智能机器人无法进行单台作业，对大工件货物进行搬运，必须通过两台甚至更多的机器人一起工作才有可能实现货物的转运。所以对多台机器人进行协同作业控制，实现了机器人共同搬运大型工件货物，且可以满足实际工况的需要。

现有的多台移动机器人协同搬运货物时，多依靠工装将机器人进行柔性连接，或者使用拖车进行辅助搬运，这样被动跟随的机器人没有智能而言，这样的方法也使得机器人搬运系统缺乏灵活性，并且工装成本也较高。在协同工作的机器人系统中，移动机器人的轮子大多采用麦克纳姆轮，依靠轮子的平移特性，多台机器人也可以进行一致的移动，但是在遇到转弯的情况下就难以应付，故亟需做出进一步的完善及改进，以满足目前日益提高的工艺要求。

发明内容

针对现有技术的以上缺陷或改进需求，在对多移动机器人在智能工厂中的协同作业模式特征开展深入研究和分析的基础上，本发明提供了一种多移动机器人协同控制方法及系统，其一方面对于单智能移动机器人采用各类传感器模块来准确采集环境障碍信息和自身运行信息等在内的多种数据，另一方面还经过专门设计的控制决策算法来实现多移动机器人之间的沟通协商，进而达到协同搬运工件行走的目的，因而使得现有技术的智能移动机器人的应用场合得到加大扩展，为多台移动机器人协同搬运大工件的作业提供了有效保障。

为实现上述目的，按照本发明的一个方面，提供了一种多移动机器人协同控制方法，所述控制方法包括以下步骤：

(1)在多个机器人子系统中任选两个机器人子系统构成协商领航层，剩余的机器人子系统构成跟随层；其中被选中的两个机器人子系统分别命名为机器人子系统1及机器人子系统2，且两者用于协商领航，其余机器人子系统跟随以协同搬运；

(2)下发搬运指令到每个机器人子系统，所述机器人子系统1及所述机器人子系统2同时接收指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划；

(3)所述机器人子系统1及所述机器人子系统2分别依据各自的路径规划及位姿计算得到速度控制量；

(4)所述机器人子系统1或者所述机器人子系统2获取所述机器人子系统2或者所述机器人子系统1的位姿及速度控制量，并将获取的数据与自身位姿及速度控制量进行对比计算，以协商确定沿工件方向的共同速度；

(5)所述机器人子系统1及所述机器人子系统2分别依据所述共同速度对各自原速度控制量进行修正以达到协同运行；同时，跟随层的机器人子系统以协商领航层的两个机器人子系统为目标以保证自身与协商领航层的两个机器人子系统的相对距离不变而进行动作控制，由此多个机器人子系统协同完成工件转运。

进一步地，机器人子系统1依据接收到的搬运指令来确定工件搬运的目的位姿[x_g,y_g,θ_g]，继而通过信息分析计算得到自身需到达的目标位姿[x_1g,y_1g,θ_1g]，目标位姿采用以下公式进行计算，机器人子系统2同理：

当机器人子系统1位于前端时，

当机器人子系统1位于后端时，

式中，[x_g,y_g,θ_g]为调度系统所要求货物放置的位置姿态，即目的位姿；d₁为机器人子系统1的支撑货架到工件中心的距离。

进一步地，协商领航层的机器人子系统根据自身的位姿信息及需到达的目标位姿进行路径f(x,y)的规划和行驶速度V的规划，行驶速度V采用以下公式来计算得出：：

(i)当

且

时，

(ii)当

且

时，

其中，V₀为针对轮式机器人预设的减速截止速度，

且表示对应的减速时间，

表示对应的减速距离，t表示时间变量；V_max和a_max分别为针对轮式机器人预设的行驶最大速度和行驶最大加速度；

为机器人的剩余行驶距离；(x_g,yg)和(x,y)分别为目标坐标和机器人当前位置坐标。

进一步地，协商领航层的机器人子系统以自身位姿与规划路径f(x,y)上点的误差为输入，并按照行驶速度V计算出使得机器人子系统按照规划路径进行运动的左右车轮速度控制量[v_l,v_r]，速度控制量[v_l,v_r]采用以下公式来计算得出：

其中，V为机器人的行驶速度，机器人子系统的规划模块已进行规划；L为机器人左右车轮之间的距离；k₁和k₂为控制参数，其值是依据实际情况在0.1至2之间进行调整的；Δy为机器人当前位置与预设轨迹的横向误差；Δθ为机器人当前姿态与追踪点姿态的角度差值。

进一步地，机器人子系统1首先由自身坐标[x₁,y₁]和机器人子系统2的位置[x₂,y₂]计算出工件货物的角度α；然后利用自身的姿态角度θ₁和速度控制量[v_1l,v_1r]以及机器人子系统2的姿态角度θ₂和速度控制量[v_2l,v_2r]分别计算出自身在工件货物方向上的分速度V₁和机器人子系统2在工件方向上的分速度V₂；通过协商算法使V₁和V₂趋于一致，以得到相同的分速度V_Z。

进一步地，协商算法采用以下公式进行：

(i)当V₁≠0且V₂≠0时，

(ii)当V₁＝0或V₂＝0时，V_z＝0

其中，[x₁,y₁,θ₁]为机器人子系统1的位姿；[x₂,y₂,θ₂]为机器人子系统2的位姿；[v_1l,v_1r]为机器人子系统1的速度控制量；[v_2l,v_2r]为机器人子系统2的速度控制量；α为工件货物的姿态角度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；V_Z为经过协商之后，机器人子系统1和机器人子系统2均要在连线方向上达到的速度。

进一步地，机器人子系统1采用以下公式对原速度控制量进行修正，机器人子系统2同理；

(i)当V₁＝0时，

(ii)当V₂＝0时，

若cos(θ₁-α)≠0，则

若cos(θ₁-α)＝0，则

(iii)当V₁≠0且V₂≠0时，

其中，[v_1l,v_1r]为机器人子系统1的原速度控制量；d为两机器人子系统搬运货物时相距的距离；L为机器人子系统的左右车轮距离；V_Z为协商速度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；θ₁为机器人子系统1的姿态角度；α为机器人子系统1和机器人子系统2连线的角度。

进一步地，跟随层的机器人子系统首先计算自身在机器人子系统1和机器人子系统2方向上的跟随速度；之后，对自身在机器人子系统1和机器人子系统2方向上的跟随速度进行补偿；接着，依据自身在机器人子系统1和机器人子系统2连线方向上的跟随分速度

和

计算得到自身的最终输出动作控制量[V_kl,V_kr]。

进一步地，最终输出动作控制量采用以下公式进行计算：

其中，k_p为控制参数，其值依据实际情况在0.1到2之间进行调整；

为机器人子系统1和机器人子系统k连线的角度；

为机器人子系统2和机器人子系统k连线的角度。

按照本发明的另一个方面，提供了一种多移动机器人协同转运控制系统，所述系统包括调度系统及多个机器人子系统，所述调度系统用于给所述机器人子系统发送搬运指令；多个所述机器人子系统是采用如上所述的多移动机器人协同转运控制方法来协同转运工件的；

多个所述机器人子系统分别构成协商领航层及跟随层，协商领航层的机器人子系统用于接收搬运指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划，进而计算得到速度控制量，并相互协商确定沿工件方向的共同速度，由此对各自原速度控制量进行修正以达到协同运行；跟随层的机器人子系统用于以协商领航层的机器人子系统为目标以保证自身与协商领航层的两个机器人子系统的相对距离不变而进行动作控制，由此实现多个机器人子系统协同完成工件转运。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的多移动机器人协同控制方法及系统主要具有以下有益效果：

1.所涉及的机器人子系统可以为智能小车，智能小车皆有单独作业的能力，为标准的背驮式轮式智能小车，在平时搬运工件货物时可以独立完成作业，在需要大工件搬运时又可以组合协同进行搬运，具有较高的利用效率。

2.所述控制方法中没有特殊的辅助搬运工装或者保证工件不会掉落的特殊工装，仅仅依靠多个机器人的协作控制算法即可实现大工件的协同搬运，具有较好的通用性，节约了大量成本。

3.机器人子系统可以绕自身的托举装置中心进行旋转，在搬运过程中每个机器人子系统与搬运工件之间的夹角可以浮动变化，如此使得机器人子系统的运动路线更加灵活，搬运行走过程中的路径也更加灵活多变，可以适应直线、圆弧的多种路线。

4.在本控制方法中，协商领航层的两个机器人子系统使用了协商修正的控制算法，该控制算法利用机器人子系统本身的速度控制量进行协商决策，在两个机器人子系统各自追踪路径的基础上进行速度的协商从而保证工件的协同搬运，在本控制算法中采用了各退一步的折中算法，即保证了各个机器人子系统的利益也符合了协同任务的利益，使得整个系统安全稳定运行。

5.在本控制方法中，协商领航层的机器人子系统的最终控制量的得出由初始速度控制量进行修正得来，修正的过程考虑了另一台协作机器人子系统的速度控制量，本控制方法中对修正的各种情况均予以考虑并得出了较好的修正方法，使双车系统可以适用于各种情况环境。

6.在本控制方法中，双车领航、多车跟随的决策方法可无视队形的计算模型，对不同的编队模型具有普适性。

附图说明

图1是本发明所构建的多移动机器人协同转运的机器人子系统组成的模块及决策分层图；

图2是本发明提供的双移动机器人搬运大工件的工作示意图；

图3是发明提供的多移动机器人协同转运控制方法涉及的协商领航层机器人子系统的控制框图；

图4是图3中的多移动机器人协同转运控制方法涉及的跟随层机器人子系统的控制框图；

图5是本发明提供的协商领航层双车系统速度协商计算的数据模型示意图；

图6是本发明提供的协商领航层单台移动机器人确定自身速度控制量的流程图；

图7是本发明提供的协商领航层双移动机器人的协商控制作业流程示意图。

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：10-通信模块，20-感知模块，30-规划模块，40-决策控制模块，50-执行模块。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

请参阅图7，本发明提供的多移动机器人转运协同控制方法，所述控制方法主要包括以下步骤：

步骤一，在多个机器人子系统中任选两个机器人子系统构成协商领航层，剩余的机器人子系统构成跟随层；其中被选中的两个机器人子系统分别为机器人子系统1及机器人子系统2，且两者用于协商领航，其余机器人子系统跟随以协同搬运。

具体地，请参阅图1及图2，本发明还提供了一种多移动机器人协同转运控制系统，所述控制系统包括多个机器人子系统及调度系统，所述调度系统用于给所述机器人子系统发送搬运指令，所述机器人子系统的数量大于等于2，任选其中的两个机器人子系统构成协商领航层，该两个机器人子系统分别为机器人子系统1及机器人子系统2；剩余的机器人子系统为跟随层(用机器人子系统3到n表示)，如此形成双车协商领航、多车跟随的多移动机器人协同搬运的决策控制方法。其中，协商领航层的两个机器人子系统进行各自的路径规划和控制，并经过动作协作完成两个机器人子系统的协同运行，而跟随层的机器人子系统则以协商领航层的两个机器人子系统为目标保证自身与协商领航层的机器人子系统的相对距离不变而进行动作决策控制。

每个机器人子系统包括通信模块10、感知模块20、规划模块30、决策控制模块40及执行模块50，所述通信模块10包括两层数据交互结构，上层主要对中央调度系统进行数据收发，包括但不限于工件起始位置姿态[x₀,y₀,θ₀]，工件目标位置姿态[x_g,y_g,θ_g]，上层通信数据主要发送给所述规划模块30作为路径规划的输入信息；下层主要面向相互协作的机器人子系统进行数据传输交互，包括但不限于机器人子系统自身的位置姿态[x,y,θ]和速度控制量[v_l,v_r](左右驱动轮的速度)，下层通信数据主要发送给所述决策控制模块40作为决策判断和控制的输入，并同时向其他机器人子系统发送相关信息；

该感知模块20包括位置姿态感知单元及障碍物感知单元，其中所述位置姿态感知单元包括装于机器人前后位置的感知传感器，通过扫描环境信息确定机器人子系统自身在世界坐标系中的位置姿态[x,y,θ]，并将所得位姿信息传输至所述决策控制模块40作为自身动作控制的输入信息；其中，所述障碍物感知单元包括安装于机器人四周位置的传感器，以对机器人周边物体进行观察，进而确定可能出现的障碍物位置及尺寸信息，所得环境及障碍物信息传输至所述规划模块30作为机器人轨迹动作规划的输入信息。

该规划模块30主要为机器人子系统自身进行轨迹规划。首先，依据工件起始位姿信息[x₀,y₀,θ₀]和终点位姿信息[x_g,y_g,θ_g]计算出机器人子系统本身所需的搬运位置姿态和放货位置姿态；然后根据工件起始和终止的位置姿态对机器人子系统进行轨迹规划，并结合障碍物信息合理进行避障路径设计。

该决策控制模块40主要包括动作控制单元、协商决策单元、动作修正单元3个单元，其中，所述动作控制单元主要利用机器人子系统的实时位姿信息[x,y,θ]和预设轨迹上点的误差计算出使机器人回到预定轨迹上来的动作控制量[v_l,v_r](机器人左右车轮速度)；所述协商决策单元则通过获取协作机器人子系统的位姿信息[x',y',θ']和速度控制量[v_l',v_r']，使用协商算法计算出自身的合理协作搬运速度V_z；所述动作修正单元则利用V_z对原速度控制量进行修正以得到最终的速度控制量[V_l,V_r](机器人左右车轮速度)。

该执行模块50包括举升机构和左右行走驱动电机，举升机构主要用于抬升支架进而将工件搬运起来，每个机器人子系统可以相对于举升台的中心进行旋转，从而使工件运输过程中更加灵活；左右行走驱动电机接收控制模块的控制指令，驱动轮子进行转动行走，从而使机器人子系统搬运前行。

轮式的机器人子系统优选差速式小车，其中左右轮为独立驱动的主动轮、前后轮为两个万向从动轮，以此方式提供动力进行转动使机器人前进后退，并可实现在垂直平面内转动以改变方向。该方法使得仅能独立工作的智能轮式移动小车在不需要特定工装的支持下就可以进行多车协同搬运大型工件，并保证运输过程中货物不掉落，提升了智能工厂中移动机器人使用的灵活性。

步骤二，下发搬运指令到每个机器人子系统，所述机器人子系统1及所述机器人子系统2同时接收指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划。

步骤三，所述机器人子系统1及所述机器人子系统2分别依据各自的路径规划及位姿计算得到速度控制量。

步骤四，所述机器人子系统1或者所述机器人子系统2获取所述机器人子系统2或者所述机器人子系统1的位姿及速度控制量，并将获取的数据与自身位姿及速度控制量进行对比计算，以确定沿工件方向的共同速度。

步骤五，所述机器人子系统1及所述机器人子系统2分别依据所述共同速度对各自原速度控制量进行修正以达到协同运行；同时，跟随层的机器人子系统以协商领航层的两个机器人子系统为目标以保证自身的相对距离不变而进行动作控制，由此多个机器人子系统协同完成工件转运。

其中，请参阅图3，所述协商领航层的决策控制主要包括以下步骤：

(1.1)确定终点位姿。

机器人子系统的通信模块接收调度系统的任务指令，明确工件要搬运到的位置及其姿态[x_g,y_g,θ_g]，通过信息分析计算机器人子系统需要到达的终点位置[x_1g,y_1g,θ_1g]或[x_2g,y_2g,θ_2g]。

其中，目标位置姿态优选采用以下公式来计算得出(以机器人子系统1为例)：

当机器人子系统1位于前端时，

当机器人子系统1位于后端时，

式中，[x_g,y_g,θ_g]为调度系统所要求货物放置的位置姿态；d₁为机器人子系统1的支撑货架到工件中心的距离。

(1.2)规划行驶路径。

机器人子系统的规划模块根据自身的位姿信息和需要到达的终点位姿信息进行路径f(x,y)的规划和行驶速度V的规划，其中规划速度V优选采用以下公式来计算得出：

(i)当

且

时，

(ii)当

时，

其中，V₀为针对轮式机器人预设的减速截止速度，

表示对应的减速时间，

表示对应的减速距离，t表示时间变量；V_max和a_max分别为针对轮式机器人预设的行驶最大速度和行驶最大加速度；

为机器人的剩余行驶距离；(x_g,yg)和(x,y)分别为目标坐标和机器人当前位置坐标。

(1.3)轨迹跟踪控制。

请参阅图6，机器人子系统的控制决策模块以自身位姿与规划路径f(x,y)上点的误差为输入，并按照特定的行驶速度V，计算出使得机器人子系统可以按照规划路径进行运动的左右车轮速度控制量[v_l,v_r]。

其中，速度控制量[v_l,v_r]优选采用以下公式来计算得出：

其中，V为机器人的行驶速度(规划模块已进行规划)；L为机器人左右车轮之间的距离；k₁和k₂为控制参数可依据实际情况在0.1至2之间进行调整；Δy为机器人当前位置与预设轨迹的横向误差；Δθ为机器人当前姿态与追踪点姿态的角度差值。

(1.4)动作协商。

请参阅图5，机器人子系统的通信模块获取协作机器人子系统的位姿和速度控制量信息，决策控制模块利用这些信息与自身姿态和速度控制量进行对比计算，确定在工件方向上的共同速度，以保证工件稳定运输。以机器人子系统1为例，首先由自身坐标[x₁,y₁]和机器人子系统2的位置[x₂,y₂]计算出工件货物的角度α；然后利用自身的姿态角度θ₁和速度控制量[v_1l,v_1r]以及机器人子系统2的姿态角度θ₂和速度控制量[v_2l,v_2r]分别计算出自身在工件货物方向上的分速度V₁和机器人子系统2在工件方向上的分速度V₂；通过协商算法使V₁和V₂趋于一致，得到相同的分速度V_Z。

其中，协商算法优选采用以下公式进行计算：

(i)当V₁≠0且V₂≠0时，

(ii)当V₁＝0或V₂＝0时，V_z＝0

其中，[x₁,y₁,θ₁]为机器人子系统1的位姿；[x₂,y₂,θ₂]为机器人子系统2的位姿；[v_1l,v_1r]为机器人子系统1的速度控制量；[v_2l,v_2r]为机器人子系统2的速度控制量；α为工件货物的姿态角度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；V_Z为经过协商之后，机器人子系统1和机器人子系统2均要在连线方向上达到的速度。

(1.5)动作修正。

机器人子系统的控制决策模块对机器人原速度控制量进行修正以达到协同的目的，通过协商得出的一致速度V_Z来调整机器人子系统本身的速度控制量得出修正后的控制动作[V_1l,V_1r]或[V_2l,V_2r]，由此即可满足各个机器人子系统的循迹要求又可保证两个机器人子系统之间的相对距离不变，达成协同一致的搬运工件任务。

其中，修正控制速度[V_l,V_r]优选采用以下公式进行计算(以机器人子系统1为例)：

(i)当V₁＝0时，

(ii)当V₂＝0时，

若cos(θ₁-α)≠0，则

若cos(θ₁-α)＝0，则

(iii)当V₁≠0且V₂≠0时，

其中，[v_1l,v_1r]为机器人子系统1的原速度控制量；d为两机器人子系统搬运货物时相距的距离；L为机器人子系统的左右车轮距离；V_Z为协商速度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；θ₁为机器人子系统1的姿态角度；α为机器人子系统1和机器人子系统2连线的角度。机器人子系统2的相关计算与上面相同。

(1.6)动作执行。

机器人子系统的控制决策模块将修正的速度控制量传输至执行模块的驱动电机，控制电机转动进行协同搬运工作。

(1.7)重复步骤(1.3)至步骤(1.6)直至机器人子系统到达目的地。

请参阅图4，跟随层的决策控制方法包括以下步骤(用机器人子系统k表示跟随层的一个机器人子系统)：

(2.1)计算机器人子系统k在机器人子系统1和机器人子系统2方向上的跟随速度

机器人子系统k的通信模块获取协商领航层的机器人子系统1和机器人子系统2的位姿信息[x₁,y₁,θ₁]、[x₂,y₂,θ₂]和两个机器人子系统输出的动作控制量[V_1l,V_1r]、[V_2l,V_2r]，以此计算出机器人子系统k在与机器人子系统1和机器人子系统2连线方向上的跟随分速度v₁和v₂。

其中，跟随分速度优选采用以下公式进行计算：

其中，[V_1l,V_1r]为机器人子系统1的输出动作控制量；[V_2l,V_2r]为机器人子系统2的输出动作控制量；

为机器人子系统1和机器人子系统k连线的角度；

为机器人子系统2和机器人子系统k连线的角度；[x₁,y₁,θ₁]、[x₂,y₂,θ₂]和[x_k,y_k,θ_k]分别为机器人子系统1、机器人子系统2和机器人子系统k的位姿信息。

(2.2)补偿机器人子系统k在机器人子系统1和机器人子系统2方向上的跟随速度。

机器人子系统k的决策控制模块通过计算机器人子系统k与机器人子系统1和机器人子系统2的当前距离d_1t、d_2t和初始队形距离d_1o、d_2o的差值Δd₁和Δd₂，以对上一步计算的控制量v₁和v₂进行补偿而得到

和

以保证机器人子系统k与机器人子系统1和机器人子系统2的相对距离位置。

其中，补偿后的跟随分速度优选采用以下公式进行计算：

其中，k₁和k₂为控制参数，其值可依据实际情况在0.1到2之间进行调整；d_1o为机器人子系统k与机器人子系统1初始的距离；d_2o为机器人子系统k与机器人子系统2初始的距离。

(2.3)计算机器人子系统k的输出动作控制量

依据机器人子系统k在机器人子系统1、机器人子系统2连线方向上的跟随分速度

和

计算得出机器人子系统k的最终输出动作控制量[V_kl,V_kr]。

其中，最终输出动作控制量优选采用以下公式进行计算：

其中，k_p为控制参数，其值可依据实际情况在0.1到2之间进行调整；

为机器人子系统1和机器人子系统k连线的角度；

为机器人子系统2和机器人子系统k连线的角度。

本实施方式中，首先由调度系统下发搬运指令至每台机器人子系统，机器人子系统1和机器人子系统2同时接收指令后根据工件目的地分别计算各自的目标位姿，并依据环境信息进行路径规划；然后在每个控制指令周期内，每台机器人子系统首先根据自己的规划轨迹和位姿进行速度控制量计算，然后经通信模块获得协作机器人子系统的位姿和速度控制量，利用协商控制算法进行对比协商以确定沿工件方向的共同速度，接下来便依据协商的速度对自身的速度控制量进行修正，以获得最终的速度控制量。机器人子系统行走后判断是否到达目的地，当机器人子系统1和机器人子系统2均达到各自的目的地时流程结束。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种多移动机器人协同转运控制方法，其特征在于，该方法包括以下步骤：

(1)在多个机器人子系统中任选两个机器人子系统构成协商领航层，剩余的机器人子系统构成跟随层；其中被选中的两个机器人子系统分别命名为机器人子系统1及机器人子系统2，且两者用于协商领航，其余机器人子系统跟随以协同搬运；

(2)下发搬运指令到每个机器人子系统，所述机器人子系统1及所述机器人子系统2同时接收指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划；

(3)所述机器人子系统1及所述机器人子系统2分别依据各自的路径规划及位姿计算得到速度控制量；

(4)所述机器人子系统1或者所述机器人子系统2获取所述机器人子系统2或者所述机器人子系统1的位姿及速度控制量，并将获取的数据与自身位姿及速度控制量进行对比计算，以协商确定沿工件方向的共同速度；

(5)所述机器人子系统1及所述机器人子系统2分别依据所述共同速度对各自原速度控制量进行修正以达到协同运行；同时，跟随层的机器人子系统以协商领航层的两个机器人子系统为目标以保证自身与协商领航层的两个机器人子系统的相对距离不变而进行动作控制，由此多个机器人子系统协同完成工件转运；

机器人子系统1或机器人子系统2依据接收到的搬运指令来确定工件搬运的目的位姿[x_g,y_g,θ_g]，继而通过信息分析计算得到自身需到达的目标位姿[x_1g,y_1g,θ_1g]或[x_2g,y_2g,θ_2g]，其中，机器人子系统1的目标位姿采用以下公式进行计算：

当机器人子系统1位于前端时，

当机器人子系统1位于后端时，

式中，[x_g,y_g,θ_g]为调度系统所要求货物放置的位置姿态，即目的位姿；d₁为机器人子系统1的支撑货架到工件中心的距离。

2.如权利要求1所述的多移动机器人协同转运控制方法，其特征在于：协商领航层的机器人子系统根据自身的位姿信息及需到达的目标位姿进行路径f(x,y)的规划和行驶速度V的规划，行驶速度V采用以下公式来计算得出：

(i)当

且

时，

(ii)当

且

时，

其中，V₀为针对轮式机器人预设的减速截止速度，

且表示对应的减速时间，

表示对应的减速距离，t表示时间变量；V_max和a_max分别为针对轮式机器人预设的行驶最大速度和行驶最大加速度；

为机器人的剩余行驶距离；(x_g,y_g)和(x,y)分别为目标坐标和机器人当前位置坐标。

3.如权利要求2所述的多移动机器人协同转运控制方法，其特征在于：协商领航层的机器人子系统以自身位姿与规划路径f(x,y)上点的误差为输入，并按照行驶速度V计算出使得机器人子系统按照规划路径进行运动的左右车轮速度控制量[v_l,v_r]，速度控制量[v_l,v_r]采用以下公式来计算得出：

其中，V为机器人的行驶速度，机器人子系统的规划模块已进行规划；L为机器人左右车轮之间的距离；k₁和k₂为控制参数，其值依据实际情况在0.1至2之间进行调整；Δy为机器人当前位置与预设轨迹的横向误差；Δθ为机器人当前姿态与追踪点姿态的角度差值。

4.如权利要求1所述的多移动机器人协同转运控制方法，其特征在于：机器人子系统1或2首先由机器人子系统1的位置[x₁,y₁]和机器人子系统2的位置[x₂,y₂]计算出工件货物的角度α；然后利用机器人子系统1的姿态角度θ₁和速度控制量[v_1l,v_1r]以及机器人子系统2的姿态角度θ₂和速度控制量[v_2l,v_2r]分别计算出机器人子系统1在机器人子系统1与机器人子系统2连线方向上的分速度V₁和机器人子系统2在机器人子系统1与机器人子系统2连线方向上的分速度V₂；通过协商算法使V₁和V₂趋于一致，以得到相同的分速度V_Z。

5.如权利要求4所述的多移动机器人协同转运控制方法，其特征在于：协商算法采用以下公式进行：

(i)当V₁≠0且V₂≠0时，

(ii)当V₁＝0或V₂＝0时，V_z＝0

其中，[x₁,y₁,θ₁]为机器人子系统1的位姿；[x₂,y₂,θ₂]为机器人子系统2的位姿；[v_1l,v_1r]为机器人子系统1的速度控制量；[v_2l,v_2r]为机器人子系统2的速度控制量；α为工件货物的姿态角度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；V_Z为经过协商之后，机器人子系统1和机器人子系统2均要在连线方向上达到的速度。

6.如权利要求1所述的多移动机器人协同转运控制方法，其特征在于：机器人子系统1采用以下公式对原速度控制量进行修正，机器人子系统2同理；

(i)当V₁＝0时，

(ii)当V₂＝0时，

若cos(θ₁-α)≠0，则

若cos(θ₁-α)＝0，则

(iii)当V₁≠0且V₂≠0时，

其中，[v_1l,v_1r]为机器人子系统1的原速度控制量；d为两机器人子系统搬运货物时相距的距离；L为机器人子系统的左右车轮距离；V_Z为协商速度；V₁为机器人子系统1在机器人子系统1和机器人子系统2连线方向上的速度；V₂为机器人子系统2在机器人子系统1和机器人子系统2连线方向上的速度；θ₁为机器人子系统1的姿态角度；α为机器人子系统1和机器人子系统2连线的角度。

7.如权利要求1-6任一项所述的多移动机器人协同转运控制方法，其特征在于：跟随层的机器人子系统首先计算自身在机器人子系统1和机器人子系统2方向上的跟随速度；之后，对自身在机器人子系统1和机器人子系统2方向上的跟随速度进行补偿；接着，依据自身在机器人子系统1和机器人子系统2连线方向上的跟随分速度

和

计算得到自身的最终输出动作控制量[V_kl,V_kr]。

8.如权利要求7所述的多移动机器人协同转运控制方法，其特征在于：最终输出动作控制量采用以下公式进行计算：

其中，k_p为控制参数，其值依据实际情况在0.1到2之间进行调整；

为机器人子系统1和机器人子系统k连线的角度；

为机器人子系统2和机器人子系统k连线的角度。

9.一种多移动机器人协同转运控制系统，其特征在于：所述控制系统包括调度系统及多个机器人子系统，所述调度系统用于给所述机器人子系统发送搬运指令；多个所述机器人子系统是采用权利要求1-8任一项所述的多移动机器人协同转运控制方法来协同转运工件的；

多个所述机器人子系统分别构成协商领航层及跟随层，协商领航层的机器人子系统用于接收搬运指令后根据工件的目的地分别计算各自的目标位姿，并依据环境信息进行路径规划，进而计算得到速度控制量，并相互协商确定沿工件方向的共同速度，由此对各自原速度控制量进行修正以达到协同运行；跟随层的机器人子系统用于以协商领航层的机器人子系统为目标以保证自身与协商领航层的两个机器人子系统的相对距离不变而进行动作控制，由此实现多个机器人子系统协同完成工件转运。

Images