CN113172627A - 多移动机械手协同搬运系统运动学建模与分布式控制方法 - Google Patents

Abstract

本发明涉及一种多移动机械手协同搬运系统运动学建模方法，包括以下步骤：定义关键节点和坐标系；运用运动链描述相邻关键节点之间的位姿变换、关键节点在坐标系内的位姿以及坐标系之间的位姿关系；之后得到非邻居节点间运动链，对非邻居节点间运动链进行非线性映射，获得多移动机械手协同搬运系统运动学模型。此外，还提供了一种多移动机械手协同搬运系统分布式控制方法。上述多移动机械手协同搬运系统运动学建模与分布式控制方法，实现了系统整体分析和建模，降低了系统计算负担，提高了系统鲁棒性和抗毁性，提高了系统稳定性和均衡各移动机械手间的载荷分配。

Description

多移动机械手协同搬运系统运动学建模与分布式控制方法

技术领域

本发明涉及多智能控制领域，特别是涉及一种多移动机械手协同搬运系统运动学建模与分布式控制方法。

背景技术

在工业自动化、物流以及军事领域，固定基座机械臂越来越无法满足大范围工作空间的需要，移动机械手因其移动平台灵活性和移动机械手可操作性而受到广泛关注。例如，当前工业机器人的应用大多是每个工位以机器人独立工作为主，此类机器人只能适应特定的产品和工作环境，并且依赖于所提供的专用设备和工装夹具。然而，对于许多任务而言，这种独立工作的机器人操作是不够的。为了适应任务复杂性、操作智能性以及系统柔顺性的要求，需要拓展为多移动机械手智能协同作业。在救灾、输送设备、勘察、救援等领域，同样需要多移动机器人系统完成单体机器人难以胜任的协同操作任务。

多移动机械手协同操作具有灵活性、操作性、负载大等优势，但应用过程中仍存在以下问题，从建模方法上来看，由于移动机械手具有很强的冗余性，多移动机械手协同过程中会产生更多冗余自由度和复杂的运动约束，采用传统建模方式，无法有效建立其运动学表达，不能保证多移动机械手协同操作的一致性与稳定性。

发明内容

基于此，有必要针对传统的建模方法无法有效建立运动学表达，不能保证多移动机械手协同操作的一致性与稳定性问题，提供一种多移动机械手协同搬运系统运动学建模与分布式控制方法。

一种多移动机械手协同搬运系统运动学建模方法，所述移动机械手包括移动平台与设置在所述移动平台上的机械臂，所述机械臂具有至少一个机械臂关节，其特征在于，所述运动学建模方法包括以下步骤：

定义关键节点和坐标系；其中，所述关键节点包括表示被搬运物体质心的上虚节点n_v,s、表示所述移动机械手抓取点的上实节点n_r,si、表示多个移动平台组成编队几何中心点的下虚节点n_v,o与表示移动平台几何中心的下实节点n_r,ci；所述坐标系包括工作空间坐标系∑g、上虚节点坐标系∑s、下虚节点坐标系∑o，上实节点坐标系∑si和下实节点坐标系∑ci；

运用运动链描述所述相邻关键节点之间的位姿变换、所述相邻关键节点在所述坐标系内的位姿以及所述坐标系之间的位姿关系；

根据所述相邻关键节点间运动链定义非邻居节点间运动链；

对所述非邻居节点间运动链进行非线性映射，获得多移动机械手协同搬运系统运动学模型。

进一步的，所述运用运动链描述所述相邻关键节点之间的位姿变换、所述相邻关键节点在所述坐标系内的位姿以及所述坐标系之间的位姿关系的步骤包括以下步骤：

运用运动链

描述上虚节点坐标系∑s在工作空间坐标系∑g中的位姿关系；用齐次坐标变换矩阵

描述运动链

即

其中，

式中，

分别为上虚节点n_v,s沿工作空间坐标系∑g的x_g、y_g、z_g轴位移，

分别为上虚节点坐标系∑s绕工作空间坐标系∑g的x_g、y_g、z_g轴旋转角度，trans()为平移式，rot()为旋转式，

为平移参数，

为旋转参数；

运用运动链

描述下虚节点坐标系∑o在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为下虚节点n_v,o沿x_s、y_s、z_s轴的位移，

分别为下虚节点坐标系∑o的x_o、y_o、z_o轴分别绕x_s、y_s、z_s轴的旋转角度；

运用运动链

描述下虚节点n_b,o与各下实节点n_r,ci间的位姿变换；即

式中，

分别为下实节点n_r,ci在x_o、y_o、z_o轴上的位移，

分别为下实节点坐标系∑ci的的x_ci、y_ci、z_ci轴分别绕x_o、y_o、z_o轴的旋转角度；

运用运动链

描述各上实节点n_r,si在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为上实节点n_r,si在x_s、y_s、z_s轴上的位移，

分别为上实节点n_r,si绕x_s、y_s、z_s轴的旋转角度。

进一步的，所述定义非邻居节点间运动链的步骤包括以下步骤：

运用运动链

描述下实节点n_r,ci在工作空间坐标系∑g内的位姿关系，即

运用运动链

描述上实节点n_r,si在下实节点n_r,ci内的位姿关系，即

