CN106546759B

CN106546759B - 用于全自动血型血清学分析系统的双机械臂协同控制方法

Info

Publication number: CN106546759B
Application number: CN201610938230.1A
Authority: CN
Inventors: 田应和; 阮建蓉; 段学超; 米建伟; 顾顺利; 阮清源; 刘连强; 黄陆光
Original assignee: Guangzhou Kang Kang Pharmaceutical Co Ltd
Current assignee: Guangzhou Kang Kang Pharmaceutical Co Ltd
Priority date: 2016-10-25
Filing date: 2016-10-25
Publication date: 2018-04-13
Anticipated expiration: 2036-10-25
Also published as: CN106546759A

Abstract

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

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

步骤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；

第一阶段：匀加速运动；

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

第三阶段：匀减速运动；

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

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

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

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

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

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

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

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

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

第一阶段：匀加速运动；

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

第二阶段：匀速运动；

第三阶段：匀减速运动；

由于将机械臂的路径划分为若干个直线段，故t_f是机械臂A和机械臂B的在所有直线段的运行时间之和。

与现有技术相比，本发明技术方案的有益效果是：

本发明的技术构思为：基于空间三角形的投影图形快速进行路径干涉判断，若无路径干涉，则在相同时间内独立控制双机械臂同步完成各自指令动作，并以此做两个机械臂的梯形速度模式轨迹规划；若两机械臂路径存在干涉，则根据路径长度特点，进行时间片合理分配，有效避开路径障碍，实现双机械臂的安全、协同工作。

本发明的优点是，能够适应时变的任务指令，针对不同操作要求，双机械臂能够在有限时间内快速、无干涉完成既定任务，且避免刚性冲击，运行平稳性好。

附图说明

图1为本发明所涉及的双机械臂协同控制示意图

图2为本发明所述工作路径干涉判别示意图

图3为本发明机械臂空间合成轨迹速度曲线图

图4为本发明中机械臂A的第一段直线段运行中轨迹曲线。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。下面结合附图和实施例对本发明的技术方案做进一步的说明。

参照附图，本发明所述的用于全自动血型血清学分析系统的双机械臂协同控制方法，包含如下步骤:

步骤1：如图1所示，建立系统全局坐标系OXYZ，机械臂A和机械臂B的零位坐标分别记为和根据血样或试剂搬运操作的需求，计算出机械臂A本次搬运动作的起点坐标为终点坐标为以及计算出机械臂B本次搬运动作的起点坐标为终点坐标为式中，其余各坐标值以此类推。

步骤2：对机械臂A和机械臂B的动作路径进行干涉判断

2.1如图2所示，记空间三角形在平面XOY的投影三角形为ΔQRS，其三条边的边长分别记为q,r,s，则

则ΔQRS的半周长

2.2计算ΔQRS面积为

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

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

步骤3：根据2.4的判断结果，如果两机械臂的动作路径无干涉，则根据操作血样的时间T_f，按照如下步骤进行机械臂A和机械臂B的轨迹规划；

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

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

3.3确定机械臂A和机械臂B的最小(极限)合成速度值分别为

3.4，确定机械臂A和机械臂B的最大(极限)合成速度值分别为

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

式中，常量λ_A,λ_B∈(0,1)，两者取值可以相同。进一步可得机械臂A和机械臂B的加速时间、加速度分别为

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

第一阶段：匀加速运动；

第二阶段：匀速运动；

第三阶段：匀减速运动；

图4所示为机械臂A从利用6秒钟运动到的轨迹曲线。

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

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

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

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

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

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

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

第一阶段：匀加速运动；

第二阶段：匀速运动；

第三阶段：匀减速运动；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims

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

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

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

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

令

则ΔQRS面积为

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

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

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

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

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

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>m</mi> <mi>A</mi> </msubsup> <mo>=</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>A</mi> </msubsup> <mo>+</mo> <msub> <mi>&lambda;</mi> <mi>A</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mn>2</mn> <mi>A</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>A</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>m</mi> <mi>B</mi> </msubsup> <mo>=</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>B</mi> </msubsup> <mo>+</mo> <msub> <mi>&lambda;</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mn>2</mn> <mi>B</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>B</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

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

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

第一阶段：匀加速运动；

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

第二阶段：匀速运动；

第三阶段：匀减速运动；

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

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

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>m</mi> <mi>A</mi> </msubsup> <mo>=</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>A</mi> </msubsup> <mo>+</mo> <msub> <mi>&lambda;</mi> <mi>A</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mn>2</mn> <mi>A</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>A</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>m</mi> <mi>B</mi> </msubsup> <mo>=</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>B</mi> </msubsup> <mo>+</mo> <msub> <mi>&lambda;</mi> <mi>B</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mn>2</mn> <mi>B</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mn>1</mn> <mi>B</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

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

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

第一阶段：匀加速运动；

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

第二阶段：匀速运动；

第三阶段：匀减速运动；