[發明專利]用于全自動血型血清學分析系統的雙機械臂協同控制方法

有效

• 申請號: 201610938230.1
• 申請日: 2016-10-25
• 公開（公告）號: CN106546759B
• 公開（公告）日: 2018-04-13
• 發明（設計）人: 田應和;阮建蓉;段學超;米建偉;顧順利;阮清源;劉連強;黃陸光
• 申請（專利權）人: 廣州譽康醫藥有限公司
• 主分類號: G01N35/00
• 分類號: G01N35/00
• 代理公司: 廣州粵高專利商標代理有限公司44102
• 代理人: 李佳佳

技術領域

本發明涉及血液分析設備領域，具體為一種用于全自動血型血清學分析系統的雙機械臂協同控制方法，實現在有限時間內的雙機械臂無干涉、快速血樣搬運操作。

背景技術

機械臂伺服系統在醫療設備、航空航天、工業自動化領域已經得到了廣泛的應用，在全自動血型血清學分析系統中，一個機械臂往往難以在有限時間內，完成較為復雜的血樣搬運、試劑添加等一系列動作，因此，引入雙機械臂控制系統來滿足任務需求。

單個機械臂的控制技術，例如示教-再現、軌跡規劃、力位混合、順應控制等已經趨于成熟，能夠以較高的精度快速完成操作任務。目前，雙機械臂的協同控制技術側重于解決兩套機械臂合力操作同一目標，或者是采用示教-再現模式操作周而復始完成固定的任務，其控制技術的適應性有待提高，以滿足在全自動血型血清學分析系統中不斷變化的任務指令要求。

發明內容

本發明要解決的技術問題是克服現有雙機械臂的協同控制技術中的不足，提供一種用于全自動血型血清學分析系統的雙機械臂協同控制方法，是一種能夠適應時變任務指令的高效避障、軌跡規劃一體的雙機械臂協同控制技術。該方法是采用一種快速避障算法解決雙機械臂路徑干涉的判斷和處理，基于梯形速度曲線模式對雙機械臂的運動軌跡進行規劃，充分利用給定的有限時間約束，完成血樣或試劑的搬運任務。

為解決上述技術問題，本發明的技術方案如下：

一種用于全自動血型血清學分析系統的雙機械臂協同控制方法，包含如下步驟：

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

第一階段：勻加速運動；