TWI764820B

TWI764820B - 機械手臂避障方法及機械手臂避障系統

Info

Publication number: TWI764820B
Application number: TW110131915A
Authority: TW
Inventors: 林柏廷; 林照益; 林士偉; 黎昆政; 楊長運; 吳佩芬; 藍順謙
Original assignee: 正崴精密工業股份有限公司
Priority date: 2021-08-27
Filing date: 2021-08-27
Publication date: 2022-05-11
Also published as: TW202308815A

Abstract

一種機械手臂避障方法適用於機械手臂，並包含模型化步驟、蒐集評估座標步驟、取得控制變量步驟、建立佔據函數步驟及尋找避障姿態步驟。本發明透過將執行所述模型化步驟、所述蒐集評估座標步驟、所述取得控制變量步驟，及所述建立佔據函數步驟中所得到的數據先預存入一資料庫，藉此讓後續機械手臂執行任務時，能快速評估是否會發生碰撞行為，若評估會產生碰撞，則執行所述尋找避障姿態步驟來閃避障礙物。本發明採非接觸式的防撞設計，能改善現有接觸式防撞設計所面臨到的缺點。

Description

機械手臂避障方法及機械手臂避障系統

本發明是有關於一種自動化設備技術領域，特別是指一種非接觸式的機械手臂避障方法，及一種使用該機械手臂避障方法的機械手臂避障系統。

隨著工業技術的快速發展，自動化技術已被廣泛地應用於各行各業中，舉例來說，通過機械手臂或其他類似的設備，來協助生產以提高工廠的生產效率。然而機械手臂在運作過程中，其工作範圍內可能會有人員或其他障礙物的存在，目前為了避免機械手臂因碰撞而損壞，常見的防撞安全技術大致包括以下兩種：

第一種、利用阻力與工作電流的限制關係來達成保護目的，其原理是當機械手臂碰撞到障礙物時，會因為遇到阻力而使得用於控制機械手臂的馬達的電流加大，進而可藉此設定當工作電流超出限制時，便停止機械手臂運作。

第二種、利用智慧皮膚的方式來達成保護的目的，其原理是在機械手臂的表面上佈滿壓力感測器，當機械手臂碰撞到障礙物時，所述壓力感應器便會發出信號，控制機械手臂停止運作，然而，此種方式必須在機械手臂上盡可能佈滿感測器，所以除了有增加機械手臂整體重量的缺點外，也會因為配裝感測器的工序麻煩，進而增加生產成本。

由於上述兩種技術皆是採用接觸式，即必須碰觸到障礙物後，才會觸發停止運作的限制條件，故當機械手臂碰撞到人員或障礙物時，往往已發生人員受傷、產品損毀或機械手臂損壞的情形。另外現有機械手臂只針對其末端效應器(end effector)進行防撞效果的設計，並非是以機械手臂整體為考量，所以防撞效果實屬有限。

因此，本發明之目的，即在提供一種非接觸式且能實現總體防撞效果的機械手臂避障方法。

於是，本發明機械手臂避障方法適用於一機械手臂，該機械手臂包括多個關節與多個分別連接不同關節的連接臂。本發明機械手臂避障方法包含模型化步驟、蒐集評估座標步驟、建立佔據函數步驟、取得控制變量步驟及尋找避障姿態步驟。

所述模型化步驟是將所述連接臂均構建成固定直徑的柱體，且所述柱體的直徑不小於最粗連接臂的直徑。

所述蒐集評估座標步驟是蒐集模型化後的機械手臂在一工作範圍內的第i個工作姿態下的第k個採樣點，並將所述採樣點所對應的評估座標標記為

，i與k均屬正整數。

所述建立佔據函數步驟是建立用以判斷第m個空間座標是否為部分該機械手臂所佔據的佔據函數O，m屬正整數，當第m個空間座標與待比較的評估座標之間距小於一設定值時，定義佔據狀態下所對應的佔據函數O的函數值範圍為大於0.5且1以下，而非佔據狀態下所對應的佔據函數O的函數值範圍為0以上且0.5以下。

