TWI766598B - 基於軸物理限制的傳動機構進給率規劃方法 - Google Patents

基於軸物理限制的傳動機構進給率規劃方法 Download PDF

Info

Publication number
TWI766598B
TWI766598B TW110107038A TW110107038A TWI766598B TW I766598 B TWI766598 B TW I766598B TW 110107038 A TW110107038 A TW 110107038A TW 110107038 A TW110107038 A TW 110107038A TW I766598 B TWI766598 B TW I766598B
Authority
TW
Taiwan
Prior art keywords
acceleration
union
intersection
torque
axis
Prior art date
Application number
TW110107038A
Other languages
English (en)
Other versions
TW202219669A (zh
Inventor
鄭銘揚
黃子源
俊龍 黃
許忠湛
Original Assignee
台達電子工業股份有限公司
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 台達電子工業股份有限公司 filed Critical 台達電子工業股份有限公司
Publication of TW202219669A publication Critical patent/TW202219669A/zh
Application granted granted Critical
Publication of TWI766598B publication Critical patent/TWI766598B/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/41—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path
    • G05B19/4103—Digital interpolation
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/34—Director, elements to supervisory
    • G05B2219/34169—Coarse interpolator, path calculator delivers position, speed, acceleration blocks

Landscapes

  • Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)

Abstract

一種傳動機構進給率規劃方法，應用於具有一或多軸的傳動機構，包括：設定路徑，並於路徑上設置複數粗插值點；基於各粗插值點的三維空間資訊、逆向運動學函式、微分手段及動態方程式計算各軸在各粗插值點上的軸位置、軸速度、軸加速度及軸轉矩；基於軸速度、軸加速度及軸轉矩計算傳動機構於各粗插值點上的初始進給率上限；基於軸加速度及軸轉矩執行雙向掃描演算法，以更新各粗插值點上的進給率上限；取得各粗插值點上的初始進給率上限、順向掃描結果及逆向掃描結果中最小者為傳動機構的工具於各粗插值點上的最終進給率。

Description