所述对所述非邻居节点间运动链进行非线性映射，获得多移动机器人协同搬运系统运动学模型的步骤包括以下步骤：

对运动链

进行所述移动平台关节空间内非线性映射，得到移动平台运动学模型；

对运动链

进行所述机械臂关节空间内非线性映射获得机械臂运动学模型；

将所述移动平台运动学模型与所述机械臂运动学模型结合，获得移动机械手运动学模型；

将各移动机械手运动学模型统一描述，获得多移动机械手协同搬运系统运动学模型。

进一步的，所述移动平台为后轮驱动差速系统，所述的运动链

到所述移动平台关节空间内非线性映射为：

其中，

式中，θ_ci＝[θ_cil θ_cir]^T，为移动平台的驱动轮参数，

为移动平台的任务空间期望速度，r为驱动轮半径，w为驱动轮之间距离；

所述机械臂为6自由度串联机械臂，所述对运动链

进行所述机械臂关节空间内非线性映射的步骤包括以下步骤：

输入初始关节角θ(0)_si|_t＝0＝[θ(0)_i，j]，j∈(1，6)和相对期望末端轨迹

si∈1×6，

计算6自由度串联机械臂齐次变换矩阵

计算6自由度串联机械臂雅克比矩阵

根据式

计算机械臂当前末端位姿p_step，

定义迭代误差

迭代误差e与期望阀值Δ比较，e≥Δ利用下式进行迭代：

对迭代结果进行机械臂关节空间赋值：

θ(0)_si|_t＝t+1＝θ_step+1。

进一步的，所述对运动链

进行所述机械臂关节空间内非线性映射的步骤之后还包括进行所述机械臂关节空间运动描述的步骤，包括以下步骤：

根据D-H法建立关节坐标系和连杆参数(α_k-1 a_k-1 θ_k d_k)，利用下式建立机械臂关机坐标系k与关节坐标系k-1的坐标变换，同时利用微分变换法求解机械臂关节空间的雅克比矩阵，根据下式建立机械臂关节空间雅克比矩阵

其中，

式中，α_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1旋转到z_k的角度，a_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1移动到z_k的距离，θ_k为沿关节k坐标系z_k轴，从x_k-1旋转到x_k的角度，d_k为沿关节k坐标系x_k轴，从x_k-1移动到x_k的距离。

一种多移动机械手协同搬运系统分布式控制方法，针对多移动机械手协同搬运系统进行控制，包括以下步骤：

定义多移动机械手协同操作系统的通信拓扑；

其中，下实节点n_r，ci与邻居下实节点n_r，cj、j∈n且j≠i，n为节点数，下实节点n_r，ci与下虚节点n_v，o，下实节点n_r，ci与相应上实节点n_r，si间进行通信，并建立拓扑面S_c，

将下虚节点n_v，o的在工作空间内的期望轨迹、下虚节点n_v，o与下实节点n_r，ci相对期望轨迹作为控制输入，与实际观察值进行同步编队跟踪控制；

将机械臂期望关节角度作为控制输入，与实际关节角的差值进行分布式自适应控制。

进一步的，所述上虚节点n_v，s和邻居下实节点n_r，cj实时状态信息由所述移动机械手获取，所述下虚节点n_v，o的实时状态信息由被搬运物体实时状态通过运动链

转换获得。

进一步的，所述的通信拓扑用图G＝(V E)表示，其中，V是节点的集合，

是节点间的链路，多移动机械手系统节点间的导通情况为

式中，1表示节点间可以通信，0表示节点间不能通信。

进一步的，移动平台的最终控制率为

式中，u_i为移动平台的控制率，

为下虚节点n_v，o的期望速度，e_s，ci为下实节点n_r，ci与上虚节点n_r，s的相对跟踪误差，e_ci，cj为下实节点n_r，ci与下邻居节点n_r，cj间的跟踪误差，

为运动链

的逆运算，k_p，si、K_p，ij为控制参数。

进一步的，机械臂的控制率为

其中，

式中，u_θ，i为机械臂的控制率，e(θ)_i为关节期望角度与实际角度误差，

为关节期望角速度与实际角速度误差，

为自适应控制参数，K_p、K_d为控制参数，Γ(θ_d，i)切换增益。

上述多移动机械手协同搬运系统运动学建模与分布式控制方法，利用上虚节点n_v，s和下虚节点n_v，o，将系统中的各实节点(n_r，si，n_r，ci)有机纳入同一描述体系，实现系统整体分析和建模。上虚节点n_v，s的期望轨迹是唯一需要在任务空间坐标系∑ci规划的，其他节点无需进行整体规划，只需知道自身和邻居节点状态，降低了系统计算负担，提高了系统鲁棒性和抗毁性。通过对运动链

中位移和旋转参数进行规划，提高系统稳定性和均衡各移动机械手间的载荷分配。

附图说明

图1为本发明多移动机械手协同搬运系统运动学建模方法流程图；

图2为本发明多移动机械手协同搬运系统运动链示意图；

图3为本发明多移动机械手协同搬运系统通信拓扑结构图；

图4为本发明多移动机械手协同搬运系统分布式控制原理图；

图5为本发明示例1和示例2中的多移动机械手协同搬运过程示意图；

图6为本发明示例1和示例2中的被搬运物体位置x_gy_gz_g变化图；