所述取得控制變量步驟是取得移動路徑上機械手臂的第i個工作姿態所對應的控制變量X ⁱ。

所述尋找避障姿態步驟是根據下列方程式Min(X ⁱ _new-X ⁱ)² s.t. O(X ⁱ _new)

O_T，以得到一避障姿態，其中，Xⁱ _new為避障姿態所對應的控制變量，O_T為一閥值。

本發明之另一目的，即在提供一種利用上述機械手臂避障方法的機械手臂避障系統。

於是，本發明機械手臂避障系統包含一機械手臂與一主機。該機械手臂固定於一基座，並包括多個關節與多個分別連接不同關節的連接臂。該主機電性連接該機械手臂並包括一資料庫及一運算控制模組。該資料庫儲存如前述所述模型化步驟、所述蒐集評估座標步驟、所述取得控制變量步驟，及所述建立佔據函數步驟中所得到的數據。該運算控制模組電性連接該資料庫，並用以執行如前述所述尋找避障姿態步驟，以控制該機械手臂閃過障礙物。

本發明之功效至少有：透過將機械手臂在執行任務中所可能採用的姿態參數先預存於資料庫中，以便後續機械手臂執行任務時能快速評估其在移動過程中是否會產生與障礙物碰撞之行為，若評估會產生碰撞，則執行所述尋找避障姿態步驟來閃避障礙物。本發明不僅能使機械手臂實現整體避障的防撞效果，也因為是採非接觸式的防撞設計，故也能改善現有接觸式防撞設計所面臨到的缺點。

1:機械手臂

1’:機械手臂

11:關節

12:連接臂

2:主機

21:資料庫

22:運算控制模組

23:訊號接收模組

3:基座

S101:模型化步驟

S102:蒐集評估座標步驟

S103:取得控制變量步驟

S104:建立佔據函數步驟

S105:尋找避障姿態步驟

W:工作範圍

、

:變異量

X ⁱ:控制變量

P:移動路徑

P _i、P _i+1:移動路徑上的路徑座標b障礙物

d_i,k,m、d_i,k+1,m:距離

、

、

:評估座標

R _m:空間座標

本發明之其他的特徵及功效，將於參照圖式的實施方式中清楚地呈現，其中：〔第一圖〕是本發明機械手臂避障方法的一流程圖；〔第二圖〕是本發明機械手臂避障系統的一實施例的一示意圖；〔第三圖〕是本發明機械手臂避障系統的一主機的一方塊圖；〔第四圖〕是本發明機械手臂避障系統的該實施例的一機械手臂在一工作範圍內的一示意圖；〔第五圖〕是本發明機械手臂避障系統的該實施例的該機械手臂的一示意圖，說明機械手臂在第1個工作姿態下且只有兩個關節自由度；〔第六圖〕是本發明機械手臂避障方法的一佔據函數的一分布圖，其橫軸上有第i個工作姿態的控制變量X ⁱ，而縱軸為佔據函數的函數值；〔第七圖〕是本發明機械手臂避障系統的該實施例的另一機械手臂在第i個工作姿態下的一示意圖，說明機械手臂在第i個工作姿態下的第k個採樣點

與第m個空間座標R _m兩者間距為d_i,k,m；〔第八圖〕是本發明該實施例的機械手臂沿一移動路徑移動的一示意圖；及〔第九圖〕是類似於第八圖的一示意圖，說明本發明機械手臂被控制為以一避障姿態來閃避障礙物。

在本發明被詳細描述之前，應當注意在以下的說明內容中，類似的元件是以相同的編號來表示，此外圖式中元件的形狀、尺寸、厚度、以及角度等相關參數並未依照比例繪製，其簡化用意僅為方便清楚說明。

