CN112394732B - 一种适用于机器人集群协同搬运的接触力动态分配方法 - Google Patents

Abstract

本发明一种适用于机器人集群协同搬运的接触力动态分配方法。该方法采用分布式计算策略，使各移动机器人根据预先规划出的被搬运物体标称运动轨迹以及实时获取的被搬运物体实际运动状态信息，解算出对被搬运物体的所需的总控制力和控制力矩，并结合各移动机器人与被搬运物体的接触点真实位置和真实接触力，计算被搬运物体当前时刻所需的总控制力增量和总控制力矩增量；各个移动机器人根据移动机器人集群当前时刻的实际相对位置，将总控制力增量和总控制力矩增量分解给自身以及其他移动机器人，自身对被搬运物体所需施加的正向接触力增量追加到自身与被搬运物体的正向接触力上；从而使机器人集群共同确定各机器人对被搬运物体所需施加的正向接触力。

Description

一种适用于机器人集群协同搬运的接触力动态分配方法

技术领域

本发明涉及移动机器人集群领域，适用于工业生产或物流运输中的仓储问题，也适用于月球基地建设中的大型物资协同搬运问题，尤其涉及机器人集群协同搬运问题中机器人与被搬运物体间的接触力动态分配问题。

背景技术

随着多智能体协同控制技术的发展，机器人集群在物流运输领域逐渐获得广泛应用，对机器人集群控制领域的研究也逐渐深入。其中，移动机器人协同搬运(cooperativetransportation)问题便是一个重要研究方向。但现有的研究侧重于机器人集群最优包围构型的生成以及机器人与被搬运物体存在固定连接下的动力学控制问题，而松散连接下的协同搬运问题容易退化成平面推箱(box-pushing)问题，无法体现物体离开地面后的动力学特性。并且许多控制策略依赖于集中式控制结构以对被搬运物体进行较精确的控制，机器人运动决策的自主性以及系统的可扩展性不强，并且容易引起通信阻塞，从而限制了控制的实时性。

发明内容

本发明的目的在于为移动机器人集群对箱式物体的协同搬运问题提供适用于机器人集群协同搬运的接触力动态分配方法，从而解决现有技术中存在的诸如系统可扩展性不强、机器人与对象间的松散连接难以对被搬运物体进行精确的动力学控制等问题。

本发明是通过以下技术方案来实现：

一种适用于机器人集群协同搬运的接触力动态分配方法，包含如下步骤：

S1：明确被搬运物体的大小、尺寸和质心位置，以及搬运被搬运物体的移动机器人个数N和尺寸；

S2：由被搬运物体的标称运动轨迹计算被搬运物体当前时刻的期望运动状态，并测量出被搬运物体当前时刻的实际运动状态；以及N个移动机器人与被搬运物体的接触点在被搬运物体本体坐标系下的真实位置及在惯性系下的真实接触力；

S3：各个移动机器人根据被搬运物体的标称运动轨迹，以及被搬运物体当前时刻的实际运动状态，计算出被搬运物体当前时刻所需的总控制力和总控制力矩，并结合S2中获取的各移动机器人与被搬运物体的接触点真实位置和真实接触力，计算被搬运物体当前时刻所需的总控制力增量和总控制力矩增量；

S4：各个移动机器人根据移动机器人集群当前时刻的实际相对位置，将总控制力增量和总控制力矩增量分解给自身以及其他移动机器人，对所有移动机器人需施加的正向接触力增量进行解算；

S5：各个移动机器人对所有移动机器人对自身需施加的正向接触力增量的解算值进行平均，作为自身对被搬运物体所需施加的正向接触力增量，追加到自身与被搬运物体的正向接触力上；

S6：重复执行S2-S5，直到移动机器人集群整体运动到目标位置。

优选的，S2中，被搬运物体的标称运动轨迹由路径规划算法事先规划好或者人为设计成指定函数曲线。

优选的，S2中，所述期望运动状态包括质心期望位置、质心期望速度、质心期望加速度、期望方位角、期望方位角速度、期望方位角加速度。

优选的，S2中，所述实际运动状态包括质心真实位置、质心真实速度、真实方位角和真实方位角速度。

优选的，S3中，采用比例-微分控制方法计算出被搬运物体当前时刻所需的总控制力和总控制力矩。

