CN108638065B - 一种排爆机器人双臂协同控制系统 - Google Patents

Abstract

本发明公开了一种排爆机器人双臂协同控制系统，排爆机器人两机械臂可在工作空间中协作完成系统分配到的排爆任务，系统在排爆过程中，智能为两机械臂分配排爆任务，根据排爆数据库的任务知识对当前排爆任务进行任务分解，得到各机械臂操作序列，两机械臂通过排爆知识库得到的任务操作序列对机械臂末端执行器的关节空间坐标进行规划，在机械臂运动过程中同时进行智能避免碰撞操作，避免机械臂运动过程中的碰撞，直至机械臂完成排爆任务结束。采用此排爆机器人双臂协同控制系统进行排爆操作可提高机器人排爆效率，且在复杂的地形环境下，仍可保证排爆任务快速稳定进行。

Description

一种排爆机器人双臂协同控制系统

技术领域

本发明属于排爆机器人领域，具体涉及一种排爆机器人双臂协同控制系统。

背景技术

随着科学技术的发展和社会需求的提高，单臂机器人无法完成恶劣环境下较复杂的工作，特别是针对排爆机器人来说，工作环境往往都相对复杂，因此，研究双排爆机器人成为排爆机器人研究领域的重点之一。

发明内容

针对上述现有技术中存在的不足，本发明提供一种排爆机器人双臂协同控制系统，本发明将双机械臂排爆机器人的机械臂运动过程分为以下几个过程：首先对排爆任务进行确定，智能进行各个机械臂的任务分配；对各个机械臂进行路径规划；机械臂向停驻点进行运动过程中进行智能避碰。

为解决上述技术问题，本发明所采用的技术方案如下：一种排爆机器人双臂协同控制系统，步骤如下：

S1，建立排爆数据库。

机器人通过学习训练传统排爆过程，得出一个排爆数据库；所述排爆数据库包括任务知识数据库、任务库和机械臂知识能力库。

S2，获取任务并将任务分配。

排爆机器人探寻到未爆物时，得到排爆任务，并采用一种基于trader集中式的双臂空间机器人任务分配方法将任务分配。

具体步骤为：

S2.1建立机械臂任务能力知识库，包括机械臂能力信息和任务的能力信息；

S2.1.1，机械臂的能力。

所述机械臂的能力包括能力类型和与能力类型相对应的能力水平。

S2.1.1.1，机械臂的能力类型。

机械臂具有多种不同的能力，如抓握能力、负载能力、灵活程度、指端力觉能力，视觉能力、腕力觉能力、运算能力、通信能力、灵巧能力，这些能力细化为单个的原子能力，并将这些原子能力组成能力类型集合。

机械臂的能力类型集合是由m个原子能力c_j组成的能力集合：

C＝{c_j}，1≤j≤m；

S2.1.1.2，机械臂的能力水平。

机械臂的每个原子能力都具有一个对应的能力水平，第i个机械臂的原子能力 c_j的能力水平为L_ij，取值范围为0≤L_ij≤1，第i个机械臂的第c_j项能力越高，则L_ij越大，即

L_ij＝diag{L_i1,L_i2,…,L_ij,…,L_im}。

S2.1.2，任务的能力信息。

S2.1.2.1，任务对机械臂某一原子能力c_j的需求为N_kj；即如果任务需要第c_j项能力，N_kj取 1，否则取0，即

S2.1.2.2，完成任务对机械臂某一原子能力c_j的需求强度为W_kj。

取值范围为0≤W_kj≤1，第c_j项能力对任务的影响越大，则W_kj越大，即

W_kj＝diag{W_k1,W_k2,…,W_kj,…,W_km}。

S2.2，根据分解动作序列对应的机械臂能力信息和任务能力信息选择执行动作的机械臂。

比较每一个分解动作序列所对应的机器臂能力信息和任务能力信息的大小，当机械臂能力水平大于任务对机械臂能力的需求强度，表示当前机械臂能完成该分解动作。

S2.3，使两个机械臂的控制器之间进行通信，分别以对方为参照物，判断将已经分解的任务如何分配给两个机械臂，使两个机械臂可以合作完成。

