[發(fā)明專利]一種基于EtherCAT的六軸機械臂安全限位方法

在審

• 申請?zhí)枺?/td>: 201811332428.0
• 申請日: 2018-11-09
• 公開（公告）號: CN109108987A
• 公開（公告）日: 2019-01-01
• 發(fā)明（設(shè)計）人: 林偉陽;李圣;劉雨;李湛;佟明斯;高會軍
• 申請（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 哈爾濱市松花江專利商標事務(wù)所 23109
• 代理人: 劉冰

1.一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述方法包括以下步驟：

步驟一、設(shè)計基于EtherCAT的控制機械臂運作的主控算法；所述的主控算法實時循環(huán)任務(wù)，在每一個循環(huán)周期檢測一遍接收到的指令并進行指令的執(zhí)行；

步驟二、設(shè)計安全判斷算法，且在主控算法的每個檢測并執(zhí)行的周期內(nèi)執(zhí)行一次；其中，所述的安全判斷算法包括：

機械臂關(guān)節(jié)位姿超限判斷算法、機械臂末端位姿超限判斷算法、機械臂停止算法；

步驟三、機械臂主控算法的每一個循環(huán)周期內(nèi)調(diào)用一次安全算法，檢查機械臂的運行情況，并判斷運行情況是否超限；

若超限，則返回運行信息，并調(diào)用機械臂停止算法，控制機械臂減速；

若未超限，不進行停機操作，機械臂主控算法繼續(xù)運行。

2.根據(jù)權(quán)利要求1所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的主控算法進行檢查的循環(huán)周期為1ms。

3.根據(jù)權(quán)利要求1或2所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的步驟二中設(shè)計的機械臂關(guān)節(jié)位姿超限判斷算法，具體為：

從機械臂電機驅(qū)動器中讀取各關(guān)節(jié)的角位移，之后與所設(shè)定的各關(guān)節(jié)的角位移的限制范圍進行比較，若超出限定范圍則返回錯誤原因的編號；其中，所述的從機械臂電機驅(qū)動器中讀取各關(guān)節(jié)的角位移為檢查機械臂關(guān)節(jié)變量，包括：角位移上限、角位移下限、最大角速度、最大角加速度。

4.根據(jù)權(quán)利要求3所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的步驟二中設(shè)計的機械臂末端位姿超限判斷算法，具體為：

從機械臂電機驅(qū)動器中讀取各關(guān)節(jié)的角位移，之后由機械臂的正運動學(xué)計算出末端的位置姿態(tài)信息，再與所設(shè)定的末端姿態(tài)的限制范圍進行比較，若超出限制范圍則返回錯誤原因的編號；其中，所述的從機械臂電機驅(qū)動器中讀取各關(guān)節(jié)的角位移為機械臂末端在以機器人基座建立的坐標系中x軸，y軸，z軸方向上的位置。

5.根據(jù)權(quán)利要求1、2或4所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的步驟二中設(shè)計的機械臂停止算法，具體為：

調(diào)用機械臂關(guān)節(jié)位姿超限判斷算法和機械臂末端位姿超限判斷算法，當(dāng)兩種算法之一返回錯誤時，則控制機械臂按設(shè)定方式減速停止；在減速過程中，機械臂停止算法讀取各關(guān)節(jié)轉(zhuǎn)軸處電機的實際角速度，將所述的實際角速度減去預(yù)設(shè)的角加速度后的值，作為期望的角速度指令輸出到關(guān)節(jié)電機驅(qū)動器；當(dāng)所有電機的實際角速度低于設(shè)定的閾值時，判斷機械臂已完全停止，輸出停機指令。

6.根據(jù)權(quán)利要求5所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的步驟三中判斷運行情況是否超限的過程具體為：

安全算法通過主控算法的接口讀取關(guān)節(jié)轉(zhuǎn)軸處電機驅(qū)動器的數(shù)據(jù)，獲得機械臂轉(zhuǎn)軸的關(guān)節(jié)變量，并通過正運動學(xué)計算獲得機械臂末端的位置姿態(tài)信息，同時調(diào)用機械臂關(guān)節(jié)位姿超限判斷算法、機械臂末端位姿超限判斷算法，根據(jù)兩個算法返回值判斷機械臂運行情況是否超限，若超限，則調(diào)用機械臂停止算法，該算法向電機驅(qū)動器發(fā)送指令，使機械臂減速。

7.根據(jù)權(quán)利要求6所述的一種基于EtherCAT的六軸機械臂安全限位方法，其特征在于：所述的步驟三中若超限返回的運行信息為運行錯誤原因的錯誤編號；其中，錯誤編號包括：0、1、2；如果運行順利，程序返回0，表示運行期間沒有出現(xiàn)問題；如果是機械臂末端位姿超限判斷算法判定出超限結(jié)果，則返回1；如果是機械臂關(guān)節(jié)位姿超限判斷算法判定出超限結(jié)果，則返回2。從而，當(dāng)機械臂自己因為超限停了，根據(jù)返回的數(shù)字就可以判斷是出了什么問題。