优选的，S3中，被搬运物体当前时刻所需的总控制力和总控制力矩均为两部分的加权和，一部分为被搬运物体标称运动轨迹的二阶状态量，另一部分为被搬运物体期望运动状态量与真实运动状态量间的误差。

优选的，S3中，被搬运物体所需的总控制力和总控制力矩表示为：

其中，m_b为被搬运物体的质量；J_b为被搬运物体在z方向相对于质心的转动惯量；k_X、k_V、k_θ、k_ω为常值比例系数；

为质心期望位置；

为质心期望速度；

为质心期望加速度；θ_bd(t)为期望方位角；ω_bd(t)为期望方位角速度；β_bd(t)为期望方位角加速度；

为被搬运物体的质心真实位置，

为质心真实速度，θ_b(t)为真实方位角，ω_b(t)为真实方位角速度。

优选的，S4中，各移动机器人对所有移动机器人的正向接触力增量解算均遵循所有移动机器人正向接触力增量的合力与S3中被搬运物体所需的总控制力增量相等，所有移动机器人正向接触力增量的合力矩与S3中被搬运物体所需的总控制力矩增量相等。

优选的，S4中，各移动机器人i将总控制力增量在被搬运物体同侧的各移动机器人间分配，不涉及移动机器人i相对于被搬运物体对侧的各移动机器人与被搬运物体间接触力的调整。

优选的，各移动机器人如果未能从S2中获得当前时刻移动机器人与被搬运物体间的接触点的真实位置和真实接触力，则S3中各移动机器人与被搬运物体间的真实接触力分别沿用上一时刻所用的的真实接触力信息。

与现有技术相比，本发明具有以下有益的技术效果：

本发明针对移动机器人对物体进行协同搬运的问题，提出了一种用于移动机器人集群协同搬运的移动机器人与被搬运物体间接触力的分布式动态分配方法。该方法各个移动机器人根据移动机器人集群当前时刻的实际相对位置，将总控制力增量和总控制力矩增量分解给自身以及其他移动机器人，因而不要求集群相对于被搬运物体时刻保持初始编队构型，即利用了移动机器人相对于被搬运物体的编队构型信息，但不要求移动机器人集群时刻保持初始编队构型，因此适用于机器人构成松散编队对物体进行协同搬运的情形。并且，该方法采用去中心化的分布式计算与个体间协商策略，充分发挥各个移动机器人的自主计算与决策能力，无需依赖中心式的控制结构，使得该算法具有一定的可扩展性。本发明采用增量形式的控制力解算与分配策略，无需依赖于各移动机器人时刻获取其他移动机器人与被搬运物体准确的接触力信息，在减少信息传输量的同时，适用于移动机器人与被搬运物体间作用力跳变、无法准确测量的情形。本发明中的移动机器人接触力分配仅在被搬运物体同侧的机器人之间进行，不涉及单个移动机器人与相对于被搬运物体另一侧的各移动机器人间的接触力分配，能够缓解接触力动态分配的高频通信需求与由被搬运物体遮挡作用造成的移动机器人间通信衰减之间的矛盾。

进一步的，各移动机器人如果未能从S2中获得移动机器人与被搬运物体间的接触点的真实位置和真实接触力，则S3中各移动机器人与被搬运物体间的真实接触力分别沿用上一时刻所用的的真实接触力信息，从而允许通信发生短时中断。

附图说明

图1为本发明的一种适用于移动机器人集群协同搬运的接触力动态分配方法执行流程图；

图2为本发明所采用的一种类叉车移动机器人结构图；

图3为移动机器人集群执行协同搬运任务时的编队构型示意图；(a)编队构型侧视图，(b)编队构型俯视图；

图4为坐标系的定义(图中放大了被搬运物体质心期望位置与真实位置之间的距离)；

图5为移动机器人侧视图中的各尺寸参数；

图6为移动机器人俯视图中的各尺寸参数(省去顶端的传感器)；

图7表示机器人集群在被搬运物体相对于运动方向前后两侧均匀分散过程；

图8表示机器人集群整体编队构型中心与被搬运物体质心对齐的过程；(a)对齐前，(b)对齐后。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

一种适用于机器人集群协同搬运的接触力动态分配方法，包含如下步骤：

S1：明确被搬运物体大小、尺寸、质心位置，以及搬运被搬运物体的移动机器人个数N、尺寸；