机器人以构建的能力集合为标准，将每一个子任务按序与机械臂能力集合做对比，设当前其中一个子任务为p_k，若存在L_ij>＝W_kj,则表示机械臂能完成任务，则优先为机械臂A分配此任务，将机械臂A已分配的任务对应的能力L_ij值降低或归零，然后进行下一个子任务与机械臂能力集合的对比，优先进行机械臂A的现有能力匹配，当机械臂A能力集合不满足L_ij>＝W_kj时，则进行机械臂B的能力集合匹配，机械臂B的能力集合匹配同机械臂A。

S3，进行任务分解。

机械臂的任务描述表示为一个隐式动作计划，它指定了一个根据隐式基本操作和执行顺序关系的子目标集合，其中机械臂主要用来处理未爆物，动作序列主要包括初始状态、接近未爆物、调整姿态、到达、抓取、提升、旋转、腕部翻转、闭合手爪、暂停工作等子任务，如下表所示：

机械臂动作序列表

序号	子任务
		1	初始
2	接近
		3	调整
4	到达
		5	提升
6	腕部旋转
		7	闭合手爪
8	暂停工作
		……	……

当机械臂获得各自分配的任务之后通过任务知识将其各自任务分为一个子任务序列，同时产生与子任务相对应的动作序列，在机械臂动作序列表中查找到相应的动作序列，对其进行链接得到机械臂的详细动作序列，每一个步骤的动作序列都对应一系列的运动控制函数再进行相应的函数描述。其中，机器人的主要运动控制函数如下：相对于机器人基坐标系的位姿控制、相对于机器人基坐标系的位置向量控制、相对于机器人基坐标系的姿态向量控制、相对于机器人末端坐标系的位置向量控制、相对于机器人末端坐标系的姿态向量控制、关节角度控制、机器人暂停函数、机器人返回初始位形函数、末端执行器闭合，进行工作、末端执行器张开，到其初始状态为止。

机械臂的动作序列主要由上述运动函数构成，当机械臂的运动序列确定之后，根据确切情况，确定各个动作的运动参数，用以上运动控制函数进行表示。

S4，对机械臂进行路径规划；

机械臂上的控制器接收末端执行器的各传感器实时采集的自身位置和方位信息，控制器将接收检测信息并将检测信息通过卡尔曼滤波器进行滤波并预测下一时刻的位置和方位信息，控制器将得到的预测信息传输给另一机械臂的控制器上，实现两个机械臂之间的相互通讯；通过两臂控制器之间的通信使得一个机械臂对于另一个机械臂的反应做出相应的动作、轨迹规划和决策。

具体步骤为：

S4.1，对末端执行器的关节空间坐标进行规划；

通过排爆知识库得到的任务操作序列对机械臂末端执行器的关节空间坐标进行规划；

S4.2，机械臂路径规划；

采用栅格法进行机械臂的运动路径规划，具体步骤如下：

S4.2.1，经过步骤S2和步骤S3，机械臂获取到任务以及子任务序列，也就是得知机械臂的目标点位置；

S4.2.2，以机械臂的底座在空间的位置为原点，建立三维坐标系，设置以机械臂底座为中心，机械臂连杆长度之和为半径的球形空间；在球形空间中以i为栅格宽度，将球形空间划分为若干三维栅格；

S4.2.3，通过机械臂配备的传感器对当前机械臂附近环境进行信息采集，实时判断障碍物的空间位置，将存在障碍物的栅格定义为障碍栅格，不存在障碍物的栅格定义为自由栅格；

S4.2.4，机械臂根据自身当前状态、目标点和自由度分布信息，在现有的自由栅格范围内，使用A*算法搜索出最优的通向目标点的路径。

S5，机械臂向停驻点进行运动过程中进行智能避免碰撞；

所述碰撞包括机械臂之间的碰撞以及机械臂与障碍物的碰撞；机械臂连杆进行障碍物的探测，并通过状态感知器将环境状态映射为机械臂的内部感知，工作选择器根据当前策略选择避障动作作用于环境，训练决策器通过更新学习策略知识用于后续动作的选择。