參閱第一圖至第三圖，本發明機械手臂避障方法適用於機械手臂避障系統。所述機械手臂避障系統包括高自由度的機械手臂1，及與機械手臂1電性連接的主機2，在本實施例中，機械手臂1固定於一基座3，機械手臂1包括多個關節11與多個分別連接不同關節11的連接臂12。主機2包括彼此電性連接的資料庫21(database)、運算控制模組22與訊號接收模組23等等元件，所述運算控制模組22包括有處理器(processor)，所述訊號接收模組23包括有接收器(receiver)與發射器(emitter)，於實際應用上，主機2可以為機械手臂控制器(Robot Controller)、伺服器(sever)、桌上型電腦(desk computer)或筆記型電腦(laptop)等。主機2與機械手臂1的電性連接方式，不限於是透過訊號接收模組23以無線方式傳送訊號，也可以是透過有線方式傳送訊號。

本發明機械手臂避障方法至少包含下列步驟S101~步驟S105：參閱第一圖及第二圖、步驟S101、模型化步驟：將所述連接臂12均構建成固定直徑的柱體，且所述柱體的直徑不小於最粗連接臂的直徑。舉例來說，實體機械手臂1的多個連接臂12分別有徑寬為130mm與220mm兩種尺寸，則模型化後的連接臂12 之徑寬須至少等於220mm或大於220mm。此步驟是模擬機械手臂1在較嚴苛的條件下來進行閃避障礙物b(如第八圖所示)之距離估算，若後續模擬結果是順利閃避障礙物b，則表示機械手臂1在實際操作上，機械手臂1與障礙物b之間仍有相當程度的緩衝空間，也就是說，本發明多了模型化步驟能提高在使用時的可靠度。

參閱第一圖、第二圖及第四圖，步驟S102、蒐集評估座標步驟：蒐集模型化後的機械手臂1在一工作範圍W內的第i個工作姿態下的第k個採樣點，並將所述採樣點所對應的評估座標位置標記為

，i與k均屬正整數。不同i值表示機械手臂1的不同工作姿態，舉例來說，

表示為機械手臂1在第1個工作姿態中的第3個採樣點的評估座標，在第四圖中，採樣點是以三角形符號來表示，所述採樣點分別有各自的評估座標。當建構出一定數量的採樣點後，便能建立出採樣點的評估座標與機械手臂1的工作姿態兩者間的對應關係。

參閱第一圖、第二圖及第五圖，步驟S103、取得控制變量步驟：取得移動路徑P上機械手臂1的第i個工作姿態所對應的控制變量X ⁱ。所述控制變量X ⁱ可以是直接從控制器設定的控制變量X ⁱ；或是從已知的工作姿態，再根據逆向運動學得出所對應的控制變量，也就是說，在步驟S103中只要能得到機械手臂1在其移動路徑P上的第i個工作姿態所對應的控制變量X ⁱ，其控制變量X ⁱ的來源如何產生並不需特別為限。

第i個工作姿態所對應的控制變量X ⁱ符合下列方程式：

其中N屬於正整數且表示機械手臂1的最高自由度，在本實施例中採用的是八軸機械手臂1(如第二圖所示)，因此N為8，x_j表示第j個關節11的變異量大小，e _j表示第j個正規基底(normal basis)，j屬於正整數，經由方程式(1)便可讓所述控制變量X ⁱ，涵蓋關節11間的轉動或移動的變異量。

為了方便且清楚說明公式(1)，第四圖與第五圖是以關節數量為二的機械手臂來舉例，藉此進一步供理解以類推高自由度的八軸機械手臂的避障運作。第五圖顯示機械手臂1’的第1個工作姿態，所述第1個工作姿態可以是直接設定控制變量X ¹來操作，或是所述的第1個工作姿態是根據逆向運動學得出所對應的控制變量X ¹，應用於方程式(1)後，可表示成如下

而當機械手臂1’有另一個工作姿態時，其控制變量可記錄成X ²，依此類推當第i個工作姿態時，其控制變量可記錄成X ⁱ。

