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

权利要求书

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

步骤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，则两机械臂的动作路径发生干涉；否则，两机械臂的动作路径无干涉。

2.根据权利要求1所述的方法，其特征在于，所述步骤3中，当两机械臂的动作路径无干涉，则根据操作血样的总时间T_f，按照如下步骤进行机械臂A和机械臂B的轨迹规划；

301、控制机械臂在动作路径的每个直线段上先匀加速运动后匀减速运动，以机械臂A

从零位到机械臂B从零位到的点位运动进行轨迹规划，其余各直线段以此类推；

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

tfA=||X1A-X0A||2Tf||X1A-X0A||2+||X2A-X1A||2+||X2A-X0A||2tfB=||X1B-X0B||2Tf||X1B-X0B||2+||X2B-X1B||2+||X2B-X0B||2---(1)]]>

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

V1A=||X1A-X0A||2tfAV1B=||X1B-X0B||2tfB---(2)]]>

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

V2A=2||X1A-X0A||2tfAV2B=2||X1B-X0B||2tfB---(3)]]>

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

VmA=V1A+λA(V2A-V1A)VmB=V1B+λB(V2B-V1B)---(4)]]>

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

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

tatA=tfA-||X1A-X0A||2VmtatB=tfB-||X1B-X0B||2Vm---(5)]]>

a0A=VmAtatAX1A-X0A||X1A-X0A||2a0B=VmBtatBX1B-X0B||X1B-X0B||2---(6)]]>

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

第一阶段：匀加速运动；

a(t)=a0v(t)=a0tX(t)=X0+0.5a0t2,0≤t≤tat]]>

式中，a(t)表示t时刻机械臂的加速度，a₀表示预定的具体加速度值，v(t)表示t时刻机械臂的速度，t_at表示机械臂加速运动的时间总长，X(t)分别表示t时刻动平台的位置，X₀表示机械臂的零位坐标；

第二阶段：匀速运动；

a(t)=0v(t)=v(tat)X(t)=X(tat)+v(t)(t-tat),tat<t≤tf-tat]]>

第三阶段：匀减速运动；

a(t)=-a0v(t)=v(tf-tat)-a0(t+tat-tf)X(t)=X0+v(tf-tat)(t+tat-tf)-0.5a0(t+tat-tf)2,tf-tat<t≤tf;]]>

当两机械臂的动作路径发生干涉，则根据操作血样的时间T_f，按照如下步骤进行机械臂A和机械臂B的轨迹规划：

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

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

tfA=||X1A-X0A||2Tf||X1A-X0A||2+||X2A-X1A||2+||X2A-X0A||2tfB=(||X1A-X0A||2+||X2A-X1A||2)Tf||X1A-X0A||2+||X2A-X1A||2+||X2A-X0A||2---(7)]]>

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

V1A=||X1A-X0A||2tfAV1B=||X1B-X0B||2tfB---(8)]]>

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

V2A=2||X1A-X0A||2tfAV2B=2||X1B-X0B||2tfB---(9)]]>

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

VmA=V1A+λA(V2A-V1A)VmB=V1B+λB(V2B-V1B)---(10)]]>

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

进而机械臂A和机械臂B的加/减速时间、加速度分别为:

tatA=tfA-||X1A-X0A||2VmtatB=tfB-||X1B-X0B||2Vm---(11)]]>

a0A=VmAtatAX1A-X0A||X1A-X0A||2a0B=VmBtatBX1B-X0B||X1B-X0B||2---(12)]]>

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

第一阶段：匀加速运动；

a(t)=a0v(t)=a0tX(t)=X0+0.5a0t2,0≤t≤tat]]>

式中，a(t)表示t时刻机械臂的加速度，v(t)分别表示t时刻机械臂的速度，X(t)分别表示t时刻动平台的位置；

第二阶段：匀速运动；

a(t)=0v(t)=v(tat)X(t)=X(tat)+v(t)(t-tat),tat<t≤tf-tat]]>

第三阶段：匀减速运动；

a(t)=-a0v(t)=v(tf-tat)-a0(t+tat-tf)X(t)=X0+v(tf-tat)(t+tat-tf)-0.5a0(t+tat-tf)2,tf-tat<t≤tf.]]>