机械臂通过超声波传感器得到当前障碍物的空间位置，且通过各个关节连杆状态感知器反馈回的信息得到机械臂当前姿态偏差角和转动增量；当机械臂与障碍物或另一机械臂之间的距离d_i＜d时，其中，d为系统设置的机械臂与障碍物或另一机械臂之间最小距离，机械臂通过栅格法，在当前的自由栅格范围内，通过训练决策器和动作选择器选择动作X，使机械臂与障碍物之间的距离d_i≥d，完成避障。

在每节关节连杆内部都配备有状态感知器，训练决策器和动作选择器三个模块，状态感知器将环境状态映射成其内部感知，动作选择器则是根据当前策略选择合理的动作作用于环境，训练决策器则是根据环境反馈以及内部感知来训练更新策略知识以用于机械臂后续工作动作的选择，且训练决策器提前对各种环境进行感知学习训练构建数据库，确定机械臂避免碰撞时各连杆运动方向、转动角度、移动路程等；动作选择器根据训练决策器得到的当前策略选择动作X作用于环境达到避免碰撞的效果。

本发明主要致力于通过双机械臂智能协作提高机器人排爆效率，相对于传统单臂排爆机器人的排爆效率大幅提高。此排爆机器人双臂协同控制系统具有以下特点：排爆机器人两机械臂可在工作空间中协作完成系统分配到的排爆任务，系统在排爆过程中，智能为两机械臂分配排爆任务，根据排爆数据库的任务知识对当前排爆任务进行任务分解，得到各机械臂操作序列，两机械臂通过排爆知识库得到的任务操作序列对机械臂末端执行器的关节空间坐标进行规划，在机械臂运动过程中同时进行智能避免碰撞操作，避免机械臂运动过程中的碰撞，直至机械臂完成排爆任务结束。采用此排爆机器人双臂协同控制系统进行排爆操作可提高机器人排爆效率，且在复杂的地形环境下，仍可保证排爆任务快速稳定进行。

附图说明

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

图1为本发明的系统流程图。

具体实施方式

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

具体实施方式

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

如图1所示，一种排爆机器人双臂协同控制系统，步骤如下：

S1，建立排爆数据库。

机器人通过学习训练传统排爆过程，得出一个排爆数据库；所述排爆数据库包括任务知识数据库、任务库和机械臂知识能力库。

S2，获取任务并将任务分配。

排爆机器人探寻到未爆物时，得到排爆任务，并采用一种基于trader集中式的双臂空间机器人任务分配方法将任务分配。

具体步骤为：

S2.1建立机械臂任务能力知识库，包括机械臂能力信息和任务的能力信息；

S2.1.1，机械臂的能力。

所述机械臂的能力包括能力类型和与能力类型相对应的能力水平。

S2.1.1.1，机械臂的能力类型。

机械臂具有多种不同的能力，如抓握能力、负载能力、灵活程度、指端力觉能力，视觉能力、腕力觉能力、运算能力、通信能力、灵巧能力，这些能力细化为单个的原子能力，并将这些原子能力组成能力类型集合。

机械臂的能力类型集合是由m个原子能力c_j组成的能力集合：

C＝{c_j}，1≤j≤m；

S2.1.1.2，机械臂的能力水平。

机械臂的每个原子能力都具有一个对应的能力水平，第i个机械臂的原子能力 c_j的能力水平为L_ij，取值范围为0≤L_ij≤1，第i个机械臂的第c_j项能力越高，则L_ij越大，即

L_ij＝diag{L_i1,L_i2,…,L_ij,…,L_im}。

S2.1.2，任务的能力信息。

S2.1.2.1，任务对机械臂某一原子能力c_j的需求为N_kj；即如果任务需要第c_j项能力，N_kj取 1，否则取0，即

S2.1.2.2，完成任务对机械臂某一原子能力c_j的需求强度为W_kj。

取值范围为0≤W_kj≤1，第c_j项能力对任务的影响越大，则W_kj越大，即

W_kj＝diag{W_k1,W_k2,…,W_kj,…,W_km}。

S2.2，根据分解动作序列对应的机械臂能力信息和任务能力信息选择执行动作的机械臂。

比较每一个分解动作序列所对应的机器臂能力信息和任务能力信息的大小，当机械臂能力水平大于任务对机械臂能力的需求强度，表示当前机械臂能完成该分解动作。