图7为本发明示例1和示例2中的被搬运物体姿态角ψ_gθ_gφ_g变化图；

图8为本发明示例1和示例2中的移动平台设计控制率；

图9为本发明示例1和示例2中的第一移动平台轨迹跟踪误差；

图10为本发明示例1和示例2中的第一机械臂轨迹观察值；

图11为本发明示例1和示例2中的第一机械臂轨迹跟踪误差；

图12为本发明示例3中的多移动机械手协同搬运装配过程示意图；

图13为本发明示例3中的被搬运物体位置x_gy_gz_g变化图；

图14为本发明示例3中的被搬运物体姿态角ψ_gθ_gφ_g变化图；

图15为本发明示例3中的移动平台设计控制率；

图16为本发明示例3中的第一移动平台轨迹跟踪误差；

图17为本发明示例3中的第一机械臂轨迹观察值；

图18为本发明示例3中的第一机械臂轨迹跟踪误差。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1和图2所示，在一个实施例中，一种多移动机械手协同搬运系统运动学建模方法，移动机械手10包括移动平台12与机械臂14，机械臂14设置在移动平台12上。移动平台12具有驱动轮与从动轮。机械臂14具有至少一个机械臂关节。建立多移动机器人协同搬运系统运动学模型关键是建立机器人所在工作空间到机器人关节(车轮和机械臂关节)所在的关节空间的运动学描述，以实现给定关节期望运动(例如关节转动角度、角速度和角加速度)实现机器人在工作空间内的操作任务。该工作空间是直角坐标空间，关节空间是一种关节变量(旋转或移动)的描述方法。移动平台关节空间是对移动平台驱动轮转动的描述，机械臂关节空间是指对机械臂各移动或旋转关节的描述。

协同搬运场景为：协同搬运系统由被搬运物体(object)和m台移动机械手(mobilemanipulators)组成，m为自然数。操作目标为：移动机械手相互配合实现对被搬运物体抓取、平移、旋转、放置等操作。该运动学建模方法包括以下步骤：

步骤S110，定义关键节点和坐标系。

其中，关键节点包括表示被搬运物体质心的上虚节点n_v，s、表示移动机械手10抓取点的上实节点n_r，si、表示多个移动平台12组成编队几何中心点的下虚节点n_v，o与表示移动平台12几何中心的下实节点n_r，ci。

上实节点n_r，si和上虚节点n_v，s所在的平面为上平面s_u，下实节点n_r，ci和下虚节点n_v，o所在的平面为下平面s_d。

坐标系包括工作空间坐标系∑g、上虚节点坐标系∑s、下虚节点坐标系∑o和下实节点坐标系∑ci。其中，工作空间坐标系o_g-x_gy_gz_g表示坐标原点o_g和x_gy_gz_g三个坐标轴，由∑g表示。同理，上虚节点坐标系由∑s表示，下虚节点坐标系由∑o表示，下实节点坐标系由∑ci表示。

为便于理解，这里对定义符号进行解释，上虚节点n_v，s中，n取“node(节点)”的首字母，v取“virtual(虚拟的)”首字母，s为被搬运物体质心。下实节点n_r，ci中，n取“node”的首字母，r取“real(实际的)”首字母，ci为移动平台几何中心。上平面s_u中，s和u由“surface(平面)”和“upper(上面的)”的首字母组成。

步骤S120，运用运动链描述相邻关键节点之间的位姿(位置和姿态)变换、相邻关键节点在坐标系内的位姿与坐标系之间的位姿关系。

在本实施例中，首先，运用运动链

描述上虚节点坐标系∑s在工作空间坐标系∑g中的位姿关系。为便于计算，用齐次坐标变换矩阵

描述运动链

即

其中，

式中，

分别为上虚节点n_v，s沿工作空间坐标系∑g的x_g、y_g、z_g轴位移，

分别为上虚节点坐标系∑s绕工作空间坐标系∑g的x_g、y_g、z_g轴旋转角度，trans()为平移式，rot()为旋转式，

为平移参数，

为旋转参数，平移参数和旋转参数统称为运动参数。

通过式(1)可以建立上虚节点n_v，s在工作空间坐标系∑g内位姿数学表达和计算。

为便于描述和理解，公式中涉及的运动链(如三维空间向量

)，矩阵(如

)，公式运算(如

)进行加粗强调，平移参数和旋转参数置于公式运算括号内。

上虚节点n_v，s位姿可由矢量集合

其中

根据式(1)对

中元素求解可得到上虚节点n_v，s的运动学正解。规划好期望的

(即给定上虚节点n_v，s的期望轨迹)，对式(1)中的平移和旋转参数求解称为对上虚节点n_v，s的运动学逆解。

其次，运用运动链

描述下虚节点坐标系∑o在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为下虚节点n_v，o沿x_s、y_s、z_s轴的位移，

分别为下虚节点坐标系∑o的x_o、y_o、z_o坐标轴分别绕x_s、y_s、z_s轴的旋转角度。式(2)描述的运动链将上平面s_u和下平面s_d联系起来，它们之间的相对位姿直接影响搬运效果。通过设计和约束平移和旋转参数

可以保证两个平面间的位姿关系，进而提高搬运效果。例如规定

