TWI688844B

TWI688844B - 用於控制機械設備的控制裝置與方法

Info

Publication number: TWI688844B
Application number: TW107145993A
Authority: TW
Inventors: 陳文泉; 陳立業; 陳金聖
Original assignee: 國立臺北科技大學
Priority date: 2018-12-19
Filing date: 2018-12-19
Publication date: 2020-03-21
Also published as: JP2020102183A; TW202024820A

Abstract

控制裝置包含記憶體以及處理器。記憶體儲存指令。處理器用以存取並執行指令以接收命令佇列，其中命令佇列用以控制機械設備運動以通過轉向角。處理器用以判斷機械設備的變速能力是否足以於轉向角執行符合精度規格的平滑程序。若判斷機械設備的變速能力不足以執行符合精度規格的平滑程序，處理器用以於命令佇列中插入弧線命令以使平滑程序符合精度規格。處理器用以根據平滑程序控制機械設備運動以通過轉向角。

Description

用於控制機械設備的控制裝置與方法

本案涉及一種控制裝置以及控制方法。詳細而言，本案涉及一種用以對機械設備進行控制的裝置以及方法。

在製造業/加工業領域，如機械手臂等等的加工裝置被大幅的使用。然而，當加工裝置的加工程序包含多段不同方向(或弧度)的軌跡時，目前的現有技術難以滿足對轉角處的軌跡精度之需求。

為了解決前述問題，本案實施例提供一種機械設備控制裝置以及一種機械設備控制方法。

本案的一實施態樣涉及一種控制裝置，用以控制一機械設備。該控制裝置包含一記憶體以及一處理器。該記憶體儲存至少一指令。該處理器耦接於該記憶體以及該機械設備。該處理器用以存取並執行該至少一指令以接收一命令佇列，其中該命令佇列用以控制該機械設備運動以通過一轉向角。該處理器用以判斷該機械設備的一變速能力是否足以於該轉向角執行符合一精度規格的一平滑程序。若判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，該處理器用以於該命令佇列中插入一弧線命令以使該平滑程序符合該精度規格。該處理器用以根據該平滑程序控制該機械設備運動以通過該轉向角。

本案的另一實施態樣涉及一種控制方法。該控制方法用以控制一機械設備，該控制方法包含：接收一命令佇列，其中該命令佇列用以控制該機械設備運動以通過一轉向角；判斷該機械設備的一變速能力是否足以於該轉向角執行符合一精度規格的一平滑程序；若判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，於該命令佇列中插入一弧線命令以使該平滑程序符合該精度規格；以及根據該平滑程序控制該機械設備運動以通過該轉向角。

根據本案提供的控制裝置以及控制方法，當機械設備根據多段不同方向或弧度的軌跡運作時，本案可以滿足轉角處的軌跡精度之需求。

應注意的是，前述的發明內容以及後述的實施方式皆僅係舉例說明而已，其主要目的係為詳細地解釋本案申請專利範圍當中的內容。

100‧‧‧控制裝置

120‧‧‧記憶體

140‧‧‧處理器

160‧‧‧輸入裝置

200‧‧‧機械設備

300‧‧‧控制方法

S310~S340‧‧‧方法步驟

T1‧‧‧第一軌跡

T2‧‧‧第二軌跡

RA‧‧‧轉向角

ARC‧‧‧圓弧

AD‧‧‧分角線

D‧‧‧圓心

T1‧‧‧軌跡

θ₁、θ₂‧‧‧角度

r‧‧‧半徑

d‧‧‧距離

ε‧‧‧轉角誤差值

V、v _c、v _s、v _f‧‧‧速度

t、t _f、t _s、t _af‧‧‧時間

t1~t7‧‧‧時間

V₀~V₇‧‧‧速度

S₁~S₇‧‧‧區段

a、a _eq、a _c、a _s‧‧‧加速度

參照後續段落中的實施方式以及下列圖式，當可更佳地理解本案的內容：第1圖為根據本案一實施例繪示的控制裝置之示意圖；第2圖為根據本案一實施例繪示的控制裝置之示意圖；第3圖為根據本案一實施例繪示的控制方法之步驟流程圖；第4圖為根據本案一實施例繪示的機械設備的運動軌跡之示意圖；第5圖為根據本案一實施例繪示的機械設備的運動軌跡之示意圖；第6圖為根據本案一實施例繪示的機械設備的梯型加減速插值演算法之速度曲線插值點示意圖；第7圖為根據本案一實施例繪示的機械設備的S型加減速插值演算法之速度曲線插值點示意圖；第8圖為根據本案一實施例繪示的機械設備的不具等加速段加減速插值演算法之示意圖；第9圖為根據本案一實施例繪示的機械設備的具等加速段加減速插值演算法之示意圖；第10圖為根據本案一實施例繪示的機械設備的加減速插值演算法之加速度插值點示意圖；以及第11圖為根據本案一實施例繪示的機械設備的加減速插值演算法之減速度插值點示意圖。

