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

摘要

本发明公开一种用于全自动血型血清学分析系统的双机械臂协同控制方法，包含在接收到血样或试剂搬运任务指令后，首先基于两个机械臂任务路径进行快速干涉判断，其次根据判断结果进行两个机械臂运动时间的合理分配；并且基于梯形速度模式对各机械臂的运动轨迹进行规划，有效避免了系统启停过程的冲击，并能充分发挥时间优势，提高血样搬运效率。该控制技术能够适应时变的任务指令，针对不同操作要求，双机械臂能够在有限时间内快速、无干涉完成既定任务，且运算量小、易于拓展、工程实用性强。

主权项

一种用于全自动血型血清学分析系统的双机械臂协同控制方法，其特征在于，包含如下步骤：步骤1：建立控制系统的全局坐标系OXYZ，机械臂A和机械臂B的零位坐标分别记为和根据血样或试剂搬运操作的需求，计算出机械臂A本次搬运动作的起点坐标为终点坐标为计算出机械臂B本次搬运动作的起点坐标为终点坐标为式中，三维位置坐标其余各坐标值的组成形式以此类推；步骤2：基于空间三角形的投影图形，对机械臂A和机械臂B的动作路径进行干涉判断；步骤3：根据干涉判断结果，当两机械臂的动作路径无干涉，则在相同时间内独立控制机械臂A和机械臂B同步完成各自指令动作，并以此做两个机械臂的梯形速度模式轨迹规划；当两机械臂的动作路径存在干涉，则根据路径长度特点，进行时间片分配，避开路径障碍，实现机械臂A和机械臂B的安全、协同工作；所述步骤2对机械臂A和机械臂B的动作路径进行干涉判断的具体过程为：201、记空间三角形在平面XOY的投影三角形为ΔQRS，其三条边的边长分别记为q,r,s，则有：q=(x1A-x2A)2+(y1A-y2A)2]]>r=(x2A-x0A)1+(y2A-y0A)2]]>s=(x1A-x0A)2+(y1A-y0A)2]]>令则ΔQRS面积为202、记空间三角形在平面XOY的投影三角形为ΔUVW，并根据步骤201依次计算ΔWQS,ΔWSR,ΔWQR的面积，分别记为SΔWQS,SΔWSR,SΔWQR；203、进行数值比较判断，如果SΔWQS+SΔWSR+SΔWQR＞SΔQRS，则两机械臂的动作路径发生干涉；否则，两机械臂的动作路径无干涉。