S2：由被搬运物体的标称运动轨迹(事先规划好，并存储在各个移动机器人及上位机中)，计算被搬运物体当前时刻的期望运动状态，包括期望位置、期望速度、期望加速度、期望方位角和期望方位角速度，并测量出被搬运物体当前时刻的实际运动状态，包括质心真实位置、质心真实速度、真实方位角和真实方位角速度，以及N个移动机器人与被搬运物体的接触点在被搬运物体本体坐标系下的真实位置及在惯性系下的接触力；各移动机器人i当前时刻t与被搬运物体的接触点位置

取在被搬运物体边缘，由机器人i自身携带的传感器测定，在惯性坐标系下表示，并通过移动机器人间通信在移动机器人集群中实时共享。各移动机器人i当前时刻t与被搬运物体的接触力

作用在该机器人与被搬运物体接触点

处，由机器人i前部的力传感器测定，在惯性坐标系下表示，并通过机器人间通信在机器人集群中实时共享。

S3：各个移动机器人根据被搬运物体的标称运动轨迹，以及被搬运物体当前时刻的实际运动状态，计算出被搬运物体当前时刻所需的总控制力和控制力矩，并结合S2中获取的各机器人与被搬运物体的接触点真实位置和真实接触力，计算被搬运物体当前时刻所需的总控制力增量和总控制力矩增量(在初始时刻，即机器人集群刚开始搬运物体时，机器人与被搬运物体接触点的真实位置允许进行调整，以尽可能相对于被搬运物体质心均匀分布，因而各个机器人与被搬运物体接触点真实位置根据S1中的各参数确定，接触力取为0，即被搬运物体所需的总控制力增量/总控制力矩增量恰好等于所需的总控制力/总控制力矩)；

S4：各个移动机器人根据移动机器人集群当前时刻的实际相对位置，将总控制力增量和控制力矩增量分解给自身以及其他移动机器人，对所有移动机器人需施加的正向接触力增量(在初始时刻恰好等于该移动机器人需施加的正向接触力)进行解算；

S5：各个移动机器人对所有移动机器人对自身应需施加的正向接触力增量的解算值进行平均，作为自身对被搬运物体所需施加的正向接触力增量；各移动机器人i如果未能获得当前时刻t移动机器人j与被搬运物体间的接触点位置

和接触力

则对其分别沿用上一时刻所采用的接触点真实位置

和真实接触力

Δt为采样周期。

S6：重复执行S2-S5，直到移动机器人集群整体运动到目标位置。

上述步骤可表示为流程图如图1所示。

本发明以如下类叉车移动机器人(图2)为例说明接触力的动态分配过程。被搬运物体形状限制为长方体，可以使用空的或盛有物体的长方体纸箱，其一条长边朝向运动方向，移动机器人集群分别贴附在其前后两条长边上，宽边不贴附，如图3所示。该类叉车移动机器人是通过将货叉叉入箱式物体底部将物体举起，并且通过类叉车移动机器人前部的与箱式物体间的正向夹持作用来对箱式物体进行协同搬运(图3)。该类叉车移动机器人货叉可以在类叉车移动机器人前部与被搬运物体正向接触力不足时对物体施加摩擦力作用，甚至可以使类叉车移动机器人施加朝类叉车移动机器人后向的作用力。该类叉车移动机器人与被搬运物体正向接触面配有左右两个压力传感器，可通过两个压力传感器的差值测得当前时刻类叉车移动机器人与被搬运物体间的实际正向接触力大小和方向。每个类叉车移动机器人可以存储被搬运物体各时刻的标称运动轨迹，可以对自身以及被搬运物体的实时运动状态进行测量，并可通过自身通讯设备实现彼此间信息交互。本发明提供的方法具有一定的普适性，适用范围不限于该类叉车移动种机器人，如对于与被搬运物体固连的移动机器人仍适用。

为了问题讨论方便，本发明假设除了对移动机器人最小正向接触力的计算之外，问题的讨论限制在二维运动平面之内。移动机器人对被搬运物体只考虑力作用，不考虑由于单个机器人接触力的分布特性引起的附加扭矩。并且为了避免被搬运物体的失控，其运动状态只发生缓慢变化，由其期望运动状态确定的期望平动和转动加速度近似为0。

为了方便各移动机器人与被搬运物体的状态量及接触力的表示，定义如下坐标系(图4)：

惯性坐标系Oxyz：与地面固连的坐标系，z轴垂直于地面向上。