以下將以圖式及詳細敘述清楚說明本案之精神，任何所屬技術領域中具有通常知識者在瞭解本案之實施例後，當可由本案所教示之技術，加以改變及修飾，其並不脫離本案之精神與範圍。

本文之用語只為描述特定實施例，而無意為本案之限制。單數形式如“一”、“這”、“此”、“本”以及“該”，如本文所用，同樣也包含複數形式。

關於本文中所使用之『耦接』或『連接』，均可指二或多個元件或裝置相互直接作實體接觸，或是相互間接作實體接觸，亦可指二或多個元件或裝置相互操作或動作。

關於本文中所使用之『包含』、『包括』、『具有』、『含有』等等，均為開放性的用語，即意指包含但不限於。

關於本文中所使用之『及/或』，係包括所述事物的任一或全部組合。

關於本文中所使用之用詞(terms)，除有特別註明外，通常具有每個用詞使用在此領域中、在本案之內容中與特殊內容中的平常意義。某些用以描述本案之用詞將於下或在此說明書的別處討論，以提供本領域技術人員在有關本案之描述上額外的引導。

第1圖為根據本案一實施例繪示的控制裝置之示意圖。如第1圖所示，在一些實施例中，控制裝置100可包含記憶體120以及處理器140。在一些實施例中，處理器140電性/通訊耦接於記憶體120。在一些實施例中，控制裝置100當中的處理器140電性/通訊耦接於機械設備200，控制裝置100用以控制機械設備200的動作。

在一些實施例中，記憶體120可包含但不限於快閃(flash)記憶體、硬碟(HDD)、固態硬碟(SSD)、動態隨機存取記憶體(DRAM)或靜態隨機存取記憶體(SRAM)。在一些實施例中，作為一種非暫態電腦可讀取媒體，記憶體120可儲存關聯於一控制方法的至少一指令，前述至少一指令可供處理器140存取並執行。

在一些實施例中，處理器140可包含但不限於單一處理器以及多個微處理器之集成，例如，中央處理器(Central Processing Unit,CPU)或繪圖處理器(Graphic Processing Unit,GPU)等。該些(微)處理器電性耦接於記憶體120，藉此，處理器140可用以自記憶體120存取前述的至少一指令，以依據此至少一指令執行前述控制方法。為了更佳地理解前述控制方法，將於後面段落中詳細解釋前述控制方法。

在一些實施例中，機械設備200可包含但不限於切削工具機、打磨工具機或用以執行該等功能的機械手臂等等。在一些實施例中，機械設備200的動作所指涉的特別是機械手臂的末端(例如，執行加工程序的一端)的行進動作。

第2圖為根據本案一實施例繪示的控制裝置之示意圖。如第2圖所示，在一些實施例中，控制裝置100可更包含輸入裝置160。在一些實施例中，處理器140電性/通訊耦接於輸入裝置160，輸入裝置160可用以將使用者所輸入的資訊或指令傳輸至處理器140。

在一些實施例中，輸入裝置160可包含多種用以接收使用者輸入的裝置，例如，鍵盤、滑鼠以及觸控螢幕等。在一些實施例中，輸入裝置160電性耦接於處理器140，用以將使用者所輸入的資訊傳輸至處理器140。在一些實施例中，處理器140用以自記憶體120存取並執行指令，並根據使用者所輸入的資訊控制機械設備200的動作。

在一些實施例中，第2圖所示的記憶體120、處理器140以及機械設備200可根據第1圖所示的對應元件而實施。

第3圖為根據本案一實施例繪示的控制方法之步驟流程圖。在一些實施例中，控制方法300可由第1圖或第2圖中所示的處理器140所執行。在一些實施例中，控制方法300之詳細步驟將於下面段落中敘述。

步驟S310：接收用以控制機械設備沿第一軌跡運動的第一命令以及用以控制機械設備沿第二軌跡運動的第二命令。

在一些實施例中，處理器140耦接於機械設備200的控制器(圖中未示)及/或記憶體(圖中未示)，處理器140可自機械設備200的控制器及/或記憶體100獲取機械設備200的命令佇列，並進行後續處理程序。在一些實施例中，機械設備200的命令佇列當中包含預設的複數命令，該些命令分別對應於機械設備200被預設的某一段運動軌跡，該些命令可包含機械設備200於對應的運動軌跡當中被預設的運動速度。