即移动平台编队几何中心o与搬运物体质心s在x_s方向上没有平移，进而在x_s方向上没有附加弯矩，可提高在x_s方向搬运稳定性。

再次，运用运动链

描述下虚节点n_v，o与各下实节点n_r，ci间的位姿变换；即

式中，

分别为下实节点n_r，ci在x_o、y_o、z_o轴上的位移，

分别为下实节点坐标系∑ci的x_ci、y_ci、z_ci坐标分别轴绕x_o、y_o、z_o轴的旋转角度。

式(3)描述下实节点n_r，ci相对于下虚节点n_v，o的位姿变换，为保持移动平台编队队形，只需知道各移动平台几何中心ci与移动平台编队几何中心点o的相对位姿关系即可，这样的规划方式可有效减少期望轨迹规划维度，更易于控制器设计。

最后，运用运动链

描述各上实节点n_r，si在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为上实节点n_r，si在x_s、y_s、z_s轴上的位移，

分别为上实节点n_r，si绕x_s、y_s、z_s轴的旋转角度。

步骤130，根据相邻关键节点间运动链定义非邻居节点间运动链。

首先，运用运动链

描述下虚节点n_v，o在工作空间坐标系∑g内的位姿关系，即

其次，运用运动链

描述下实节点n_r，ci在工作空间坐标系∑g内的位姿关系，即

再次，运用运动链

描述上实节点n_r，si在下实节点n_r，ci内的位姿关系，即

式(5)描述了下虚节点n_v，o(移动平台编队几何中心o)在工作空间坐标系∑g中的位姿转换关系。通过给定下虚节点n_v，o期望轨迹，对式(5)运动学逆解可以得到位移和旋转参数表达式。

通过式(7)可以描述上实节点n_r，si在下实节点坐标系∑ci中的位姿变换。上实节点n_r，si即为机械臂14的末端抓取点，根据D-H法建立机械臂关节空间坐标系与齐次变换表达式，与式(7)联立即可求解机械臂14的各关节角运动情况。

步骤S140，对非邻居节点间运动链进行非线性映射，获得多移动机械手协同搬运系统运动学模型。包括以下步骤：

首先，对运动链

进行移动平台关节空间内非线性映射，得到移动平台运动学模型。其次，对运动链

进行机械臂关节空间内非线性映射获得机械臂运动学模型。再次，将移动平台运动学模型与机械臂运动学模型结合，获得移动机械手运动学模型。最后，将各移动机械手运动学模型统一描述，获得多移动机械手协同搬运系统运动学模型。

具体的，建立移动平台ci运行学模型如式(8)。移动平台12为后轮驱动差速系统，运动链

到移动平台关节空间内非线性映射为：

其中，

式中，θ_ci＝[θ_cil θ_cir]^T为移动平台的驱动轮参数，

为移动平台的任务空间期望速度，r为驱动轮半径，w为驱动轮之间距离。

移动机械手运动学逆解具体为：机械臂14为6自由度串联机械臂，对运动链

进行机械臂关节空间内非线性映射的步骤包括以下步骤：

输入初始关节角θ(0)_si|_t＝0＝[θ(0)_i，j]，j∈(1，6)和相对期望末端轨迹

si∈1×6，

计算6自由度串联机械臂齐次变换矩阵

计算6自由度串联机械臂雅克比矩阵

根据式

计算机械臂当前末端位姿p_step，定义迭代误差

迭代误差e与期望阀值Δ比较，e≥Δ利用下式进行迭代：

对迭代结果进行机械臂关节空间赋值：

θ(0)_si|_t＝t+1＝θ_step+1。

在对运动链

进行机械臂关节空间内非线性映射的步骤之后还包括进行机械臂关节空间运动描述的步骤：

机械臂关节空间运动描述过程为：根据D-H法建立关节坐标系和连杆参数(α_k-1a_k-1 θ_k d_k)，利用下式(9)建立机械臂关机坐标系k与关节坐标系k-1的坐标变换，

其中，

式中，α_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1旋转到z_k的角度，a_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1移动到z_k的距离，θ_k为沿关节k坐标系z_k轴，从x_k-1旋转到x_k的角度，d_k为沿关节k坐标系x_k轴，从x_k-1移动到x_k的距离。

再根据式(9)，利用微分变换法求解机械臂关节空间的雅克比矩阵，根据下式建立机械臂关节空间雅克比矩阵

上述多移动机械手协同搬运系统运动学建模方法，利用上虚节点n_v，s和下虚节点n_v，o，将系统中的各实节点(n_r，si，n_r，ci)有机纳入同一描述体系，实现系统整体分析和建模。上虚节点n_v，s的期望轨迹是唯一需要在任务空间坐标系∑ci规划的，其他节点无需进行整体规划，只需知道自身和邻居节点状态，降低了系统计算负担，提高了系统鲁棒性和抗毁性。通过对运动链

中位移和旋转参数进行规划，可准确保证上平面s_u和下平面s_d相对位姿关系，提高系统稳定性和均衡各移动机械手间的载荷分配。

此外，还提供了一种多移动机械手协同搬运系统分布式控制方法。

如图3与图4所示，在一个实施例中，一种多移动机械手协同搬运系统分布式控制方法，针对多移动机械手协同搬运系统进行控制，包括以下步骤：