參閱第一圖、第二圖、第六圖及第七圖，步驟S104、建立佔據函數步驟：建立一佔據函數O，其是用以判斷在機械手臂1的工作範圍內的第m個空間座標是否為部分該機械手臂所佔據，m屬正整數，所述第m個空間座標可以記作為R _m，而R _m與

兩者距離可以用d_i,k,m表示，其中

。

佔據函數O(X ⁱ)被描述為如下

數學式(2)說明當第m個空間座標R _m與待比較的評估座標

之間距小於一設定值ε時，且對機械手臂1的第i個工作姿態下的所有第k個採樣點而言，定義佔據狀態下所對應的佔據函數O的函數值O_m範圍為大於0.5且1以下，而非佔據狀態下所對應的佔據函數O的函數值O_m範圍為0以上且0.5以下，其中ε為實數。舉例來說，如果空間中第m個空間座標R _m被障礙物佔據，且當佔據函數O(X ⁱ)經計算後為1時，因為佔據函數的函數值O_m只要是大於0.5且是1以下則表示空間座標R _m實質上會被評估座標

佔據，又評估座標

是機械手臂1在第i個工作姿態下的第k個採樣點，所以機械手臂的第i個工作姿態會被評估為碰觸到障礙物，而當O(X ⁱ)為0時，因為佔據函數的函數值O_m是0以上且0.5以下則表示空間座標R _m實質上不會被評估座標

佔據，所以機械手臂的第i個工作姿態會被評估為不會碰觸到障礙物。較佳地，在所述建立佔據函數步驟中，為了讓後續機械手臂之避障移動路徑能有最佳數值解，則需進一步將所述佔據函數O(X ⁱ)轉換成高斯函數，其所繪製出來的曲線如第六圖中的虛線所示。

需特別說明的是，步驟S104不一定要在步驟S103的後面，也可以是先執行步驟S103後再執行步驟S104，甚至也可以是步驟S103、S104同時執行。

參閱第一圖，步驟S105、尋找避障姿態步驟：根據下列方程式

以得到避障姿態，其中，X ⁱ _new為避障姿態所對應的控制變量。補充說明，由於控制變量X ⁱ舉例來說可以是表示每個關節受各自馬達所調控的改變量，因此，藉由對公式(3)最小化問題(minimization problem)，使各馬達所需調變的改變量最小，藉此取得符合公式(3)的避障姿態。由於依定義是當佔據函數O(X ⁱ)的函數值O_m為0.5<O_m

1時，表示機械手臂被評估為會碰觸到障礙物，而當佔據函數O(X ⁱ)的函數值O_m為0

O_m

0.5時，表示機械手臂被評估為未碰觸到障礙物，因此本發明還可透過將該閥值O_T從0.5調整成另一個閥值O_T’，整後的閥值O_T’係小於0.5，例如O_T’=0.4，藉此提升避障的安全層級，此時所計算的佔據函數的函數值O_m必須是小於等於0.4，機械手臂的避障姿態才會被評估為不會碰觸到障礙物，而當所計算的佔據函數的函數值O_m是大於0.4時，則機械手臂的避障姿態會被評估為碰觸到障礙物，且隨著佔據函數值O_m值越大，意味著其碰撞程度越嚴重。由上可知，調整後的閥值O_T’更小於該閥值O_T的限制條件，在實際使用上是加大機械手臂與障礙物兩者間距，而能降低機械手臂碰觸到障礙物的機率，故可藉由調整閥值O_T的大小來控制避障的安全層級。

於實際應用上，舉例來說，當機械手臂是在一水平面上沿特定路徑執行如點膠(dispensing)作業時，此時機械手臂用以點膠的末端部位須維持在該水平面上，因此，在符合方程式(3)的條件下，可以增加機械手臂自基座依序向外設置的第八個關節，其與基座兩者間的相對位置在所述尋找避障姿態步驟之前與之後均一樣的限制條件，確保機械手臂在閃開障礙物的同時，還能維持末端效應器在執行任務時的正確位置。此外，還能透過增加一個末端向量不變的限制條件，也就是讓機械手臂的末端效應器本身在世界座標系中的姿態不隨時間改變，如此便可應用於孔位須精確對準(與孔位進入方向不傾斜)的作業(如：鎖螺絲作業)或是對目標物品進行尋邊作業。

