TWI579669B - Automatic obstacle avoidance method and control device for arm type robot - Google Patents

Automatic obstacle avoidance method and control device for arm type robot Download PDF

Info

Publication number
TWI579669B
TWI579669B TW104121463A TW104121463A TWI579669B TW I579669 B TWI579669 B TW I579669B TW 104121463 A TW104121463 A TW 104121463A TW 104121463 A TW104121463 A TW 104121463A TW I579669 B TWI579669 B TW I579669B
Authority
TW
Taiwan
Prior art keywords
posture
robot
final
geometric model
determined
Prior art date
Application number
TW104121463A
Other languages
English (en)
Other versions
TW201624161A (zh
Inventor
Atsushi Nakaya
Masaya Yoshida
Shohei Mase
Hiroki Takahashi
Original Assignee
Kawasaki Heavy Ind Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Kawasaki Heavy Ind Ltd filed Critical Kawasaki Heavy Ind Ltd
Publication of TW201624161A publication Critical patent/TW201624161A/zh
Application granted granted Critical
Publication of TWI579669B publication Critical patent/TWI579669B/zh

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B25J11/0095—Manipulators transporting wafers
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/06—Safety devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • H—ELECTRICITY
    • H01—ELECTRIC ELEMENTS
    • H01L—SEMICONDUCTOR DEVICES NOT COVERED BY CLASS H10
    • H01L21/00—Processes or apparatus adapted for the manufacture or treatment of semiconductor or solid state devices or of parts thereof
    • H01L21/67—Apparatus specially adapted for handling semiconductor or electric solid state devices during manufacture or treatment thereof; Apparatus specially adapted for handling wafers during manufacture or treatment of semiconductor or electric solid state devices or components ; Apparatus not specifically provided for elsewhere
    • H01L21/683—Apparatus specially adapted for handling semiconductor or electric solid state devices during manufacture or treatment thereof; Apparatus specially adapted for handling wafers during manufacture or treatment of semiconductor or electric solid state devices or components ; Apparatus not specifically provided for elsewhere for supporting or gripping
    • H01L21/687—Apparatus specially adapted for handling semiconductor or electric solid state devices during manufacture or treatment thereof; Apparatus specially adapted for handling wafers during manufacture or treatment of semiconductor or electric solid state devices or components ; Apparatus not specifically provided for elsewhere for supporting or gripping using mechanical means, e.g. chucks, clamps or pinches
    • H01L21/68707—Apparatus specially adapted for handling semiconductor or electric solid state devices during manufacture or treatment thereof; Apparatus specially adapted for handling wafers during manufacture or treatment of semiconductor or electric solid state devices or components ; Apparatus not specifically provided for elsewhere for supporting or gripping using mechanical means, e.g. chucks, clamps or pinches the wafers being placed on a robot blade, or gripped by a gripper for conveyance

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Condensed Matter Physics & Semiconductors (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Hardware Design (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Power Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Description

臂型機器人之障礙物自動回避方法及控制裝置
本發明係關於一種臂型機器人之障礙物自動回避方法及控制裝置。
先前以來，已知有使已停止之機器人自動地回避與障礙物之干擾而到達規定姿勢的方法。
專利文獻1中揭示有沿路徑逆向返回至作業原點之方法。具體而言，係藉由依序執行包含移動指令而記述之控制程式，而沿所需之路徑進行一系列動作之機器人自停止位置返回至作業原點的控制方法，當依序逆執行已執行之控制程式時，使用其前一個移動指令之位置參數來執行各移動指令。
專利文獻2中揭示有一種機器人；即便機器人異常停止於移動路徑以外時亦能返回至原點。具體而言，具備：將至少包含機器人進行動作之動作區域的區域劃分為各個具有規定區域之區域塊的區域塊作成工程、及針對每個所劃分之區域塊設定機器人之返回方向的方向設定工程。藉此，能回避與障礙物之干擾。
[先前技術文獻] [專利文獻]
[專利文獻1]日本專利特開平7-28520號公報
[專利文獻2]日本專利特開2009-90383號公報
專利文獻1所述之方法中，當機器人異常停止於移動路徑以外時，無法返回至作業原點。另一方面，專利文獻2之方法中，當機器人異常停止於移動路徑以外時，則能夠返回至作業原點。然而，專利文獻2所述之方法中，須預先針對每個區域塊設定機器人之返回方向，從而，存在作業者因設定作業而令作業量增大的問題。而且，返回方向之決定亦須作業者根據經驗法則進行判斷，從而存在作業者難以確保返回方向的問題。進而，針對每個區域塊設定有機器人之返回方向的資料會導致資料量龐大的問題。換言之，存在須針對機器人之返回方向進行複雜設定的問題。
本發明係鑒於上述課題完成，其目的在於不論機器人停止於哪個位置均能以簡單的設定使其安全地到達規定姿勢的機器人之障礙物自動回避方法及控制裝置。
為了解決上述問題，本發明之一態樣之機器人之障礙物自動回避方法係以如下方式構成：該臂型機器人具備利用關節連結有1個以上的連桿且於前端部設有手之機器人臂，該臂型機器人之障礙物自動回避方法包括如下步驟：幾何模型表現步驟，其係對於上述機器人以使其具有幾何形狀之方式建模，從而表現為幾何模型；區域設定步驟，其係設定上述幾何模型不得進入之禁止進入區域、與藉指定由該禁止進入區域而上述幾 何模型進行動作之動作區域；最終姿勢決定步驟，其係決定上述機器人之最終姿勢；初始軌道決定步驟，其係決定當上述機器人自當前姿勢向上述最終姿勢變化時上述手的初始軌道；假想姿勢算出步驟，其係算出與上述初始軌道上之規定的點對應的上述機器人的假想姿勢；干擾判定步驟，其係判定上述假想姿勢下之上述幾何模型是否干擾上述禁止進入區域；經過姿勢決定步驟，其係當上述干擾判定步驟中判定為未干擾時，將上述假想姿勢決定為經過姿勢，而當判定為產生干擾時，假設產生使上述禁止進入區域之干擾部分與上述幾何模型之干擾部分相對排斥的斥力，算出因假想斥力而將上述幾何模型之干擾部分自上述禁止進入區域推擠至上述動作區域之狀態下的姿勢，將該所算出之姿勢決定為經過姿勢；更新軌道決定步驟，其係將自上述當前姿勢經由上述經過姿勢而變化為上述最終姿勢時上述手的軌道決定為更新軌道；及，將最新的經過姿勢決定步驟中所決定之經過姿勢假設為上述初始軌道決定步驟中之上述當前姿勢，並反復執行上述初始軌道決定步驟、上述假想姿勢算出步驟、上述干擾判定步驟、上述經過姿勢決定步驟、及上述更新軌道決定步驟的步驟。
根據上述構成，算出當機器人自當前姿勢向最終姿勢變化時與手之初始軌道上之規定的點對應的機器人之假想姿勢，判定假想姿勢下之機器人之幾何模型是否干擾禁止進入區域(障礙物)。並且，當判定為未干擾時，將假想姿勢決定為當自當前姿勢向最終姿勢變化時經過的經過姿勢，而當判定為產生干擾時，算出因假想斥力而將幾何模型之干擾部分自禁止進入區域推擠至動作區域之狀態下的姿勢，將該姿勢決定為經過姿勢。因此，當機器人之幾何模型干擾禁止進入區域時，賦予不干擾禁止進 入區域之經過姿勢，故而，可回避與障礙物之衝突。然而，此時，因偏離原來之初始軌道，故而無法保證接近最終姿勢。另一方面，當機器人之幾何模型為干擾禁止進入區域時，賦予與初始軌道上之點對應的經過姿勢，故而接近最終姿勢。並且，根據上述構成，將新賦予的經過姿勢作為當前姿勢且反復實施該程序，故而，每當機器人之幾何模型未干擾禁止進入.區域之程序出現時，賦予與該程序中之初始軌道上之點對應的經過姿勢且接近最終姿勢。因此，藉由反復進行試行錯誤，最終，機器人一面回避與障礙物之衝突一面獲得當自當前姿勢變化為最終姿勢時手的更新軌道。
結果，藉由機器人之幾何模型、禁止進入區域及動作區域之設定該等簡單的設定，從而，不論機器人停止於哪個位置均能自動回避與障礙物之干擾，使機器人安全地到達規定姿勢(例如為退避姿勢)。
上述最終姿勢決定步驟可將設定於已知軌道上之上述機器人之複數個最終候補姿勢中的、上述機器人之當前姿勢與上述最終候補姿勢的上述各關節之角度之位移之絕對值的總和為最小值的最終候補姿勢選擇並決定為上述最終姿勢。
根據此構成，可選擇接近機器人之當前姿勢的姿勢作為最終姿勢。藉此，能縮短直至到達最終姿勢為止所需的時間。
上述經過姿勢決定步驟可算出某假想姿勢下之上述機器人之上述各關節因上述假想斥力而圍繞旋轉軸產生的轉矩，且以上述假想姿勢為起點，反復進行算出因該轉矩之影響而進一步變化的上述機器人之經過規定時間後之假想姿勢的運算，藉此算出上述機器人之假想姿勢之經時變化，且根據上述機器人之假想姿勢之經時變化收斂時的上述機器人之假 想姿勢決定上述經過姿勢。
根據此構成，能適當地賦予回避機器人與禁止進入區域之干擾的經過姿勢。
上述假想斥力可以與上述幾何模型進入至禁止進入區域之距離成正比地變大的方式構成。
根據此構成，即便機器人之幾何模型深入至禁止進入區域，亦能迅速地將其推擠至禁止進入區域外。
本發明之機器人之障礙物自動回避方法可具有如下步驟：停留狀態判定步驟，其係判斷上述機器人的姿勢是否以接近上述最終姿勢之方式未有經時變化而是陷入停留狀態；經過候補姿勢生成步驟，其係當上述停留狀態判定步驟中判定為陷入停留狀態時，生成上述機器人之因上述假想斥力而受推擠之狀態下的姿勢以外的經過候補姿勢；第1判定步驟，其係判定上述經過候補姿勢是否為接近上述最終姿勢之姿勢；及第1決定步驟，其係當上述第1判定步驟中判定為上述經過候補姿勢為向接近上述最終姿勢之方向變化的姿勢時，根據該經過候補姿勢而決定上述經過姿勢。
根據此構成，例如，當因斥力而回推之方向與初始軌道之方向一致時，被回推至動作區域之姿勢對應於初始軌道上之點，故而，幾何模型反復對禁止進入區域產生干擾且陷入停留狀態，機器人不接近最終姿勢。然而，根據上述構成，可獲得與非初始軌道上之點對應的經過姿勢，故而，即便陷入該停留狀態，亦可經由接近最終姿勢之其他經過姿勢而到達最終姿勢。
本發明之機器人之障礙物自動回避方法可具有如下步驟：第 2判定步驟，其係當上述第1判定步驟中判定為上述經過候補姿勢為向遠離上述最終姿勢之方向變化的姿勢時，根據機率值判定是否選擇上述經過候補姿勢；及第2決定步驟，其係當上述第2判定步驟中判定為選擇時，根據該經過候補姿勢決定上述經過姿勢。
根據此構成，即便無法生成接近最終姿勢之其他姿勢，亦可嘗試是否可經由其他經過姿勢而到達最終姿勢。藉此，可較佳地脫離當前之無法到達最終姿勢的狀態。
為了解決上述問題，本發明之一態樣之臂型機器人之控制裝置係以如下方式構成，該臂型機器人具備利用關節連結有1個以上的連桿且於前端部設有手之機器人臂，該臂型機器人之控制裝置具備：幾何模型表現部，其對於上述機器人以使其具有幾何形狀之方式建模，從而表現為幾何模型；區域設定部，其設定上述幾何模型不得進入之禁止進入區域、與藉由指定該禁止進入區域而上述幾何模型進行動作之動作區域；最終姿勢決定部，其決定上述機器人之最終姿勢；初始軌道決定部，其決定上述機器人自當前姿勢向上述最終姿勢變化時上述手的初始軌道；假想姿勢算出部，其算出與上述初始軌道上之規定的點對應的上述機器人的假想姿勢；干擾判定部，其判定上述假想姿勢下之上述幾何模型是否干擾上述禁止進入區域；經過姿勢決定部，其決定經過姿勢；及更新軌道決定部；且，上述經過姿勢決定部係當上述干擾判定部判定為未干擾時，將上述假想姿勢決定為經過姿勢，而當判定為產生干擾時，假設產生使上述禁止進入區域之干擾部分與上述幾何模型之干擾部分相對排斥的斥力，算出因假想斥力而將上述幾何模型之干擾部分自上述禁止進入區域推擠至上述動作區域 之狀態下的姿勢，將該算出之姿勢決定為經過姿勢；上述更新軌道決定部係將自上述當前姿勢經由上述經過姿勢而變化為上述最終姿勢時上述手的軌道決定為更新軌道，將上述經過姿勢決定部所決定之最新的經過姿勢假設為上述初始軌道決定部之當前姿勢，由上述初始軌道決定部、上述假想姿勢算出部、上述干擾判定部、上述經過姿勢決定部、及上述更新軌道決定部反復進行處理。
根據上述構成，算出當機器人自當前姿勢向最終姿勢變化時與手之初始軌道上之規定的點對應的機器人之假想姿勢，判定假想姿勢下之機器人之幾何模型是否干擾禁止進入區域(障礙物)。並且，當判定為未干擾時，將假想姿勢決定為當自當前姿勢向最終姿勢變化時經過的經過姿勢，而當判定為產生干擾時，算出因假想斥力使幾何模型之干擾部分自禁止進入區域推擠至動作區域之狀態下的姿勢，將該姿勢決定為經過姿勢。因此，當機器人之幾何模型干擾禁止進入區域時，賦予不干擾禁止進入區域之經過姿勢，故而，可回避與障礙物之衝突。然而，此時，因脫離原來之初始軌道，故而無法保證接近最終姿勢。另一方面，當機器人之幾何模型未干擾禁止進入區域時，賦予與初始軌道上之點對應的經過姿勢，故而接近最終姿勢。並且，根據上述構成，將新賦予之經過姿勢作為當前姿勢且反復實施該程序，故而，每當機器人之幾何模型未干擾禁止進入區域之程序出現時，均賦予與該程序之初始軌道上之點對應的經過姿勢，且接近最終姿勢。因此，藉由反復進行試行錯誤，最終，機器人可一面回避與障礙物之衝突一面獲得當自當前姿勢變化為最終姿勢時手的更新軌道。
本發明可利用簡單的設定而發揮如下效果，即，不論機器人停止於哪個位置均能自動回避與障礙物之干擾而使機器人安全地到達規定姿勢。
B‧‧‧分界線
L1‧‧‧旋轉軸線
L2‧‧‧旋轉軸線
L3‧‧‧旋轉軸線
M1‧‧‧幾何模型
M2‧‧‧幾何模型
M3‧‧‧幾何模型
Pp‧‧‧假想姿勢
Ps‧‧‧當前姿勢
Pt‧‧‧最終姿勢
PtC‧‧‧最終候補姿勢
Pv‧‧‧經過姿勢
Pvc‧‧‧經過候補姿勢
S1‧‧‧禁止進入區域
S2‧‧‧動作區域
Ta‧‧‧初始軌道
Tb‧‧‧更新軌道
1‧‧‧基台
2‧‧‧第1臂
2a‧‧‧第1關節軸
3‧‧‧第2臂
3a‧‧‧第2關節軸
4‧‧‧手
4a‧‧‧第3關節軸
5‧‧‧控制器
12‧‧‧第1臂驅動部
13‧‧‧第2臂驅動部
14‧‧‧手驅動部
15‧‧‧第1臂角度位置檢測部
16‧‧‧第2臂角度位置檢測部
17‧‧‧手角度位置檢測部
21‧‧‧控制部
22‧‧‧記憶部
31‧‧‧第1臂控制部
32‧‧‧第2臂控制部
33‧‧‧手控制部
34‧‧‧最終姿勢決定部
35‧‧‧初始軌道決定部
36‧‧‧假想姿勢算出部
37‧‧‧幾何模型表現部
38‧‧‧干擾判定部
39‧‧‧經過姿勢決定部
40‧‧‧區域設定部
41‧‧‧更新軌道決定部
100‧‧‧機器人
110‧‧‧設備
圖1係表示具備本發明之實施形態1之機器人的設備之構成例的俯視圖。
圖2係概略性地表示圖1之機器人之控制系統之構成例的方塊圖。
圖3係表示圖1之機器人之最終姿勢之選擇方法的說明圖。
圖4係表示圖1之機器人之經過姿勢之算出方法的流程圖。
圖5A係表示圖1之機器人之經過姿勢之算出方法的說明圖。
圖5B係表示圖1之機器人之經過姿勢之算出方法的說明圖。
圖5C係表示圖1之機器人之經過姿勢之算出方法的說明圖。
圖6係表示圖1之機器人之動作例的流程圖。
圖7A係表示圖1之機器人之動作例的圖。
圖7B係表示圖1之機器人之動作例的圖。
圖7C係表示圖1之機器人之動作例的圖。
圖7D係表示圖1之機器人之動作例的圖。
圖7E係表示圖1之機器人之動作例的圖。
圖7F係表示圖1之機器人之動作例的圖。
圖8係表示本發明之實施形態2之機器人之動作例的流程圖。
圖9係表示圖8之機器人之處理中使用之轉移機率的坐標圖。
圖10A係表示圖8之機器人之模擬條件的圖。
圖10B係表示圖8之機器人之模擬條件的圖。
圖10C係表示圖8之機器人之模擬結果的圖。
以下，參照圖式對本發明之實施形態進行說明。再者，本發明並不受本實施形態之限制。而且，以下之所有圖中，對於相同或同等之要素標注相同的參照符號，且省略其重複說明。
(實施形態1)
圖1係表示具備本發明之實施形態1之機器人100的設備110之構成例之俯視圖。
設備110例如為進行基板處理之設備，且內部設置有機器人100，且例如具有包圍腔室113之壁111、及於腔室內部自壁111向內側突出之壁112。該等壁111及壁112構成機器人100動作中之障礙物。
[機器人之構成]
機器人100例如為搬送半導體晶圓、玻璃晶圓等基板之機器人。
如圖1所示，機器人100係具備利用關節連結有1個以上的連桿且於前端部設有手之機器人臂的臂型機器人。更具體而言，機器人100係水平多關節機器人，且具備基台1、第1臂2、第2臂3及手4，其等以直鏈狀連結。本實施形態中，第1臂2、第2臂3、手4構成機器人臂。而且，機器人100具備控制機器人100之動作的控制器5(參照圖2)。
基台1例如為中空的圓筒狀構件。於基台1之內部，配設有 包含伺服馬達之第1臂驅動部12(參照圖2)。第1臂驅動部12係以使後述之第1關節軸2a旋轉的方式構成。
第1臂2例如為中空的板狀構件，當俯視時大致形成為條狀。於第1臂2，以自第1臂2之基端部之底面向下方突出的方式形成有第1關節軸2a。並且，第1關節軸2a能以沿z方向延伸之旋轉軸線L1為中心旋轉地安裝於基台1，該部分構成第1關節。因此，第1臂2係以於xy平面旋轉之方式構成。再者，本實施形態中，旋轉軸線L1構成為xy平面上之基準點O。
於第1臂2之內部配設有包含伺服馬達之第2臂驅動部13(參照圖2)。第2臂驅動部13係以使後述之第2關節軸3a旋轉之方式構成。
並且，藉由第1臂驅動部12之伺服馬達之編碼器，檢測第1臂2相對於基台1之圍繞旋轉軸線L1之相對的角度位置q1(參照圖3)。該編碼器構成第1臂角度位置檢測部15(參照圖2)。
再者，第1臂2有時稱為第1連桿，而且有時將第1關節軸2a稱為1軸。
第2臂3例如為中空的板狀構件，且當俯視時大致形成為條狀。於第2臂3，以自第2臂3之基端部之底面向下方突出之方式設有第2關節軸3a。並且，第2關節軸3a能以與旋轉軸線L1平行地延伸之旋轉軸線L2為中心旋轉地安裝於第1臂2，該部分構成第2關節。因此，第2臂3係以於xy平面上旋轉之方式構成。
於第2臂3之內部，配設有包含伺服馬達之手驅動部14(參 照圖2)。手驅動部14係以使後述之第3關節軸4a旋轉之方式構成。
並且，藉由第2臂驅動部13之伺服馬達之編碼器，檢測第2臂3相對於第1臂2之圍繞旋轉軸線L2的相對的角度位置q2(參照圖3)。該編碼器構成第2臂角度位置檢測部16(參照圖2)。
再者，第2臂3有時稱為第2連桿，而且第2關節軸3a有時稱為2軸。
手4係保持基板者，且形成為平板狀。於手4，具有以自其基端部之底面向下方突出之方式形成的第3關節軸4a。並且，第3關節軸4a能以與旋轉軸線L1、L2平行地延伸之旋轉軸線L3為中心旋轉地安裝於第2臂3，該部分構成第3關節。因此，手4係以於xy平面旋轉之方式構成。
並且，藉由手驅動部14之伺服馬達之編碼器，檢測手4相對於第2臂3之圍繞旋轉軸線L3之相對的角度位置q3(參照圖3)。該編碼器構成手角度位置檢測部17(參照圖2)。
再者，手4有時稱為第3連桿，而且第3關節軸4a有時稱為3軸。
[控制部]
圖2係概略性地表示機器人100之控制系統之構成例的方塊圖。
機器人100所具備之控制器5例如具備CPU等具有運算器之控制部21、及ROM及RAM等具有記憶體之記憶部22。控制器5既可由進行集中控制之單獨的控制器構成，亦可由彼此協作地進行分散控制之複 數個控制器構成。控制部21包含第1臂控制部31、第2臂控制部32、手控制部33、最終姿勢決定部34、初始軌道決定部35、假想姿勢算出部36、幾何模型表現部37、區域設定部40、干擾判定部38、經過姿勢決定部39及更新軌道決定部41。該等功能部31~41係藉由控制部21執行記憶部22中儲存之規定的控制程式而實現的功能塊(功能模組)。
第1臂控制部31控制第1臂驅動部12，使第1臂2於xy平面圍繞旋轉軸線L1旋轉。
第2臂控制部32控制第2臂驅動部13，使第2臂3於xy平面圍繞旋轉軸線L2旋轉。
手控制部33控制手驅動部14，使手4於xy平面圍繞旋轉軸線L3旋轉。
再者，本實施形態中，例如，控制部21之主控制部(未圖示)根據機器人100之第1臂角度位置檢測部15、第2臂角度位置檢測部16及手角度位置檢測部17各自檢測出之角度位置、及最終姿勢，分別向第1臂控制部31、第2臂控制部32及手控制部33輸出最終角度位置。第1臂控制部31、第2臂控制部32及手控制部33分別以使對應之控制對象物(第1關節軸2a、第2關節軸3a及第3關節軸4a)之角度位置成為與最終姿勢對應之最終角度位置的方式，根據對應之角度位置檢測部15、16、17檢測出的旋轉軸之角度位置，對於對應之第1臂驅動部12、第2臂驅動部13及手驅動部14分別進行反饋控制。
幾何模型表現部37對於機器人100以使其具有幾何形狀之方式建模，從而表示為幾何模型M。
本實施形態中，幾何模型表現部37係利用幾何模型M1、M2及M3表現第1臂2、第2臂3及手4。
如圖1所示，第1臂2之幾何模型M1例如為長圓形之模型。幾何模型M1表現為，於將第1臂2投影至xy平面所得的圖形之外側，與該圖形相離。
第2臂3之幾何模型M2例如為長圓形之模型。幾何模型M2表現為，於將第2臂3投影至xy平面所得的圖形之外側，與該圖形相離。
手4之幾何模型M3例如為水滴型之模型。幾何模型M3指定(劃定)為，於將保持基板之手4投影至xy平面所得的圖形之外側，與該圖形相離。
如圖1及2所示，區域設定部40設定幾何模型M1~M3不得進入之禁止進入區域S1、及藉由禁止進入區域S1指定(劃定)且幾何模型M1~M3進行動作之動作區域S2。
本實施形態中，禁止進入區域S1及動作區域S2係儲存於記憶部22，且藉由區域設定部40自記憶部22讀取禁止進入區域S1及動作區域S2而設定禁止進入區域S1及動作區域S2。
而且，禁止進入區域S1係以還包含較之設備110之腔室113之內壁面(壁111及壁112之壁面)更靠內側的區域的方式設定。而且，沿禁止進入區域S1與動作區域S2之分界延伸的假想線構成分界線B。
最終姿勢決定部34決定機器人100之最終姿勢Pt。
本實施形態中，最終姿勢決定部34自記憶部22中記憶之複 數個最終候補姿勢Ptc中選擇並決定一個最終姿勢Pt。
最終候補姿勢Ptc係設定於不干擾禁止進入區域S1之已知軌道上的姿勢。藉此，機器人100只要可到達任一姿勢，則之後可沿已知軌道進行動作而不會干擾禁止進入區域S1。
圖3係表示機器人100之最終姿勢Pt之選擇方法的說明圖。
如圖3所示，當最終姿勢決定部34自複數個最終候補姿勢Ptc(圖3中表示其中之一)中選擇一個最終姿勢Pt時，首先，根據以下式子，算出當前姿勢Ps與最終候補姿勢Ptc下各關節軸之關節角度之位移之絕對值的總和E。
其中，n係軸數，qi係i軸之目標姿勢下之關節角度，qdi係i軸之目標候補姿勢下之關節角度。
繼而，選擇E為最小值之最終候補姿勢Ptc，且將其決定為最終姿勢Pt。
初始軌道決定部35係決定當機器人100自當前姿勢Ps向最終姿勢決定部34所決定之最終姿勢Pt變化時手4的初始軌道Ta。本實施形態中，初始軌道Ta係藉由賦予當前姿勢Ps及最終姿勢Pt而唯一性地決定的軌道。
假想姿勢算出部36算出初始軌道Ta上之規定的點上的機器 人100之假想姿勢Pp。
干擾判定部38分別判定假想姿勢Pp下之機器人100之幾何模型M1~M3是否干擾禁止進入區域S1。
經過姿勢決定部39算出當自當前姿勢Ps向最終姿勢Pt變化時經過的經過姿勢Pv。
進而，當干擾判定部38判定為機器人100之幾何模型M1~M3與禁止進入區域S1完全無干擾時，經過姿勢決定部39將假想姿勢Pp決定為經過姿勢Pv。
另一方面，當判定為幾何模型M1~M3中之至少一個幾何模型與禁止進入區域S1產生干擾時，經過姿勢決定部39假設產生使禁止進入區域S1之干擾部分與幾何模型之干擾部分相對排斥的斥力fi，算出因假想斥力fi將幾何模型之干擾部分自禁止進入區域S1推擠至動作區域S2之狀態下的姿勢，且將該算出之姿勢決定為經過姿勢Pv。
圖4係表示機器人100之經過姿勢Pv之算出方法的流程圖。
圖5A~5C係表示機器人100之經過姿勢Pv之算出方法的說明圖。
以下，關於經過姿勢決定部39為了算出經過姿勢Pv而進行之處理進行詳細說明。再者，本實施形態中，機器人100係具有3個連桿之機器人，以下，表示作為具有n個連桿之機器人普及時的見解。
首先，決定機器人100之幾何模型進入至禁止進入區域S1的距離di(步驟S71)。距離di係根據假想姿勢Pp下各幾何模型與禁止進入區域S1之干擾形態而算出。
例如，於圖5A所示之情況下，首先，於幾何模型Mi之干擾部分(圖5A中淡墨色之部分)，算出與分界線B之距離達到最大的幾何模型Mi上之點P(x，y)。並且，於分界線B之法線方向，將點P與分界線B之距離設定為距離di。
而且，於圖5B所示之情況下，首先，於幾何模型Mi之干擾部分(圖5B中淡墨色之部分)，算出與分界線B之距離達到最大的幾何模型Mi上之點P(x，y)。並且，將點P與分界線B之頂點Q的距離設定為距離di。
進而，於圖5C所示之情況下，首先，於幾何模型Mi之干擾部分(圖5C中淡墨色之部分)，算出位於自分界線B之頂點Q朝幾何模型與分界線B之一個交點R之側的、與分界線B之距離達到最大的幾何模型Mi上之點P1(x，y)。並且，於線段QR之法線方向上，將點P1與分界線B之距離設定為距離di1。
而且，於幾何模型Mi之干擾部分，算出位於自分界線B之頂點Q朝幾何模型與分界線B之另一交點R’之側的、與分界線B之距離達到最大的幾何模型上之點P2(x，y)。並且，於線段QR’之法線方向上，將點P2與分界線B之距離設定為距離di2。
如此，距離di係構成為根據假想姿勢Pp下幾何模型與禁止進入區域S1之干擾形態而算出，但並不限於上述構成。
繼而，根據所決定之距離di，依據以下式子(2)針對各個連桿算出假想斥力fi(步驟S72)。
[數2]
其中，i係連桿編號，k係任意假想剛性係數，di係相對於禁止進入分界線B之進入距離。
再者，於圖5C所示之情況下，根據所決定之距離di1及di2，依據式子(2)分別算出假想斥力fi1及fi2，之後將假想斥力fi1及fi2合成，算出假想斥力fi。
即，對於判定為干擾禁止進入區域S1之第i個連桿，假設產生與幾何模型進入至禁止區域S2之距離成正比之假想斥力fi。藉此，當幾何模型深入禁止進入區域S1時，會強力地推擠。因此，可縮短直至幾何模型受推擠為止所需的時間。
另一方面，判定為未干擾禁止進入區域S1之第i連桿係以不會產生假想斥力fi之方式構成。
繼而，算出因假想斥力fi而產生之圍繞機器人100之各軸而產生的假想轉矩τ(步驟S73)。
其中，τi係第i軸上產生之轉矩， li係第i(1in)連桿之長度，wn係以越遠離第i連桿則第i<n假想斥力fi之影響越小之方式設定的加權係數，(例：)
ψ i係禁止進入分界線B之法線方向與x軸於xy平面上所成的角度，φ i係第i(1in)連桿與x軸於xy平面上所成的角度。
即，判定為干擾分界線B之第i連桿上產生之假想斥力fi係以不僅影響第i連桿而且亦影響第i<n連桿之方式構成。藉此，即便於第i連桿上產生假想斥力fi之點為該連桿之死點時，該假想斥力fi亦會影響其他連桿且產生轉矩。藉此，可利用該轉矩使其他連桿旋轉，從而生成與假想姿勢Pp不同的經過姿勢Pv。
而且，角度φ i係根據各幾何模型與禁止進入區域S1之干擾形態而算出。
例如，於圖5A所示之情形時，角度ψ i係分界線B之法線方向與x軸於xy平面上所成的角度。
而且，於圖5B所示之情形時，角度φ i係線段PQ與x軸所成的角度。
進而，於圖5C所示之情形時，角度ψ i係上述假想斥力之合成向量與x軸所成的角度。
繼而，算出因上述假想轉矩τ i之影響而產生的、機器人100之假想姿勢之經時變化。
[數4]
其中，q[0]係當前姿勢， τ[0]係由式子(3)算出，α、β係以使機器人進行預期之舉動的方式設定的常數。
此處，q(‧)係於q上方標注一個符號‧(點)而得的記號。
並且，藉由反復計算式子(2)與式子(3)，算出當τ[k]與q(‧)[k]收斂為0時的q[k]。該q[k]係第1臂2、第2臂3、及手4自禁止進入區域S1推擠至動作區域S2的姿勢。
並且，根據q[k]設定假想姿勢Pp(步驟S74)。
回避軌道決定部41將當自當前姿勢Ps經由經過姿勢Pv而變化為最終姿勢Pt時手4的軌道決定為更新軌道Tb。
[動作例]
繼而，說明機器人100之動作例。
圖6係表示本發明之實施形態之機器人100之動作例的流程圖。圖7A~圖7F係表示本發明之實施形態之機器人100之動作例的圖。
首先，如圖7A所示，控制部21之最終姿勢決定部34決定機器人100之最終姿勢Pt(最終姿勢決定步驟)(步驟S10)。本實施形態中，控制部21之最終姿勢決定部34針對4個最終候補姿勢Ptc1~Ptc4求出E值，且將E值最小的最終候補姿勢Ptc4選擇為最終姿勢Pt。如此，以將接近當前姿勢Ps之姿勢選擇為最終姿勢Pt之方式構成，故而，能減少直至自 當前姿勢Ps到達最終姿勢Pt為止所需的各軸之位移，從而能縮短直至到達為止所需的平均時間。
繼而，如圖7B所示，控制部21之初始軌道決定部35係決定機器人100自當前姿勢Ps向最終姿勢Pt變化之初始軌道Ta(初始軌道決定步驟)(步驟S20)。
繼而，如圖7C所示，控制部21之假想姿勢算出部36係算出初始軌道Ta上的當前姿勢Ps附近的，自當前姿勢Ps朝最終姿勢Pt之側的規定的點上的機器人100之假想姿勢Pp(假想姿勢算出步驟)(步驟S30)。此處，所謂附近係指相對於當前姿勢而對各軸施以微小地變更所得的姿勢。
繼而，控制部21之區域設定部40自記憶部22讀取禁止進入區域S1及動作區域S2且進行設定(區域設定步驟)(步驟S35)。
繼而，控制部21之幾何模型表現部37係對於處於假想姿勢Pp之機器人100的第1臂2、第2臂3及手4建模，從而分別以幾何模型M1、M2及M3表現(幾何模型表現步驟)(步驟S40)。
幾何模型M1~M3係使機器人100之第1臂2、第2臂3及手4簡化後呈現的模型，故而，能削減後述之干擾判定所需的計算量，從而能迅速進行干擾判定。
而且，如上所述，由幾何模型M1~M3之假想線描繪出之圖形的外周係以如下方式指定(劃定)，於將保持基板之手4投影至xy平面所得的圖形之外側，與該圖形相離。即，幾何模型表現為較第1臂2、第2臂3及手4更厚，故而，即便後述之步驟S50中控制部21之干擾判定部38判定為幾何模型與禁止進入區域S1產生干擾，亦能使機器人100與設備110 不產生干擾，從而，能防止機器人100或者機器人100保持之基板實際上接觸於設備110。
而且，禁止進入區域S1係以包含較之設備110之腔室113之內壁面更靠內側的區域的方式設定，故而，能更確實地防止機器人100或者機器人100保持之基板實際上接觸於設備110。
繼而，控制部21之干擾判定部38分別判定假想姿勢Pp下之幾何模型M1~M3是否干擾禁止進入區域S1(干擾判定步驟)(步驟S50)。即，所謂幾何模型與禁止進入區域S1產生干擾係指，幾何模型之分界線與分界線B交叉的狀態。
再者，本實施形態中，幾何模型表現部37係於緊靠干擾判定部38之干擾判定之前的步驟中生成幾何模型，但並不限於此，亦可於干擾判定部38之干擾判定之前的任意步驟中執行。同樣，區域設定部40之禁止進入區域S1及禁止進入區域S2的設定亦可於干擾判定部38之干擾判定之前的任意步驟中執行。
並且，當判定為絲毫未干擾時(步驟S50中為否)，控制部21之經過姿勢決定部39將假想姿勢Pp決定為經過姿勢Pv(經過姿勢決定步驟)(步驟S60)。
因此，當機器人100之幾何模型未干擾禁止進入區域S1時，賦予與初始軌道Ta上之點對應的經過姿勢，故而，接近最終姿勢Pt。
另一方面，如圖7D所示，步驟S50中，當控制部21之干擾判定部38判定為幾何模型與分界線B產生干擾時(步驟S50中為是)，控制部21之經過姿勢決定部39如圖7E所示，算出幾何模型之干擾部分自 禁止進入區域S1推擠至動作區域S2之狀態下的姿勢，將該算出之姿勢決定為經過姿勢Pv(經過姿勢決定步驟)(步驟S70)。
因此，當機器人100之幾何模型干擾禁止進入區域S1時，賦予不干擾禁止進入區域S1之經過姿勢Pv，故而可回避與障礙物之衝突。
再者，當將上述假想姿勢決定為經過姿勢Pv時，亦可進而判定自當前姿勢至上述推擠狀態之姿勢的軌道是否干擾禁止進入區域S1。
若於步驟S60或者步驟S70中決定經過姿勢Pv，則繼而，控制部21之更新軌道決定部41將當自當前姿勢Ps經由經過姿勢Pv而變化為最終姿勢Pt時手4的軌道決定為更新軌道Tb(更新軌道決定步驟)(步驟S80)。
繼而，控制部21將經過姿勢Pv決定(暫定)為新的當前姿勢Ps，且將更新軌道Tb決定(暫定)為新的初始軌道Ta(步驟S90)。
繼而，控制部21之假想姿勢算出部36係算出初始軌道Ta之當前姿勢Ps附近的、自當前姿勢Ps朝最終姿勢Pt之側的規定的點上的新的假想姿勢Pp(步驟S100)。
繼而，判定步驟S100中算出之假想姿勢Pp與最終姿勢Pt是否一致。並且，當判定為不一致時(步驟S110中為否)，反復執行上述步驟S40~步驟S110。即，步驟S40~步驟S110係執行至當前姿勢Ps與最終姿勢Pt一致為止。
並且，若步驟S100中算出之假想姿勢Pp與最終姿勢Pt一致(步驟S110中為是)，則結束處理。藉此，如圖7F所示，生成已回避與禁止進入區域S1之干擾的最終的更新軌道Tb。
再者，本實施形態中，機器人100係以如下方式構成，即，當上述處理結束之後，使機器人100實際上沿最終的更新軌道Tb自當前姿勢Ps向最終姿勢Pt活動。即，以如下方式構成，當步驟S90中將經過姿勢Pv再設定為新的當前姿勢Ps時，控制部21並不使第1臂2、第2臂3、手4向該當前姿勢Ps活動，而是將所決定之更新軌道Tb之歷程記憶至記憶部22。然而，並不限於此， 亦可每當步驟S90中將經過姿勢Pv決定為新的當前姿勢Ps時，由控制部21使第1臂2、第2臂3及手4實際上向該當前姿勢Ps活動。
如以上說明所述，本發明之機器人100算出當機器人100自當前姿勢Ps向最終姿勢Pt變化時與手4之初始軌道Ta上之規定的點對應的機器人100之假想姿勢Pp，且判定假想姿勢Pp下之機器人100之幾何模型M1~M3是否干擾對應於障礙物而指定(劃定)的禁止進入區域S1。並且，當判定為未干擾時，將假想姿勢Pp決定為當自當前姿勢Ps向最終姿勢Pt變化時經過的經過姿勢Pv，而當判定為產生干擾時，算出因假想斥力fi使幾何模型之干擾部分自禁止進入區域S1推擠至動作區域S2之狀態下的姿勢，且將該姿勢決定為經過姿勢Pv。因此，當機器人100之幾何模型干擾禁止進入區域S1時，賦予不干擾禁止進入區域S1之經過姿勢Pv，故而，可回避與障礙物之衝突。
另一方面，當機器人100之幾何模型未干擾禁止進入區域S1時，賦予與初始軌道Ta上之點對應的經過姿勢Pv，故而，接近最終姿勢Pt。並且，根據上述構成，將新賦予之經過姿勢Pv作為當前姿勢Ps且反復實施該程序，故而，每當機器人100之幾何模型不干擾禁止進入區域 S1之程序出現時，均賦予與該程序之初始軌道Ta上之點對應的經過姿勢Pv，且接近最終姿勢Pt。因此，藉由反復進行試行錯誤，最終，機器人100可一面回避與障礙物之衝突一面獲得當自當前姿勢Ps變化為最終姿勢Pt時手4的更新軌道Tb。
結果，藉由機器人之幾何模型、禁止進入區域及動作區域之設定該等簡單的設定，從而，不論機器人停止於哪個位置均能自動回避與障礙物之干擾，使機器人安全地到達規定姿勢。
(實施形態2)
以下，關於實施形態2之構成、動作，以與實施形態1之不同點為中心進行敘述。
上述實施形態1中，初始軌道Ta係藉由賦予當前姿勢Ps及最終姿勢Pt而唯一性地決定的軌道。而且，步驟S70中決定之經過姿勢Pv係於假想姿勢Pp下之各幾何模型與分界線B的位置關係下唯一性地決定的姿勢。藉此，實施形態1中，例如，當因斥力而回推之方向與初始軌道Ta之方向一致時，根據受假想斥力fi推擠之狀態下的姿勢而決定的更新軌道Tb有時不接近最終姿勢Pt。
本實施形態係考慮到更新軌道Tb不接近最終姿勢Pt之情況的實施形態。
圖8係表示本發明之實施形態2之機器人100之動作例的流程圖。
本實施形態中，步驟S70之經過姿勢之決定係以如下方式執行。
首先，判定是否陷入停留狀態(停留狀態判定步驟)(步驟S271)。本實施形態中，判定是否陷入停留狀態時，係根據以下式子(5)算出假想姿勢Pp與最終姿勢Pt下各關節軸之關節角度之位移之絕對值的總和E。
其中，n係軸數，qri係i軸之假想姿勢Pp下之關節角度，qgi係i軸之目標姿勢Pt下之關節角度。
並且，若E值減小，則判定為接近最終姿勢。另一方面，若E值未變化或者增加，則判定為不接近最終姿勢Pt、或者遠離最終姿勢Pt。並且，若E值未變化或者增加之狀態以規定的時間間隔持續，則判定為陷入停留狀態。
並且，若步驟S271中判定為控制部21並非陷入停留狀態，則執行步驟S71~S74。步驟S71~S74係與上述實施形態1中之步驟S71~S74相同，故而，省略其說明。
另一方面，若步驟S271中判定為控制部21陷入停留狀態，則控制部21之經過姿勢決定部39算出當前姿勢Ps附近之、受假想斥力fi推擠之狀態下的姿勢以外的經過候補姿勢Pvc(經過候補姿勢生成步驟)(步驟S272)。本實施形態中，經過候補姿勢Pvc係隨機生成之姿勢。然而，並不限於此。亦可取而代之而例如根據當前姿勢Ps周圍的狀況生成。
繼而，判定經過候補姿勢Pvc是否為較之當前姿勢Ps更接近最終姿勢Pt的姿勢(第1判定步驟)(步驟S273)。本實施形態中，當判定經過候補姿勢Pvc是否為接近最終姿勢Pt之姿勢時，係藉由將根據以下式子(6)算出之E值與根據以下式子(7)算出之E值進行比較而進行。
其中，n係軸數，qgi係i軸之目標姿勢Pt下之關節角度，qri係i軸之經過候補姿勢Pvc下之關節角度，qi係i軸之當前姿勢Ps下之關節角度。
即，上述式子(6)係求出經過候補姿勢Pvc與最終姿勢Pt下各關節軸之關節角度之位移之絕對值的總和E(qr)。而且，上述式子(7)係求出當前姿勢Ps與最終姿勢Pt下各關節軸之關節角度之位移之絕對值的總和E(q)。並且，上述式子(8)係將E(qr)與E(q)之值進行比較的式子。
並且，例如，上述式子(8)中，若△0，即，若經過候補姿勢Pvc之E(qr)值等於或小於當前姿勢Ps之E(q)值，則判定為接近最終姿勢Pt之姿勢。另一方面，若△>0，即，若經過候補姿勢Pvc之E(qr)值大於當前姿勢Ps之E(q)值，則判定為並非接近最終姿勢Pt之姿勢。
並且，若控制部21之經過姿勢決定部39判定為接近最終姿 勢Pt之姿勢(步驟S273中為是)，則根據經過候補姿勢Pvc決定經過姿勢Pv(第1決定步驟)(步驟S274)。進而，判定自當前姿勢至該經過姿勢Pv之軌道是否干擾禁止進入區域S1。
另一方面，若控制部21之經過姿勢決定部39判定為並非接近最終姿勢Pt之姿勢(步驟S273中為否)，則進而決定是否將經過候補姿勢Pvc選擇為經過姿勢Pv(第2判定步驟)(步驟S275)。
該選擇係使用機率值進行。本實施形態中，該選擇係利用以下式子(9)算出與經過候補姿勢Pvc之評估值相應的轉移機率P(△，t)，且根據該機率，決定是否將經過候補姿勢Pvc選擇為經過姿勢Pv。而且，以圖中之a所示之線來例示該轉移機率之坐標圖。
[數7] P(△,t)=e -△/t …(9)
其中，t係參數，△係由式子(8)算出。
藉此，就轉移機率P(△，t)而言，△越接近0，即，經過候補姿勢Pvc較之當前姿勢Ps、背離(遠離)最終姿勢Pt之程度越小，則經過候補姿勢Pvc被選擇為經過姿勢Pv之可能性越高(接近1)，而經過候補姿勢Pvc較之當前姿勢Ps、背離最終姿勢Pt之程度越大，則經過候補姿勢Pvc被選擇為經過姿勢Pv之可能性越低(接近0)。
並且，若經過候補姿勢Pvc被選擇為經過姿勢Pv，則根據經過候補姿勢Pvc決定經過姿勢Pv(第2決定步驟)(步驟S276)。
並且，控制部21係與上述實施形態相同地執行步驟S80之後的處理。
再者，於經過候補姿勢Pvc較之當前姿勢Ps、背離最終姿勢Pt之程度少的姿勢下，有時難以發現能脫離停留狀態之姿勢。此時，亦可如圖9之坐標圖中之b表示之線所示進行調整，即，當陷入停留狀態時，藉由增大上述式子(9)之參數t的值，使得即便為較之當前姿勢Ps、背離最終姿勢Pt之程度相對較大的經過候補姿勢Pvc亦容易選擇。
而且，較佳為，隨著當前姿勢Ps接近最終姿勢Pt，而降低選擇背離最終姿勢Pt之姿勢的機率。因此，隨著當前姿勢Ps接近最終姿勢Pt，如圖9之坐標圖中b表示之線所示，亦可藉由減小參數t的值，而降低選擇背離最終姿勢Pt之姿勢的機率。
如此，本實施形態中，就機器人100而言，例如當受斥力fi推擠之方向與初始軌道Ta之方向一致時，推擠至動作區域S2之姿勢與初始軌道Ta上之點對應，故而，幾何模型會反復干擾禁止進入區域S1而陷入停留狀態，機器人100並不接近最終姿勢Pt(無法到達最終姿勢)。此時，能獲得與並非初始軌道Ta上之點對應的經過姿勢Pv，且能根據該姿勢決定更新軌道Tb，且經由接近最終姿勢Pt之其他經過姿勢Pv而到達最終姿勢Pt。
而且，即便無法算出接近其他最終姿勢Pt之姿勢，亦可嘗試是否可經由別的姿勢而到達最終姿勢Pt。藉此，可較佳地脫離當前之無法到達最終姿勢pt的狀態。並且，根據和最終姿勢pt與經過候補姿勢Pvc之背離度成正比地下降的機率值，判定是否選擇經過候補姿勢，故而，與 當前姿勢pS之背離度小的經過候補姿勢Pvc被選擇為經過姿勢Pv的可能性變大。藉此，可更佳地適當脫離當前之無法到達最終姿勢的狀態。
圖10A及圖10B係表示本實施形態之機器人100之模擬條件的圖。如圖10A所示，設定機器人100之當前姿勢Ps、禁止進入區域S1、動作區域S2、幾何模型M。而且，如圖10B所示，設定機器人100之最終姿勢Pt。再者，最終姿勢Pt下之手之教示點為[x，y]=[0，450]。
圖10C係表示機器人100之模擬結果的圖，且係表示手之軌跡的圖。如圖10C所示，可知，機器人100係自當前姿勢Ps到達最終姿勢Pt。
<變形例>
上述實施形態中，由水平多關節機器人構成機器人100，於xy平面表現幾何模型，未判定z軸方向上干擾之有無，但並不限於此。亦可取而代之，由垂直多關節構成機器人、立體地表現幾何模型、於3次元空間進行干擾之判定。
而且，上述實施形態中，假想姿勢算出部36係算出機器人100之假想姿勢Pp，但亦可取而代之而算出機器人100之幾何模型之假想姿勢Pp。
根據上述說明可知，本發明所屬技術領域中具有通常知識者瞭解本發明之多種改良或其他實施形態。因此，上述說明應僅作為例示進行解釋，且係為了將執行本發明之最佳態樣教示給業者而提供。可於不脫離本發明之精神的情況下，實質性地變更其構造及/或者功能之細節。
[產業上之可利用性]
本件發明可適用於產業用機器人。

Claims (7)

  1. 一種臂型機器人之障礙物自動回避方法，該臂型機器人具備利用關節連結有1個以上的連桿且於前端部設有手的機器人臂，該方法中包括如下步驟：幾何模型表現步驟，其係對於上述機器人以使其具有幾何形狀之方式建模，從而表現為幾何模型；區域設定步驟，其係設定上述幾何模型不得進入之禁止進入區域、與藉由指定該禁止進入區域而上述幾何模型進行動作之動作區域；最終姿勢決定步驟，其係決定上述機器人之最終姿勢；初始軌道決定步驟，其係決定當上述機器人自當前姿勢向上述最終姿勢變化時上述手的初始軌道；假想姿勢算出步驟，其係算出與上述初始軌道上之規定的點對應的上述機器人的假想姿勢；干擾判定步驟，其係判定上述假想姿勢下之上述幾何模型是否干擾上述禁止進入區域；經過姿勢決定步驟，其係當上述干擾判定步驟中判定為未干擾時，將上述假想姿勢決定為經過姿勢，而當判定為產生干擾時，假設產生使上述禁止進入區域之干擾部分與上述幾何模型之干擾部分相對排斥的斥力，算出因假想斥力而將上述幾何模型之干擾部分自上述禁止進入區域推擠至上述動作區域之狀態下的姿勢，將該所算出之姿勢決定為經過姿勢；更新軌道決定步驟，其係將自上述當前姿勢經由上述經過姿勢而變化為上述最終姿勢時上述手的軌道決定為更新軌道；及 將最新的經過姿勢決定步驟中所決定之經過姿勢假設為上述初始軌道決定步驟中之上述當前姿勢，並反復執行上述初始軌道決定步驟、上述假想姿勢算出步驟、上述干擾判定步驟、上述經過姿勢決定步驟及上述更新軌道決定步驟的步驟。
  2. 如申請專利範圍第1項之機器人之障礙物自動回避方法，其中，上述最終姿勢決定步驟係將已知軌道上所設定之上述機器人之複數個最終候補姿勢中的、上述機器人之當前姿勢與上述最終候補姿勢下上述各關節之角度之位移之絕對值的總和為最小值的最終候補姿勢選擇並決定為上述最終姿勢。
  3. 如申請專利範圍第1項之機器人之障礙物自動回避方法，其中，上述經過姿勢決定步驟係算出某假想姿勢下之上述機器人之上述各關節因上述假想斥力而圍繞旋轉軸產生的轉矩，且以上述假想姿勢為起點，反復進行算出因該轉矩之影響而進一步變化的上述機器人之經過規定時間後之假想姿勢的運算，藉此算出上述機器人之假想姿勢之經時變化，且根據上述機器人之假想姿勢之經時變化收斂時的上述機器人之假想姿勢決定上述經過姿勢。
  4. 如申請專利範圍第1至3中任一項之機器人之障礙物自動回避方法，其中，上述假想斥力係以與上述幾何模型進入至禁止進入區域的距離成正比地變大的方式構成。
  5. 如申請專利範圍第1項之機器人之障礙物自動回避方法，其具有如下步驟：停留狀態判定步驟，其係判斷上述機器人的姿勢是否以接近上述最終 姿勢之方式未有經時變化而陷入停留狀態；經過候補姿勢生成步驟，其係當上述停留狀態判定步驟中判定為陷入停留狀態時，生成上述機器人之因上述假想斥力而受推擠之狀態下的姿勢以外的經過候補姿勢；第1判定步驟，其係判定上述經過候補姿勢是否為接近上述最終姿勢之姿勢；及第1決定步驟，其係當上述第1判定步驟中判定為上述經過候補姿勢為向接近上述最終姿勢之方向變化的姿勢時，根據該經過候補姿勢而決定上述經過姿勢。
  6. 如申請專利範圍第5項之機器人之障礙物自動回避方法，其具有如下步驟：第2判定步驟，其係當上述第1判定步驟中判定為上述經過候補姿勢為向遠離上述最終姿勢之方向變化的姿勢時，根據機率值判定是否選擇上述經過候補姿勢；及第2決定步驟，其係當上述第2判定步驟中判定為選擇時，根據該經過候補姿勢決定上述經過姿勢。
  7. 一種臂型機器人之控制裝置，該臂型機器人具備利用關節連結有1個以上的連桿且於前端部設有手之機器人臂，該臂型機器人之控制裝置具備：幾何模型表現部，其對於上述機器人以使其具有幾何形狀之方式建模，從而表現為幾何模型；區域設定部，其設定上述幾何模型不得進入之禁止進入區域、與藉由指定該禁止進入區域而上述幾何模型進行動作之動作區域； 最終姿勢決定部，其決定上述機器人之最終姿勢；初始軌道決定部，其決定上述機器人自當前姿勢向上述最終姿勢變化時上述手的初始軌道；假想姿勢算出部，其算出與上述初始軌道上之規定的點對應的上述機器人的假想姿勢；干擾判定部，其判定上述假想姿勢下之上述幾何模型是否干擾上述禁止進入區域；經過姿勢決定部，其決定經過姿勢；及更新軌道決定部；且上述經過姿勢決定部係當上述干擾判定部判定為未干擾時，將上述假想姿勢決定為經過姿勢，而當判定為產生干擾時，假設產生使上述禁止進入區域之干擾部分與上述幾何模型之干擾部分相對排斥的斥力，算出因假想斥力而將上述幾何模型之干擾部分自上述禁止進入區域推擠至上述動作區域之狀態下的姿勢，將該算出之姿勢決定為經過姿勢，上述更新軌道決定部係將自上述當前姿勢經由上述經過姿勢而變化為上述最終姿勢時上述手的軌道決定為更新軌道，將上述經過姿勢決定部所決定之最新的經過姿勢假設為上述初始軌道決定部之當前姿勢，由上述初始軌道決定部、上述假想姿勢算出部、上述干擾判定部、上述經過姿勢決定部及上述更新軌道決定部反復進行處理。
TW104121463A 2014-12-25 2015-07-02 Automatic obstacle avoidance method and control device for arm type robot TWI579669B (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2014/006475 WO2016103297A1 (ja) 2014-12-25 2014-12-25 アーム型のロボットの障害物自動回避方法及び制御装置

Publications (2)

Publication Number Publication Date
TW201624161A TW201624161A (zh) 2016-07-01
TWI579669B true TWI579669B (zh) 2017-04-21

Family

ID=56149401

Family Applications (1)

Application Number Title Priority Date Filing Date
TW104121463A TWI579669B (zh) 2014-12-25 2015-07-02 Automatic obstacle avoidance method and control device for arm type robot

Country Status (6)

Country Link
US (1) US10350756B2 (zh)
JP (1) JP6378783B2 (zh)
KR (1) KR101941147B1 (zh)
CN (1) CN107000223B (zh)
TW (1) TWI579669B (zh)
WO (1) WO2016103297A1 (zh)

Families Citing this family (36)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6309990B2 (ja) * 2016-03-24 2018-04-11 ファナック株式会社 複数の機構ユニットにより構成されたロボットを制御するロボットシステム、該機構ユニット、およびロボット制御装置
WO2018143003A1 (ja) * 2017-01-31 2018-08-09 株式会社安川電機 ロボットパス生成装置及びロボットシステム
JP6856447B2 (ja) * 2017-05-24 2021-04-07 ファナック株式会社 パラレルリンク機構の制御装置及び制御方法、並びにパラレルリンク機構及び制御装置を含むシステム
JP6879464B2 (ja) * 2017-08-02 2021-06-02 オムロン株式会社 干渉判定方法、干渉判定システム及びコンピュータプログラム
JP7041492B2 (ja) * 2017-10-31 2022-03-24 川崎重工業株式会社 ロボットシステム
US11458626B2 (en) * 2018-02-05 2022-10-04 Canon Kabushiki Kaisha Trajectory generating method, and trajectory generating apparatus
JP6955702B2 (ja) 2018-03-06 2021-10-27 オムロン株式会社 情報処理装置、情報処理方法、及びプログラム
US10899006B2 (en) * 2018-05-01 2021-01-26 X Development Llc Robot navigation using 2D and 3D path planning
JP7077800B2 (ja) * 2018-06-11 2022-05-31 オムロン株式会社 経路計画装置、経路計画方法、及び経路計画プログラム
CN108858162B (zh) * 2018-06-28 2020-10-30 广州视源电子科技股份有限公司 四轴机械臂的位置确定方法和装置
JP6833777B2 (ja) * 2018-09-10 2021-02-24 株式会社東芝 物体ハンドリング装置及びプログラム
DE102018122376B3 (de) 2018-09-13 2019-11-07 Pilz Gmbh & Co. Kg Verfahren und Vorrichtung zur kollisionsfreien Bewegungsplanung eines Manipulators
EP3639983A1 (en) 2018-10-18 2020-04-22 Technische Universität München Anti-collision safety measures for a reconfigurable modular robot
DE102018133472B3 (de) * 2018-12-21 2020-03-12 Franka Emika Gmbh Bewegungsüberwachung eines Robotermanipulators
JP7147571B2 (ja) * 2019-01-15 2022-10-05 オムロン株式会社 経路生成装置、経路生成方法、及び経路生成プログラム
US20220097231A1 (en) * 2019-01-30 2022-03-31 Nec Corporation Obstacle avoidance control device, obstacle avoidance control system, obstacle avoidance control method, and recording medium
US12030192B2 (en) * 2019-02-08 2024-07-09 Nec Corporation Motion model calculation device, control device, joint mechanism, and motion model calculation method
JP7222803B2 (ja) * 2019-04-25 2023-02-15 株式会社日立製作所 軌道計画装置、軌道計画方法及びプログラム
CN110202575B (zh) * 2019-06-05 2022-07-19 武汉工程大学 一种用于工业测量的机器人目标轨迹精度补偿方法
JP2020199625A (ja) * 2019-06-13 2020-12-17 ファナック株式会社 シミュレーション装置
JP7384602B2 (ja) * 2019-09-12 2023-11-21 ファナック株式会社 ロボット制御装置
JP6792184B1 (ja) * 2019-11-27 2020-11-25 株式会社安川電機 シミュレーションシステム、シミュレーション方法、シミュレーションプログラム、ロボットの製造方法、およびロボットシステム
EP4069471A1 (en) * 2019-12-04 2022-10-12 ABB Schweiz AG Method of controlling industrial actuator, control system and actuator system
CN113561171B (zh) * 2020-01-05 2023-03-24 牧今科技 具有动态运动调整机制的机器人系统及其操作方法
JP7375587B2 (ja) * 2020-02-05 2023-11-08 株式会社デンソー 軌道生成装置、多リンクシステム、及び軌道生成方法
CN112276953B (zh) * 2020-10-27 2021-12-28 清华大学深圳国际研究生院 连续型超冗余机械臂的臂型协同规划方法、终端设备和存储介质
US20220193907A1 (en) * 2020-12-22 2022-06-23 X Development Llc Robot planning
TWI741943B (zh) * 2021-02-03 2021-10-01 國立陽明交通大學 機器人控制方法、動作計算裝置及機器人系統
US11350578B1 (en) * 2021-02-17 2022-06-07 Firefly Automatix, Inc. Dynamically calculating a stacking head's path during a stacking operation
CN113134832A (zh) * 2021-03-16 2021-07-20 无锡信捷电气股份有限公司 一种桁架机器人安全区域的设定方法
US11673264B2 (en) 2021-03-25 2023-06-13 Mitsubishi Electric Research Laboratories, Inc. System and method for robotic assembly based on adaptive compliance
US11787055B2 (en) * 2021-03-30 2023-10-17 Honda Research Institute Europe Gmbh Controlling a robot using predictive decision making
CN113103231A (zh) * 2021-03-31 2021-07-13 北京北方华创微电子装备有限公司 机械手归位方法及半导体热处理设备
CN115723121A (zh) * 2021-08-26 2023-03-03 富士能电子(昆山)有限公司 机械手臂避障方法及机械手臂避障系统
CN113910300B (zh) * 2021-10-25 2023-05-23 苏州灵猴机器人有限公司 一种机器人末端防撞方法
KR102629021B1 (ko) * 2023-01-30 2024-01-25 주식회사 마키나락스 산업용 로봇의 작업 경로 생성 방법

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5305652A (en) * 1990-01-23 1994-04-26 Kuka Schweissanlagen + Robotor Gmbh Multiaxial industrial robot
JP2001054889A (ja) * 1999-08-20 2001-02-27 Denso Corp 組立用垂直多関節型ロボット
US6250174B1 (en) * 1996-12-17 2001-06-26 Fanuc Ltd. Robot construction
CN101763119B (zh) * 2009-12-16 2012-01-04 东南大学 基于遥操作移动机器人的辅助避障方法
CN102126210B (zh) * 2011-03-22 2012-07-11 浙江理工大学 七自由度气动肌肉柔性机械手臂
TWI461270B (zh) * 2007-11-26 2014-11-21 Yaskawa Denki Seisakusho Kk Vertical multi-articular manipulator

Family Cites Families (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61134807A (ja) * 1984-12-06 1986-06-21 Mitsubishi Heavy Ind Ltd ロボツトア−ム制御方法
JPH0728520A (ja) 1993-07-14 1995-01-31 Toyota Motor Corp ロボットの原点復帰制御方法
JPH09207089A (ja) * 1996-02-01 1997-08-12 Matsushita Electric Ind Co Ltd 多関節構造体制御装置
JP2001154706A (ja) * 1999-11-29 2001-06-08 Japan Atom Energy Res Inst 移動体の経路生成法
JP2003136466A (ja) * 2001-10-25 2003-05-14 Murata Mach Ltd 産業用ロボットの制御装置
JP4027350B2 (ja) * 2004-06-29 2007-12-26 ファナック株式会社 ロボットの待機位置復帰プログラム作成装置
JP4958608B2 (ja) 2007-04-06 2012-06-20 三菱電機株式会社 ロボット制御装置、ロボット制御方法及びロボット搬送装置
JP2009090383A (ja) 2007-10-04 2009-04-30 Nidec Sankyo Corp ロボットの原点復帰方法
WO2009055707A1 (en) * 2007-10-26 2009-04-30 Honda Motor Co., Ltd. Real-time self collision and obstacle avoidance
FR2930472B1 (fr) 2008-04-24 2010-08-13 Univ Havre Robot manipulateur et commande associee pour un positionnement fin de l'extremite terminale
JP5228783B2 (ja) 2008-10-15 2013-07-03 株式会社デンソーウェーブ ロボットの原点復帰装置
JP4730440B2 (ja) * 2009-01-01 2011-07-20 ソニー株式会社 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム
JP2010188437A (ja) 2009-02-16 2010-09-02 Amada Co Ltd ロボットハンドの歪み検出方法および自動交換システム
DE102010007458A1 (de) * 2010-02-10 2011-08-11 KUKA Laboratories GmbH, 86165 Verfahren für eine kollisionsfreie Bahnplanung eines Industrieroboters
US8340820B2 (en) * 2010-02-26 2012-12-25 Agilent Technologies, Inc. Robot arm and method of controlling robot arm to avoid collisions
JP5476225B2 (ja) 2010-06-23 2014-04-23 日東精工株式会社 干渉判定装置および干渉判定方法
JP2012216151A (ja) 2011-04-01 2012-11-08 Mitsubishi Electric Corp 干渉回避制御装置
CN102819264B (zh) * 2012-07-30 2015-01-21 山东大学 移动机器人路径规划q学习初始化方法
JP6110636B2 (ja) 2012-08-09 2017-04-05 日本電産サンキョー株式会社 産業用ロボット
CN104812535A (zh) * 2012-10-11 2015-07-29 Abb技术有限公司 自动产生用于将机器人从停止位置返回预设重启位置的无碰撞返回程序的方法和设备
CN104029203B (zh) * 2014-06-18 2017-07-18 大连大学 实现空间机械臂避障的路径规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5305652A (en) * 1990-01-23 1994-04-26 Kuka Schweissanlagen + Robotor Gmbh Multiaxial industrial robot
US6250174B1 (en) * 1996-12-17 2001-06-26 Fanuc Ltd. Robot construction
JP2001054889A (ja) * 1999-08-20 2001-02-27 Denso Corp 組立用垂直多関節型ロボット
TWI461270B (zh) * 2007-11-26 2014-11-21 Yaskawa Denki Seisakusho Kk Vertical multi-articular manipulator
CN101763119B (zh) * 2009-12-16 2012-01-04 东南大学 基于遥操作移动机器人的辅助避障方法
CN102126210B (zh) * 2011-03-22 2012-07-11 浙江理工大学 七自由度气动肌肉柔性机械手臂

Also Published As

Publication number Publication date
US20170348856A1 (en) 2017-12-07
TW201624161A (zh) 2016-07-01
WO2016103297A1 (ja) 2016-06-30
KR20170094351A (ko) 2017-08-17
JPWO2016103297A1 (ja) 2017-07-13
JP6378783B2 (ja) 2018-08-22
CN107000223A (zh) 2017-08-01
CN107000223B (zh) 2019-11-01
KR101941147B1 (ko) 2019-04-12
US10350756B2 (en) 2019-07-16

Similar Documents

Publication Publication Date Title
TWI579669B (zh) Automatic obstacle avoidance method and control device for arm type robot
US11498214B2 (en) Teaching device, teaching method, and robot system
JP5998816B2 (ja) 経路探索方法、経路探索装置、ロボット制御装置、ロボット及びプログラム
CN108145709B (zh) 控制机器人的方法和设备
US9727053B2 (en) Information processing apparatus, control method for information processing apparatus, and recording medium
CN110722552B (zh) 自动路径生成装置
US20220193894A1 (en) Supervised Autonomous Grasping
CN118269089A (zh) 用于机械手的运动模拟的方法
JP7210201B2 (ja) 情報処理方法、プログラム、記録媒体、情報処理装置、ロボットシステム、物品の製造方法
Hudson et al. Model-based autonomous system for performing dexterous, human-level manipulation tasks
CN113442129A (zh) 用于确定工作空间的传感器布置的方法和系统
CN114080304A (zh) 控制装置、控制方法及控制程序
US20220382253A1 (en) Machining program conversion device, numerical control device, and machining program conversion method
JP2021169149A (ja) 分解ベースのアセンブリ計画
JP7263987B2 (ja) 制御装置、制御方法、及び制御プログラム
JP2021082222A (ja) 情報処理方法、ロボットシステム、物品の製造方法、および情報処理装置
JP7314215B2 (ja) シミュレータ、ロボット教示装置、ロボットシステム、物品の製造方法、シミュレーション方法、プログラム及び記録媒体
US20220193906A1 (en) User Interface for Supervised Autonomous Grasping
WO2022259600A1 (ja) 情報処理装置、情報処理システム、および情報処理方法、並びにプログラム
Kim et al. Improvement of Door Recognition Algorithm using Lidar and RGB-D camera for Mobile Manipulator
Puiu et al. Real-time collision avoidance for redundant manipulators
US20240058961A1 (en) Path generation device, path generation method, and path generation program
WO2023053374A1 (ja) 制御装置及びロボットシステム
CN117916742A (zh) 用于基于神经网络输出来更新神经网络的训练的机器人系统和方法
CN117813603A (zh) 生成用于神经网络的扩充训练数据的系统和方法