在一些實施例中，機械設備200的命令佇列當中包含第一命令以及第二命令，其中第一命令用以控制機械設備200沿第一軌跡運動，第二命令用以控制機械設備200沿第二軌跡運動。在一些實施例中，第一軌跡以及第二軌跡之間具有一轉向角。在一些實施例中，若機械設備200根據預設的命令佇列，依序執行第一命令以及第二命令，機械設備200將沿第一軌跡行進，並於第一軌跡與第二軌跡的交會處根據轉向角轉向進入第二軌跡，再沿第二軌跡行進。為易於理解，相關內容將於後述段落參照第4圖說明。

在一些實施例中，處理器140可作為機械設備200的控制器，記憶體120可作為機械設備200的記憶體，處理器140可自記憶體120獲取機械設備200的命令佇列，並進行後續處理程序。

步驟S320：判斷機械設備由第一軌跡經轉向角導入第二軌跡的平滑程序是否符合精度規格。

在一些實施例中，處理器140獲取機械設備200的命令佇列後，處理器140可根據第一命令以及第二命令模擬機械設備200由第一軌跡經轉向角導入第二軌跡的平滑程序。

詳細而言，在一些實施例中，由於第一軌跡以及第二軌跡之間具有轉向角，當機械設備200根據第一命令的預定速度沿第一軌跡行進至接近轉向角時，機械設備200應自預定速度降低至一定速度，方便進行轉向。在一些實施例中，根據轉向角的不同，機械設備200可能於第一軌跡末端減速至靜止，再向第二軌跡的方向重新加速，方能完成轉向。

承前所述，在一些實施例中，當機械設備200於第一軌跡以及第二軌跡的交會處根據轉向角轉向後，機械設備200將重新加速至第二命令的預定速度，以沿第二軌跡行進。換言之，當機械設備200根據第一命令以及第二命令運動時，機械設備200需要進行先減速再加速的平滑程序，以適應第一軌跡以及第二軌跡之間的轉向角。然而，由於前述變速程序將延長機械設備200於轉角處的運動時間，其降低了機械設備200的效率。

在一些實施例中，機械設備200係根據一種混合補間(Blending)程序決定加減速策略，以完成前述平滑程序。應理解，前述平滑程序旨在使機械設備200能以較佳的速度通過轉角處。

在一些實施例中，在平滑程序中，機械設備200被預設有一精度規格，前述精度規格係為機械設備200自第一軌跡轉向進入第二軌跡過程中被容許的軌跡精度。亦即，此軌跡精度即為使用者/系統對機械設備200於轉角處的運動所能容許的誤差。應理解，機械設備200於轉角處執行平滑程序時的加減速規劃應符合此軌跡精度，如此可提高機械設備200整體的運動效率亦不犧牲精度規格。

在一些實施例中，使用者可藉由控制裝置100的輸入裝置160輸入關聯於前述精度規格的資訊，處理器140可於記憶體120中儲存前述精度規格。亦即，機械設備200的精度規格可由使用者自行設定。

在一些實施例中，處理器140可先判斷機械設備200的變速能力(即，加速能力以及減速能力)是否足以執行符合前述精度規格的平滑程序。為了更佳地理解，請一併參照第4圖，其係為根據本案一實施例繪示的機械設備的運動軌跡之示意圖。如第4圖所示，在一些實施例中，根據命令佇列當中的第一命令以及第二命令，機械設備200被預設沿第一軌跡T1 以及第二軌跡T2運動。在一些實施例中，第一軌跡T1以及第二軌跡T2皆為直線路徑，第一軌跡T1以及第二軌跡T2於交會處具有轉向角RA。

在一些實施例中，根據命令佇列當中的第一命令，機械設備200於第一軌跡T1當中的預設速度(可理解為切線速度)為V_ab。在一些實施例中，根據命令佇列當中的第二命令，機械設備200於第二軌跡T2當中的預設速度(可理解為切線速度)為V_bc。如第4圖所示，在一些實施例中，轉向角RA的外角值為φ。在一些實施例中，機械設備200於轉向角RA轉向時的向心加速度值設為a_c。

在一些實施例中，處理器140可根據下列方法以及計算式判斷機械設備200的變速能力是否足以符合平滑程序當中被容許的精度規格。首先，機械設備200的速度值V，其中V=max(V_ab,V_bc)。假定速度的取樣時間非常短，此時，機械設備200的速度變化量△V可視為△V=V*θ_ac。根據向心加速度的定義，a_c=△V/△t=V*(θ_ac/△t)。因此，處理器140可以計算θ_ac，其中θ_ac=(a_c/V)*△t。

在一些實施例中，若θ_ac≧φ(轉向角RA的外角值)，處理器140可判斷機械設備200的變速能力足以符合平滑程序當中被容許的精度規格。

在一些實施例中，若θ_ac<φ(轉向角RA的外角值)，處理器140可判斷機械設備200的變速能力不足以符合平滑程序當中被容許的精度規格。