轨迹坐标系O_tx_ty_tz_t：沿被搬运物体标称运动轨迹运动的坐标系，原点在被搬运物体质心当前期望位置处，x轴沿被搬运物体由标称运动轨迹确定的当前时刻前进方向，z轴向上。

被搬运物体本体坐标系O_bx_by_bz_b：与被搬运物体固连，坐标原点为被搬运物体质心，三坐标轴分别与被搬运物体的棱平行，z轴向上，x轴指向被搬运物体由标称运动轨迹确定的当前时刻前进方向。

移动机器人本体坐标系O_ix_iy_iz_i(i＝1,2,3,...,N)：与移动机器人固连，坐标原点为移动机器人质心，z轴向上，x轴指向移动机器人前进方向。

为了方便公式表达，已知下列参数：

m_b——箱式物体的质量；

J_b——箱式物体在z方向相对于质心的转动惯量；

a——箱式物体的长；

b——箱式物体的宽；

l₁——移动机器人质心到牛眼轮最下方的水平距离；

l₂——移动机器人质心到其后轮中心的水平距离；

l₃——移动机器人的货叉水平面前缘到牛眼轮最下方的水平距离；

l₄——移动机器人质心到移动机器人的货叉上方垫片中心的垂直距离；

l₅——移动机器人货叉上方垫片中心到货叉水平面的垂直距离；

l₆——移动机器人货叉厚度；

h——移动机器人货叉底部离地面的高度(在移动机器人集群整体运动阶段保持为定值)；

l_r——移动机器人与被搬运物体接触面的宽度；

b_r——移动机器人的最大宽度(取为移动机器人两个驱动轮外缘间的距离)；

R——移动机器人驱动轮半径。

移动机器人的尺寸参数在移动机器人侧视图和俯视图中的表示分别如图5和图6所示。

为了使箱式物体与尽可能多的移动机器人相贴附，假设箱式物体运动方向前方为其一条长边(边长为a)，机器人仅贴附在箱子长边上，如图4所示。

为了方便描述，在移动机器人集群初始搬运构型确定中，为了使移动机器人集群编队构型相对于被搬运物体质心尽可能对称分布，以便于对移动机器人施加的正向接触力的解算以及移动机器人对被搬运物体的控制，移动机器人个数可以灵活变化，在尺寸条件允许的情况下，可以取为[2,N_max]内的任意正整数。

具体地，移动机器人最大个数N_max由如下公式确定

其中a为箱子的长，b_r为移动机器人最大宽度(取为移动机器人两个驱动轮外缘间的距离，见图6)，

为向下取整符号。

本发明给出一种移动机器人初始编队构型配置方案：

1.将N个移动机器人分成两拨，分别设于被搬运物体前方和后方。如果N为奇数，则被搬运物体后方设置

个机器人执行搬运任务(

为向下取整符号)，其余

个机器人在被搬运物体前方执行搬运任务；如果N为偶数，则被搬运物体前后分配相同数量的移动机器人，即

2.分别使被搬运物体前方和后方的移动机器人均匀分散，即使得被搬运物体任意一侧的移动机器人彼此间的边缘距离以及两端的移动机器人边缘与被搬运体侧边缘的距离均相等(分别记为δ_f与δ_b)(图7)。

3.如果被搬运物体质心偏向其某一条宽边，则保持最靠近该宽边的移动机器人不动，使移动机器人编队构型整体向该宽边方向压缩，使编队整体质心与被搬运物体质心对齐(图8)。

根据以上方案，可以确定N个移动机器人的初始搬运点在被搬运物体本体坐标系下的位置

其中x_i＝±b/2，具体取决于该移动机器人位于被搬运物体的前方(正号)或后方(负号)。

为了方便描述，当移动机器人集群初始搬运构型确定后，各移动机器人与被搬运物体间的初始接触力通过以下策略进行分配(采用平衡条件，即各移动机器人与被搬运物体的接触力合力和合力矩均为0)：

1.如果移动机器人集群个数为奇数，则先使被搬运物体运动方向前方的各个移动机器人i与被搬运物体的接触力

取为相同值，然后确定被搬运物体运动方向后方的各个移动机器人j对被搬运物体相同大小和方向的接触力

以平衡移动机器人集群对被搬运物体的力作用；

2.如果移动机器人集群个数为偶数，则使各个移动机器人i与被搬运物体的接触力