參閱第二圖、第三圖、第八圖及第九圖，以下將透過機械手臂1採用上述所述避障方法來避開障礙物b的流程，藉此以了解本發明的優點：首先，將執行步驟S101~步驟S104中所得到的數據存入於主機2的資料庫21中，然後將機械手臂1與所述主機2進行相關驅動程式之安裝。本發明執行步驟S101~步驟S104所需時間為評估座標數量乘以採樣姿態數量再乘以每組姿態佔據評估座標時之排列組合之計算時間，較佳地，執行步驟S101~步驟S104所需時間是在一天之內完成。

接著，機械手臂1在執行任務時，運算控制模組22控制機械手臂1沿一移動路徑P移動，所述移動路徑P上有多個路徑座標，每一個路徑座標能匹配於相應的評估座標，而當有障礙物b位於機械手臂1的工作範圍內時(障礙物之相關座標，舉例來說能透過三維點雲技術取得)，運算控制模組22會計算機械手臂1是否會碰撞到障礙物b，此計算過程能在10毫秒內完成。若此計算過程評估為不會發生碰撞，則機械手臂1繼續沿著移動路徑P移動，反之，若評估為會發生碰撞(如第八圖所示)，則運算控制模組22會執行所述尋找避障姿態步驟，從而使機械手臂1以避障姿態閃過障礙物b來繼續執行任務(如第九圖所示)。執行所述尋找避障姿態步驟所需時間舉例來說，可以是如下計算公式t=D * (E * F+G) (4)

其中t是指所述尋找避障姿態步驟所需的時間，D是迭代次數，E是每次迭代所需的佔據函數計算次數，F是每次佔據函數計算的所需時間，G是適用於此計算公式(4)之演算法計算所需的時間。

倘若，運算控制模組22評估為會發生碰撞，但在執行所述尋找避障姿態步驟的時間超過一運行時間時，運算控制模組22會控制機械手臂1停止動作，等待障礙物b移開於可能產生碰撞的位置後，運算控制模組22才會控制機械手臂1繼續執行任務。

本發明透過將機械手臂在執行任務中所可能面臨到的相關參數先預存於資料庫，因此在使用時便能快速評估機械手臂在移動過程中是否會發生碰撞，此外，若評估為會發生碰撞，本發明機械手臂避障方法因為已預先計算所有會碰觸到障礙物的各種可能工作姿態，因此計算上只要針對閃避該些會碰觸到障礙物的工作姿態去進行運算，便可快速找到合適的避障姿態來閃避障礙物，從而使本發明機械手臂在執行任務的過程中，不會因為碰觸到障礙物而發生任務中斷的情形。

綜上所述，本發明機械手臂避障方法及使用所述避障方法的機械手臂避障系統，確實能達成本發明之目的。

本創作所揭露之技術內容並不限於上述之實施例，凡是與本創作所揭露之創作概念及原則相同者，皆落入本創作之申請專利範圍。需注意的是，元件的定義，例如“第一”和“第二”並不是限定之文字，而是區別性的用語。而本案所用之“包括”或“包含”涵蓋“包括”和“具有”的概念，並表示元件、操作步驟及/或組或上述的組合，並不代表排除或增加的意思。又，除非有特別說明，否則操作之步驟順序並不代表絕對順序。更，除非有特別說明，否則以單數形式提及元件時(例如使用冠詞“一”或“一個”)並不代表“一個且只有一個”而是“一個或多個”。本案所使用的“及/或”是指“及”或“或”，以及“及”和“或”。本案所使用的範圍相關用語係包含全部及/或範圍限定，例如“至少”、“大於”、“小於”、“不超過”等，是指範圍的上限或下限。