步驟S330：若機械設備的變速能力不足以符合精度規格，於第一命令與第二命令之間加入弧線命令，使平滑程序符合精度規格。

在一些實施例中，若處理器140判斷機械設備200的變速能力已足以符合前述精度規格(亦即，前述實施例中θ_ac≧φ的狀況)時，處理器140可維持機械設備200的命令佇列當中的第一命令以及第二命令。

在一些實施例中，若處理器140判斷機械設備200的變速能力不足以符合前述精度規格(亦即，前述實施例中θ_ac<φ的狀況)時，處理器140將於第一命令與第二命令之間插入一弧線命令。在一些實施例中，前述弧線命令用以使機械設備200於轉向角RA處根據弧線行進，進而使機械設備200於轉向角RA處的運動符合前述精度規格。

為了更佳地理解，請一併參照第5圖，其係為根據本案一實施例繪示的機械設備的運動軌跡之示意圖。如第5圖所示，在一些實施例中，若需於第一軌跡T1以及第二軌跡T2之間插入一圓弧ARC，可先根據轉向角RA的分角線AD將轉向角RA的內角等分為相等的二個角度θ₁以及θ₂。如第5圖所示，圓弧ARC的圓心D位於轉向角RA的分角線上，圓弧ARC的半徑為r。

如第5圖所示，在一些實施例中，圓弧ARC的圓心D至第一軌跡T1以及第二軌跡T2的交會點之距離為d。根據前述算式，可以推得r=d*sin θ₁。應理解，在一些實施例中，使用者所容許的轉角誤差值為ε(即圓弧ARC至第一軌跡T1以及第二軌跡T2的交會點的最短距離)。如第5圖所示，可知 d=ε+r。根據前述算式，可得d=ε/(1-sin θ 1)。在一些實施例中，處理器140可根據前述計算方式獲得圓弧ARC的圓心D的位置以及圓弧ARC的半徑長度值r。根據圓弧ARC的圓心D以及半徑長度值r，處理器140可產生對應圓弧ARC的弧線命令。

在一些實施例中，處理器140可將對應圓弧ARC的弧線命令插入對應於第一軌跡T1的第一命令以及對應於第二軌跡T2的第二命令之間，藉以更新機械設備200的命令佇列。

步驟S340：根據平滑程序控制機械設備運動。

在一些實施例中，若處理器140判斷機械設備200的變速能力已足以符合前述精度規格，處理器140可直接根據命令佇列控制機械設備200，使機械設備200可直接根據命令以及第二命令執行平滑程序。藉此，機械設備200將沿第一軌跡T1運動，於轉向角RA附近變速以直接轉向進入第二軌跡T2，再沿第二軌跡T2運動。

在一些實施例中，若處理器140判斷機械設備200的變速能力不足以符合前述精度規格，處理器140可根據更新後的命令佇列控制機械設備200，使機械設備200可根據第一命令、弧線命令以及第二命令執行平滑程序。藉此，機械設備200將沿第一軌跡T1運動，於轉向角RA附近變速以根據圓弧ARC繞轉進入第二軌跡T2，再沿第二軌跡T2運動。

承前所述，在一些實施例中，處理器140係根據混合補間程序決定機械設備200的加減速策略，以完成前述平滑程序。在一些實施例中，前述混合補間程序可根據梯型加減速插值演算法或S型加減速插值演算法實施。應理解，在一些實施例中，若將第一軌跡T1以及第二軌跡T2以複數切分點分為複數區段，前述混合補間程序係用以計算機械設備200於該些區段當中的各個切分點的速度值(以及加速度值)。亦即，該些切分點可視為平滑程序當中的複數插值點，前述混合補間程序用以計算該些插值點的速度值(以及加速度值)。

為了更佳地理解，請一併參照第6圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以理解其內容。第6圖的縱軸標示了速度(V)而橫軸標示了時間(t)。

在一些實施例中，以T表示機械設備200的目標位置(例如，第一軌跡T1當中的某個切分點位置)，以P表示目前已插值的位置，以v _m表示機械設備200於此段命令(例如，第一命令)中的最大速度(例如，第一軌跡T1當中的預設速度)，以v _c表示機械設備200的當前速度，以v _s表示下個切分點所建議的速度，以v _f表示此段命令的終止速度值，以a _m表示此段命令的最大加速度，以t _f表示機械設備200的運動完成時間，以t _s表示切分點之間的取樣時間。應理解，第6圖所繪示的梯形面積即為機械設備200根據此段命令所需移動的距離。根據前述定義，可得下列計算式(1)。

在前述計算式(1)中，計算式(1)右側的第一項表示了第6圖所繪示的梯形面積當中落於上側的三角形面積，計算式(1)右側的第一項以及第二項則表示了第6圖所繪示的梯形面積當中的剩餘面積。