首先，定义多移动机械手协同操作系统的通信拓扑。根据建立的多移动机械手协同搬运系统运动学模型，为实现运动链间信息有效传递，定义多移动机械手协同操作系统通信方式：移动平台节点间、移动平台和下虚节点间可以通信，实现下平面s_d内节点间位姿、速度等信息交互；移动平台和安装在移动平台上的机械臂之间可以进行通信。下实节点n_r,ci与邻居下实节点n_r,cj、j∈n且j≠i，n为节点数，下实节点n_r,ci与下虚节点n_v,o，下实节点n_r,ci与相应上实节点n_r,si间进行通信，并建立拓扑面S_c，

进一步的，所述的通信拓扑可以用图G＝(VE)，V是节点的集合，

是节点间的链路，多移动机械手系统节点间的导通情况如下式所示，

其中，1表示节点间可以通信，0表示节点间不能通信，otherwise表示否则。

就通信机制而言，传统的通讯机制，若由n个移动机械手组成的协同操作系统会有n个节点，每个节点又由移动平台和机械臂组成，加之需要实时观测状态的操作对象，数据有效交互成为一个研究重点。

本实施例的通讯机制，根据上述通信方式，移动机械手仅需知道邻居节点状态信息，大大降低系统通信负担和计算复杂度。将全局信息定义在被搬运物体上，便于对搬运效果实时跟踪与反馈。上虚节点n_v,s期望轨迹、实时状态通过运动链

计算得到下虚节点n_v,o(移动平台编队几何中心)期望轨迹和实时状态，为下平面s_d内节点编队控制和实际工程应用提供一种可行方案。

其次，将下虚节点n_v,o的在工作空间内的期望轨迹、下虚节点n_v,o与下实节点n_r,ci相对期望轨迹作为控制输入，与实际观察值进行同步编队跟踪控制。

基于PID(Proportion Integral Differential，比例积分微分)的编队控制，移动平台的控制率为，

式中，u_i为移动平台的控制率，

为下虚节点n_v,o的期望速度，e_o,ci为下实节点n_r,ci与下虚节点n_v,o的相对跟踪误差，e_ci,cj为下实节点n_r,ci与下邻居节点n_r,cj间的跟踪误差，k_p,i、K_p,ij为控制参数。

由于下虚拟节点n_v，o实时状态不易观察。实际应用时，移动机械手可以获取上虚节点n_v，s和邻居移动平台(邻居下实节点n_r，cj)实时状态信息。下虚节点n_v，o的实时状态信息由被搬运物体实时状态(即上虚节点n_v，s)通过运动链

转换得到。因此，移动平台的最终控制率为，

式中，e_s，ci为下实节点n_r，ci与上虚节点n_r，s的相对跟踪误差，

为运动链

的逆运算，k_p，si、K_p，ij为控制参数。

再次，将机械臂期望关节角度作为控制输入，与实际关节角的差值进行分布式自适应滑模控制。

基于自适应滑模控制，机械臂的控制率为，

其中，

式(12)中，e(θ)_i为关节期望角度与实际角度误差，

为关节期望角速度与实际角速度误差，

为自适应控制参数，K_p、K_d为控制参数，Γ(θ_d，i)切换增益。

在控制策略上说，传统的控制策略，移动机械手具有位置、速度、加速度和操作力等几个维度的控制方式，根据期望控制性能又有PID、滑模、自适应等多种控制方法，在庞杂的控制策略中选取有效的控制维度和方法是研究的难点。

本实施例的移动平台间采用基于PID的编队控制，可在跟踪误差较大时保证相对误差快速收敛，保持节点间相对位姿关系和编队队形；机械臂采用自适应滑模控制，在实现快速收敛的同时实现抓取点一定自适应调节，避免操作力突变造成抓取点滑移。

为了更好地理解本发明的功能和特点，以下结合图1至图17进行说明。

为描述方便，所涉及的任务空间位姿用6维向量P表示，P＝[P_trans P_rot]＝[x_g y_g z_gψ_g θ_g φ_g]，P_trans＝[x_g y_g z_g]相对于坐标系o_g平移变化，P_rat＝[ψ_g θ_g φ_g]相对于坐标系o_g旋转欧拉角RPY(Roll-pitch-yaw，滚转角-俯仰角-偏航角)。

示例一，结合图1至图11对多移动机械手协同搬运系统运动学建模过程进行描述。

该协同搬运系统由6台移动机械手组成。实现在15m×5m的场地内，对被搬运物体沿x_g移动9m，绕y_g旋转10°的操作，被搬运物体长宽高为5m×0.3m×0.45m，质量为25kg的长条板。

步骤一、设定初始条件。

被搬运物体初始位姿：P_o，int＝[-5m 2.5m 1.3157m 0° 0° 0°]；

移动平台初始位姿：P_1，int＝[x_g y_g φ_g]＝[-6.8m 3m 0°]、P_2，int＝[-5m 3m 0°]、P_3，int＝[-3.2m 3m 0°]、P_4，int＝[-3.2m 2m 0°]、P_5，int＝[-5m 2m 0°]、和P_6，int＝[-6.8m 2m0°]。

机械臂初始关节角：θ_1，int＝θ_2，int＝θ_3，int＝[90° 0° 0° 0° 0° 0°]、θ_4，int＝θ_5，int＝θ_6，int＝[-90° 0° 0° 0° 0° 0°]。

