[发明专利]用于全自动血型血清学分析系统的双机械臂协同控制方法

说明书

技术领域

本发明涉及血液分析设备领域，具体为一种用于全自动血型血清学分析系统的双机械臂协同控制方法，实现在有限时间内的双机械臂无干涉、快速血样搬运操作。

背景技术

机械臂伺服系统在医疗设备、航空航天、工业自动化领域已经得到了广泛的应用，在全自动血型血清学分析系统中，一个机械臂往往难以在有限时间内，完成较为复杂的血样搬运、试剂添加等一系列动作，因此，引入双机械臂控制系统来满足任务需求。

单个机械臂的控制技术，例如示教-再现、轨迹规划、力位混合、顺应控制等已经趋于成熟，能够以较高的精度快速完成操作任务。目前，双机械臂的协同控制技术侧重于解决两套机械臂合力操作同一目标，或者是采用示教-再现模式操作周而复始完成固定的任务，其控制技术的适应性有待提高，以满足在全自动血型血清学分析系统中不断变化的任务指令要求。

发明内容

本发明要解决的技术问题是克服现有双机械臂的协同控制技术中的不足，提供一种用于全自动血型血清学分析系统的双机械臂协同控制方法，是一种能够适应时变任务指令的高效避障、轨迹规划一体的双机械臂协同控制技术。该方法是采用一种快速避障算法解决双机械臂路径干涉的判断和处理，基于梯形速度曲线模式对双机械臂的运动轨迹进行规划，充分利用给定的有限时间约束，完成血样或试剂的搬运任务。

为解决上述技术问题，本发明的技术方案如下：

一种用于全自动血型血清学分析系统的双机械臂协同控制方法，包含如下步骤：

步骤1：建立控制系统的全局坐标系OXYZ，机械臂A和机械臂B的零位坐标分别记为和根据血样或试剂搬运操作的需求，计算出机械臂A本次搬运动作的起点坐标为终点坐标为计算出机械臂B本次搬运动作的起点坐标为终点坐标为式中，三维位置坐标其余各坐标值的组成形式以此类推；；

步骤2：基于空间三角形的投影图形，对机械臂A和机械臂B的动作路径进行干涉判断；

步骤3：根据干涉判断结果，当两机械臂的动作路径无干涉，则在相同时间内独立控制机械臂A和机械臂B同步完成各自指令动作，并以此做两个机械臂的梯形速度模式轨迹规划；当两机械臂的动作路径存在干涉，则根据路径长度特点，进行时间片分配，避开路径障碍，实现机械臂A和机械臂B的安全、协同工作。

优选地，所述步骤2对机械臂A和机械臂B的动作路径进行干涉判断的具体过程为：

201、记空间三角形在平面XOY的投影三角形为ΔQRS，其三条边的边长分别记为q,r,s，则有：

令

则ΔQRS面积为

202、记空间三角形在平面XOY的投影三角形为ΔUVW，并根据步骤201依次计算ΔWQS,ΔWSR,ΔWQR的面积，分别记为S_ΔWQS,S_ΔWSR,S_ΔWQR；

203、进行数值比较判断，如果S_ΔWQS+S_ΔWSR+S_ΔWQR＞S_ΔQRS，则两机械臂的动作路径发生干涉；否则，两机械臂的动作路径无干涉。

优选地，所述步骤3中，当两机械臂的动作路径无干涉，则根据操作血样的总时间T_f，按照如下步骤进行机械臂A和机械臂B的轨迹规划；

301、控制机械臂在动作路径的每个直线段上先匀加速运动后匀减速运动，以机械臂A从零位到机械臂B从零位到的点位运动进行轨迹规划，其余各直线段以此类推；

302、根据血样搬运的路径长度和总运行时间，确定机械臂A和机械臂B的在该直线段的运行时间分别为：

303、确定机械臂A和机械臂B的最小许可合成速度分别为：

304、确定机械臂A和机械臂B的最大许可合成速度值分别为：

305、确定机械臂A和机械臂B的梯形运动曲线中拟实现的最大合成速度分别为：

式中，常量λ_A,λ_B∈(0,1)；

从而得机械臂A和机械臂B的加速时间、加速度分别为：

306、机械臂A和机械臂B在该直线段运行过程中的三阶段运动方程均采用如下函数，式中X和a等变量略去右上标A，B；

第一阶段：匀加速运动；