在一些實施例中，若欲計算建議速度v _s，假設機械設備200根據最大減速度進行減速，機械設備200將於時間t _f時停止移動。藉此，可得出如下列計算式(2)。

v _s=a _m(t _f-t _s)+v _f．．．(2)

將計算式(2)代入計算式(1)後可得計算式(3)，計算式(3)如下所示。

整理後計算式(3)可得計算式(4)，如下所示。

由計算式(4)可得計算式(5)，如下所示。計算式(5)係用以計算t _f，即為機械設備200的運動完成時間。

將根據計算式(5)獲取的t _f代回計算式(2)，可得下個切分點所建議的速度v _s，如下列計算式(6)所示。

當根據前述計算式(6)所求出的v _s≧v _m時，則設 v _s=v _m，再重新計算加速度值a _s，如下列計算式(7)所示。

當根據前述計算式(7)所求出的a _s≧a _m時，則設a _s=a _m；當根據前述計算式(7)所求出的a _s≦-a _m時，則設a _s=-a _m，再重新計算速度值v _s，如下列計算式(8)所示。

v _s=v _c+a _c t _s．．．(8)

根據計算式(8)，插值計算出機械設備200的下一個位置(P _new)，如下列計算式(9)所示。

P _new=P+v _s t _s．．．(9)

在一些實施例中，根據關聯於前述計算式(1)~(8)的加減速插值演算法，處理器140可根據機械設備200的變速能力實施前述混合補間程序，令機械設備200可平滑地通過第一軌跡T1以及第二軌跡T2之間的轉向角RA。

為了更佳地理解，請一併參照第7圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以理解其內容。圖中的縱軸標示了速度(V)而橫軸標示了時間(t)。

如第7圖所示，在S型加減速插值演算法中，大致上可根據時間(t1~t7)將機械設備200的變速(加減速)過程劃分為七個區段(S₁~S₇)，每個區段對應不同的計算式以及判斷是否切換的條件，各區段當中的速度可標記為V₀~V₇(V₀為初始速度)。在一些情況中，當在S型加減速插值演算法當中應用即時變速率(override speed)時，將大幅複雜化每個區段中的判斷計算。因此，本案實施例提供以下的新型S型加減速插值演算法，可較佳地適應於具有即時變速率的應用。

在一些實施例中，處理器140可先計算機械設備200的變速過程當中的等效加速度，其可表示為a _eq。應理解，在處理器140計算機械設備200的等效加速度時，具有二種可能性。第一種可能性係為變速過程當中的速度差可達到最大加速度且變速過程當中包含等加速度區段。第二個可能性則是變速過程當中的速度差達到最大加速度後將馬上減速，或者變速過程無法達到最大加速度。以下段落中將分別推導二種可能性的計算過程。

第一種可能性的狀況可藉由下列不等式表示。

(v _m-v _f)

為了更佳地理解此可能性的計算過程，請一併參照第8圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以及第7圖以理解其內容。圖中的縱軸標示了速度(V)而橫軸標示了時間(t)。

在一些實施例中，如第8圖所示，其顯示了機械設備200於變速程序中可能達到最大加速度後需立刻進行減速的情況，或者機械設備200於變速程序中無法達到最大加速度的情況。在本實施例中，可先計算機械設備200於減速區段當中所能達到的最大的減速度值，其可表示為a _n，如下列計算式(10)所示。

在計算式(10)當中，t _f代表減速區段所經過的時間長度，圖中的倒三角形面積表示了機械設備200於減速區段當中的速度差，其關係如下列計算式(11)所示。

將根據計算式(10)代入計算式(11)，可得下列的計算式(12)。

根據計算式(12)可推得減速區段中的等效加速度a _eq，如下列計算式(13)所示。

第二種可能性的狀況可藉由下列不等式表示。

(v _m-v _f)>

為了更佳地理解此可能性的計算過程，請一併參照第9圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以及第7圖以理解其內容。圖中的縱軸標示了速度(V)而橫軸標示了時間(t)。

在一些實施例中，如第9圖所示，其顯示了機械設備200於變速程序中能達到最大加速度並具有等加速區段的情況。在本實施例中，可先計算此區段的時間長度值，其可表示為t _f，如下列計算式(14)所示。

t _f=2t ₁+t ₂．．．(14)

在計算式(14)當中，時間t _f可藉由下列計算式(15)表示。

第9圖所示的倒梯形面積可藉由下列計算式(16)表示，其代表了此區段的速度差。

v _m-v _f=a _m t ₁+a _m t ₂．．．(16)

將計算式(15)代入計算式(16)整理後可得下列的計算式(17)。

將計算式(17)以及計算式(15)代入計算式(14)可得得下列的計算式(18)，可得此區段的時間長度值t _f。