基於軸物理限制的傳動機構進給率規劃方法
本發明涉及傳動機構，尤其涉及傳動機構運動時的進給率的規劃方法。
近年來，為了實現工業自動化，各式的傳動機構例如機械手臂、CNC加工機等，已普遍運用在各類型的工廠之中。
如上所述的傳動機構，其內部通常會設置有複數馬達(即，為傳動機構的軸或關節)。藉由控制各個馬達的轉動，控制器可控制傳動機構的整體運動，藉此達到讓傳動機構依據預先規劃的運動軌跡進行移動，最終令傳動機構上設置的工具到達目的位置並執行指定動作的主要目的。
一般來說，要令傳動機構以最快的速度移動，只需要讓傳動機構內的每一顆馬達皆以最快的轉速進行運轉即可。然而，若要令傳動機構穩定地沿著預先規劃的運動軌跡移動，就必須要按照特定比例來計算並分配各個馬達的轉速及轉角，而無法對馬達進行任意的控制。
再者，一個傳動機構中的複數馬達可能會分別具有不同的物理限制(例如具有不同的速度上限、加速度上限、轉矩上限等)，若要令傳動機構 盡可能以最快的速度移動，可能會不小心令部分馬達的運轉超出其物理上限(即，產生超頻的效果)。如此一來，將可能會導致馬達的毀損。
為了避免馬達毀損的問題，部分傳動機構的控制器會在控制傳動機構的移動時，同步調降內部所有馬達的轉速或轉矩，藉此避免馬達的運轉超出其物理限制。
然而，如上所述，同一個傳動機構中的複數馬達可能會分別具有不同的物理限制，若以相同幅度來同時調降所有馬達的轉速或轉矩，雖可避免上述問題，但將會無法有效發揮馬達的最大效力。如此一來，將會導致傳動機構的移動速度過慢，進而拉長加工時間、提高加工成本並降低傳動機構的產值。
本發明之主要目的，在於提供一種基於軸物理限制的傳動機構進給率規劃方法，係可基於軸物理限制來規劃傳動機構在預定的運動軌跡上的進給率，藉此確保傳動機構可以在令各軸馬達皆符合本身的物理限制的情況下達到較佳的移動速度。
為了達成上述之目的，本發明的基於軸物理限制的傳動機構進給率規劃方法主要是應用於具有一或多軸的一傳動機構中，並且包括下列步驟：設定一路徑，並於該路徑上設置複數粗插值點；基於各該粗插值點的三維空間資訊、一逆向運動學函式、一微分手段及一動態方程式計算各該軸在各該粗插值點上的一軸位置、一軸速度、一軸加速度及一軸轉矩；基於該軸速度、該軸加速度及該軸轉矩計算該傳動機構於各該粗插值點上的一初始進給率上限；基於該軸加速度及該軸轉矩執行一雙向掃描演算法，以更新該傳動機構於各該粗插 值點上的一進給率上限；取得各該粗插值點上的該初始進給率上限、一順向掃描結果及一逆向掃描結果中最小者，以做為該傳動機構於各該粗插值點上的一最終進給率。
本發明結合了求解初始進給率(Initial Feed-rate)上限的演算法以及雙向掃描演算法(Bidirectional Scan Algorithm)，並且再加上傳動機構在各個粗插值點之間為等加速度的假設。相較於相關技術所採用的技術手段，本發明在不通過運動學奇異點(Singular Point)的條件下，只要初始進給率上限大於零，就不需要尋找切換點，且不會有無解而找不到進給率的問題。
本發明的技術方案在執行雙向掃描演算法之前，先利用傳動機構中各軸的物理量來計算初始進給率上限，因此只要傳動機構採用的進給率不超過此上限，則無論是在等速、加速或減速的狀態下，都不會有馬達運轉超出其物理限制的情形。並且，本發明於雙向掃描演算法執行完畢且更新了傳動機構於各個粗插值點上的進給率後，即可直接控制傳動機構進行移動與加工，不需要再重新計算進給率，因此在加工過程中可以有效節省額外的運算資源。
另外，本發明基於傳動機構上的各軸的軸加速度及軸轉矩來執行雙向掃描演算法，以更新傳動機構的進給率，因此傳動機構在實際移動與加工時，於任何時間點下都有至少一軸運作在某一項物理限制之下。藉此，可以確保在所有軸皆不超出各自的物理限制的情況下，令傳動機構達到較佳的移動速度。
1:傳動機構
11:工具
2:運動軌跡
21:粗插值點
211:起點位置
212:終點位置
3:初始進給率上限
41:順向掃描結果
42:逆向掃描結果
43:最終進給率
S10~S28:規劃步驟
S40~S62:計算步驟
S70~S84:係數計算步驟
S90~S104:上、下界計算步驟
圖1為傳動機構的運動示意圖。
圖2為本發明的規劃流程圖的第一具體實施例。
圖3為本發明的粗插值點示意圖。
圖4為本發明的初始進給率上限的示意圖的第一具體實施例。
圖5A為本發明的順向掃描示意圖。
圖5B為本發明的逆向掃描示意圖。
圖5C為本發明的順向掃描結果與逆向掃描結果的組合示意圖。
圖5D為本發明的最終進給率示意圖。
圖6A為本發明的最終進給率的第一計算流程圖的第一具體實施例。
圖6B為本發明的最終進給率的第二計算流程圖的第一具體實施例。
圖7為本發明的係數計算流程圖的第一具體實施例。
圖8為本發明的上、下界計算流程圖的第一具體實施例。
圖9為本發明的進給率修正的示意圖。
圖10為本發明的各軸表現的示意圖。
圖11A為本發明的各軸速度表現的示意圖。
圖11B為本發明的各軸加速度表現的示意圖。
圖11C為本發明的各軸轉矩表現的示意圖。
茲就本發明之一較佳實施例，配合圖式，詳細說明如後。
本發明揭露了一種基於軸物理限制的傳動機構進給率規劃方法(下面將於說明書中簡稱為規劃方法)，所述規劃方法主要應用於內部具有一或多個軸(即，一或多個關節)，並且可接受外部電腦或處理器發送的控制命令，並沿著控制命令中指出的運動軌跡進行移動的傳動機構。
值得一提的是，下列於說明書中所指的傳動機構，可為單軸的傳動機構(例如導螺桿)或多軸的傳動機構，其中多軸的傳動機構還可為非正交組態傳動機構(即，各軸的運動會相互影響的傳動機構)，而不侷限於特定的傳動機構類型。
為便於說明及理解，下面於說明書中將以多軸的傳動機構來進行舉例說明，但並不以多軸為傳動機構的必要限制。
具體地，所述傳動機構在每一個軸的位置皆設置有一個對應的馬達，所述處理器主要是於控制命令中記錄各個馬達於對應時間中的轉速及/或轉角，以藉由各個馬達的對應轉動，來令傳動機構以指定的速度移動至指定的位置。
本發明的其中一個技術特徵在於，於規劃傳動機構的進給率(Feedrate)時會一併考量傳動機構內部的多個軸的各種物理限制(例如速度上限、加速度上限、轉矩上限等)，並且基於各個軸的各種物理限制來規劃傳動機構在沿著所述運動軌跡進行移動時的速度(即，進給率)。藉此，避免在令傳動機構快速移動時，因部分軸的運轉超出了其物理限制，而造成馬達的毀損。另，通過本發明的規劃方法，亦可解決為了避免各軸的毀損而降低傳動機構整體的進給率度，進而導致加工效率下降的問題。
參閱圖1，為傳動機構的運動示意圖。本發明中所指的傳動機構1，可以泛指任何可藉由內部設置的多個馬達之運轉來進行移動與加工的機構，例如SCARA機械手臂(SCARA Robot)、六軸機械手臂(6-Axis Robot Manipulator)、三軸/五軸CNC加工機(3/5 Axis CNC Machine Tool)等。於圖1的實施例中，主要是以六軸機械手臂為例，進行說明，但並不以此為限。
如圖1所示，傳動機構1為了達到特定的加工目的，一般會在末端位置設置對應的工具11(例如刀片、夾子等)。為了藉由傳動機構1來進行加工，使用者(例如生產線的管理者)可預先規劃所需的一條運動軌跡2，並且將對應的控制命令傳遞至傳動機構1。傳動機構1接收上述控制命令後，可依據控制命令的內容來控制內部的多個軸(即，多個馬達)的運轉參數(例如轉動速度、轉動角度等)，藉此傳動機構1可以依據指定速度來沿著運動軌跡2移動至指定位置，最終於指定位置上藉由工具11來完成加工。
值得一提的是，傳動機構1的末端位置上並不必然設置有所述工具11。若傳動機構1上沒有設置所述工具11，則可將工具11的尺寸視為零，此時，工具11的進給率相等於傳動機構1的末端位置的進給率。
為便於說明與理解，下面於說明書中將以傳動機構1於末端位置上設置的工具11的進給率為例，進行說明，但本發明的技術方案並不以運用在設置有工具11的傳動機構1為限。
本發明的規劃方法主要是用來規劃傳動機構1的進給率。所述進給率，指的是傳動機構1上的工具11在運動軌跡2上的移動速度。更具體地，所述進給率指的是單位時間下，工具11於運動軌跡2上前進的量，或是指在運動軌跡2上的切線速度。如前文所述，工具11的進給率係受傳動機構1中 各個軸的運轉參數所影響，若進給率規劃不當，容易造成傳動機構1內的一或多個軸的運轉超出其物理限制，進而導致軸的毀損。
請參閱圖2，為本發明的規劃流程圖的第一具體實施例。具體地，本發明的規劃方法主要是以軟體或韌體的方式實現，並且所述軟體運作於與傳動機構1通訊連接，用以控制傳動機構1並搜集對應資料的電子裝置中。所述電子裝置可例如為個人電腦(Personal Computer,PC)、筆記型電腦(Laptop)、平板電腦(Tablet)、工業電腦(Industrial PC,IPC)、伺服器(Server)、處理器(Processor)、微處理單元(Micro Control Unit,MCU)、可程式邏輯控制器(Programmable Logic Controller,PLC)、系統單晶片(System on a Chip,SoC)等，但不加以限定。當所述電子裝置載入並執行所述軟體或韌體後，即可執行本發明的規劃方法。
於本發明的規劃方法中，首先，管理者選擇或設定要控制的傳動機構1(步驟S10)，並且設定加工所需的運動軌跡2(步驟S12)。
具體地，管理者於步驟S10中主要可藉由電子裝置上的人機介面、程式介面或網頁介面(圖未標示)來設定電子裝置要控制的傳動機構1的基本資料，例如型號、軸的數量(即，馬達的數量)、各軸的物理限制等。於一實施例中，所述物理限制可例如為軸轉矩限制、軸位置限制(包括正極限與負極限)、軸速度限制、軸加速度限制、MDH(Modified Denavit-Hartenberg)參數等，但並不以此為限。
於步驟S12中，管理者主要可於所述介面上設定所述運動軌跡2於一個三維空間中的空間資訊，例如起點位置的位置資訊(X,Y,Z)與轉動角度資訊(α,β,γ)、終點位置的位置資訊與轉動角度資訊、路徑總長、路徑角度等，但並不以此為限。
步驟S12後，管理者或所述電子裝置可藉過特定演算法將運動軌跡2邏輯切割為複數個區段，藉此於運動軌跡2上設置複數粗插值點(Rough-Interpolation Point)，並取得各個粗插值點於所述三維空間中的三維空間資訊(步驟S14)。於一實施例中，各個粗插值點的三維空間資訊至少包括了於三維空間中的位置資訊以及轉動角度資訊。
請同時參閱圖3，為本發明的粗插值點示意圖。如圖3所示，電子裝置可自動或是基於外部操作來於設定完成的運動軌跡2上設置複數粗插值點21，並且分別記錄各個粗插值點21在三維空間中的位置資訊以及轉動角度資訊(X,Y,Z,α,β,γ)。所述粗插值點又可稱為預插值點(Pre-Interpolation Point)，並且用以產生粗插值點21的所述特定演算法屬於相關技術領域中的常用技術手段，於此不再贅述。
值得一提的是，電子裝置藉由上述特定演算法，主要可在運動軌跡2的起點位置211與終點位置212之間產生複數粗插值點21，而基於電子裝置本身的計算能力，所產生的粗插值點2的數量不盡相同。粗插值點2的數量越多，最終規劃所得的進給率將會越精準，但進給率的規劃會耗費較多的計算資源。反之，粗插值點2的數量越少，最終規劃所得的進給率會較不精確，但電子裝置不需要具備太高的計算能力。
於一實施例中，各個粗插值點21之間可為等距離或不等距離。於另一實施例中，所述特定演算法可基於實際參數(例如運動軌跡2的方向與長度等)來自動計算各個相鄰的粗插值點21之間的距離，但不以此為限。
回到圖2。於步驟S14後，電子裝置獲得了規劃進給率所需的所有資料。藉此，電子裝置可基於各個粗插值點21的三維空間資訊、一個逆向 運動學函式、微分手段以及一個動態方程式來計算傳動機構1中的各個軸於各個粗插值點21上的軸位置資訊、軸速度資訊、軸加速度資訊以及軸轉矩資訊。具體地，電子裝置主要是通過上述程序，將傳動機構1的工具11於三維空間上的空間資訊(即，各個粗插值點21的三維空間資訊)分別轉換為傳動機構1內的各軸於各個粗插值點21上的對應資訊。
本發明中，電子裝置主要是取得一個逆向運動學函式，並且通過逆向運動學函式來將各個粗插值點21的三維空間資訊轉換為各軸於各個粗插值點21上的軸位置資訊(步驟S16)。於一實施例中，所述粗插值點21的三維空間資訊為(X,Y,Z,α,β,γ)，轉換後的軸位置資訊為：q i =M i (X,Y,Z,α,β,γ)，其中i為傳動機構1中的各個軸的編號，qi為各軸的軸位置資訊，M為所述逆向運動學函式。所述逆向運動學函式為相關技術領域中的常用技術手段，於此不再贅述。
值得一提的是，所述軸位置資訊還可表示為：q i =M i (X(s 1),Y(s 1),Z(s 1),α(s 2),β(s 2),γ(s 2))，其中，s1為所述運動軌跡2的路徑長度，s2為傳動機構1的轉動角度總和。意即，三維空間資訊中的位置資訊為運動軌跡2的路徑長度的函數，而三維空間資訊中的轉動角度資訊為傳動機構1的轉動角度總和的函數。
於步驟S16後，電子裝置取得了傳動機構1的各軸於各個粗插值點21上的軸位置資訊。接著，電子裝置對這些軸位置資訊分別執行微分手段，以獲得各軸於各個粗插值點21上的軸速度資訊。並且，電子裝置進一步對各軸於各個粗插值點21上的軸速度資訊分別執行微分手段，以獲得各軸於各個粗插值點21上的軸加速度資訊(步驟S18)。
所述軸速度資訊可表示為：
Figure 110107038-A0305-02-0012-1
，所述軸加速 度資訊可表示為：
Figure 110107038-A0305-02-0012-3
，其中，
Figure 110107038-A0305-02-0012-97
代表軸位置資訊q的一次 微分(也就是軸速度資訊)，
Figure 110107038-A0305-02-0012-98
代表軸位置資訊q的二次微分(也就是軸加速度資訊)，而f代表傳動機構1的工具11於三維空間中的進給率，
Figure 110107038-A0305-02-0012-99
代表進給率的一次微分(也就是工具11於三維空間中的加速度)。
具體地，對所述軸位置資訊進行微分，即可得到基於時間變化的三維空間位置變化量，也就是進給率的物理意義。因此，通過上述的微分計算，即可產生上述用來描述傳動機構1的工具11於三維空間中的進給率f與傳動機構1中的各軸於軸空間中的速度
Figure 110107038-A0305-02-0012-100
間的關係之關係式。
於上述實施例中，軸速度資訊的計算公式主要是對運動軌跡 2的路徑長度s1進行微分，因此表示為
Figure 110107038-A0305-02-0012-95
。然而，於其他實施 例中，亦可對軸位置資訊q中的三維空間的距離總量(即，X、Y、Z的平方和開根號)、轉動角度總量(即，α、β、γ的平方和開根號)等參數進行微分，而不對路徑長度s1進行微分。再者，上述總量除了以平方和開根號來表示以外，亦可使用四元數(Quaternion)來表示，而不以此為限。換句話說，上述軸速度資訊的計算 公式可以進一步改寫為一通式：
Figure 110107038-A0305-02-0012-6
，其中A1代表所述運動軌跡 2的路徑長度s1、所述距離總量或轉動角度總量。
步驟S18後，電子裝置進一步取得一個動態方程式，並且將各軸的所述軸速度資訊及軸加速度資訊帶入動態方程式中，以分別獲得各軸於各個粗插值點21上的軸轉矩資訊(步驟S20)。
具體地，所述動態方程式是基於古典動力學的歐拉-拉格朗日方程式(Lagrange-Euler Equation)所形成，以將所述軸轉矩資訊表示成一個由慣 性力(Inertia Torque)、科氏力與向心力(Coriolis Force and Centripetal Torque)、重力 (Gravity Torque)、及動摩擦力所組合而成的公式，此公式表示為：
Figure 110107038-A0305-02-0013-7
Figure 110107038-A0305-02-0013-8
，其中τ為各軸的軸轉矩資訊，q為各軸的軸位 置資訊，
Figure 110107038-A0305-02-0013-101
為各軸的軸速度資訊，
Figure 110107038-A0305-02-0013-103
為各軸的軸加速度資訊，D(q)為慣性力矩陣，C(q,
Figure 110107038-A0305-02-0013-102
)為科氏力與向心力矩陣，F c sign(
Figure 110107038-A0305-02-0013-104
)+F v
Figure 110107038-A0305-02-0013-105
為動摩擦力向量。
於前述步驟S20中，電子裝置將各軸的軸速度資訊以及軸加速度資訊分別帶入上述動態方程式中，即可將上述動態方程式轉換為用來描述 軸轉矩資訊的下列方程式：
Figure 110107038-A0305-02-0013-9
Figure 110107038-A0305-02-0013-10
，其中d為慣量 力矩陣中的元素，Cind為科氏力與向心力矩陣中與軸速度相關的元素。於各個軸的軸速度資訊以及軸加速度資訊已知的情況下，電子裝置可藉由上述方程式計算出各軸於各個粗插值點21上的軸轉矩資訊。其中，由於傳動機構1的進給率必定為正數，因此上述方程式中的sign(
Figure 110107038-A0305-02-0013-107
)可以改寫為sign(M s,i )。
步驟S20後，電子裝置獲得了傳動機構1內各軸於各個粗插值點21上的軸速度資訊、軸加速度資訊以及軸轉矩資訊，並且可基於這些資訊來分別計算傳動機構1於各個粗插值點21上的一個初始進給率上限(步驟S22)。
本發明的其中一個技術特徵在於，在不考慮工具11於三維空間中可以實現的加速度(即，將所述加速度
Figure 110107038-A0305-02-0013-108
設定為零)的前提下，先基於各軸的軸速度上限來初始計算傳動機構1在各個粗插值點21上可以實現而又不會損壞各軸的一個初始進給率上限。換句話說，只要傳動機構1沿著運動軌跡2移動時，在各個粗插值點21上的進給率不超過此初始進給率上限，則各軸的各項物理量都將不會超過其本身的物理限制。
如前文所述，各軸於各個粗插值點上的軸速度資訊可表示為：
Figure 110107038-A0305-02-0014-12
，而軸加速度資訊可表示為：
Figure 110107038-A0305-02-0014-14
，而在將工具11的加 速度設定為零後，即可得到各個軸的進給率上限為：
Figure 110107038-A0305-02-0014-16
。並 且，基於各軸於各個粗插值點上的軸轉矩資訊，亦可得到各軸的進給率上限為：f min (τ max,i ,M s,i ,M ss,i ,d ij ,C ind,i ,F c,i ,F v,i )。
於上述步驟S22中，電子裝置主要是將傳動機構1的工具11 的加速度設定為零(即，
Figure 110107038-A0305-02-0014-11
)，並且基於各軸的軸速度資訊分別計算傳動機構1 在各個粗插值點21上的第一進給率上限f1max(即，
Figure 110107038-A0305-02-0014-18
)，基於各軸的軸 加速度資訊分別計算傳動機構1在各個粗插值點21上的第二進給率上限f2max(即，
Figure 110107038-A0305-02-0014-17
)，基於各軸的軸轉矩資訊分別計算傳動機構1在各個粗插值點 21上的第三進給率上限f3max(即，f 3 max =f min (τ max,i ,M s,i ,d ij ,C ind,i ,F c,i ,F v,i ))。
為了確保各軸在運轉時，其軸速度、軸加速度以及軸轉矩等參數皆不會超過物理限制，因此本實施例中，電子裝置會分別以各個粗插值點21上的第一進給率上限、第二進給率上限以及第三進給率上限中最小者，做為傳動機構1在此粗插值點21上的初始進給率上限。於一實施例中，電子裝置主要可藉由以下計算公式來計算工具11於各個粗插值點21上的初始進給率上限：
Figure 110107038-A0305-02-0014-19
，其中 f initial max 為初始進給率上限，i=1至M，M為傳動機構1內部具備的軸的總數。
請同時參閱圖4，為本發明的初始進給率上限的示意圖的第一具體實施例。如圖4所示，電子裝置通過特定演算法的執行，將預設的運動 軌跡2邏輯切割成等距或不等距的多個區段，並且在各個區段上設置了複數粗插值點21。並且，通過上述圖3的步驟S22，電子裝置可以獲得傳動機構1的工具11於各個粗插值點21上的初始進給率上限3。
如前文所述，本發明中，電子裝置是在將工具11的加速度設定為零的情況下計算所述初始進給率上限3。然而，傳動機構1在移動時的加速度是無法避免的，因此所述初始進給率上限3只能夠確保各軸的軸速度皆可落在其各自的物理限制範圍內。若要讓各軸的軸加速度以及軸轉矩也能夠落在物理限制範圍內，電子裝置必須於後續程序中，藉由工具11在三維空間中的加速度來對初始進給率上限3進行修正。
回到圖3。於步驟S22後，電子裝置接著基於各軸的軸加速度資訊以及軸轉矩資訊來執行雙向掃描演算法，以對前述計算所得的初始進給率上限3進行更新，並且得到對應的順向掃描結果以及逆向掃描結果(即，執行了雙向掃描演算法中的順向掃描程序以及逆向掃描程序後所得到的結果)(步驟S24)。
本實施例中，電子裝置在步驟S22中主要是將傳動機構1的 工具11於三維空間中的加速度設定為等加速度，即，設定
Figure 110107038-A0305-02-0015-88
，其中
Figure 110107038-A0305-02-0015-109
為工具11於三維空間中的加速度，fold為上一個被掃描的粗插值點21的進給率，s為上一個被掃描的粗插值點21與當前被掃描的粗插值21點間的距離。
於工具11被設定為等加速度的情況下，電子裝置可基於各軸於各個粗插值點21上的軸加速度資訊以及軸轉矩資訊來分別執行雙向掃描演算法中的順向掃描程序以及逆向掃描程序，藉此於各個粗插值點21上分別得到符合條件的一筆順向掃描結果以及一筆逆向掃描結果(容後詳述)。
步驟S24後，電子裝置進一步取得各個粗插值點21上的初始進給率上限3、順向掃描結果以及逆向掃描結果，並且以其中最小者做為傳動機構1的工具11於各個粗插值點21上的最終進給率(步驟S26)。於各個粗插值點21的最終進給率皆計算完成後，電子裝置即完成了傳動機構1於預設的運動軌跡2上的進給率規劃程序。據此，電子裝置可以基於此進給率來產生並發送對應的控制命令至傳動機構1，以令傳動機構1控制其上的工具11基於所述最終進給率來沿著運動軌跡2進行移動(步驟S28)，藉此完成加工目的。
如上所述，本發明是由電子裝置依據上述步驟計算傳動機構1於各個粗插值點21上的最終進給率後，再控制傳動機構1基於此最終進給率來進行移動與加工。因此，在傳動機構1的移動過程中不需要再重新計算、更新進給率。如此一來，可以有效節省電子裝置與傳動機構1的運算資源。
請同時參閱圖5A、圖5B、圖5C、圖5D、圖6A及圖6B，其中圖5A為本發明的順向掃描示意圖、圖5B為本發明的逆向掃描示意圖、圖5C為本發明的順向掃描結果與逆向掃描結果的組合示意圖、圖5D為本發明的最終進給率示意圖，而圖6A與圖6B分別為本發明的最終進給率的第一計算流程圖以及第二計算流程圖的第一具體實施例。
如圖5A及圖5B所示，所述雙向掃描演算法中包括了順向掃描程序以及逆向掃描程序。順向掃描程序主要是由運動軌跡2的起點位置211開始，朝向終點位置212的方向依序掃描運動軌跡2中的多個粗插值點。相對地，逆向掃描程序是由同一個運動軌跡2的終點位置212開始，朝向起點位置211的方向依序掃描運動軌跡2中的多個粗插值點。
本發明的其中一個技術特徵在於，於所述掃描程序(包括順向掃描程序以及逆向掃描程序)中，下一個被掃描的粗插值點21的初始進給率上限3必須大於當前被掃描的粗插值點21的初始進給率上限3。如圖5A及圖5B所示，由於傳動機構1在移動時的進給率具有高低起伏，因此無論是順向或逆向，下一個被掃描的粗插值點21的初始進給率上限3都有可能小於、大於或等於當前被掃描的粗插值點21的初始進給率上限3。換句話說，順向掃描程序並不會掃描運動軌跡2中的所有粗插值點21，而逆向掃描程序同樣不會掃描運動軌跡2中的所有粗插值點21。
由圖5A中可看出，順向掃描程序是從運動軌跡2的起點位置211開始掃描，並且下一個掃描的粗插值點21的初始進給率上限3必須大於當前掃描的粗插值點21的初始進給率上限3(稱為順向掃描條件)。換句話說，順向掃描程序主要是用於處理傳動機構1從低速開始進行加速的所有情況。相似地，從圖5B可看出逆向掃描程序是從運動軌跡2的終點位置212開始掃描，而下一個掃描的粗插值點21的初始進給率上限3同樣也必須大於當前掃描的粗插值點21的初始進給率上限3(稱為逆向掃描條件)。換句話說，逆向掃描程序主要是用於處理傳動機構1從高速開始進行減速的所有情況。
再如圖5C與圖5D所示，於順向掃描程序結束後，電子裝置可為每一個被掃描的粗插值點21皆產生一個順向掃描結果41，並且於逆向掃描程序結束後，為每一個被掃描的粗插值點21皆產生一個逆向掃描結果42。最後，針對運動軌跡2中的每一個粗插值點21，電子裝置會於已知的初始進給率上限3、順向掃描結果41(若存在)以及逆向掃描結果42(若存在)中選擇最小者，做為各個粗插值點21的最終進給率43。
於圖6A、圖6B的實施例中，是以電子裝置先執行順向掃描程序，再執行逆向掃描程序為例，舉例說明。然而，於其他實施例中，電子裝置亦可先執行逆向掃描程序後，再執行順向掃描程序，或是藉由多工處理而同時執行順向掃描程序以及逆向掃描程序，而不以圖6A、圖6B中所示者為限。
如圖6A所示，首先，電子裝置由運動軌跡2的起點位置211開始，朝向終點位置212的方向執行順向掃描程序(步驟S40)，其中每一個被掃描的粗插值點21的初始進給率上限3，皆大於上一個被掃描的粗插值點21的初始進給率上限3。值得一提的是，本發明中的順向掃描程序可以直接從所述起點位置211掃描至所述終點位置212(即，完整掃描整個運動軌跡2)，亦可視實際的記憶體限制，以順向的方向掃描運動軌跡2的其中一段路徑，而不以完整掃描整個運動軌跡2為必要。
於本實施例中，電子裝置藉由上述
Figure 110107038-A0305-02-0018-24
的計算式將傳動 機構1的工具11設定為等加速度。藉由等加速度的設定，電子裝置可將上述用以表示各軸於各個粗插值點21上的軸加速度資訊的計算式分別轉換為一個第一二次方程式以及一個第二二次方程式，並且將上述用以表示各軸於各個粗插值點21上的軸轉矩資訊的計算式分別轉換為一個第三二次方程式以及一個第四二次方程式(步驟S42)。
如前文所述，各軸的軸加速度資訊可以表示為
Figure 110107038-A0305-02-0018-30
Figure 110107038-A0305-02-0018-31
，將其中的加速度
Figure 110107038-A0305-02-0018-114
以前述的等加速度
Figure 110107038-A0305-02-0018-170
來取代，則可以得到用來代表 各軸的速度與加速度限制的方程式：-
Figure 110107038-A0305-02-0018-112
M ss,i f 2+M s,i (
Figure 110107038-A0305-02-0018-86
)
Figure 110107038-A0305-02-0018-171
。 值得一提的是，此處的fold為上一個被掃描的粗插值點21的進給率，因此在計算下一個被掃描的粗插值點21的進給率時，fold可視為一個已知的常數。
本技術領域中具有通常知識者皆知，二次方程式一般可表示為：ax 2+bx+c，其中的a、b、c即為二次方程式的係數。為便於理解，本發明進一步將上述用來代表各軸的速度與加速度限制的方程式改寫成所述第一二次方程式：
Figure 110107038-A0305-02-0019-115
(M SS,i ,M s,i )f 2+
Figure 110107038-A0305-02-0019-116
(M s,i ,
Figure 110107038-A0305-02-0019-117
)
Figure 110107038-A0305-02-0019-118
0，以及所述第二二次方程式：
Figure 110107038-A0305-02-0019-119
(M SS,i ,M s,i )f 2+
Figure 110107038-A0305-02-0019-120
(M s,i ,
Figure 110107038-A0305-02-0019-121
)
Figure 110107038-A0305-02-0019-124
0，其中
Figure 110107038-A0305-02-0019-125
與
Figure 110107038-A0305-02-0019-126
為加速度交集係數，
Figure 110107038-A0305-02-0019-127
與
Figure 110107038-A0305-02-0019-128
為加速度聯集係數。更具體地，藉由對上述方程式進行化簡，可得出
Figure 110107038-A0305-02-0019-46
如前文所述，各軸的軸轉矩資訊可以表示為
Figure 110107038-A0305-02-0019-87
Figure 110107038-A0305-02-0019-41
Figure 110107038-A0305-02-0019-172
，將其中的加速度
Figure 110107038-A0305-02-0019-174
以前述的等加速度
Figure 110107038-A0305-02-0019-39
來取代， 則可以得到用來代表各軸的轉矩限制的方程式：
Figure 110107038-A0305-02-0019-42
Figure 110107038-A0305-02-0019-44
同樣地，為便於理解，本發明基於二次方程式的基本形式，將上述用以代表各軸的轉矩限制的方程式改寫成所述第三二次方程式：a p,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b p,τ (F v,i ,M s,i )f+c p,τ (F c,i ,M s,i ,d ij ,τ max,i )
Figure 110107038-A0305-02-0019-130
0，以及所述第四二次方程式：a n,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b n,τ (F v,i ,M s,i )f+c n,τ (F c,i ,M s,i ,d ij ,τ max,i )
Figure 110107038-A0305-02-0019-131
0，其中a p,τ 、b p,τ 與c p,τ 為轉矩交集係數，a n,τ 、b n,τ 與c n,τ 為轉矩聯集係數。
更具體地，藉由對上述方程式進行化簡，可得出
Figure 110107038-A0305-02-0019-47
Figure 110107038-A0305-02-0019-50
，b p,τ =F v,i M s,i ，
Figure 110107038-A0305-02-0020-51
，a n,τ =-a p,τ ，b n,τ = -b p,τ ，
Figure 110107038-A0305-02-0020-52
。
於步驟S42後，電子裝置可分別對各軸於各個粗插值點21(即，符合上述順向掃描條件的所有粗插值點21)上的第一二次方程式以及第二二次方程式進行整理，以藉由方程式求解而獲得對應的加速度交集係數以及加速度聯集係數。同樣的，電子裝置分別對各軸於各個粗插值點21上的第三二次方程式以及第四二次方程式進行整理，以藉由方程式求解而獲得對應的轉矩交集係數以及轉矩聯集係數(步驟S44)。具體的求解方式如後述之圖7所示。
步驟S44後，電子裝置獲得了求解各個第一二次方程式及第二二次方程式所需的加速度交集係數以及加速度聯集係數，並且獲得了求解各個第三二次方程式以及第四二次方程式所需的轉矩交集係數以及轉矩聯集係數。
承步驟S44，電子裝置進一步將這些加速度交集係數以及加速度聯集係數分別帶入對應的第一二次方程式以及第二二次方程式中進行求解，以分別獲得各軸於各個粗插值點21上的順向加速度交集上界、順向加速度交集下界、順向加速度聯集上界以及順向加速度聯集下界，並且將這些轉矩交集係數以及轉矩聯集係數分別帶入對應的第三二次方程式以及第四二次方程式以進行求解，以分別獲得各軸於各個粗插值點21上的順向轉矩交集上界、順向轉矩交集下界、順向轉矩聯集上界及順向轉矩聯集下界(步驟S46)。具體的求解方式如後面之圖8所示。
步驟S46後，電子裝置即可基於所獲得的這些順向加速度交集上界、順向加速度交集下界、順向加速度聯集上界、順向加速度聯集下界、順向轉矩交集上界、順向轉矩交集下界、順向轉矩聯集上界及順向轉矩聯集下界， 分別計算各軸於各個粗插值點21上的交集上界U i,in 、交集下界D i,in 、聯集上界U i,un 以及聯集下界D i,un (步驟S48)。藉此，電子裝置可基於這些交集上界、交集下界、聯集上界以及聯集下界來分別建立傳動機構1的進給率於符合順向掃描條件的各個粗插值點21上的一個順向解空間(步驟S50)。本實施例中，順向掃描程序針對各個粗插值點21所給出的順向掃描結果41，係分別落在對應的順向解空間之中。
於一實施例中，電子裝置於上述步驟S50中可通過下面計算 公式來建立各個粗插值點21的順向解空間：
Figure 110107038-A0305-02-0021-91
Figure 110107038-A0305-02-0021-92
，其中，U i,in 為各軸於各個粗插值點21上的交集 上界，D i,in 為各軸於各個粗插值點21上的交集下界，U i,un 為各軸於各個粗插值點21上的聯集上界，D i,un 為各軸於各個粗插值點21上的聯集下界，i為傳動機構1內部的軸的編號，n為傳動機構1內部的複數軸的總數。
由上述計算公式可看出，針對各個粗插值點21，電子裝置先取得第一軸至第i軸之交集空間，並且取得第一軸至第i軸之聯集空間，最後再對所述交集空間以及所述聯集空間取其交集，藉此建立各個粗插值點21的順向解空間。
於步驟S50後，電子裝置即完成了雙向掃描演算法中的順向掃描程序。於此之後，電子裝置可以進一步執行雙向掃描演算法中的逆向掃描程序。值得一提的是，本發明中的逆向掃描程序是指基於與前述順向掃描程序相反的方向(即，由終點位置212朝向起點位置211的方向)，對已經完成順向掃描程序的一段路徑(或是完整的運動軌跡2)執行與順向掃描程序相同的動作。意即， 本發明中的逆向掃描程序同樣不以掃描整個完整的運動軌跡2為必要，也就是說不以從運動軌跡2的終點位置212開始掃描為必要。
為便於說明與理解，下面將仍以從運動軌跡2的終點位置212開始，朝向起點位置211的方向執行逆向掃描程序為例，但並不以此為限。
具體地，電子裝置由運動軌跡2的終點位置212開始，朝向起點位置211的方向執行逆向掃描程序(步驟S52)，並且每一個被掃描的粗插值點21的初始進給率上限3，皆大於上一個被掃描的粗插值點21的初始進給率上限3(此時是沿著與順向掃描程序相反的方向進行判斷與掃描)。
本實施例中，電子裝置採用與在順向掃描程序中相同的計算式將傳動機構1的工具11設定為等加速度，藉此將用以表示各軸於各個粗插值點21上的軸加速度資訊的計算式分別轉換為前述的第一二次方程式以及第二二次方程式，並且將用以表示各軸於各個粗插值點21上的軸轉矩資訊的計算式分別轉換為前述的第三二次方程式以及第四二次方程式(步驟S54)。
具體地，電子裝置於本實施例的逆向掃描程序中，同樣將以表示各軸於各個粗插值點21上的軸加速度資訊的計算式分別轉換為前述第一二次方程式：
Figure 110107038-A0305-02-0022-132
(M SS,I ,M s,i )f 2+
Figure 110107038-A0305-02-0022-133
(M s,i ,
Figure 110107038-A0305-02-0022-134
)
Figure 110107038-A0305-02-0022-135
0及第二二次方程式：
Figure 110107038-A0305-02-0022-136
(M SS,i ,M s,i )f 2+
Figure 110107038-A0305-02-0022-137
(M s,i ,
Figure 110107038-A0305-02-0022-138
)
Figure 110107038-A0305-02-0022-139
0，並且將用以表示各軸於各個粗插值點21上的軸轉矩資訊分別轉換為前述第三二次方程式：a p,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b p,τ (F v,i ,M s,i )f+c p,τ (F c,i ,M s,i ,d ij ,τ max,i )
Figure 110107038-A0305-02-0022-140
0及第四二次方程式：a n,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b n,τ (F v,i ,M s,i )f+c n,τ (F c,i ,M s,i ,d ij ,τ max,i )
Figure 110107038-A0305-02-0022-141
0。
與順向掃描程序相同，於步驟S54後，電子裝置分別對各軸於符合逆向掃描條件的各個粗插值點21上的第一二次方程式以及第二二次方程式進行整理，以藉由方程式求解而獲得對應的加速度交集係數以及加速度聯集係數，並且分別對各軸於各個粗插值點21上的第三二次方程式以及第四二次方程式進行整理，以藉由方程式求解而獲得對應的轉矩交集係數以及轉矩聯集係數(步驟S56)。具體的求解方式如後面之圖7所示。
與順向掃描程序相似，於步驟S56後，電子裝置將這些加速度交集係數及加速度聯集係數分別帶入對應的第一二次方程式以及第二二次方程式中進行求解，以分別獲得各軸於各個粗插值點21上的逆向加速度交集上界、逆向加速度交集下界、逆向加速度聯集上界以及逆向加速度聯集下界，並且將這些轉矩交集係數及轉矩聯集係數分別帶入對應的第三二次方程式以及第四二次方程式以進行求解，以分別獲得各軸於各個粗插值點21上的逆向轉矩交集上界、逆向轉矩交集下界、逆向轉矩聯集上界及逆向轉矩聯集下界(步驟S58)。具體的求解方式如後面之圖8所示。
步驟S58後，電子裝置可基於所獲得的這些逆向加速度交集上界、逆向加速度交集下界、逆向加速度聯集上界、逆向加速度聯集下界、逆向轉矩交集上界、逆向轉矩交集下界、逆向轉矩聯集上界及逆向轉矩聯集下界，分別計算各軸於各個粗插值點21上的交集上界U i,in 、交集下界D i,in 、聯集上界U i,un 以及聯集下界D i,un (步驟60)。藉此，電子裝置可以基於這些交集上界、交集下界、聯集上界以及聯集下界來分別建立傳動機構1的進給率於符合逆向掃描條件的各個粗插值點21上的一個逆向解空間(步驟S62)。本實施例 中，逆向掃描程序針對各個粗插值點21所給出的逆向掃描結果42，係分別落在對應的逆向解空間之中。
相似於前述順向掃描程序，電子裝置於上述步驟S62中，可通過所述計算公式來建立進給率在各個粗插值點21上的逆向解空間：
Figure 110107038-A0305-02-0024-93
。如同前文所 述，針對各個粗插值點21，電子裝置先取得第一軸至第i軸之交集空間，並且取得第一至第i軸之聯集空間，最後再對所述交集空間以及所述聯集空間取其交集，藉此建立傳動機構1的進給率在各個粗插值點21上的逆向解空間。
於步驟S50以及步驟S62後，電子裝置獲得了傳動機構1於運動軌跡2中的各個粗插值點21上的初始進給率上限3、順向掃描結果41(若存在)以及逆向掃描結果42(若存在)，因此，可基於前述圖2的步驟S26來決定傳動機構1於各個粗插值點21上的最終進給率43。
本發明中，所述初始進給率上限3、順向掃描結果41以及逆向掃描結果42皆是基於各軸的各項物理限制進行計算所獲得的。並且如前文所述，電子裝置在步驟S26中是將初始進給率上限3、順向掃描結果41以及逆向掃描結果42中最小者做為最終進給率43。因此，當傳動機構1以小於或等於各個粗插值點21所對應的最終進給率43的進給率來進行移動時，可以確保各軸的運轉皆不會超出其各自的物理限制。
更甚者，藉由本發明的規劃方法，傳動機構1於移動中的任一時間點，皆會有至少一軸的至少一個物理量運轉於其物理上限。如此一來，可以在不損壞傳動機構1中的任何一個軸的情況下，讓傳動機構1達到最佳的移動速度，進而得到較佳的加工效率。
續請參閱圖7，為本發明的係數計算流程圖的第一具體實施例。圖7用以說明電子裝置在執行順向掃描程序時(即，前述圖6A的步驟S44)，如何求解各軸於各個粗插值點21上的二次方程式的加速度交集係數、加速度聯集係數、轉矩交集係數以及轉矩聯集係數。並且，圖7同時可用以說明電子裝置在執行逆向掃描程序時(即，前述圖6B的步驟S56)，如何求解各軸於各個粗插值點21上的二次方程式的加速度交集係數、加速度聯集係數、轉矩交集係數以及轉矩聯集係數。
如前文所述，由於逆向掃描程序是以與順向掃描程序相反的方向執行相同的動作，因此下面將配合圖7的內容進行一次的說明，且該求解方式同時適用於順向掃描程序以及逆向掃描程序。
如圖7所示，於將傳動機構1的工具11設定為等加速度並且獲得了各軸於各個粗插值點21上的第一、第二、第三及第四二次方程式後，電子裝置首先取得傳動機構1內部的複數軸的其中之一(例如第i軸)於多個粗插值點21的其中一(例如第j個粗插值點21，即為當前掃描的粗插值點21)上的複數參數，並且將這些參數輸入這個軸於這個粗插值點21上的第一、第二、第三及第四二次方程式(步驟S70)。
於一實施例中，所述參數是與求解這些二次方程式相關的參數，例如前文中所述之M s,i 、M ss,i 、d ij 、C ind 、F c 、F v 、
Figure 110107038-A0305-02-0025-142
、τ max 等，但並不以此為限。
於步驟S70中將上述參數分別帶入所述第一與第二二次方程式後，電子裝置即可整理此軸(第i軸)於當前掃描的粗插值點21(第j個粗插值點21)上的加速度交集係數以及加速度聯集係數(步驟S72)。而於步驟S70中將 上述參數分別帶入所述第三與第四二次方程式後，電子裝置即可整理此軸(第i軸)於當前掃描的粗插值點21(第j個粗插值點21)上的轉矩交集係數以及轉矩聯集係數(步驟S74)。
二次方程式的求解方法為本技術領域中的常用技術手段，於此不另做說明。
於圖7的實施例中，電子裝置係先整理加速度交集係數與加速度聯集係數，再整理轉矩交集係數與轉矩聯集係數。惟，於其他實施例中，電子裝置亦可先整理轉矩交集係數與轉矩聯集係數，再整理加速度交集係數與加速度聯集係數，或是藉由多工處理來同時整理上述所有係數，而不以圖7中所示之順序為限。
步驟S72與步驟S74後，電子裝置判斷是否所有軸在當前掃描的粗插值點21上的係數都已經整理完畢(步驟S76)。若於步驟S76中判斷為否，則電子裝置取得下一個軸於當前掃描的粗插值點21上的相關參數，即，i+1(步驟S78)。並且，電子裝置再重覆執行上述步驟S70至步驟S74，以接著整理下一個軸於此粗插值點21上的加速度交集係數、加速度聯集係數、轉矩交集係數以及轉矩聯集係數。
當傳動機構1中的所有軸於當前掃描的粗插值點21上的係數都整理完畢(即，於步驟S76中判斷為是)時，電子裝置進一步判斷運動軌跡2中所有符合掃描條件的粗插值點21是否皆已掃描完畢(步驟S80)。若於步驟S80中判斷為否，則電子裝置接著取得符合掃描條件(即，初始進給率上限3大於當前掃描的粗插值點21的初始進給率上限3)的粗插值點21，即，j+1(步驟S82)。並且，電子裝置再重覆執行上述步驟S70至步驟S78，以整理各軸於下 一個掃描的粗插值點21上的加速度交集係數、加速度聯集係數、轉矩交集係數以及轉矩聯集係數。
若於步驟S80中判斷為是，代表所有軸於所有符合掃描條件(包括順向掃描條件與逆向掃描條件)的粗插值點21上的所有二次方程式的加速度交集係數、加速度聯集係數、轉矩交集係數以及轉矩聯集係數皆已整理完畢。因此，電子裝置可進一步輸出各軸於各個粗插值點21上的加速度交集係數
Figure 110107038-A0305-02-0027-143
與
Figure 110107038-A0305-02-0027-144
、加速度聯集係數
Figure 110107038-A0305-02-0027-145
與
Figure 110107038-A0305-02-0027-146
、轉矩交集係數a p,τ 、b p,τ 與c p,τ 以及轉矩聯集係數a n,τ 、b n,τ 與c n,τ (步驟S84)。通過這些係數，電子裝置可以對各軸相對於各個粗插值點21的二次方程式進行求解。
續請參閱圖8，為本發明的上、下界計算流程圖的第一具體實施例。圖8用以說明電子裝置在執行順向掃描程序時(前述圖6A的步驟S46)，如何求解各軸於各個粗插值點21上的加速度(或稱順向加速度)的交集上下界與聯集上下界以及轉矩(或稱順向轉矩)的交集上下界與聯集上下界。並且，圖8同時可用以說明電子裝置在執行逆向掃描程序時(前述圖6B的步驟S58)，如何求解各軸於各個粗插值點21上的加速度(或稱逆向加速度)的交集上下界與聯集上下界以及轉矩(或稱逆向轉矩)的交集上下界與聯集上下界。
如前文所述，由於逆向掃描程序是以與順向掃描程序相反的方向執行相同的動作，因此下面將配合圖8的內容進行一次的說明，且該求解方式同時適用於順向掃描程序以及逆向掃描程序。
如圖8所示，於獲得了所有軸於所有符合掃描條件的粗插值點21上的各項係數後，電子裝置首先取得複數軸的其中之一(例如第i軸)於多個粗插值點21的其中之一(例如第j個粗插值點21，即為當前掃描的粗插值點 21)上的加速度交集係數、加速度聯集係數、轉矩交集係數及轉矩聯集係數，並且將這些係數分別輸入這個軸於這個粗插值點21上的所述第一、第二、第三及第四二次方程式(步驟S90)。
於獲得了這些二次方程式的對應係數後，電子裝置即可對所述第一二次方程式以及第二二次方程式進行求解，以獲得這個軸(第i軸)於當前掃描的粗插值點21(第j個粗插值點21)上的加速度交集上界、加速度交集下界、加速度聯集上界及加速度聯集下界(步驟S92)。並且，電子裝置可對所述第三二次方程式以及第四二次方程式進行求解，以獲得這個軸(第i軸)於當前掃描的粗插值點21(第j粗插值點21)上的轉矩交集上界、轉矩交集下界、轉矩聯集上界及轉矩聯集下界(步驟S94)。
於圖8的實施例中，電子裝置係先求解加速度的交集上下界與聯集上下界後，再求解轉矩的交集上下界與聯集上下界。惟，於其他實施例中，電子裝置亦可先求解轉矩的交集上下界與聯集上下界後，再求解加速度的交集上下界與聯集上下界，或是藉由多工處理來同時求解加速度以及轉矩的交集上下界與聯集上下界，而不以圖8中所示之順序為限。
於步驟S92與步驟S94後，電子裝置判斷是否所有軸在當前掃描的粗插值點21上的加速度的交集上下界與聯集上下界以及轉矩的交集上下界與聯集上下界都已求解完畢(步驟S96)。若於步驟S96中判斷為否，則電子裝置取得下一個軸於當前掃描的粗插值點21上的對應係數，即，i+1(步驟S98)。並且，電子裝置再重覆執行上述步驟S90至步驟S94，以求解下一個軸於此粗插值點21上的加速度交集上界、加速度交集下界、加速度聯集上界、 加速度聯集下界、轉矩交集上界、轉矩交集下界、轉矩聯集上界及轉矩聯集下界。
當傳動機構中的所有軸於當前掃描的粗插值點21上的交集上下界及聯集上下界都求解完畢時(即，於步驟S96中判斷為是)，電子裝置進一步判斷運動軌跡2中所有符合掃描條件(包括順向掃描條件與逆向掃描條件)的粗插值點21是否皆已掃描完畢(步驟S100)。若於步驟S100中判斷為否，則電子裝置接著掃描下一個符合掃描條件(即，初始進給率上限3大於當前掃描的粗插值點21的初始進給率上限3)的粗插值點21，即，j+1(步驟S102)。並且，電子裝置再重覆執行上述步驟S90至步驟S98，以求解各軸於下一個掃描的粗插值點21上的加速度交集上界、加速度交集下界、加速度聯集上界、加速度聯集下界、轉矩交集上界、轉矩交集下界、轉矩聯集上界及轉矩聯集下界。
若於步驟S100中判斷為是，代表所有軸於所有符合掃描條件(包括順向掃描條件與逆向掃描條件)的粗插值點21上的加速度交集上下界、加速度聯集上下界、轉矩交集上界下及轉矩聯集上下界皆已求解完畢。據此，電子裝置可進一步輸出各軸於各個粗插值點21上的加速度交集上界、加速度交集下界、加速度聯集上界、加速度聯集下界、轉矩交集上界、轉矩交集下界、轉矩聯集上界及轉矩聯集下界(步驟S104)。
於步驟S104後，電子裝置即可於前述圖6A的步驟S48以及圖6B步驟S60中，分別計算各軸於各個粗插值點21上的交集上界U i,in 、交集下界D i,in 、聯集上界U i,un 以及聯集下界D i,un ，進而建立傳動機構1的進給率於各個粗插值點21上的順向解空間以及逆向解空間。
如前文所述，本發明的技術方案基於各軸相對於各個粗插值點21的軸位置資訊、軸速度資訊、軸加速度資訊以及軸轉矩資訊來計算各個粗插值點21的初始進給率上限3,並且再通過雙向掃描演算法來對初始進給率上限3進行修正，以獲得各個粗插值點21的最終進給率43。若傳動機構1基於計算所得的最終進給率43進行移動(即，令傳動機構1的進給率最高不超過所述最終進給率43)，將可以在所有軸的各項物理量皆不會超出其物理限制的情況下，達到最快的移動速度以及最佳的加工效率。
值得說明的是，只要傳動機構1實際工作時的進給率低於以雙向掃描法所求得的進給率限制，就一定有與其對應的三維空間加速度之限制，且其總和量值不會超過各軸的物理限制。本發明先假設傳動機構1為等加速度後，再執行雙向掃描法以計算最終進給率43的技術手段，至少具有下列兩點優勢：一、必然存在順向掃描/逆向掃描的解，不會有無解的狀況發生；二、進給率只要低於所述最終進給率43，則只要修正二次的fold(即，上一個被掃描的粗插值點21的進給率)後，再重新解一次上述的二次不等式，則必可安全通過，期間不必再重新計算二次不等式中的各項係數，亦不用執行任何的疊代，不會浪費計算資源。
值得一提的是，相關技術中所採用的技術方案皆不保證只要傳動機構1實際工作時採用的進給率低於所規劃的進給率上限，就不會有任一軸超出其軸物理限制的問題。然而，藉由本發明的技術方案，可以確保上述情況不會發生。
再者，通過上述規劃方法，只要初始進給率上限3大於零，則電子裝置在執行雙向掃描演算法時就不需要尋找切換點，並且不會在計算後 發現無解而找不到最終進給率43的問題。更具體地，在任何加工程序中，只要傳動機構有運動，進給率就一定會大於零。換句話說，採用本發明的上述技術方案，則絕不會有無解的問題發生。
參閱圖9，為本發明的進給率修正的示意圖。如圖9(a)及(b)所示，若採用相關技術的規劃方法來規劃傳動機構1的進給率，則當傳動機構1基於給定的進給率進行移動時，可能會有部分的軸的運轉超出了其物理限制。於圖9(b)的實施例中，是以第i軸的速度超出了其速度上限為例。
於相關技術中，若發現傳動機構1中某一軸的運轉超出了其物理限制，則電子裝置一般只能夠如圖9(c)所示，降低傳動機構1整體的進給率。如此一來，即如圖9(d)所示，雖然第i軸的速度不會超出其速度上限，但因為轉動速度成比率下降，因此大幅延長了傳動機構1的移動時間，進而降低了加工效率。
如圖9(e)所示，本發明的技術方案是基於各軸的各項物理上限來計算傳動機構1於各個粗插值點21上的進給率，故相較於相關技術中降低整體進給率的規劃方法，藉由本發明的規劃方法所給出的進給率，會因為傳動機構1所在的位置不同而有較大幅度的變化。因此，如圖9(f)所示，當傳動機構1基於圖9(e)所示的進給率來進行移動與加工時，不但可以確保第i軸的速度不會超過其速度上限，並且相較於相關技術的規劃方法，可以有效提高傳動機構1的移動速度，進而提高了加工效率。
為證明上述規劃方法的有效性，本發明具體將此規劃方法應用於真實的傳動機構中進行模擬。具體地，以下所述的模擬對象為一台六軸標準型機械手臂，並且此機械手臂的實際MDH參數及六軸的物理極限參數如下表1 及表2所示。其中，MDH參數為本技術領域中描述機械手臂機構參數的標準方法之一，於此不再贅述。
Figure 110107038-A0305-02-0032-55
Figure 110107038-A0305-02-0032-56
於本次模擬中，係以此機械手臂的末端位置以斜直線移動作為範例，其運動軌跡資訊如下表3所示。並且，該運動軌跡藉由本發明的規劃方法邏輯切分為一千個粗插值點，並且每一個粗插值點間為等距離。
Figure 110107038-A0305-02-0033-57
請同時參閱圖10，為本發明的各軸表現的示意圖。依據上述本發明的規劃方法，用以控制機械手臂的電子裝置可基於上述表1至表3的資訊來規劃機械手臂於預定的運動軌跡上的進給率以及加速度。並且，圖10同時還記錄了進給率規劃與限制的情形，其呈現了機械手臂實際的進給率與雙向掃描法掃描出的進給率限制的比較結果。
請同時參閱圖11A、圖11B及圖11C，分別為本發明的各軸速度表現、各軸加速度表現以及各軸轉矩表現的示意圖。
如圖11A、圖11B及圖11C所示，當機械手臂依照由本發明的規劃方法所給出的進給率/加速度來進行移動時，機械手臂中的所有軸的各項物理量皆不會超過其各自的物理限制。
更值得一提的是，如圖11A、圖11B及圖11C所示，於機械手臂移動過程中的任一時間點中，必然有某一軸的某一項物理量頂在其物理限制上。例如，在0-2600微秒時，第一軸的馬達轉矩頂在其轉矩上限(如圖11C所示)；在2600-2800微秒時，第二軸的馬達速度頂在其速度上限(如圖11A所示)；在2800-3100微秒時，第三軸的馬達轉矩頂在其轉矩下限(如圖11C所 示)；在3100-3200微秒時，第二軸的馬達加速度頂在其加速度下限(如圖11B所示)；在3200-3400微秒時，第二軸的馬達轉矩頂在其轉矩下限(如圖11C所示)。
由上述圖11A至圖11C的模擬實驗結果可以看出，基於本發明的規劃方法所給出的進給率，傳動機構在沿著預定的運動軌跡進行移動時，確實可以避免各個軸的轉動超出其既有的物理限制，進而可以有效避免這些軸的損壞。
並且，如前文所述，本發明的規劃方法除了可避免傳動機構中各軸的轉動超出其各自的物理限制之外，因為在任一時間點皆有至少一個軸的某一項物理量頂在其物理限制上，因此可以令傳動機構達到最快的移動速度。
以上所述僅為本發明之較佳具體實例，非因此即侷限本發明之專利範圍，故舉凡運用本發明內容所為之等效變化，均同理皆包含於本發明之範圍內，合予陳明。
S10~S28:規劃步驟