惟以上所述者，僅為本發明之實施例而已，當不能以此限定本發明實施之範圍，凡是依本發明申請專利範圍及專利說明書內容所作之簡單的等效變化與修飾，皆仍屬本發明專利涵蓋之範圍內。

S101:模型化步驟

S102:蒐集評估座標步驟

S103:取得控制變量步驟

S104:建立佔據函數步驟

S105:尋找避障姿態步驟

Claims

一種機械手臂避障方法，適用於一機械手臂，該機械手臂包括多個關節與多個分別連接不同關節的連接臂，該機械手臂避障方法包含：模型化步驟：將所述連接臂均構建成固定直徑的柱體，且所述柱體的直徑不小於最粗連接臂的直徑；蒐集評估座標步驟：蒐集模型化後的機械手臂在一工作範圍內的第i個工作姿態下的第k個採樣點，並將所述採樣點所對應的評估座標標記為
，i與k均屬正整數；建立佔據函數步驟：建立用以判斷第m個空間座標是否為部分該機械手臂所佔據的佔據函數O，m屬正整數，當第m個空間座標與待比較的評估座標之間距小於一設定值時，定義佔據狀態下所對應的佔據函數O的函數值範圍為大於0.5且1以下，而非佔據狀態下所對應的佔據函數O的函數值範圍為0以上且0.5以下；取得控制變量步驟：取得移動路徑上機械手臂的第i個工作姿態所對應的控制變量X ⁱ；及尋找避障姿態步驟：根據下列方程式Min(X ⁱ _new-X ⁱ)² s.t. O(X ⁱ _new)
O_T，以得到一避障姿態，其中，X ⁱ _new為避障姿態所對應的控制變量，O_T為一閥值。
如請求項1所述的機械手臂避障方法，其中，在所述尋找避障姿態步驟中還包括將該閥值O_T調整成另一閥值O_T’步驟，調整後的閥值O_T’係小於該閥值O_T，該閥值O_T’係小於0.5。
如請求項1所述的機械手臂避障方法，其中，該機械手臂還包括一末端效應器，該末端效應器在世界座標系中的姿態不隨時間改變。
如請求項3所述的機械手臂避障方法，其中，當所述尋找避障姿態步驟所需時間是大於一運行時間時，該機械手臂停止移動。
如請求項1所述的機械手臂避障方法，其中，所述模型化步驟、所述建立評估座標步驟、所述取得控制變量步驟，及所述建立佔據函數步驟中所得到的數據均預存於一資料庫。
如請求項5所述的機械手臂避障方法，其中，當所述尋找避障姿態步驟所需時間是大於一運行時間時，該機械手臂停止移動。
一種機械手臂避障系統，包含：一機械手臂，固定於一基座，該機械手臂包括多個關節與多個分別連接不同關節的連接臂；及一主機，電性連接該機械手臂，並包括一資料庫，儲存如請求項1所述的機械手臂避障方法之所述模型化步驟、所述建立評估座標步驟、所述取得控制變量步驟，及所述建立佔據函數步驟中所得到的數據；及一運算控制模組，電性連接該資料庫，並用以執行如請求項1所述的機械手臂避障方法之所述尋找避障姿態步驟，以控制該機械手臂閃過障礙物。
如請求項7所述的機械手臂避障系統，其中，該機械手臂還包括一末端效應器，該末端效應器在世界座標系中的姿態不隨時間改變。
如請求項7所述的機械手臂避障系統，其中，當所述尋找避障姿態步驟所需時間是大於一運行時間時，該機械手臂停止移動。
如請求項7所述的機械手臂避障系統，其中，在所述尋找避障姿態步驟中還包括將該閥值O_T調整成另一閥值O_T’步驟，調整後的閥值O_T’係小於該閥值O_T，該閥值O_T’係小於0.5。