S2.3，使两个机械臂的控制器之间进行通信，分别以对方为参照物，判断将已经分解的任务如何分配给两个机械臂，使两个机械臂可以合作完成。

机器人以构建的能力集合为标准，将每一个子任务按序与机械臂能力集合做对比，设当前其中一个子任务为p_k，若存在L_ij>＝W_kj,则表示机械臂能完成任务，则优先为机械臂A分配此任务，将机械臂A已分配的任务对应的能力L_ij值降低或归零，然后进行下一个子任务与机械臂能力集合的对比，优先进行机械臂A的现有能力匹配，当机械臂A能力集合不满足L_ij>＝W_kj时，则进行机械臂B的能力集合匹配，机械臂B的能力集合匹配同机械臂A。

S3，进行任务分解。

机械臂的任务描述表示为一个隐式动作计划，它指定了一个根据隐式基本操作和执行顺序关系的子目标集合，其中机械臂主要用来处理未爆物，动作序列主要包括初始状态、接近未爆物、调整姿态、到达、抓取、提升、旋转、腕部翻转、闭合手爪、暂停工作等子任务，如下表所示：

机械臂动作序列表

序号	子任务
		1	初始
2	接近
		3	调整
4	到达
		5	提升
6	腕部旋转
		7	闭合手爪
8	暂停工作
		……	……

当机械臂获得各自分配的任务之后通过任务知识将其各自任务分为一个子任务序列，同时产生与子任务相对应的动作序列，在机械臂动作序列表中查找到相应的动作序列，对其进行链接得到机械臂的详细动作序列，每一个步骤的动作序列都对应一系列的运动控制函数再进行相应的函数描述。其中，机器人的主要运动控制函数如下：相对于机器人基坐标系的位姿控制、相对于机器人基坐标系的位置向量控制、相对于机器人基坐标系的姿态向量控制、相对于机器人末端坐标系的位置向量控制、相对于机器人末端坐标系的姿态向量控制、关节角度控制、机器人暂停函数、机器人返回初始位形函数、末端执行器闭合，进行工作、末端执行器张开，到其初始状态为止。

机械臂的动作序列主要由上述运动函数构成，当机械臂的运动序列确定之后，根据确切情况，确定各个动作的运动参数，用以上运动控制函数进行表示。

S4，对机械臂进行路径规划；

机械臂上的控制器接收末端执行器的各传感器实时采集的自身位置和方位信息，控制器将接收检测信息并将检测信息通过卡尔曼滤波器进行滤波并预测下一时刻的位置和方位信息，控制器将得到的预测信息传输给另一机械臂的控制器上，实现两个机械臂之间的相互通讯；通过两臂控制器之间的通信使得一个机械臂对于另一个机械臂的反应做出相应的动作、轨迹规划和决策。

具体步骤为：

S4.1，对末端执行器的关节空间坐标进行规划；

通过排爆知识库得到的任务操作序列对机械臂末端执行器的关节空间坐标进行规划；

S4.2，机械臂路径规划；

采用栅格法进行机械臂的运动路径规划，具体步骤如下：

S4.2.1，经过步骤S2和步骤S3，机械臂获取到任务以及子任务序列，也就是得知机械臂的目标点位置；

S4.2.2，以机械臂的底座在空间的位置为原点，建立三维坐标系，设置以机械臂底座为中心，机械臂连杆长度之和为半径的球形空间；在球形空间中以i为栅格宽度，将球形空间划分为若干三维栅格；

S4.2.3，通过机械臂配备的传感器对当前机械臂附近环境进行信息采集，实时判断障碍物的空间位置，将存在障碍物的栅格定义为障碍栅格，不存在障碍物的栅格定义为自由栅格；

S4.2.4，机械臂根据自身当前状态、目标点和自由度分布信息，在现有的自由栅格范围内，使用A*算法搜索出最优的通向目标点的路径。

S5，机械臂向停驻点进行运动过程中进行智能避免碰撞；

所述碰撞包括机械臂之间的碰撞以及机械臂与障碍物的碰撞；机械臂连杆进行障碍物的探测，并通过状态感知器将环境状态映射为机械臂的内部感知，工作选择器根据当前策略选择避障动作作用于环境，训练决策器通过更新学习策略知识用于后续动作的选择。