Claims (8)

  1. 一種基於軸物理限制的傳動機構進給率規劃方法，應用於沿著一運動軌跡移動的一傳動機構，其中該傳動機構具有一或多個軸以及一末端位置，該規劃方法包括：a)於該運動軌跡上設置複數粗插值點(Rough-Interpolation Point)，並取得各該粗插值點的一三維空間資訊；b)基於各該粗插值點的該三維空間資訊、一逆向運動學函式、一微分手段以及一動態方程式計算各該軸在各該粗插值點上的一軸位置資訊、一軸速度資訊、一軸加速度資訊及一軸轉矩資訊；c)基於各該軸的該軸速度資訊、該軸加速度資訊及該軸轉矩資訊計算該末端位置於各該粗插值點上的一初始進給率上限；d)基於各該軸的該軸加速度資訊及該軸轉矩資訊執行一雙向掃描演算法，以更新各該粗插值點上的該初始進給率上限，並得到一順向掃描結果以及一逆向掃描結果；e)取得各該粗插值點上的該初始進給率上限、該順向掃描結果及該逆向掃描結果中最小者，做為該末端位置於各該粗插值點上的一最終進給率；及f)控制該末端位置基於各該粗插值點的該最終進給率沿著該運動軌跡進行移動。
  2. 如請求項1所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟b包括下列步驟：b1)通過該逆向運動學函式計算各該粗插值點的該三維空間資訊，以獲得各該軸於各該粗插值點上的該軸位置資訊，其中該軸位置資訊為q i = M i (X,Y,Z,α,β,γ)=M i (X(s 1),Y(s 1),Z(s 1),α(s 2),β(s 2),γ(s 2))，其中s1為該運動軌跡的路徑長度，s2為該傳動機構的轉動角度總和；b2)對該些軸位置資訊執行該微分手段，以獲得各該軸於各該粗插值點上的 該軸速度資訊，其中該軸速度資訊為
    Figure 110107038-A0305-02-0037-58
    ，其中Mi為該逆向 運動學函式，i為該軸的編號，M為未限定軸的該逆向運動學函式，A1為該運動軌跡的路徑長度、該軸位置資訊中的三維空間的距離總量或該軸位置資訊中的三維空間的轉動角度總量，Ms為該逆向運動學函式對A1進行微分，f為該末端位置的一進給率；b3)對該些軸速度資訊執行該微分手段，以獲得各該軸於各該粗插值點上的 該軸加速度資訊，其中該軸加速度資訊為
    Figure 110107038-A0305-02-0037-96
    ，其中 Mss為該逆向運動學函式對A1進行二次微分，
    Figure 110107038-A0305-02-0037-147
    為該末端位置的一加速度；及b4)將該些軸速度資訊及該些軸加速度資訊帶入該動態方程式，以獲得各該 軸於各該粗插值點上的該軸轉矩資訊，其中該動態方程式為
    Figure 110107038-A0305-02-0037-62
    Figure 110107038-A0305-02-0037-63
    ，其中τ為該軸轉矩資訊，q為該軸位置 資訊，
    Figure 110107038-A0305-02-0037-148
    為該軸速度資訊，
    Figure 110107038-A0305-02-0037-149
    為該軸加速度資訊，D(q)為一慣性力矩陣，C(q,
    Figure 110107038-A0305-02-0037-150
    )為一科氏力與向心力矩陣(Coriolis Force and Centripetal Matrix)， F c sign(
    Figure 110107038-A0305-02-0037-151
    )+F v
    Figure 110107038-A0305-02-0037-152
    為一動摩擦力向量，該軸轉矩資訊為
    Figure 110107038-A0305-02-0037-64
    Figure 110107038-A0305-02-0037-60
    Figure 110107038-A0305-02-0037-61
    ，其中d為該慣量力矩陣中的元素，Cind為該科氏 力與向心力矩陣中與該軸速度相關的元素。
  3. 如請求項2所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟c是將該末端位置的該加速度設定為零、基於該些軸速度資訊分別計算各該粗插值點上的一第一進給率上限、基於該些軸加速度資訊分別計算各該粗插值點上的一第二進給率上限、基於該些軸轉矩資訊分別計算各該粗插值點上的一第三進給率上限，並且以各該粗插值點上的該第一進給率上限、該第二進給率上限及該第三進給率上限中最小者做為各該粗插值點的該初始進 給率上限，其中該第一進給率上限為
    Figure 110107038-A0305-02-0038-65
    ，該第二進給率上限為
    Figure 110107038-A0305-02-0038-67
    ，該第三進給率上限為f 3 max = f min (τ max,i ,M s,i ,d ij ,C ind,i ,F c,i ,F v,i )，其中i=1~M，M為該一或多軸的總數。
  4. 如請求項2所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟d是將該末端位置設定為一等加速度，並基於該些軸加速度資訊以及該些軸轉矩資訊分別執行該雙向掃描演算法中的一順向掃描程序以及一逆向掃描程序，以得到該順向掃描結果及該逆向掃描結果。
  5. 如請求項4所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟d包括：d1)執行該順向掃描程序，其中該順向掃描程序是基於由該運動軌跡的一起點位置朝向一終點位置的方向掃描該運動軌跡中的複數個該粗插值點，其中下一個被掃描的該粗插值點的該初始進給率上限大於當前被掃描的該粗插值點的該初始進給率上限；d2)該步驟d1後，設定該末端位置為等加速度以將該些軸加速度資訊分別轉換為一第一二次方程式及一第二二次方程式，並將該些軸轉矩資訊分別轉 換為一第三二次方程式及一第四二次方程式，其中該第一二次方程式為
    Figure 110107038-A0305-02-0039-153
    (M SS,I ,M s,i )f 2+
    Figure 110107038-A0305-02-0039-154
    (M s,i ,
    Figure 110107038-A0305-02-0039-155
    )
    Figure 110107038-A0305-02-0039-156
    0，該第二二次方程式為
    Figure 110107038-A0305-02-0039-160
    (M SS,i ,M s,i )f 2+
    Figure 110107038-A0305-02-0039-159
    (M s,i ,
    Figure 110107038-A0305-02-0039-158
    )
    Figure 110107038-A0305-02-0039-157
    0，該第三二次方程式為a p,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b p,τ (F v,i ,M s,i )f+c p,τ (F c,i ,M s,i ,d ij ,τ max,i )
    Figure 110107038-A0305-02-0039-161
    0，該第四二次方程式為a n,τ (d ij ,C ind,i ,M ss,i ,M s,i )f 2+b n,τ (F v,i ,M s,i )f+c n,τ (F c,i ,M s,i ,d ij ,τ max,i )
    Figure 110107038-A0305-02-0039-163
    0，其中
    Figure 110107038-A0305-02-0039-164
    與
    Figure 110107038-A0305-02-0039-165
    為該加速度交集係數，
    Figure 110107038-A0305-02-0039-166
    與
    Figure 110107038-A0305-02-0039-167
    為該加速度聯集係數，a p,τ 、b p,τ 與c p,τ 為該轉矩交集係數，a n,τ 、b n,τ 與 c n,τ 為該轉矩聯集係數，其中
    Figure 110107038-A0305-02-0039-72
    ，
    Figure 110107038-A0305-02-0039-75
    ，
    Figure 110107038-A0305-02-0039-81
    Figure 110107038-A0305-02-0039-77
    ，b p,τ =F v,i M s,i ，
    Figure 110107038-A0305-02-0039-78
    Figure 110107038-A0305-02-0039-79
    ，a n,τ =-a p,τ ，b n,τ = -b p,τ ，
    Figure 110107038-A0305-02-0039-80
    ，DOF為該 一或多軸的總數；d3)該步驟d2後，分別對該些第一二次方程式及該些第二二次方程式進行整理以獲得對應的一加速度交集係數及一加速度聯集係數，並且分別對該些第三二次方程式及該些第四二次方程式進行整理以獲得對應的一轉矩交集係數及一轉矩聯集係數；d4)該步驟d3後，分別將該些加速度交集係數及該些加速度聯集係數帶入對應的各該第一二次方程式及各該第二二次方程式並進行求解，以分別獲得各該軸於各該粗插值點上的一順向加速度交集上界、一順向加速度交集下界、一順向加速度聯集上界及一順向加速度聯集下界，並且分別將該些轉矩交集係數及該些轉矩聯集係數帶入對應的各該第三二次方程式及各該第 四二次方程式並進行求解，以分別獲得各該軸於各該粗插值點上的一順向轉矩交集上界、一順向轉矩交集下界、一順向轉矩聯集上界及一順向轉矩聯集下界；d5)該步驟d4後，基於該些順向加速度交集上界、該些順向加速度交集下界、該些順向加速度聯集上界、該些順向加速度聯集下界、該些順向轉矩交集上界、該些順向轉矩交集下界、該些順向轉矩聯集上界及該些順向轉矩聯集下界分別計算各該軸於各該粗插值點上的一交集上界、一交集下界、一聯集上界及一聯集下界；d6)該步驟d5後，基於該些交集上界、該些交集下界、該些聯集上界及該些聯集下界，通過一第一計算公式分別建立該進給率於各該粗插值點上的一順向解空間，其中各該順向掃描結果分別落在對應的該順向解空間之中， 其中該第一計算公式為：
    Figure 110107038-A0305-02-0040-82
    Figure 110107038-A0305-02-0040-83
    ，其中U i,in 為該交集上界，D i,in 為該交集下界，U i,un 為該 聯集上界，D i,un 為該聯集下界，i為該一或多軸的數量；d7)執行該逆向掃描程序，其中該逆向掃描程序是基於由該運動軌跡的該終點位置朝向該起點位置的方向掃描該運動軌跡中的複數個該粗插值點，其中下一個被掃描的該粗插值點的該初始進給率上限大於當前被掃描的該粗插值點的該初始進給率上限；d8)該步驟d7後，設定該末端位置為等加速度以將該些軸加速度資訊分別轉換為該第一二次方程式及該第二二次方程式，並將該些軸轉矩資訊分別轉換為該第三二次方程式及該第四二次方程式； d9)該步驟d8後，分別對該些第一二次方程式及該些第二二次方程式進行整理以獲得對應的該加速度交集係數及該加速度聯集係數，並且分別對該些第三二次方程式及該些第四二次方程式進行整理以獲得對應的該轉矩交集係數及該轉矩聯集係數；d10)該步驟d9後，將該些加速度交集係數及該些加速度聯集係數分別帶入對應的各該第一二次方程式及各該第二二次方程式並進行求解，以分別獲得各該軸於各該粗插值點上的一逆向加速度交集上界、一逆向加速度交集下界、一逆向加速度聯集上界及一逆向加速度聯集下界，並且分別將該些轉矩交集係數及該些轉矩聯集係數帶入對應的各該第三二次方程式及各該第四二次方程式並進行求解，以分別獲得各該軸於各該粗插值點上的一逆向轉矩交集上界、一逆向轉矩交集下界、一逆向轉矩聯集上界及一逆向轉矩聯集下界；d11)該步驟d10後，基於該些逆向加速度交集上界、該些逆向加速度交集下界、該些逆向加速度聯集上界、該些逆向加速度聯集下界、該些逆向轉矩交集上界、該些逆向轉矩交集下界、該些逆向轉矩聯集上界及該些逆向轉矩聯集下界分別計算各該軸於各該粗插值點上的該交集上界、該交集下界、該聯集上界及該聯集下界；及d12)該步驟d11後，基於該些交集上界、該些交集下界、該些聯集上界及該些聯集下界，通過該第一計算公式分別建立該進給率於各該粗插值點上的一逆向解空間，其中各該逆向掃描結果分別落在對應的該逆向解空間之中。
  6. 如請求項5所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟d2及該步驟d8通過一第二計算公式來計算該末端位置的該加 速度，該第二計算公式為
    Figure 110107038-A0305-02-0042-85
    ，其中
    Figure 110107038-A0305-02-0042-168
    為該加速度，fold為上一個被掃描的 該粗插值點的該進給率，s為上一個被掃描的該粗插值點至當前被掃描的該粗插值點間的距離。
  7. 如請求項5所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟d3包括：d31)將該一或多軸的其中之一於當前掃描的該粗插值點上的複數參數輸入該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式；d32)整理該加速度交集係數與該加速度聯集係數；d33)整理該轉矩交集係數與該轉矩聯集係數；d34)於該一或多軸皆整理完畢前重覆執行該步驟d31至該步驟d33，以整理下一個軸的該加速度交集係數、該加速度聯集係數、該轉矩交集係數與該轉矩聯集係數；d35)於所有該粗插值點皆掃描完畢前重覆執行該步驟d31至該步驟d34，以整理該一或多軸相對於下一個該粗插值點的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式的該加速度交集係數、該加速度聯集係數、該轉矩交集係數與該轉矩聯集係數；及d36)於所有該粗插值點皆掃描完畢後，輸出各該軸相對於各該粗插值點的該加速度交集係數、該加速度聯集係數、該轉矩交集係數及該轉矩聯集係數； 其中，該步驟d9包括：d91)將該一或多軸的其中之一於當前掃描的該粗插值點上的複數參數輸入該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式；d92)整理該加速度交集係數與該加速度聯集係數；d93)整理該轉矩交集係數與該轉矩聯集係數；d94)於該一或多軸皆整理完畢前重覆執行該步驟d91至該步驟d93，以整理下一個軸的該加速度交集係數、該加速度聯集係數、該轉矩交集係數與該轉矩聯集係數；d95)於所有該粗插值點皆掃描完畢前重覆執行該步驟d91至該步驟d94，以整理該一或多軸相對於下一個該粗插值點的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式的該加速度交集係數、該加速度聯集係數、該轉矩交集係數與該轉矩聯集係數；及d96)於所有該粗插值點皆掃描完畢後，輸出各該軸相對於各該粗插值點的該加速度交集係數、該加速度聯集係數、該轉矩交集係數及該轉矩聯集係數。
  8. 如請求項7所述的基於軸物理限制的傳動機構進給率規劃方法，其中該步驟d4包括：d41)將該一或多軸的其中之一於當前掃描的該粗插值點上的該加速度交集係數、該加速度聯集係數、該轉矩交集係數及該轉矩聯集係數輸入對應的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式； d42)求解該第一二次方程式及該第二二次方程式以獲得該順向加速度交集上界、該順向加速度交集下界、該順向加速度聯集上界及該順向加速度聯集下界；d44)求解該第三二次方程式及該第四二次方程式以獲得該順向轉矩交集上界、該順向轉矩交集下界、該順向轉矩聯集上界及該順向轉矩聯集下界；d45)於該一或多軸的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式皆求解完畢前重覆執行該步驟d41至該步驟d44，以求解下一個軸於當前掃描的該粗插值點上的該順向加速度交集上界、該順向加速度交集下界、該順向加速度聯集上界、該順向加速度聯集下界、該順向轉矩交集上界、該順向轉矩交集下界、該順向轉矩聯集上界及該順向轉矩聯集下界；d46)於所有該粗插值點皆掃描完畢前重覆執行該步驟d41至該步驟d45，以求解該一或多軸於下一個該粗插值點上的該順向加速度交集上界、該順向加速度交集下界、該順向加速度聯集上界、該順向加速度聯集下界、該順向轉矩交集上界、該順向轉矩交集下界、該順向轉矩聯集上界及該順向轉矩聯集下界；及d47)於所有該粗插值點皆掃描完畢後，輸出各該軸於各該粗插值點上的該順向加速度交集上界、該順向加速度交集下界、該順向加速度聯集上界、該順向加速度聯集下界、該順向轉矩交集上界、該順向轉矩交集下界、該順向轉矩聯集上界及該順向轉矩聯集下界；其中，該步驟d10包括： d101)將該一或多軸的其中之一於當前掃描的該粗插值點上的該加速度交集係數、該加速度聯集係數、該轉矩交集係數及該轉矩聯集係數輸入對應的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式；d102)求解該第一二次方程式及該第二二次方程式以獲得該逆向加速度交集上界、該逆向加速度交集下界、該逆向加速度聯集上界及該逆向加速度聯集下界；d104)求解該第三二次方程式及該第四二次方程式以獲得該逆向轉矩交集上界、該逆向轉矩交集下界、該逆向轉矩聯集上界及該逆向轉矩聯集下界；d105)於該一或多軸的該第一二次方程式、該第二二次方程式、該第三二次方程式及該第四二次方程式皆求解完畢前重覆執行該步驟d101至該步驟d104，以求解下一個軸於當前掃描的該粗插值點上的該逆向加速度交集上界、該逆向加速度交集下界、該逆向加速度聯集上界、該逆向加速度聯集下界、該逆向轉矩交集上界、該逆向轉矩交集下界、該逆向轉矩聯集上界及該逆向轉矩聯集下界；d106)於所有該粗插值點皆掃描完畢前重覆執行該步驟d101至該步驟d105，以求解該一或多軸於下一個該粗插值點上的該逆向加速度交集上界、該逆向加速度交集下界、該逆向加速度聯集上界、該逆向加速度聯集下界、該逆向轉矩交集上界、該逆向轉矩交集下界、該逆向轉矩聯集上界及該逆向轉矩聯集下界；及d107)於所有該粗插值點皆掃描後，輸出各該軸於各該粗插值點上的該逆向加速度交集上界、該逆向加速度交集下界、該逆向加速度聯集上界、該逆 向加速度聯集下界、該逆向轉矩交集上界、該逆向轉矩交集下界、該逆向轉矩聯集上界及該逆向轉矩聯集下界。