根據計算式(18)可推得減速區段中的等效加速度a _eq，如下列計算式(19)所示。

如第7圖所示，在S型加減速插值演算法中，大致上可將機械設備200的變速過程劃分為七個區段(S₁~S₇)，其中的區段S₁~S₄係為由較低速度提升到較高速度的加速度區段。處理器140可根據以下的加速度插值法決定區段S₁~S₄當中的位置插值。

為了更佳地理解此加速度插值法的計算過程，請一併參照第10圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以及第7圖以理解其內容。圖中的縱軸標示了加速度(a)，而橫軸標示了時間(t)。

在一些實施例中，以v _t代表此區段中的目標速度，以v _c代表當前切分點的速度，以a _m代表此區段中的最大加速度，以a _c代表當前切分點的加速度，以a _s代表此區段中下個切分點建議的速度值，以J _m代表為此區段中的最大加加速度(jerk)，以t _af代表為完成此區段的時間，以t _s表示切分點之間的取樣時間。應理解，第10圖所繪示的三角形面積於此區段中所需增加的速度總量。根據前述定義，可得下列計算式(20)。

在前述計算式(20)中，計算式(20)右側的第一項表示了第10圖所繪示的三角形面積當中落於上側的較小三角形面積，計算式(20)右側的第二項則表示了第10圖所繪示的三角形面積當中落於下側的較大三角形面積。

在一些實施例中，可先計算此區段中下個切分點建議的速度值a _s。應理解，若在此區段中根據最大減速度做減速運動，機械設備200於時間為t _af時將停止。因此，根據前述關係，可得下列計算式(21)。

a _s=J _m(t _af -t _s)．．．(21)

將計算式(21)代回計算式(20)可得下列計算式 (22)。

整理計算式(22)可得下列計算式(23)。

根據計算式(23)可推導出下列計算式(24)，計算式(24)用以獲取完成此區段的時間t _af。

將t _af代入計算式(21)可得計算式(25)，計算式(25)表示了此區段中下個切分點建議的速度值a _s。

當根據前述計算式(25)所求出的a _s≧a _m時，則設a _s=a _m，再重新計算最大加加速度值J _s，如下列計算式(26)所示。

當根據前述計算式(26)所求出的J _s≧J _m時，則設J _s=J _m；當根據前述計算式(26)所求出的J _s≦-J _m時，則設J _s=-J _m，再重新計算速度值a _s，如下列計算式(27)所示。

a _s=a _c+J _c t _s．．．(27)

根據計算式(27)，插值計算出機械設備200的下個切分點所建議的速度v _s，如下列計算式(28)所示。

v _s=v _c+a _c t _s．．．(28)

根據計算式(28)，計算出機械設備200的下一個位置(P _new)，如下列計算式(29)所示。

P _new=P+v _s t _s．．．(29)

如第7圖所示，在S型加減速插值演算法中，大致上可將機械設備200的變速過程劃分為七個區段(S₁~S₇)，其中的區段S₄~S₇係為由較高速度降低到較低速度的減速度區段。處理器140可根據以下的減速度插值法決定區段S₄~S₇當中的位置插值。

為了更佳地理解此減速度插值法的計算過程，請一併參照第11圖，其係為根據本案一實施例繪示的機械設備的加減速插值演算法之示意圖。可一併參照第1至5圖以及第7圖以理解其內容。圖中的縱軸標示了加速度(a)，而橫軸標示了時間(t)。

應理解，機械設備200由高速降至低速的過程對應了二種可能性。第一種可能性係為機械設備200所運動的距離快抵達終點，故作減速度運動。第二種可能性係為機械設備200的變速率(即，機械設備200的輸出比率)下降所造成。

在一些實施例中，以v _t代表此區段中的目標速度，以v _c代表當前切分點的速度，以a _m代表此區段中的最大加速度，以a _c代表當前切分點的加速度，以a _s代表此區段中下個切分點建議的速度值，以J _m代表為此區段中的最大加加速度(jerk)，以t _af代表為完成此區段的時間，以t _s表示切分點之間的取樣時間。應理解，第11圖所繪示的三角形面積於此區段中所需增加的速度總量。根據前述定義，可得下列計算式(30)。

在前述計算式(30)中，計算式(30)右側的第一項表示了第11圖所繪示的三角形面積當中落於下側的較小三角形面積，計算式(30)右側的第二項則表示了第12圖所繪示的三角形面積當中落於上側的較大三角形面積。

在一些實施例中，可先計算此區段中下個切分點建議的速度值a _s。應理解，若在此區段中根據最大減速度做減速運動，機械設備200於時間為t _af時將停止。因此，根據前述關係，可得下列計算式(31)。

a _s=-J _m(t _af-t _s)．．．(31)