抓取点为：

P_1，gra＝[x_1，gra y_1，gra z_1，gra ψ_gra θ_gra φ_gra]＝[-6.8 2.6 1.31 0° 0° 90°]，

P_2，gra＝[-5 2.6 1.31 0° 0° 90°]，P_3，gra＝[-3.2 2.6 1.31 0° 0° 90°]，

P_4，gra＝[-3.2 2.3 1.31 0° 0° -90°]，

P_5，gra＝[-5 2.3 1.31 0° 0° -90°]，

和P_6，gra＝[-6.8 2.3 1.31 0° 0° -90°]。

步骤二、规划被搬运物体期望轨迹。

式中，

表示在工作空间坐标系∑g中，搬运物体质心s在x_g方向上的期望轨迹，k＝10^-5；a＝t-99.5。

为便于理解，这里对步骤一和步骤二中符号进行说明：int为“initial(初始的)”前三个字母，表示各变量初始状态；gra为“grasp(抓取)”前三个字母，表示抓取点；

中x表示在x轴上变量，g取“global(全局的)”首字母，d取“desire(期望)”首字母，s表示被搬运物体质心s。

步骤三、建立多移动机械手系统运动学模型。

这里以6台移动机械手中的第一移动机械手101的建模过程为例。该个第一移动机械手101包括第一移动平台与第一机械臂。

根据式(1)，确定上虚节点n_v,s期望轨迹齐次变换表达：

式中，期望位移参数和旋转参数

由式(13)确定。

本设计实例中，上平面s_u和下平面s_d间无需相对运动，因此运动链

为上虚节点n_v,s到下虚节点n_v,o的垂直投影，

式中，

为下虚拟节点n_v,o与上虚节点n_v,s在z_s方向上的期望移动距离，

由式(13)确定。

由式(15)可知，被搬运物体质心s与移动平台编队几何中心o在x轴、y轴上没有相对位移，因此没有附加弯矩，进而可以保证在x轴、y轴方向上搬运平稳，更易于移动机械手抓取点载荷分配。

搬运过程中保持移动平台编队队形不变，因此下虚节点n_v,c到移动平台101的运动链

为，

式中，

为节点n_v,o与n_r,ci在x_o方向上的相对位置，

具体数值可由上述步骤一的设定初始条件确定。

上虚节点n_v,s到上实节点n_v,s1的运动链

为，

式中，

为节点n_v,s与n_r,si在x_s方向上的相对位移，区别于式(16)描述的下平面s_d中的运动链

在搬运过程中抓取点坐标系跟随期望轨迹不断变化，因此位移和旋转参数

为变量。

根据式(14)～(16)，第一移动平台在工作空间坐标系∑g中的运动链

为：

根据式(8)和式(18)即可求解第一移动平台的运动学模型。

根据式(15)、(16)和(17)抓取点s₁相对于第一移动平台的运动链为

根据式(19)和运动链

到机械臂关节空间内非线性映射的步骤即可求解第一机械臂的运动学模型。

同理，其他移动机械手运动学模型可通过步骤上述步骤求解得到。

示例2，为示例1所描述的搬运场景结果分析。

如图5所示，t＝0s时，6台移动机械手和搬运物体处于初始状态，t＝50s时，6台移动机械手协调配合完成对被搬运物体水平移动一定距离，t＝100s时，被搬运物体到达指定搬运位置(4,2.5,1.31)，t＝190s时，6台移动机械手协调配合完成对被搬运物体旋转10°的搬运操作。

图6和图7为被搬运物体位姿变化图，为更好辨别具体位置和姿态变化情况，图6为被搬运物体位置变化图，在t＝0～50s内，被搬运物体在x_g方向移动，在t＝50～100s内，移动机械手和移动平台协调配合，实现被搬运物体同时在x_g和z_g方向同时移动，证明所设计的运动学模型的移动平台和移动机械手的运动协调性。t＝0～190s，通过设计的分布式控制器可以实现位置保持稳定；图7为被搬运物体姿态变化图，在t＝100～190s内，被搬运物体绕y_g匀速旋转10°。搬运物体初始误差也可以快速收敛到期望范围内。

图8为根据式(11)设计的移动平台控制率，图9为第一移动平台在任务空间内轨迹跟踪误差。可以看出控制率输出较为平滑，第一移动平台轨迹跟踪误差在一定范围内有效收敛，可有效保证移动平台队形稳定。图10为机械臂1轨迹观测值，图11为第一机械臂关节轨迹跟踪误差，这里为便于图中观察，只选取第一机械臂的数据进行采集分析。各个关节轨迹平滑，跟踪误差整体收敛，但在初始阶段和搬运任务切换时，跟踪误差出现一定幅度波动，通过设计控制率的快速收敛性和自适应性，能够保证抓取点稳定并实现搬运任务。

示例3，如图12所示，为3台移动机械手对物体实现搬运和装配操作的应用场景。

该协同搬运系统由3台移动机械手组成。实现在15m×5m的场地内，对被搬运物体延x_g移动10m，随后将搬运物体实现与工作台上零件装配操作，被搬运物体长宽高为1.8m×1m×0.45m，质量为8kg的平板。