机械臂通过超声波传感器得到当前障碍物的空间位置，且通过各个关节连杆状态感知器反馈回的信息得到机械臂当前姿态偏差角和转动增量；当机械臂与障碍物或另一机械臂之间的距离d_i＜d时，其中，d为系统设置的机械臂与障碍物或另一机械臂之间最小距离，机械臂通过栅格法，在当前的自由栅格范围内，通过训练决策器和动作选择器选择动作X，使机械臂与障碍物之间的距离d_i≥d，完成避障。

而且在每节关节连杆内部都配备有状态感知器，训练决策器和动作选择器三个模块，状态感知器将环境状态映射成其内部感知，动作选择器则是根据当前策略选择合理的动作作用于环境，训练决策器则是根据环境反馈以及内部感知来训练更新策略知识以用于机械臂后续工作动作的选择，且训练决策器提前对各种环境进行感知学习训练构建数据库，确定机械臂避免碰撞时各连杆运动方向、转动角度、移动路程等；动作选择器根据训练决策器得到的当前策略选择动作X作用于环境达到避免碰撞的效果。

譬如机械臂连杆编号从肩部开始到末端执行器分别为1、2、3，则d₃、θ₃输入连杆3，输出量是Δθ₃，连杆3将信息传送给连杆2，连杆2同连杆3，最终信息传递给连杆1。

下面对本发明的原理进行说明

将双机械臂排爆机器人的机械臂运动过程分为以下几个过程：首先对排爆任务进行确定，智能进行各个机械臂的任务分配；对各个机械臂进行路径规划；机械臂向停驻点进行运动过程中进行智能避碰。

将双臂机器人协调操作问题可分为两种类型：松协调和紧协调，松协调任务的特点是在共享工作空间内，每一个机械臂独立执行各自的任务，例如：检测到未爆物之后，通过排爆任务数据库获得排爆任务，对任务进行计算分解得到一只机械臂抓取固定未爆物，另一个机械臂在此基础上进行排爆操作；紧协调任务的特点是双臂机器人是强耦合的，而物体的期望路径完全决定了每一个操作臂的操作空间运动轨迹，例如：当获取排爆任务进行计算分解之后，得到任务为双臂协调转移未爆物，也即双臂末端执行器与未爆物之间没有相对运动，机械臂需要将未爆物从初始位置转移到规划位置。

机械臂任务分配：

任务分解：

针对排爆机器人排爆任务分解，采用一种基于任务知识库的分解方法，基本任务由机械臂的基本操作组成，将任务分解为一系列的操作序列，用隐式基本操作序列表示，然后再将隐式基本操作序列转化为机械臂可以进行运动规划的显式基本操作序列。

机器人通过学习训练传统排爆过程，得出一个排爆任务数据库，当机器人进行排爆时，将当前排爆任务与数据库中排爆任务对比，判断进行紧协调/松协调任务，系统根据排爆数据库的任务知识对当前排爆任务进行任务分解，得到一系列子任务操作序列，生成各个子任务的能力矩阵，同时产生与子任务相对应的动作序列。将动作序列用隐式操作序列进行表示，每一个动作序列用隐式操作序列进行表示时，表达式中指定了被操作物体、操作动作和机械臂。将隐式操作序列中的操作动作通过一系列运动控制函数表示出来，即转化为机械臂可直接进行调用的显示操作序列。

任务分配：

针对排爆机器人的任务分配，对双臂机器人采用形式化的描述方法，通过定义能力向量，对任务和机械臂的能力进行量化描述，采用了一种基于trader集中式的双臂空间机器人任务分配方法。