TW110107038A 2020-11-13 2021-02-26 基於軸物理限制的傳動機構進給率規劃方法 TWI766598B (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
US202063113595P 2020-11-13 2020-11-13
US63/113,595 2020-11-13

Publications (2)

Publication Number Publication Date
TW202219669A TW202219669A (zh) 2022-05-16
TWI766598B true TWI766598B (zh) 2022-06-01

Family

ID=81491527

Family Applications (1)

Application Number Title Priority Date Filing Date
TW110107038A TWI766598B (zh) 2020-11-13 2021-02-26 基於軸物理限制的傳動機構進給率規劃方法

Country Status (2)

Country Link
CN (1) CN114488953B (zh)
TW (1) TWI766598B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6757583B2 (en) * 2002-06-27 2004-06-29 Joe Giamona Interpolated motion control over a serial network
TW201220010A (en) * 2010-11-02 2012-05-16 Syntec Inc Parameter learning controller in a machine device and learning method thereof
TW201344383A (zh) * 2012-04-27 2013-11-01 Syntec 使用於工具機之加工路徑最佳化方法
US20190021802A1 (en) * 2015-07-01 2019-01-24 Mako Surgical Corp. Robotic Systems And Methods For Controlling A Tool Removing Material From A Workpiece
EP1869531B1 (en) * 2005-03-23 2020-04-22 Hurco Companies Inc. Method of tolerance-based trajectory planning
US10768602B2 (en) * 2015-02-11 2020-09-08 Shenzhen A&E Intelligent Technology Institute Co., Ltd. Curve velocity planning method, device, and numerical control machining route data processing method thereof

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0682287B2 (ja) * 1986-10-23 1994-10-19 新明和工業株式会社 ロボツトの走査制御方法
WO1996010221A1 (de) * 1994-09-29 1996-04-04 Siemens Aktiengesellschaft Verfahren zur konturfehlerfreien begrenzung von achsbeschleunigungen
JP3034843B2 (ja) * 1998-05-28 2000-04-17 ファナック株式会社 加工機の制御装置
KR100871456B1 (ko) * 2007-02-28 2008-12-03 부산대학교 산학협력단 5축 밀링가공 시간 예측방법
CN102707671A (zh) * 2012-05-29 2012-10-03 苏州新代数控设备有限公司 应用于工具机的加工路径最佳化方法
CN103760827B (zh) * 2014-01-10 2016-08-17 大连理工大学 跃度约束的数控加工进给率的离线规划方法
CN106970589B (zh) * 2016-12-06 2019-07-30 大连理工大学 一种减小多轴加工轮廓误差的进给率松弛方法
CN107608313B (zh) * 2017-09-11 2019-09-27 大连理工大学 一种五轴双样条曲线插补速度规划方法
CN110531709B (zh) * 2019-08-15 2020-07-10 华中科技大学 一种分析曲面零件加工误差与进给率关系的方法
CN111240275B (zh) * 2020-01-16 2021-01-22 北京航空航天大学 基于对数几率函数在运动和误差限制下的进给率规划方法
CN111487929B (zh) * 2020-04-17 2021-04-20 中国航发北京航空材料研究院 一种基于双向比例调整的多约束数控加工进给率定制方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6757583B2 (en) * 2002-06-27 2004-06-29 Joe Giamona Interpolated motion control over a serial network
EP1869531B1 (en) * 2005-03-23 2020-04-22 Hurco Companies Inc. Method of tolerance-based trajectory planning
TW201220010A (en) * 2010-11-02 2012-05-16 Syntec Inc Parameter learning controller in a machine device and learning method thereof
TW201344383A (zh) * 2012-04-27 2013-11-01 Syntec 使用於工具機之加工路徑最佳化方法
US10768602B2 (en) * 2015-02-11 2020-09-08 Shenzhen A&E Intelligent Technology Institute Co., Ltd. Curve velocity planning method, device, and numerical control machining route data processing method thereof
US20190021802A1 (en) * 2015-07-01 2019-01-24 Mako Surgical Corp. Robotic Systems And Methods For Controlling A Tool Removing Material From A Workpiece

Also Published As

Publication number Publication date
CN114488953B (zh) 2024-02-27
CN114488953A (zh) 2022-05-13
TW202219669A (zh) 2022-05-16

Similar Documents

Publication Publication Date Title
JP2019517929A (ja) ロボット関節空間におけるポイントツーポイント移動の軌跡計画方法
JP6859756B2 (ja) 制御システム、コントローラ、制御方法、および制御プログラム
CN111553239B (zh) 一种机器人关节视觉伺服控制方法、终端设备及存储介质
WO2020151406A1 (zh) 运动控制方法、装置和系统及存储介质
JPH09265313A (ja) ロボットの速度演算装置、およびロボットの速度演算方法
CN104460525B (zh) 用于构建零件加工方法的方法和系统
Zhang et al. Efficient computation of smooth minimum time trajectory for CNC machining
CN106826839A (zh) 对机器人的控制
CN108608425B (zh) 六轴工业机器人铣削加工离线编程方法及系统
TWI766598B (zh) 基於軸物理限制的傳動機構進給率規劃方法
CN115723137A (zh) 一种基于平面恒定力的柔顺任务控制方法
CN113084792A (zh) 关节奇异区域的确定方法、机器人及存储装置
US10684594B2 (en) Machine learning device, servo motor controller, servo motor control system, and machine learning method
Zhang et al. Dynamics based time-optimal smooth motion planning for the delta robot
EP3912767A1 (en) Robot control device, robot control method, and robot control program
WO2023116129A1 (zh) 一种协作机器人的柔顺力控制方法及系统
CN111515954A (zh) 一种机械臂高质量运动路径生成方法
CN114019908B (zh) 一种螺旋锥齿轮齿廓弧面倒棱控制方法
CN113145907B (zh) 一种基于能量最优机器人铣削加工进给方向优化方法
CN112147893B (zh) 一种基于直纹面空间的五轴铣削刀轴矢量优化方法
TWI541623B (zh) 五軸加工數值控制系統及其數值控制方法
Sun et al. A novel tool path smoothing algorithm of 6R manipulator considering pose-dependent dynamics by designing asymmetrical FIR filters
JP6836540B2 (ja) 情報処理装置及び情報処理方法
Medrano-Hermosillo et al. Forward Kinematics Analysis of 6-DoF Articulated Robot using Screw Theory and Geometric Algebra
CN116483026B (zh) 多模式多类型非对称s型柔性速度曲线双向快速规划方法