实例中：被搬运物体初始位姿：P_s,int＝[-5m 2.5m 1.3157m 0° 0° 0°]；

移动平台初始位姿：P_1,int＝[x_1,int y_1,int φ_1,imt]＝[-6.25m 2.5m 0°]、P_2,int＝[-5m 3.35m 0°]、和P_3,int＝[-5m 1.65m 0°]。

机械臂初始关节角：θ_1,int＝[180° 0° 0° 0° 0° 0°]、θ_2,int＝[90° 0° 0° 0° 0°0°]和θ_3,int＝[-90° 0° 0° 0° 0° 0°]。

抓取点为：

P_1,gra＝[x_1,gra y_1,gra z_1,gra ψ_gra θ_gra φ_gra]＝[-5.8 2.5 1.31 0° 90°]、

P_2,grasp＝[-5 2.96 1.31 0° 90°]、和P_3,grasp＝[-5 2 1.31 0° 0° 90°]。

规划被搬运物体期望轨迹：

式中，k＝10^-5；a＝t-50。

如图12所示，t＝0s时，3台移动机械手和搬运物体处于初始状态，t＝50s时，3台移动机械手协调配合将物体搬运到装配位置(5,2.5,1.31)，t＝50～100s内，移动机械手间协调配合，将物体与工作台上的零件实现装配操作。t＝190～220s内，3台移动机械手放开搬运物体，进行退回操作。

图13和图14为被搬运物体位姿变化图，图13为被搬运物体位置变化图，在t＝0～50s内，被搬运物体在x_g方向移动；在t＝50～100s内，移动机械手间协调配合，将物体沿z_g方向移动0.4m；在t＝100～190s内，被搬运物体在x_g方向移动0.5m，实现与工作台上的零件配合。在装配过程中t＝100～190s，移动平台通过设计的编队控制器保持队形稳定；图14为被搬运物体姿态变化图，可以看出在整个搬运过程中，物体没有发生较大的角度偏斜，证明所提出的运动学模型和相应控制方案能够满搬运与和装配任务。

图15为设计的3台移动平台的控制率，图16为选取的第一移动平台的轨迹跟踪误差，图17为示例中第一机械臂的轨迹观测值，图18为示例中第一机械臂关节空间跟踪误差。由图中可以看出，通过所提出的运动学建模方法、通信机制和相应的分布式协调控制方法，输出的移动平台的控制率和机械臂运动观测值曲线平滑，无论移动平台跟踪误差还是机械臂关节误差都具有良好收敛性，从而保证系统具有较高的控制精度。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种多移动机械手协同搬运系统运动学建模方法，所述移动机械手包括移动平台与设置在所述移动平台上的机械臂，所述机械臂具有至少一个机械臂关节，其特征在于，所述运动学建模方法包括以下步骤：

定义关键节点和坐标系；其中，所述关键节点包括表示被搬运物体质心的上虚节点n_v，s、表示所述移动机械手抓取点的上实节点n_r，si、表示多个移动平台组成编队几何中心点的下虚节点n_v，o与表示移动平台几何中心的下实节点n_r，ci；所述坐标系包括工作空间坐标系∑g、上虚节点坐标系∑s、下虚节点坐标系∑o，上实节点坐标系∑si和下实节点坐标系∑ci；

运用运动链描述所述相邻关键节点之间的位姿变换、所述相邻关键节点在所述坐标系内的位姿以及所述坐标系之间的位姿关系；

根据所述相邻关键节点间运动链定义非邻居节点间运动链；

对所述非邻居节点间运动链进行非线性映射，获得多移动机械手协同搬运系统运动学模型。

2.根据权利要求1所述的多移动机械手协同搬运系统运动学建模方法，其特征在于，所述运用运动链描述所述相邻关键节点之间的位姿变换、所述相邻关键节点在所述坐标系内的位姿以及所述坐标系之间的位姿关系的步骤包括以下步骤：

运用运动链

描述上虚节点坐标系∑s在工作空间坐标系∑g中的位姿关系；用齐次坐标变换矩阵

描述运动链

即

其中，

式中，

分别为上虚节点n_v，s沿工作空间坐标系∑g的x_g、y_g、z_g轴位移，

分别为上虚节点坐标系∑s绕工作空间坐标系∑g的x_g、y_g、z_g轴旋转角度，trans()为平移式，rot()为旋转式，

为平移参数，

为旋转参数；

运用运动链

描述下虚节点坐标系∑o在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为下虚节点n_v，o沿x_s、y_s、z_s轴的位移，

分别为下虚节点坐标系∑o的x_o、y_o、z_o轴分别绕x_s、y_s、z_s轴的旋转角度；

运用运动链

描述下虚节点n_v，o与各下实节点n_r，ci间的位姿变换；即

式中，

分别为下实节点n_r，ci在x_o、y_o、z_o轴上的位移，

分别为下实节点坐标系∑ci的的x_ci、y_ci、z_ci轴分别绕x_o、y_o、z_o轴的旋转角度；

运用运动链

描述各上实节点n_r，si在上虚节点坐标系∑s中的位姿关系，即

式中，

分别为上实节点n_r，si在x_s、y_s、z_s轴上的位移，

分别为上实节点n_r，si绕x_s、y_s、z_s轴的旋转角度。