取为相同值，方向垂直于被搬运物体表面向内。

在移动机器人集群搬运物体整体移动过程中，借助移动机器人个体间的广播通信，各个移动机器人i(i∈{1,2,...,N})与被搬运物体间的正向接触力通过如下策略进行动态分配：

1.移动机器人i获取所有移动机器人j(j∈{1,2,...,N})的当前运动状态(包括方位θ_j(t)、角速度ω_j(t)，以及搬运点的位置X_j(t)和速度V_j(t)，可借助于从移动机器人的惯性导航元件获取的自身状态信息进行解算)，以及被搬运物体的实际运动状态(包括质心真实位置

质心真实速度

质心真实加速度A_b(t)＝(a_xb(t),a_yb(t))^T、真实方位角θ_b(t)、真实方位角速度ω_b(t)，这些状态量亦在惯性坐标系下表示)。被搬运物体质心位置是指被搬运物体质心相对于其本体的位置，表征被搬运物体的质心偏移情况，而非被搬运物体质心在惯性坐标系下的坐标。该质心位置可通过调整箱内物体位置进行人为调整，也可通过实验方法测定。

2.移动机器人i根据被搬运物体的标称运动轨迹，确定被搬运物体当前时刻的期望运动状态(包括各时刻t下被搬运物体的质心期望位置

质心期望速度

质心期望加速度

期望方位角θ_bd(t)、期望方位角速度ω_bd(t)、期望方位角加速度β_bd(t))。这些期望状态量在惯性坐标系下表示。

3.移动机器人i根据被搬运物体的真实运动状态和期望运动状态，采用比例-微分控制方法(公式(8))计算出被搬运物体所需的总控制力和总控制力矩。被搬运物体当前时刻t所需的总控制力和总控制力矩均为两部分的加权和，一部分为由被搬运物体标称运动轨迹的二阶状态量(即期望质心加速度

或期望角加速度β_bd(t))，另一部分表示被搬运物体期望运动状态量与真实运动状态量间的误差，具体由公式(8)确定。

4.移动机器人i根据被搬运物体当前所需总控制力，以及所有移动机器人的当前运动状态和接触力，计算出其关于被搬运物体同侧各移动机器人j(包括自身)接触力的变化量

(上标表示该值由哪个机器人算得)，以使被搬运物体所受合力

等于其当前所需总控制力

被搬运物体所受合力矩M_b(t)等于其当前所需总控制力矩M_bd(t)；其关于被搬运物体对侧各移动机器人k的接触力变化量计算值

默认为0。各移动机器人i仅将所需的接触力在被搬运物体同侧的各移动机器人间分配，不涉及移动机器人i相对于被搬运物体对侧的各移动机器人接触力的调整，即这些移动机器人的接触力变化量设为0，以减弱被搬运物体对机器人个体间的通讯遮挡作用对控制算法有效性的影响。

5.移动机器人i将其所计算的各移动机器人的接触力变化量

进行广播，共享给所有移动机器人，同时获得其他移动机器人算得的本移动机器人与被搬运物体间的正向接触力变化量

6.移动机器人i由其当前正向接触力

以及各移动机器人解算出的自身正向接触力变化量

通过公式(14)计算出下一时刻移动机器人i自身所需施加的正向接触力

假设被搬运物体为刚体，则移动机器人与被搬运物体的接触位置被约束在被搬运物体前缘或后缘所在的线段上，可以利用被搬运物体真实质心位置、真实方位角求得在被搬运物体本体坐标系下表示的移动机器人与被搬运物体的接触位置

应满足的如下形式关系。

被搬运物体前方移动机器人：

其中x_b为大写

(质心真实位置)在横轴上的分量，y_b为大写

在纵轴上的分量。该式可改写为

被搬运物体后方移动机器人：

可改写为

进一步地，可由t时刻下移动机器人i(i＝1,2,...,N)在被搬运物体本体坐标系下表示的接触位置

通过如下坐标变换关系得到该移动机器人在惯性系下表示的接触位置X_i(t)＝(x_i(t),y_i(t))^T：

其中θ_b(t)为当前时刻被搬运物体的真实方位角。

已知被搬运物体当前时刻的位置、速度、方向的期望值和实际值，采用比例-微分控制方法，则被搬运物体所需的总控制力和总控制力矩可表示为