將計算式(31)代回計算式(30)可得下列計算式(32)。

整理計算式(32)可得下列計算式(33)。

根據計算式(33)可推導出下列計算式(34)，計算式(34)用以獲取完成此區段的時間t _af。

將t _af代入計算式(31)可得計算式(35)，計算式(35)表示了此區段中下個切分點建議的速度值a _s。

當根據前述計算式(35)所求出的a _s≦-a _m時，則設a _s=-a _m，再重新計算最大加加速度值J _s，如下列計算式(36)所示。

當根據前述計算式(36)所求出的J _s≧J _m時，則設J _s=J _m；當根據前述計算式(26)所求出的J _s≦-J _m時，則設J _s=-J _m，再重新計算速度值a _s，如下列計算式(37)所示。

a _s=a _c+J _c t _s．．．(37)

根據計算式(37)，插值計算出機械設備200的下個切分點所建議的速度v _s，如下列計算式(38)所示。

v _s=v _c+a _c t _s．．．(38)

根據計算式(38)，計算出機械設備200的下一個位置(P _new)，如下列計算式(39)所示。

P _new=P+v _s t _s．．．(39)

在一些實施例中，根據關聯於前述計算式(20)~(39)的加減速插值演算法，處理器140可根據機械設備200的變速能力實施前述混合補間程序。應理解，處理器140需要判斷插值的位置已經進入區段S₅~S₇的減速區當中，方能計算前述的混合補間程序。在一些實施例中，處理器140可根據前述的等效加速度a _eq再配合梯型加減速的判斷方法來判斷插值的位置已經進入減速區段。前述判斷方法可以下列計算式(40)以及計算式(41)表示。

在一些實施例中，處理器140可根據計算式(40)當中的速度參照值(v _ref)來判斷插值的位置是否已經進入減速區，若根據計算式(40)計算出的速度參照值v _ref大於v _m，可判斷插值的位置未進入減速區。反之，若根據計算式(40)計算出的速度參照值v _ref小於等於v _m，可判斷插值的位置將進入減速區。

在一些實施例中，計算式(41)當中的加速度參照值(a _ref)可用以判斷當變速率改變時，變速率改變後的速度是否大於或等於機械設備200的當前速度。若根據計算式(41)計算出的加速度參照值a _ref顯示變速率改變後的速度大於機械設備200的當前速度，則判斷須持續加速。若根據計算式(41)計算出的加速度參照值a _ref顯示變速率改變後的速度小於機械設備200的當前速度，則判斷須進行減速。

在一些實施例中，根據前述的計算式(10)至計算式(41)，可獲得前述的新型S型加減插值演算法的判斷條件以及參數，整理如下表所示。

根據該表，當處理器140計算出v _ref≧v _m且a _ref≧0時，可判斷此狀況對應至類型1，可執行如前述的計算式(20)至計算式(29)所述的加速度演算法，此狀況所適用的參數為v _t=v _m，亦即，處理器140可設定機械設備200於此區段中的目標速度為最大速度。

根據該表，當處理器140計算出v _ref≧v _m且a _ref<0時，可判斷此狀況對應至類型2，可執行如前述的計算式(30)至計算式(39)所述的減速度演算法，此狀況所適用的參數為v _t=v _m，亦即，處理器140可設定機械設備200於區段中的目標速度為最大速度。

根據該表，當處理器140計算出v _ref<v _m時，可判斷此狀況對應至類型3，可執行如前述的計算式(30)至計算式(39)所述的減速度演算法，此狀況所適用的參數為v _t=v _f，亦即，處理器140可設定機械設備200於區段中的目標速度為終止速度值。

應理解，在前述實施例中，第一軌跡T1以及第二軌跡T2皆為直線路徑，然而本案並不以此為限。在一些實施例中，第一軌跡T1以及第二軌跡T2更可包含以下組合：第一軌跡T1可為直線軌跡，第二軌跡T2可為弧線軌跡；第一軌跡T1可為弧線軌跡，第二軌跡T2可為直線軌跡；以及第一軌跡T1以及第二軌跡T2皆為弧線軌跡。對本領域技術人員而言，應可理解如何於第一軌跡T1以及第二軌跡T2的各種組合狀況下修改並實施前述加減速插值演算法應用。

應理解，藉由前述的判斷方式以及加減速插值演算法，本案提供了一種有效的控制裝置100以及控制方法300，可確保機械設備200於使用者需求的預定精度下於轉角處進行轉向動作。

雖然本案以詳細之實施例揭露如上，然而本案並不排除其他可行之實施態樣。因此，本案之保護範圍當視後附之申請專利範圍所界定者為準，而非受於前述實施例之限制。

對本領域技術人員而言，在不脫離本案之精神和範圍內，當可對本案作各種之更動與潤飾。基於前述實施例，所有對本案所作的更動與潤飾，亦涵蓋於本案之保護範圍內。