机器人的双臂任务分配相当于人类的大脑将任务分解分配给双臂，采用的是一种集中式的任务分配方式。在机器人集中式任务分配中，主要有两种方法：①人为预先分配，即将待分配的任务由人预先安排好，分配费系统中的agent。但是该方法灵活性较差，不能适应变化的环境，排爆机器人在灵活多变的环境中工作不适用。②系统中存在集中式分配者(trader)，其内部存储有一张关于每个机械臂(agent)技能的表格。进行任务分配的时候，它通过查询这张技能表格，可以直接指定某agent或者是两个agent协商完成任务分配。本排爆机器人采用的是双臂机器人系统，agent数目较少，只有两个，对机器人的信息负载没有太高的要求，运用集中式任务分配方法能够一定程度上提高系统的运行效率，运用了一种基于trader 的集中式双臂空间机器人任务分配方法，用于实现排爆机器人排爆时的任务分配，建立一个机械臂任务能力知识库，其中存有机械臂和任务的能力信息。

agent的能力分类：

agent具有多种不同的能力，如抓握能力、负载能力、灵活程度、指端力觉能力，视觉能力、腕力觉能力、运算能力、通信能力、灵巧能力等，而对于一个任务来说，需要agent的多种能力才能够完成。将这些能力细化为单个的原子能力，并将这些原子能力组成能力集合。

能力集合：由m个原子能力c_j组成的能力集合，即

C＝{c_j}，1≤j≤m；

每一个agent的能力描述

定义某个agent的第c_j项的能力水平为L_ij，该参数为常量，取值范围为0≤L_ij≤1，第i 个agent的第c_j项能力越高，则L_ij越大，即

L_ij＝diag{L_i1,L_i2,…,L_ij,…,L_im}；

针对agent即将要进行的任务的能力的描述：

设有l个任务t_k(1≤k≤l)。对于任务t_k，定义其对agent能力c_j的需求为N_kj，该参数为变量。即如果任务需要第c_j项能力，N_kj取1，否则取0，即

定义完成任务t_k所需要能力c_j的需求强度为W_kj，该参数为常量，取值范围为1≤W_kj≤1，第c_j项能力对任务的影响越大，则W_kj越大，即

W_kj＝diag{W_k1,W_k2,…,W_kj,…,W_km}；

Agent和任务的能力向量是与任务分解的动作序列一一对应的，即每一个动作序列所对应的agent的任务的能力向量都是不同的。通过比较每一动作序列所对用的机器人和任务的能力向量的大小即可判断agent能否完成任务，在判断agent能够完成任务的情况下，通过两个agent进行通信，分别以对方为参照物，判断将已经分解的任务如何分配给两个agent，使两个agent可以合作完成任务。

机械臂控制方式：

两机械臂协作完成排爆任务，采用分布式控制，即两只机械臂都采用力与位置结合的控制方法。双臂各有一个控制器，分别控制各自运动方向和末端执行器的工作，末端执行器分别配备一个六维力传感器，实时检测，机械臂工作过程中，为了机械臂更好进行协同推进工作，两控制器之间实时进行信息交换，即两控制器之间实时进行通信，传递当前此机械臂运动方向所在空间位置、各个关节速度、加速度等信息。但是这种信息传递有较大的滞后，特别是在机械臂运动速度快的时候尤为明显，为了克服分布式控制方式中的滞后问题，采用卡尔曼滤波辅助补偿。卡尔曼滤波补偿的基本思想是各个机械臂与对方机械臂进行通信之前，首先对当前自身位置和方位信息进行检测，同时运用卡尔曼滤波对下一时刻的位置和方位信息进行预测，再将这些信息打包发送给对方机械臂，可以很大程度上解决信息滞后的问题。

机械臂路径规划：

双臂排爆机器人在某种程序上可以看做是两个单臂机器人在一起协同工作，当把其他的机器人看做一个未知源干扰的时候，其中的一个机器人就独立于另一个机器人。但是本项目涉及到的双臂排爆机器人作为一个完整的机器人系统，其双臂之间存在着依赖关系，它们通过控制器间的通信，实现了传感器数据的共享，双臂之间通过一个共同联结的物理耦合，且两臂之间具有协调关系，也就是通过两臂控制器之间的通信使得一个臂对于另一个臂的反应可以作出相应的动作、轨迹规划和决策等，在某种程度上与人类的双臂协调类似。

在机器人进行路径规划之前，通过排爆知识库得到的任务操作序列可以对机械臂末端执行器的关节空间坐标进行规划，在满足运动和力约束的基础上进行机械臂末端执行器运动，在机械臂末端执行器向提前规划好的空间坐标运动过程中，调用机械臂智能避碰功能。