被搬运物体所需的总控制力和总控制力矩的增量可表示为

其中k_X、k_V、k_θ、k_ω为常值比例系数。

被搬运物体后方各移动机器人i根据下列公式计算出其关于被搬运物体同侧的各个移动机器人j(j＝1,2,...,k，k为被搬运物体后侧机器人个数)的控制力

写成分量形式为

写成矩阵形式为

x_i为大写X_i(移动机器人i接触点在惯性系下的位置)在惯性系横轴方向的分量；y_i为大写X_i在惯性系纵轴方向的分量。

其中各变量略去当前时刻(t)。该2k元一次方程组为欠定方程组，具有无数组解。但由于系数矩阵行满秩(将前三列施加列变换成为对角子矩阵可知)，对系数矩阵采用广义逆可得极小2-范数解。

X＝A^T(AA^T)^-1B (13)

其中：

对于被搬运物体前方的各个移动机器人i，采用公式(13)计算其关于被搬运物体同侧的各个移动机器人j(j＝k+1,k+2,...,N，k为被搬运物体后侧机器人个数)的控制力

时，只需要更改系数矩阵A和自变量矩阵X中各变量的下标，使

如果公式(8)～(13)中某些变量未能获取当前时刻值，则对该变量沿用上一时刻值作为当前时刻值。

已知移动机器人i当前时刻与被搬运物体间的实际正向接触力

以及各移动机器人对自身与被搬运物体间的正向接触力变化量的解算值

下一时刻机器人i自身所需施加的正向接触力

由下列公式解算：

其中

为了使本领域人员更好地理解本发明中的技术方案，下面列举一个具体实施例，说明本发明所述的接触力动态分配方法的计算过程。

S1：输入移动机器人个数N＝4，与被搬运物体接触面宽度l_r＝0.2，被搬运物体质量m_b＝0.1，尺寸为a＝1，b＝1，质心位于其几何中心处；

S2：输入被搬运物体当前时刻的质心期望位置

质心期望速度

质心期望加速度

(假设被搬运物体质心运动速度无突变时，期望加速度较小)，期望方位角θ_bd(t)＝0，期望方位角速度ω_bd(t)＝0(假设被搬运物体沿直线运动或无方向突变)，被搬运体当前时刻质心真实位置

质心真实速度

真实方位角θ_b(t)＝0.01，真实方位角速度ω_b(t)＝0.01，以及N个移动机器人的搬运点在惯性坐标系下的位置(x_i,y_i)(i＝1,2,...,N)与接触力(F_xi,F_yi)(i＝1,2,...,N)如表1所示。

表1移动机器人的搬运点在惯性坐标系下的位置与接触力

i	1	2	3	4
					x<sub>i</sub>	-0.3941	-0.3879	0.6056	0.6130
y<sub>i</sub>	0.312	-0.304	0.353	-0.387
					F<sub>xi</sub>	0.501	0.495	-0.504	-0.503
F<sub>yi</sub>	0.005	-0.001	-0.002	-0.003

其中长度单位默认为m，角度单位默认为rad，时间单位默认为s，质量单位默认为kg。

注：这里的x_i是由被搬运物体真实位置和方位角以及移动机器人与被搬运物体接触点纵坐标算得的。将被搬运物体看做刚体时，移动机器人i与被搬运物体接触点被约束在被搬运物体边缘形成的线段上，接触点坐标(x_i,y_i)满足

可改写为

其中正负号取决于移动机器人在被搬运物体前方(正方)还是后方(负号)。

S3：根据公式(8)，计算出当前时刻应施加的总控制力和总控制力矩(比例系数k_X、k_V、k_θ、k_ω均取为1)

S4：每个移动机器人根据各移动机器人相对于被搬运物体的分布及其施力情况，计算出自己所需施加的控制力和控制力矩。

具体地，每个移动机器人先计算当前被搬运物体所受总控制力

总控制力矩

M_b(t)＝M₁(t)+M₂(t)+M₃(t)+M₄(t)

＝(-0.5,0.312)^T×(0.501,0.005)^T+(-0.5,-0.304)^T×(0.495,-0.001)^T+(0.5,0.353)^T×(-0.504,-0.002)^T+(0.5,-0.387)^T×(-0.503,-0.003)^T

＝-0.0271(负号表示力矩沿-z方向，即顺时针方向)

从而得被搬运物体所需的总控制力增量

