TWI766807B - Safe reset method of robotic arm - Google Patents
Safe reset method of robotic arm Download PDFInfo
- Publication number
- TWI766807B TWI766807B TW110134841A TW110134841A TWI766807B TW I766807 B TWI766807 B TW I766807B TW 110134841 A TW110134841 A TW 110134841A TW 110134841 A TW110134841 A TW 110134841A TW I766807 B TWI766807 B TW I766807B
- Authority
- TW
- Taiwan
- Prior art keywords
- path
- return
- torque
- robotic arm
- point
- Prior art date
Links
Images
Landscapes
- Manipulator (AREA)
Abstract
一種機械手臂的安全復位方法,其係應用於一以馬達驅動的機械手臂,其末端可被驅動沿一第一路徑移動,其中此第一路徑上具有一位置點,而機械手臂末端移動可及的範圍中具有一不位於第一路徑上的偏移點。此復位方法包括有:在偏移點與位置點之間定義一第二路徑;計算機械手臂的末端沿第二路徑從偏移點移動至位置點的返回速度,並依據返回速度計算得出返回扭矩;判斷返回扭矩是否大於一扭矩限制,若是則令機械手臂的末端以所述扭矩限制所對應的限制速度返回位置點;若否則令機械手臂的末端以所述返回速度返回位置點。A safe reset method of a robotic arm, which is applied to a motor-driven robotic arm, the end of which can be driven to move along a first path, wherein there is a position point on the first path, and the end of the robotic arm can be moved has an offset point in the range that is not on the first path. The reset method includes: defining a second path between the offset point and the position point; calculating the return speed of the end of the robot arm moving from the offset point to the position point along the second path, and calculating the return speed according to the return speed Torque; determine whether the return torque is greater than a torque limit, if so, make the end of the robot arm return to the position point at the speed limit corresponding to the torque limit; otherwise, make the end of the robot arm return to the position point at the return speed.
Description
本發明與機器人有關,尤指一種機械手臂遭碰撞後的復位方法。The present invention is related to robots, in particular to a method for resetting a mechanical arm after being collided.
第6圖說明了傳統用於機械手臂控制的閉迴路控制系統。傳統上,當閉迴路控制系統收到一位置命令而需讓伺服馬達91移動到一目標位置時,閉迴路控制系統會根據位置命令以及從伺服馬達91所回授的一當前實際位置,經由位置控制迴路92的計算與位置補償後,產生所需的速度命令給驅動器的速度控制迴路93。之後速度控制迴路93再根據速度命令、從伺服馬達91所回授的當前實際速度以及所需的速度補償而產生一扭矩命令,驅動器再根據此扭矩命令而控制馬達的電流,讓伺服馬達91能輸出對應的輸出扭矩,進而讓機械手臂快速地移動到目標位置。Figure 6 illustrates a traditional closed-loop control system for robotic arm control. Conventionally, when the closed-loop control system receives a position command and needs to move the
然而,當機械手臂偵測到異常狀況時,例如機械手臂在運動過程中碰撞到異物,為了安全,伺服馬達91將停止回授當前實際位置等數值,並且工作人員可能將機械手臂移開以排除異物。此時,機械手臂的位置已離原運動路徑相當遠,在排除異常狀態且伺服馬達91開始重新回授當前實際位置等數值給驅動器時,可能因為當前實際位置(即機械手臂被移開後的位置)離開原運動路徑過遠,而造成驅動器需要輸出較大的馬達電流,才能使伺服馬達91快速地轉動到目標位置。過大的馬達電流可能觸發伺服馬達91的安全機制而造成系統關閉,甚至造成伺服馬達91的損壞。因此,在機械手臂發生異常的情況下,一種較佳的機械手臂的安全回復機制是相當需要的。However, when the robot arm detects an abnormal situation, for example, the robot arm collides with a foreign object during movement, for safety, the
本發明之主要目的乃在於針對現有機械手臂的控制機制的缺失進行改良,進而提出一種嶄新的機械手臂的安全回復機制,其可在機械手臂因異常狀況而被移動到比較遙遠的位置時,也能夠安全地復位。The main purpose of the present invention is to improve the lack of the control mechanism of the existing robot arm, and further propose a new safety recovery mechanism of the robot arm, which can also be used when the robot arm is moved to a relatively remote position due to abnormal conditions. Can be reset safely.
為達前述之目的,本發明提供一種機械手臂的安全復位方法,其係應用於一機械手臂,該機械手臂具有一馬達,該馬達具有一驅動器,該機械手臂的末端在該馬達的驅動下沿一第一路徑移動,定義該第一路徑上具有一位置點,該機械手臂的末端移動可及的範圍中定義有一不位於該第一路徑上的偏移點;該復位方法包括有: 在該偏移點與該位置點之間定義一第二路徑; 計算該機械手臂的末端沿該第二路徑從該偏移點移動至該位置點的一返回速度,並依據該返回速度計算得出一返回扭矩; 判斷該返回扭矩是否大於一扭矩限制,若判斷結果為是,則令該機械手臂的末端以該扭矩限制所對應的一限制速度沿該第二路徑返回該位置點;若判斷結果為否,則令該機械手臂的末端以該返回速度沿該第二路徑返回該位置點。 In order to achieve the aforementioned purpose, the present invention provides a method for safe resetting of a robotic arm, which is applied to a robotic arm, the robotic arm has a motor, the motor has a driver, and the end of the robotic arm is at the lower edge of the drive of the motor. A first path movement is defined as having a position point on the first path, and an offset point not located on the first path is defined in the range within which the distal end of the robotic arm can move; the reset method includes: define a second path between the offset point and the position point; calculating a return speed of the end of the robotic arm moving from the offset point to the position point along the second path, and calculating a return torque according to the return speed; Judging whether the return torque is greater than a torque limit, if the judgment result is yes, make the end of the robot arm return to the position point along the second path at a limited speed corresponding to the torque limit; if the judgment result is no, then The end of the robot arm is made to return to the position point along the second path at the return speed.
較佳地,該第二路徑為一直線路徑。Preferably, the second path is a straight path.
較佳地,若該返回扭矩大於該扭矩限制,則在該機械手臂的末端以該限制速度沿該第二路徑移動至一新偏移點後,執行以下步驟: 計算該機械手臂的末端沿該第二路徑從該新偏移點移動至該位置點的一新返回速度,並依據該新返回速度計算得出一新返回扭矩; 判斷該新返回扭矩是否大於該扭矩限制,若判斷結果為是,則令該機械手臂的末端以該扭矩限制所對應的該限制速度沿該第二路徑而返回該位置點;若判斷結果為否,則令該機械手臂的末端以該新返回速度沿該第二路徑而返回該位置點。 Preferably, if the return torque is greater than the torque limit, after the end of the robotic arm moves to a new offset point along the second path at the limit speed, the following steps are performed: calculating a new return speed of the end of the robotic arm moving from the new offset point to the position point along the second path, and calculating a new return torque according to the new return speed; Judging whether the new return torque is greater than the torque limit, if the judgment result is yes, make the end of the robot arm return to the position point along the second path at the limit speed corresponding to the torque limit; if the judgment result is no , then the end of the robotic arm is made to return to the position point along the second path at the new return speed.
較佳地,若該返回扭矩小於該扭矩限制,則該馬達以其額定最大輸出扭矩驅動該機械手臂的末端以該返回速度沿該第二路徑返回該位置點。Preferably, if the return torque is less than the torque limit, the motor drives the distal end of the robotic arm with its rated maximum output torque to return to the position along the second path at the return speed.
較佳地,該扭矩限制為該馬達之額定最大輸出扭矩的20%。Preferably, the torque limit is 20% of the rated maximum output torque of the motor.
本發明之上述目的與優點,不難從以下所選用實施例之詳細說明與附圖中獲得深入了解。The above-mentioned objects and advantages of the present invention can be easily understood from the detailed description and accompanying drawings of the following selected embodiments.
本發明所提供之一種機械手臂的安全復位方法係應用於一如第2、3圖所示的機械手臂,於本實施例中,是以一多軸機械手臂1作為示例,其每一軸11都設有一伺服馬達12,每一個伺服馬達12都具有一驅動器2而可被驅動轉動,進而讓機械手臂1改變其姿態。各驅動器2電性連接至一控制電腦3,控制電腦3可預設地產生一連串的位置命令而令驅動器2驅動伺服馬達12轉動,進而讓機械手臂1的末端13在伺服馬達12的驅動下沿一第一路徑L1移動(參見第5圖)。A method for safe resetting of a robotic arm provided by the present invention is applied to a robotic arm as shown in Figures 2 and 3. In this embodiment, a multi-axis robotic arm 1 is used as an example, and each
伺服馬達12的驅動器2的韌體架構如第4圖所示,可看到驅動器2是採用閉迴路控制系統,閉迴路控制系統包含有一位置控制迴路41、一速度控制迴路42、一扭矩開關43與一扭矩控制迴路44。當控制電腦3產生一位置命令而需讓機械手臂1的末端13移動到一目標位置時,位置控制迴路41依據目標位置以及伺服馬達12所回授的一當前實際位置而產生一速度命令。速度控制迴路42依據速度命令以及伺服馬達12所回授的一當前實際速度而產生一扭矩命令,扭矩開關43是用來限制輸出扭矩,後續段落將會再對扭矩開關43的作動方式做更詳細的介紹。The firmware structure of the
參照第5圖,所示情境係為使機械手臂1的末端13沿一第一路徑L1的一起點A移動到終點C以進行畫線的動作,其中該第一路徑L1上還定義有一位於起點A與終點C之間的位置點B。假設機械手臂1的末端13移動到位置點B時,其中一個關節碰觸到異物以致機械手臂1停止移動,為了避免工作人員受傷並排除異物,工作人員移動了機械手臂1而使該末端13從位置點B移動到一偏移點B’,其中該偏移點B’是位於機械手臂1的末端13移動可及的範圍中,且偏移點B’不在第一路徑L1上。又偏移點B’與位置點B之間定義有一第二路徑L2;於本說明例中,該第二路徑L2是偏移點B’到位置點B的空間中最短路徑,但本發明並不以此為限。Referring to FIG. 5, the situation shown is for the
本發明的機械手臂的安全復位方法包含有以下步驟:The safe reset method of the mechanical arm of the present invention includes the following steps:
步驟S1:控制電腦3計算機械手臂1的末端13沿第二路徑L2從偏移點B’移動至位置點B的一返回速度,配合該末端13距伺服馬達12的力臂而計算得一返回扭矩。須說明的是,於本實施例中,機械手臂1的末端13從偏移點B’移動至位置點B的過程中可能需要同時驅動多個伺服馬達12,因此必須針對每一個伺服馬達12而計算對應的返回扭矩。於以下段落中,只針對其中一個伺服馬達12進行說明,其餘的伺服馬達12則可依此類推。Step S1: The
步驟S2:控制電腦3判斷該返回扭矩是否大於一扭矩限制,若判斷結果為是,則進入步驟S3,之後再從步驟S3回到步驟S1;若判斷結果為否,則進入步驟S4。在通常情況下,如果偏移點B’與位置點B之間的距離太遠,在一預定時間(例如1毫秒)內伺服馬達12所需轉動的角度過大,很可能會造成伺服馬達12的返回速度與返回扭矩過大的問題,因此在本實施例中,伺服馬達12的驅動器2便會通過控制扭矩開關43降低驅動器2的輸出電流至一安全值內,以令伺服馬達12以較低的輸出扭矩(例如伺服馬達12的額定最大輸出扭矩的20%)進行轉動。Step S2: The
步驟S3:驅動器2驅動伺服馬達12,而使機械手臂1的末端13沿第二路徑L2於預定時間內以上述扭矩限制所對應的一限制速度移動到第二路徑L2中的另一偏移點B”,控制電腦3將以此偏移點B”迭代為步驟S1中的偏移點B’。之後回到步驟S1時,控制電腦3將計算從新偏移點B”移動至位置點B的一新返回速度,並獲得對應的一新返回扭矩。在後續重回步驟S2時,原本的步驟S2中的「返回扭矩」被迭代為「新返回扭矩」,並判斷「新返回扭矩」是否大於扭矩限制,若判斷結果為是,則重複執行步驟S3與步驟S1;若判斷結果為否,則進入步驟S4。Step S3: The
步驟S4:令機械手臂1的末端13以該返回速度或新返回速度沿第二路徑L2而返回位置點B。須說明的是,此時扭矩開關43的扭矩限制是設定在伺服馬達12的額定最大輸出扭矩,伺服馬達12是以返回速度而驅動機械手臂1的末端13返回位置點B,除非機械手臂1於移動過程中再次發生異常碰撞,才會再將扭矩開關43的扭矩限制設定在伺服馬達12的額定最大輸出扭矩的20%。Step S4: Make the
步驟S5:在機械手臂1的末端13回到位置點B時,依照原先預設的位置命令,令該末端13沿著第一路徑L1從位置點B移動到終點C,此時扭矩開關43的扭矩限制同樣是伺服馬達12的額定最大輸出扭矩。Step S5: When the
透過本發明的機械手臂的安全復位方法,在機械手臂1因意外停止而被移開第一路徑L1時,能夠利用在韌體上控制扭矩開關43的扭矩限制,而令機械手臂1慢慢地沿著第二路徑L2而回到第一路徑L1上,避免伺服馬達12以太大的返回扭矩與返回速度返回,進而有效地降低伺服馬達12觸發安全機制甚至是毀損的可能性。Through the safe reset method of the robotic arm of the present invention, when the robotic arm 1 is moved away from the first path L1 due to an accidental stop, the torque limit of the
惟以上實施例之揭示僅用以說明本發明,並非用以限制本發明,舉凡等效元件之置換仍應隸屬本發明之範疇。However, the disclosure of the above embodiments is only used to illustrate the present invention, not to limit the present invention, and the replacement of equivalent elements should still belong to the scope of the present invention.
綜上所述,可使熟知本領域技術者明瞭本發明確可達成前述目的,實已符合專利法之規定,爰依法提出申請。To sum up, those skilled in the art can understand that the present invention can achieve the above-mentioned purpose, and it complies with the provisions of the Patent Law.
1:機械手臂1: Robotic arm
11:軸11: Shaft
12:伺服馬達12: Servo motor
13:末端13: End
2:驅動器2: Drive
3:控制電腦3: Control the computer
41:位置控制迴路41: Position control loop
42:速度控制迴路42: Speed control loop
43:扭矩開關43: Torque switch
44:扭矩控制迴路44: Torque control loop
A:起點A: starting point
B:位置點B: location point
B’、B”:偏移點B', B": offset point
C:終點C: end point
L1:第一路徑L1: first path
L2:第二路徑L2: Second path
91:伺服馬達91: Servo motor
92:位置控制迴路92: Position control loop
93:速度控制迴路93: Speed control loop
第1圖為本發明方法之流程圖; 第2圖為本發明中機械手臂之示意圖; 第3圖為本發明中機械手臂與其控制電腦及驅動器之方塊示意圖; 第4圖為本發明中機械手臂驅動器之方塊示意圖; 第5圖為本發明中機械手臂的移動路徑示意圖; 第6圖為習知機械手臂驅動器之方塊示意圖。 Figure 1 is a flow chart of the method of the present invention; Figure 2 is a schematic diagram of a robotic arm in the present invention; Figure 3 is a schematic block diagram of the robotic arm and its control computer and driver in the present invention; Fig. 4 is a block schematic diagram of the robot arm driver in the present invention; Fig. 5 is a schematic diagram of the movement path of the robotic arm in the present invention; FIG. 6 is a block diagram of a conventional manipulator driver.
Claims (4)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW110134841A TWI766807B (en) | 2021-09-17 | 2021-09-17 | Safe reset method of robotic arm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW110134841A TWI766807B (en) | 2021-09-17 | 2021-09-17 | Safe reset method of robotic arm |
Publications (2)
Publication Number | Publication Date |
---|---|
TWI766807B true TWI766807B (en) | 2022-06-01 |
TW202313278A TW202313278A (en) | 2023-04-01 |
Family
ID=83103616
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
TW110134841A TWI766807B (en) | 2021-09-17 | 2021-09-17 | Safe reset method of robotic arm |
Country Status (1)
Country | Link |
---|---|
TW (1) | TWI766807B (en) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1715010A (en) * | 2004-06-29 | 2006-01-04 | 发那科株式会社 | Programming device for returning robot to waiting position |
CN107175672A (en) * | 2016-03-11 | 2017-09-19 | 精工爱普生株式会社 | Robot controller, robot and robot system |
CN107614209A (en) * | 2015-05-28 | 2018-01-19 | 生活机器人学股份有限公司 | Robot device and stepper motor control device |
-
2021
- 2021-09-17 TW TW110134841A patent/TWI766807B/en active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1715010A (en) * | 2004-06-29 | 2006-01-04 | 发那科株式会社 | Programming device for returning robot to waiting position |
CN107614209A (en) * | 2015-05-28 | 2018-01-19 | 生活机器人学股份有限公司 | Robot device and stepper motor control device |
CN107175672A (en) * | 2016-03-11 | 2017-09-19 | 精工爱普生株式会社 | Robot controller, robot and robot system |
Also Published As
Publication number | Publication date |
---|---|
TW202313278A (en) | 2023-04-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7411012B2 (en) | Industrial robots and their operating methods | |
US7102311B2 (en) | Drive control method and drive controller | |
US20160375588A1 (en) | Switching A Control Of A Robot Into A Manual Operating Mode | |
JP6717768B2 (en) | Robot for learning control considering operation in production line and control method thereof | |
CN110919627B (en) | Robot control method and control device | |
WO2003086718A1 (en) | Robot | |
JP2008302496A (en) | Device and method for controlling robot arm, robot, and control program of the robot arm | |
JP2010142910A5 (en) | ||
US20150137726A1 (en) | Motor controller for protecting tool and object to be processed in emergency stop | |
CN110315517B (en) | Robot system and control method | |
JP6814458B2 (en) | Collision detection device for machine tools equipped with a contact release device | |
JP6447141B2 (en) | Robot emergency stop method, robot control device | |
TWI766807B (en) | Safe reset method of robotic arm | |
JP5774552B2 (en) | Robot controller | |
JP5436160B2 (en) | Force control device | |
JP2002187080A (en) | Robot and method for controlling velocity of motor thereof | |
JP5593968B2 (en) | Robot system | |
JP3911258B2 (en) | Method of controlling drive device by control device and control device | |
JP7251814B2 (en) | ROBOT CONTROL SYSTEM, ROBOT CONTROL METHOD, AND PROGRAM | |
JP6088601B2 (en) | Robot controller that suppresses tool tip deflection in robot with traveling axis | |
CN113043328A (en) | Balancer abnormality detection system and balancer abnormality detection method | |
JP2021109294A (en) | Robot control method and robot system | |
JP2019084627A (en) | Method for returning horizontal multi-joint robot to original position | |
JP7469588B2 (en) | Control device for articulated robot and articulated robot | |
JP7474935B2 (en) | Robot control method and robot control device |