机械臂智能避碰：

将机械臂自机器人肩部开始到末端执行器每个关节连杆依次标记为θ₁到θ_i，每个连杆主要对一下两种信息进行感知：①连杆与最近障碍物的距离d_i，②连杆当前的姿态偏差角θ_i。系统输出量是每个连杆转动增量Δθ_i。机械臂连杆编号从肩部开始到末端执行器分别为1、2、 3，则d₃、θ₃输入连杆3，输出量是Δθ₃，连杆3将信息传送给连杆2，连杆2同连杆3，最终信息传递给连杆1。另外每节连杆内部都配备有状态感知器，训练决策器和动作选择器三个模块，状态感知器将环境状态映射成其内部感知，动作选择器则是根据当前策略选择合理的动作作用于环境，训练决策器则是根据环境反馈以及内部感知来训练更新策略知识以用于机械臂后续工作动作的选择。

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

Claims (8)

1.一种排爆机器人双臂协同控制系统，其特征在于，步骤如下：

S1，建立排爆数据库；

机器人通过学习训练传统排爆过程，得出一个排爆数据库；所述排爆数据库包括任务知识数据库、任务库和机械臂知识能力库；

S2，获取任务并将任务分配；

排爆机器人探寻到未爆物时，得到排爆任务，并采用一种基于trader集中式的双臂空间机器人任务分配方法将任务分配；

S3，进行任务分解；

S4，对机械臂进行路径规划；

机械臂上的控制器接收末端执行器的各传感器实时采集的自身位置和方位信息，控制器将接收检测信息并将检测信息通过卡尔曼滤波器进行滤波并预测下一时刻的位置和方位信息，控制器将得到的预测信息传输给另一机械臂的控制器上，实现两个机械臂之间的相互通讯；通过两臂控制器之间的通信使得一个机械臂对于另一个机械臂的反应做出相应的动作、轨迹规划和决策；

S5，机械臂向停驻点进行运动过程中进行智能避免碰撞；

所述碰撞包括机械臂之间的碰撞以及机械臂与障碍物的碰撞；机械臂连杆进行障碍物的探测，并通过状态感知器将环境状态映射为机械臂的内部感知，工作选择器根据当前策略选择避障动作作用于环境，训练决策器通过更新学习策略知识用于后续动作的选择。

2.根据权利要求1所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S2中，具体步骤为：

S2.1建立机械臂任务能力知识库，包括机械臂能力信息和任务的能力信息；

S2.1.1，机械臂的能力；

所述机械臂的能力包括能力类型和与能力类型相对应的能力水平

S2.1.2，任务的能力信息；

S2.2，根据分解动作序列对应的机械臂能力信息和任务能力信息选择执行动作的机械臂；

比较每一个分解动作序列所对应的机器臂能力信息和任务能力信息的大小，当机械臂能力水平大于任务对机械臂能力的需求强度，表示当前机械臂能完成该分解动作；

S2.3，使两个机械臂的控制器之间进行通信，分别以对方为参照物，判断将已经分解的任务如何分配给两个机械臂，使两个机械臂可以合作完成；

机器人以构建的能力集合为标准，将每一个子任务按序与机械臂能力集合做对比，设当前其中一个子任务为p_k，若存在L_pj＞＝W_pj，则表示机械臂能完成任务，则优先为机械臂A分配此任务，将机械臂A已分配的任务对应的能力水平L_pj值降低或归零，然后进行下一个子任务与机械臂能力集合的对比，优先进行机械臂A的现有能力匹配，当机械臂A能力集合不满足L_pj＞＝W_pj时，则进行机械臂B的能力集合匹配，机械臂B的能力集合匹配同机械臂A，其中，L_pj为子任务p_k的能力水平，W_pj为完成子任务p_k所需的需求强度。

3.根据权利要求2所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S2.1.1中，具体步骤为：S2.1.1.1，机械臂的能力类型；

机械臂具有多种不同的能力，包括抓握能力、负载能力、灵活程度、指端力觉能力、视觉能力、腕力觉能力、运算能力、通信能力、灵巧能力，这些能力细化为单个的原子能力，并将这些原子能力组成能力类型集合：