3.根据权利要求1所述的多移动机械手协同搬运系统运动学建模方法，其特征在于，所述定义非邻居节点间运动链的步骤包括以下步骤：

运用运动链

描述下实节点n_r，ci在工作空间坐标系∑g内的位姿关系，即

运用运动链

描述上实节点n_r，si在下实节点n_r，ci内的位姿关系，即

所述对所述非邻居节点间运动链进行非线性映射，获得多移动机器人协同搬运系统运动学模型的步骤包括以下步骤：

对运动链

进行所述移动平台关节空间内非线性映射，得到移动平台运动学模型；

对运动链

进行所述机械臂关节空间内非线性映射获得机械臂运动学模型；

将所述移动平台运动学模型与所述机械臂运动学模型结合，获得移动机械手运动学模型；

将各移动机械手运动学模型统一描述，获得多移动机械手协同搬运系统运动学模型。

4.根据权利要求3所述的多移动机械手协同搬运系统运动学建模方法，其特征在于：所述移动平台为后轮驱动差速系统，所述的运动链

到所述移动平台关节空间内非线性映射为：

其中，

式中，θ_ci＝[θ_cil θ_cir]^T，为移动平台的驱动轮参数，

为移动平台的任务空间期望速度，r为驱动轮半径，w为驱动轮之间距离；

所述机械臂为6自由度串联机械臂，所述对运动链

进行所述机械臂关节空间内非线性映射的步骤包括以下步骤：

输入初始关节角θ(0)_si|_t＝0＝[θ(0)_i，j]，j∈(1，6)和相对期望末端轨迹

si∈1×6，

计算6自由度串联机械臂齐次变换矩阵

计算6自由度串联机械臂雅克比矩阵

根据式

计算机械臂当前末端位姿p_step，

定义迭代误差

迭代误差e与期望阀值Δ比较，e≥Δ利用下式进行迭代：

对迭代结果进行机械臂关节空间赋值：

θ(0)_si|_t＝t+1＝θ_step+1。

5.根据权利要求4所述的多移动机械手协同搬运系统运动学建模方法，其特征在于：所述对运动链

进行所述机械臂关节空间内非线性映射的步骤之后还包括进行所述机械臂关节空间运动描述的步骤，包括以下步骤：

根据D-H法建立关节坐标系和连杆参数(α_k-1 a_k-1 θ_k d_k)，利用下式建立机械臂关机坐标系k与关节坐标系k-1的坐标变换，同时利用微分变换法求解机械臂关节空间的雅克比矩阵，根据下式建立机械臂关节空间雅克比矩阵

其中，

cα_k-1＝cos(α_k-1)，

式中，α_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1旋转到z_k的角度，a_k-1为沿关节k-1坐标系x_k-1轴，从z_k-1移动到z_k的距离，θ_k为沿关节k坐标系z_k轴，从x_k-1旋转到x_k的角度，d_k为沿关节k坐标系x_k轴，从x_k-1移动到x_k的距离。

6.一种多移动机械手协同搬运系统分布式控制方法，其特征在于，针对多移动机械手协同搬运系统进行控制，包括以下步骤：

定义多移动机械手协同操作系统的通信拓扑；

其中，下实节点n_r，ci与邻居下实节点n_r，cj、j∈n且j≠i，n为节点数，下实节点n_r，ci与下虚节点n_v，o，下实节点n_r，ci与相应上实节点n_r，si间进行通信，并建立拓扑面S_c，

将下虚节点n_v，o的在工作空间内的期望轨迹、下虚节点n_v，o与下实节点n_r，ci相对期望轨迹作为控制输入，与实际观察值进行同步编队跟踪控制；

将机械臂期望关节角度作为控制输入，与实际关节角的差值进行分布式自适应控制。

7.根据权利要求6所述的多移动机械手协同搬运系统分布式控制方法，其特征在于，所述上虚节点n_v，s和邻居下实节点n_r，cj实时状态信息由所述移动机械手获取，所述下虚节点n_v，o的实时状态信息由被搬运物体实时状态通过运动链

转换获得。

8.根据权利要求7所述的多移动机械手协同搬运系统分布式控制方法，其特征在于，所述的通信拓扑用图G＝(V E)表示，其中，V是节点的集合，

是节点间的链路，多移动机械手系统节点间的导通情况为

式中，1表示节点间可以通信，0表示节点间不能通信。

9.根据权利要求7所述的多移动机械手协同搬运系统分布式控制方法，其特征在于，移动平台的最终控制率为

式中，u_i为移动平台的控制率，

为下虚节点n_v，o的期望速度，e_s，ci为下实节点n_r，ci与上虚节点n_r，s的相对跟踪误差，e_ci，cj为下实节点n_r，ci与下邻居节点n_r，cj间的跟踪误差，

为运动链

的逆运算，k_p，si、K_p，ij为控制参数。

10.根据权利要求7所述的多移动机械手协同搬运系统分布式控制方法，其特征在于，机械臂的控制率为

其中，

式中，u_θ，i为机械臂的控制率，e(θ)_i为关节期望角度与实际角度误差，

为关节期望角速度与实际角速度误差，

为自适应控制参数，K_p、K_d为控制参数，Γ(θ_d，i)切换增益。

Images