100‧‧‧控制裝置

120‧‧‧記憶體

140‧‧‧處理器

200‧‧‧機械設備

Claims

一種控制裝置，用以控制一機械設備，該控制裝置包含：一記憶體，儲存至少一指令；以及一處理器，耦接於該記憶體以及該機械設備，該處理器用以存取並執行該至少一指令：接收一命令佇列，其中該命令佇列用以控制該機械設備運動以通過一轉向角；判斷該機械設備的一變速能力是否足以於該轉向角執行符合一精度規格的一平滑程序；若判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，於該命令佇列中插入一弧線命令以使該平滑程序符合該精度規格；以及根據該平滑程序控制該機械設備運動以通過該轉向角；其中該命令佇列中包含用以控制該機械設備沿一第一軌跡運動的一第一命令以及用以控制該機械設備沿一第二軌跡運動的一第二命令，該第一軌跡以及該第二軌跡之間具該轉向角，若該處理器判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，於該第一命令以及該第二命令間插入該弧線命令以使該平滑程序符合該精度規格；其中該處理器於該命令佇列中插入該弧線命令後，根據一混合補間程序計算該平滑程序當中的複數插值點對應的速度。
如請求項1所述之控制裝置，其中該混合補間程序係根據一梯型加減速插值演算法或一S型加減速插值演算法實施。
如請求項2所述之控制裝置，其中該S型加減速插值演算法對應於一速度參照值以及一加速度參照值，且該機械設備於該些插值點中的一插值點對應至一第一速度，並於該插值點對應至一第一加速度，若該第一速度大於等於該速度參照值且該第一一加速度大於等於該加速度參照值，該處理器判斷該機械設備執行該S型加減速插值演算法中的一加速度演算法，若該第一速度大於或等於該速度參照值且該第一加速度小於該加速度參照值，該處理器判斷該機械設備執行該S型加減速插值演算法中的一減速度演算法，若該第一速度小於該速度參照值，該處理器判斷該機械設備執行該減速度演算法。
如請求項1所述之控制裝置，其中該第一軌跡以及該第二軌跡包含以下組合之其中一種：該第一軌跡係為直線軌跡，該第二軌跡係為直線軌跡；該第一軌跡係為直線軌跡，該第二軌跡係為弧線軌跡；該第一軌跡係為弧線軌跡，該第二軌跡係為直線軌跡；以及該第一軌跡係為弧線軌跡，該第二軌跡係為弧線軌跡。
一種控制方法，用以控制一機械設備，該控制方法包含：接收一命令佇列，其中該命令佇列用以控制該機械設備運動以通過一轉向角；判斷該機械設備的一變速能力是否足以於該轉向角執行符合一精度規格的一平滑程序；若判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，於該命令佇列中插入一弧線命令以使該平滑程序符合該精度規格；以及根據該平滑程序控制該機械設備運動以通過該轉向角；其中該命令佇列中包含用以控制該機械設備沿一第一軌跡運動的一第一命令以及用以控制該機械設備沿一第二軌跡運動的一第二命令，該第一軌跡以及該第二軌跡之間具該轉向角，若該處理器判斷該機械設備的該變速能力不足以執行符合該精度規格的該平滑程序，於該第一命令以及該第二命令間插入該弧線命令以使該平滑程序符合該精度規格；其中該處理器於該命令佇列中插入該弧線命令後，根據一混合補間程序計算該平滑程序當中的複數插值點對應的速度。
如請求項5所述之控制方法，其中該混合補間程序係根據一梯型加減速插值演算法或一S型加減速插值演算法實施。
如請求項6所述之控制方法，其中該S型加減速插值演算法對應於一速度參照值以及一加速度參照值，且該機械設備於該些插值點中的一插值點對應至一第一速度，並於該插值點對應至一第一加速度，該控制方法更包含：若該第一速度大於等於該速度參照值且該第一加速度大於等於該加速度參照值，判斷該機械設備執行該S型加減速插值演算法中的一加速度演算法；若該第一速度大於等於該速度參照值且該第一加速度小於該加速度參照值，判斷該機械設備執行該S型加減速插值演算法中的一減速度演算法；以及若該第一速度小於該速度參照值，判斷該機械設備執行該減速度演算法。
如請求項5所述之控制方法，其中該第一軌跡以及該第二軌跡包含以下組合之其中一種：該第一軌跡係為直線軌跡，該第二軌跡係為直線軌跡；該第一軌跡係為直線軌跡，該第二軌跡係為弧線軌跡；該第一軌跡係為弧線軌跡，該第二軌跡係為直線軌跡；以及該第一軌跡係為弧線軌跡，該第二軌跡係為弧線軌跡。