机械臂的能力类型集合是由m个原子能力c_j组成的能力集合：

C＝{c_j}，1≤j≤m；

S2.1.1.2，机械臂的能力水平；

机械臂的每个原子能力都具有一个对应的能力水平，第i个机械臂的原子能力c_j的能力水平为L_ij，取值范围为0≤L_ij≤1，第i个机械臂的第c_j项能力越高，则L_ij越大，即

L_in＝diag{L_i1，L_i2，…，L_ij，…，L_im}。

4.根据权利要求2所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S2.1.2中，具体步骤为：S2.1.2.1，任务对机械臂某一原子能力c_j的需求为N_kj；

即如果任务需要第c_j项能力，N_kj取1，否则取0，即

S2.1.2.2，完成任务对机械臂某一原子能力c_j的需求强度为W_kj；

取值范围为0≤W_kj≤1，第c_j项能力对任务的影响越大，则W_kj越大，即

W_kn＝diag{W_k1，W_k2，…，W_kj，…，W_km}。

5.根据权利要求1所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S3中，机械臂的任务描述表示为一个隐式动作计划，它指定了一个根据隐式基本操作和执行顺序关系的子目标集合，其中机械臂用来处理未爆物，动作序列包括初始状态、接近未爆物、调整姿态、到达、抓取、提升、旋转、腕部翻转、闭合手爪、暂停工作的子任务；当机械臂获得各自分配的任务之后通过任务知识将其各自任务分为一个子任务序列，同时产生与子任务相对应的动作序列，在机械臂动作序列表中查找到相应的动作序列，对其进行链接得到机械臂的详细动作序列，每一个步骤的动作序列都对应一系列的运动控制函数再进行相应的函数描述。

6.根据权利要求1所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S4中，具体步骤为：

S4.1，对末端执行器的关节空间坐标进行规划；

通过排爆数据库得到的任务操作序列对机械臂末端执行器的关节空间坐标进行规划；

S4.2，机械臂路径规划；

采用栅格法进行机械臂的运动路径规划，具体步骤如下：

S4.2.1，经过步骤S2和步骤S3，机械臂获取到任务以及子任务序列，也就是得知机械臂的目标点位置；

S4.2.2，以机械臂的底座在空间的位置为原点，建立三维坐标系，设置以机械臂底座为中心，机械臂连杆长度之和为半径的球形空间；在球形空间中以i1为栅格宽度，将球形空间划分为若干三维栅格；

S4.2.3，通过机械臂配备的传感器对当前机械臂附近环境进行信息采集，实时判断障碍物的空间位置，将存在障碍物的栅格定义为障碍栅格，不存在障碍物的栅格定义为自由栅格；

S4.2.4，机械臂根据自身当前状态、目标点和自由度分布信息，在现有的自由栅格范围内，使用A*算法搜索出最优的通向目标点的路径。

7.根据权利要求1所述的一种排爆机器人双臂协同控制系统，其特征在于：在步骤S5中，机械臂通过超声波传感器得到当前障碍物的空间位置，且通过各个关节连杆状态感知器反馈回的信息得到机械臂当前姿态偏差角和转动增量；当机械臂与障碍物或另一机械臂之间的距离d_i2＜d时，其中，d为系统设置的机械臂与障碍物或另一机械臂之间最小距离，机械臂通过栅格法，在当前的自由栅格范围内，通过训练决策器和动作选择器选择动作，使机械臂与障碍物之间的距离d_i2≥d，完成避障。

8.根据权利要求7所述的一种排爆机器人双臂协同控制系统，其特征在于：在每节关节连杆内部都配备有状态感知器，训练决策器和动作选择器三个模块，状态感知器将环境状态映射成其内部感知，动作选择器则是根据当前策略选择合理的动作作用于环境，训练决策器则是根据环境反馈以及内部感知来训练更新策略知识以用于机械臂后续工作动作的选择，且训练决策器提前对各种环境进行感知学习训练构建数据库，确定机械臂避免碰撞时各连杆运动方向、转动角度、移动路程；动作选择器根据训练决策器得到的当前策略选择动作作用于环境达到避免碰撞的效果。

Images