总控制力矩增量

ΔM_b(t)＝M_b(t)-M_bd(t)＝-0.071。

根据公式(13)，各移动机器人对所有移动机器人的控制力计算结果为

1号机器人：

2号机器人：

3号机器人：

4号机器人：

由于各移动机器人所获取的其他移动机器人当前时刻位置与控制力数据存在一定差异，因此实际上1号机器人与2号机器人的计算结果以及3号机器人与4号机器人的计算结果并不会精确相同。

S5：各移动机器人对自身及其他移动机器人对自身控制力变化量的解算值进行平均，得到各移动机器人控制力的变化值

将这些控制力变化值分别加到各移动机器人当前控制力上即得到各移动机器人下一时刻所需控制力。

Claims (10)

1.一种适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，包含如下步骤：

S1：明确被搬运物体的大小、尺寸和质心位置，以及搬运被搬运物体的移动机器人个数N和尺寸；

S2：由被搬运物体的标称运动轨迹计算被搬运物体当前时刻的期望运动状态，并测量出被搬运物体当前时刻的实际运动状态；以及N个移动机器人与被搬运物体的接触点在被搬运物体本体坐标系下的真实位置及在惯性系下的真实接触力；

S3：各个移动机器人根据被搬运物体的标称运动轨迹，以及被搬运物体当前时刻的实际运动状态，计算出被搬运物体当前时刻所需的总控制力和总控制力矩，并结合S2中获取的各移动机器人与被搬运物体的接触点真实位置和真实接触力，计算被搬运物体当前时刻所需的总控制力增量和总控制力矩增量；

S4：各个移动机器人根据移动机器人集群当前时刻的实际相对位置，将总控制力增量和总控制力矩增量分解给自身以及其他移动机器人，对所有移动机器人需施加的正向接触力增量进行解算；

S5：各个移动机器人对所有移动机器人对自身需施加的正向接触力增量的解算值进行平均，作为自身对被搬运物体所需施加的正向接触力增量，追加到自身与被搬运物体的正向接触力上；

S6：重复执行S2-S5，直到移动机器人集群整体运动到目标位置。

2.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S2中，被搬运物体的标称运动轨迹由路径规划算法事先规划好或者人为设计成指定函数曲线。

3.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S2中，所述期望运动状态包括质心期望位置、质心期望速度、质心期望加速度、期望方位角、期望方位角速度、期望方位角加速度。

4.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S2中，所述实际运动状态包括质心真实位置、质心真实速度、真实方位角和真实方位角速度。

5.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S3中，采用比例-微分控制方法计算出被搬运物体当前时刻所需的总控制力和总控制力矩。

6.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S3中，被搬运物体当前时刻所需的总控制力和总控制力矩均为两部分的加权和，一部分为被搬运物体标称运动轨迹的二阶状态量，另一部分为被搬运物体期望运动状态量与真实运动状态量间的误差。

7.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S3中，被搬运物体所需的总控制力和总控制力矩表示为：

其中，m_b为被搬运物体的质量；J_b为被搬运物体在z方向相对于质心的转动惯量；k_X、k_V、k_θ、k_ω为常值比例系数；

为质心期望位置；

为质心期望速度；

为质心期望加速度；θ_bd(t)为期望方位角；ω_bd(t)为期望方位角速度；β_bd(t)为期望方位角加速度；

为被搬运物体的质心真实位置，

为质心真实速度，θ_b(t)为真实方位角，ω_b(t)为真实方位角速度。

8.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S4中，各移动机器人对所有移动机器人的正向接触力增量解算均遵循所有移动机器人正向接触力增量的合力与S3中被搬运物体所需的总控制力增量相等，所有移动机器人正向接触力增量的合力矩与S3中被搬运物体所需的总控制力矩增量相等。

9.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，S4中，各移动机器人i将总控制力增量在被搬运物体同侧的各移动机器人间分配，不涉及移动机器人i相对于被搬运物体对侧的各移动机器人与被搬运物体间接触力的调整。

10.根据权利要求1所述的适用于机器人集群协同搬运的接触力动态分配方法，其特征在于，各移动机器人如果未能从S2中获得当前时刻移动机器人与被搬运物体间的接触点的真实位置和真实接触力，则S3中各移动机器人与被搬运物体间的真实接触力分别沿用上一时刻所用的真实接触力信息。

Images