TWI444798B

TWI444798B - Multi - degree of freedom platform control device

Info

Publication number: TWI444798B
Application number: TW097127627A
Authority: TW
Inventors: Takashi Mamba
Original assignee: Yaskawa Denki Seisakusho Kk
Priority date: 2007-07-25
Filing date: 2008-07-21
Publication date: 2014-07-11
Also published as: TW200921316A; JP5168281B2; JPWO2009013985A1; US8148931B2; WO2009013985A1; US20100109593A1

Description

多自由度平台控制裝置

本發明是關於控制多自由度平台的位置或速度的多自由度平台控制裝置。

習知的多自由度平台控制裝置是因應於成為控制對象的多自由度平台的感測器構成或致動器構成，開發了專用控制裝置。

在第3圖中，110是多自由度平台，具備用以驅動平台的位置與姿勢的致動器，及用以檢測出位置與姿勢的感測器，進行輸出感測器訊號。100是多自由度平台控制裝置，依據多自由度平台110的感測器訊號來變化流在致動器的電流，來控制多自由度平台110的位置與姿勢。多自由度平台控制裝置是由指令生成器140、控制演算器150、推力變換演算器160、電流指令器170、位置演算器180、感測器訊號變換器190所構成。140是指令生成器，給予多自由度平台的位置與姿勢的指令。位置指令是以合併X軸、Y軸、Z軸方向的位置，與滾動、投擲、遙動的姿勢的6自由度所給予。依據使用者分別給予的定位指令，每當控制週期予以內插使之生成，或是將事先所決定的動作給予作為每當控制週期的位置指令。190是感測器訊號變換器，受訊感測器訊號，而算出在絕對座標所表示的感測器位置。180是位置演算器，使用藉由感測器訊號變換器190所算出的感測器位置資訊，來演算多自由度平台110的位置與姿勢。150是控制演算器，將藉由位置演算器180所算出的位置與姿勢，追隨於指令生成器140所生成的指令的方式來決定操作量。操作量是指具體上為重心位置的並進推力與重心周圍的力矩等。以X軸、Y軸、Z軸，滾動、投擲、遙動的6自由度分別作為PID控制、或位置PI控制，速度P控制的梯級控制，位置P控制、速度PI控制的梯級控制等，而使用前授控制或一次延遲濾波器，凹口波濾波器等的技術。將指令生成器140所生成的指令作為速度指令，而控制演算器150是作為算出相當於位置與姿勢的微分的量，而將位置與姿勢的微分相當量追隨於指令的方式作為決定操作量的速度控制系也可以。160是推力變換演算器，為了實現控制演算器150所算出的操作量來算出各致動器須輸出的推力。170是電流指令器，發生如由推力變換演算器160所接受的推力指令的推力的方式來控制多自由度平台110的致動器電流。

位置演算器180的演算內容是藉由多自由度平台110的感測器構成有所不同。被考慮有對應於如第4圖至第13圖所示的各種感測器構成的多自由度平台的位置姿勢算出方向。例如，參照專利文獻1。

在第4圖中，22是固定件，為被固定成無法動的感測器基準位置。21是移動件，對於固定件22進行移動的控制對象。未圖示的多自由度平台控制裝置是控制移動件21對於固定件22的位置或姿勢的裝置。1至3是間隙感測器，測定3點的Z軸方向的位置。5與6是二維感測器分別測定2點的X軸與Y軸方向的移動距離。亦即從X軸方向2點、Y軸方向2點、Z軸方向3點的資訊求出位置。

在第5圖中，1至3是間隙感測器，測定3點的Z軸方向的位置。7至9是變位感測器，測定2點的Y軸方向的移動距離與1點的X軸方向的移動距離。亦即，從X軸方向1點、Y軸方向2點、Z軸方向3點的資訊求出位置。

在第6圖中，1至3是間隙感測器，測定3點的Z軸方向的位置。7至9是變位感測器，測定2點的X軸方向的移動距離與1點的Y軸方向的移動距離。亦即，從X軸方向2點、Y軸方向1點、Z軸方向3點的資訊求出位置。

在第7圖中，1至3是間隙感測器，測定3點的Z軸方向的位置。5與6是二維感測器，分別測定2點的X軸與Y軸方向的移動距離。亦即，從X軸方向2點、Y軸方向2點、Z軸方向3點的資訊求出位置。但是，對於在第4圖中，間隙感測器1至3的感測器頭是位於固定件側，而且標位於移動件側，惟在第7圖中，感測器頭是位於移動件側，而且標位於固定件側之處不相同。

在第8圖中，1至3是間隙感測器，測定3點的Z軸方向的位置。7至9是變位感測器，測定2點的Y軸方向的移動距離與1點的X軸方向的移動距離。亦即，從X軸方向1點、Y軸方向2點、Z軸方向3點的資訊求出位置。但是，對於在第5圖中，間隙感測器1至3的感測器頭是位於固定件側，而且標位於移動件側，惟在第8圖中，感測器頭是位於移動件側，而且標位於固定件側之處不相同。

在第9圖中，1至3是間隙感測器，測定3點的Z軸方向的位置。7至9是變位感測器，測定2點的X軸方向的移動距離與1點的Y軸方向的移動距離。亦即，從X軸方向2點、Y軸方向1點、Z軸方向3點的資訊求出位置。但是，對於在第6圖中，間隙感測器1至3的感測器頭是位於固定件側，而且標位於移動件側，惟在第9圖中，感測器頭是位於移動件側，而且標位於固定件側之處不相同。

在第10圖中，1至4是間隙感測器，測定4點的Z軸方向的位置，亦即，從Z軸方向4點的資訊求出位置。

在第11圖中，10是三維感測器，測定1點的Z軸方向的位置、滾動角、投擲角。11是三維感測器，測定1點的X軸方向的位置、Y軸方向的位置、遙動角。

又，針對於如第12圖的致動器構成的情形，將算出各致動器的推力指令的方向說明如下。

在第12圖中，31至36是致動器，成為具備1個發生X軸方向的力量的致動器，2個發生Y軸方向的力量的致動器，3個Z軸方向的力量的致動器的致動器構成。控制的自由度與致動器的數一致之故，因而該情形的推力變換演算式是如下地被求出。

將重心位置作為(xG、yG、zG)，將X軸致動器的位置作為(xx1、yz1、zx1)，將2個Y軸致動器的位置分別作為(xy1、yy1、zy1)、(xy2、yy2、zy2)，將3個Z軸致動器的位置分別作為(xz1、yz1、zz1)、(xz2、yz2、zz2)、(xz3、yz3、zz3)。將X軸致動器的推力作為Fx1、將2個Y軸致動器的推力分別作為Fy1、Fy2，將3個Z軸致動器的推力分別作為Fz1、Fz2、Fz3。將重心位置的並進推力的X軸成分、Y軸成分、Z軸成分分別作為Fx、Fy、Fz，而將重心周圍的力矩的X軸成分、Y軸成分、Z軸成成分別作為Tx、Ty、Tz。這時候可成立(1)式。

(1)式是成為正方行列而可求出逆行列之故，因而將其作為G，則可得到(2)式。

在推力變換演算器160，利用(2)式的演算，就可從多自由度平台的操作量算出各致動器的推力指令。

針對於具有冗長的致動器的如第13圖的致動器構成的情形，也考量分配推力指令的方法(例如參照專利文獻2)。

在第13圖中，31至37是致動器，成為具備1個發生X軸方向的力量的致動器，2個發生Y軸方向的力量的致動器，4個Z軸方向的力量的致動器的致動器構成。控制的自由度6，對於致動器的數為7之故，因而該情形的推力變換演算式是給予任何的拘束條件求出。例如在專利文獻2中，給予未作用著過度的力量的拘束條件。

如此地，習知的多自由度平台控制裝置是未具備用以選擇符合於控制對象的感測器構成的位置演算式的感測器構成輸入裝置，或是用以選擇符合於控制對象的致動器構成的致動器推力變換演算式的致動器構成輸入裝置，而使用專用的位置算出演算或是致動器推力變換演算。

專利文獻1：日本特開2001－159901號公報(第23頁至第24頁，第1圖至第8圖)。

專利文獻2：日本特開2006－72398號公報(第9頁、第2圖)。

習知的多自由度平台控制裝置是未具備感測器構成輸入裝置，或是致動器構成輸入裝置，無法使用從對應於各種感測器構成的位置演算式中選擇符合於控制對象的感測器構成的位置演算式，或是從對應於各種致動器構成的致動器推力變換演算式中選擇符合於控制對象的致動器構成的致動器推力變換演算式之故，因而沒有通用性，而對於感測器構成或致動器構成不相同的控制對象必須開發新又專用的控制裝置的問題。擬開發新又專用的控制裝置時，而在開發上成為需要較多的時間或成本。又，也容易產生軟體上的不方便而也有降低可靠性的問題。

本發明是鑑於此種問題點所創作者，從對應於各種感測器構成的位置演算式中選擇符合於控制對象的感測器構成的位置演算式，或是從對應於各種致動器構成的致動器推力變換演算式中選擇符合於控制對象的致動器構成的致動器推力變換演算式而作成可提高通用性，作成不依賴以控制演算或指令生成器等的軟體作為控制對象而共通地可使用，提供信賴性高的多自由度平台控制裝置，作為目的。

為了解決上述問題，本發明是如下地構成。

申請專利範圍第1項所述的發明是一種多自由度平台控制裝置，具備：生成複數自由度的指令的指令生成器；及由被安裝於控制對象的複數感測器訊號來算出複數自由度的位置的位置演算器；及由上述複數自由度的指令與上述複數自由度的位置來演算複數自由度的操作量的控制演算器；及由上述複數自由度的操作量來演算複數致動器的推力指令的推力變換演算器；及演算須流在上述複數致動器的電流指令的電流指令器，其特徵為：具備由複數感測器構成中用以選擇所期望的感測器構成的感測器構成輸入裝置；上述感測器構成輸入裝置是具備用以輸入在所選擇的感測器構成所使用的各感測器的位置座標的感測器位置輸入部；使用對應於所選擇的感測器構成的位置演算式來演算上述複數自由度的位置；上述位置演算器是將對應於複數感測器構成各個的位置演算式事先保有於記憶體上。

又本發明是，上述位置演算器是具備將對應於複數感測器構成各個的位置演算式，事後追加作為函數的功能。

又，本發明是，上述感測器構成輸入裝置是具備對於使用者所定義的X軸、Y軸、Z軸，分別輸入具備幾個感測器所用的感測器數輸入部。

又，本發明是，上述感測器構成輸入裝置是具備用以輸入在所選擇的感測器構成所使用的各感測器的種別的感測器種別輸入部。

又，本發明是，上述感測器種別輸入部是至少可選擇將絕對位置感測器與起動位置作成零的相對位置感測器。

又，本發明是，上述位置演算器是具備檢測出相對位置感測器的零位置訊號的零位置訊號檢測器，具備保存檢測零位置訊號時的感測器訊號。

又，本發明是，具備由複數致動器構成中用以選擇所期望的致動器構成的致動器構成輸入裝置，使用對應於所選擇的致動器構成的推力變換演算式來演算上述複數致動器的推力指令。

又，本發明是，上述推力變換演算器是將對應於複數致動器構成各個的推力變換演算式事先保有於記憶體上。

又，本發明是，上述推力變換演算器是具備將對應於複數致動器構成各個的推力變換演算式，事後追加作為函數的功能。

又，本發明是，上述致動器構成輸入裝置是具備對於使用者所定義的X軸、Y軸、Z軸，分別輸入具備幾個致動器所用的致動器數輸入部。

又，本發明是，上述致動器構成輸入裝置是具備用以輸入在所選擇的致動器構成所使用的各致動器的位置座標的致動器位置輸入部。

又，本發明是，上述致動器構成輸入裝置是具備用以輸入在所選擇致動器構成所使用的各致動器的種別的致動器種別輸入部。

依照本發明，可選擇複數感測器構成中選擇感測器構成，可提高位置演算部的通用性，可將多自由度平台控制裝置使用於各種控制對象。

又，依照本發明，可事後僅追加位置演算部的演算式，對於控制器未對應的感測器構成也可用以最小限的勞力加以追加對應。

又，依照本發明，可選擇感測器數，對於各種感測器數的控制對象可使用控制裝置。

又，依照本發明，可輸入感測器位置，對於各種感測器位置的控制對象可使用控制裝置。

又，依照本發明，可選擇感測器種別，對於各種感測器種別的控制對象可使用控制裝置。

又，依照本發明，對於使用靜電容量感測器或絕對值編碼器等的絕對位置感測器的控制對象，或是對於使用如相對值線性標度或雷射干擾計等地以起動位置作為零的相對位置感測器的控制對象也可使用控制對象。

又，依照本發明，對於使用相對位置感測器的控制對象，在實行原點復位動作之後，也在絕對位置可進行控制。

又，依照本發明，可選擇複數致動器構成中選擇致動器構成，可提高致動器推力變換演算部的通用性，可將多自由度平台控制裝置使用於各種控制對象。

又，依照本發明，可事後僅追加推力變換演算部的演算式，對於控制器未對應的致動器構成也可用以最小限的勞力加以追加對應。

又，依照本發明，可選擇致動器數，對於各種致動器數的控制對象可使用控制裝置。

又，依照本發明，可輸入致動器位置，對於各種致動器位置的控制對象可使用控制裝置。

又，依照本發明，可選擇致動器種別，對於各種致動器種別的控制對象可使用控制裝置。

以下，參照圖式來說明本發明的實施形態。

實施例1

第1圖是表示本發明的多自由度平台控制裝置的方塊圖。在圖中，100是成為多自由度平台控制裝置、220是成為致動器構成輸入裝置，控制裝置的使用者是使用致動器構成輸入裝置220來輸入致動器構成。160是推力變換演算器，因應於所輸入的致動器構成使得推力變換演算器160的演算式會變化。170是電流指令器，因應於所輸入的致動器構成使得演算式變化。又230是成為感測器構成輸入裝置。控制裝置的使用者，是使用感測器構成輸入裝置230進行輸入感測器構成。190是感測器訊號變換器，利用所輸入的感測器使得演算式變化。180是位置演算器，因應於所輸入的感測器構成使得位置演算器180的演算式變化。

本發明與習知技術不同的部分，是具備致動器構成輸入裝置220與感測器構成輸入裝置230的部分，及將推力演算器160、電流指令器170、位置演算器180、感測器訊號變換器190作為可變的部分。

第2圖是表示本發明的多自由度平台控制裝置的動作的流程圖。在以下，使用第2圖來說明其動作。多自由度平台控制裝置100是具備快閃記憶體或硬碟等的記憶裝置，保存使用者所輸入的致動器構成與感測器構成，則可省略每當起動時每次輸入的費時。在此，準備輸入致動器構成與感測器構成所用的控制事先準備模式，及在一定控制週期實際進行控制演算的控制模式。使用者是首先在控制事先準備模式僅一次輸入致動器構成與感測器構成而加以保存，而從下一次就以控制模式就可起動。

當起動多自由度平台控制裝置100，首先在步驟1進行判定為控制事先準備模式或控制模式。該判定是將模式選擇的畫面顯示在監測器而在使用者要求輸入，或是利用雙列直插開關等的硬體進行切換，或是準備快閃記憶體或硬碟等的記憶裝置，而利用軟體旗標進行切換也可以。首先使用時，作為控制事先準備模式進行起動。在控制事先準備模式中，輸入致動器構成與感測器構成，致動器構成是指致動器數，與該致動器的各個位置及種別，而感測器構成是指感測器數，與該感測器的各個位置及種別。在控制事先準備模式中，首先輸入感測器構成。首先，在步驟11輸入感測器數。感測器數是分別輸入幾個於X軸、Y軸、Z軸，之後，在步驟12輸入感測器位置。感測器位置是針對於在步驟11所輸入的數的各個感測器，將補償施加於感測器訊號的值成為零時的感測器檢測點的絕對位置輸入在X座標、Y座標、Z座標。但是，針對於該感測器補償，則在後續的步驟15進行輸入。然後，在步驟13選擇感測器種別。感測器種別是選擇靜電容量感測器或絕對值編碼器等的絕對位置感測器，或是如相對值線性標度或雷射干擾計等地將起動位置作為零的相對位置感測器。相對位置感測器的情形，在控制模式中，使用在剛起動之後將起動位置作為零的相對位置資訊來控制平台，俾實行復置動作。在復置動作中，保存著檢測出零位置訊號時的感測器訊號，結束復置動作之後，從實際的感測器訊號減掉零位置訊號檢測時的感測器訊號，藉此，與絕對位置感測器同樣地可進行操作。然後在步驟14輸入感測器感度，而在步驟15輸入感測器補償。

然後，輸入致動器構成。首先，在步驟21輸入致動器數。致動器數是分別輸入幾個於X軸、Y軸、Z軸，之後，在步驟22輸入致動器位置。致動器位置是針對於在步驟21所輸入的數的各個致動器，輸入推力中心點的X座標、Y座標、Z座標。之後，在步驟23選擇致動器種別。致動器種別是例如選擇音圈電動機或線性電動機。若為音圈電動機，則藉由位置會把電流與推力之關係成為大約一定之故，因而僅輸入推力常數就可以。若為線性電部機，則必須變化藉由位置所流動的電流之故，因而需要極間距與磁極位置的資訊，在步驟24，輸入推入常數。在步驟25，若致動器種別為音圈電動機，則移行至步驟1，而為線性電動機，則移行至步驟26，在步驟26，輸入極間距，而在步驟27輸入磁極位置。在步驟100，保存設定，而結束控制事先準備模式。視需要，在下一次起動時，在控制模式下能起動的方式設定旗標。

在控制模式中，在步驟31進行初期化處理。在控制事先準備模式依據輸入的資訊，來進行初期化演算，具體而言，初期化演算是包括有：讀取參數，設定變數的初期值，為了演算高速化的變數的演算等。在此，例如在步驟21所輸入的致動器數，若在X軸方向為1個，Y軸方向為2個，Z軸方向為3個，則從在步驟22所輸入的致動器位置求出(2)式的行列G。該行列是在推力變換器160所使用。然後，在步驟32，讀取感測器訊號，在步驟33，從感測器訊號算出感測器位置。具體上，例如檢測出X軸的絕對位置感測器的感測器訊號SX1被檢測出時的感測器位置(xw、yw、zw)是利用次式被算出。但是，將在步驟12所輸入的感測器位置作為(x1、y1、z1)，將在步驟15所輸入的感測器補償值作為Sofs，而將感測器感度作為K。

x _w ＝K (S _X1 －S _ofs )＋x ₁ y _w ＝y ₁ z _w ＝z ₁

然後，在步驟34，算出平台位置。平台位置的演算式是藉由感測器構成有所不同。在本發明的多自由度平台控制裝置中，將對應於第4圖至第11圖所表示的感測器構成的位置演算式全部保有在記憶體上，例如在所輸入的感測器數為X軸2個、Y軸2個、Z軸3個的情形，則使用第4圖的感測器構成的位置算出式，而X軸1個、Y軸2個、Z軸3個的情形，則使用第5圖的感測器構成的位置算出式。然後，因應於所選擇的感測器構成，與習知技術同樣地，以符合X軸、Y軸、Z軸方向的位置，及滾動、投擲、遙動的姿勢的6自由度來算出位置及姿勢。然後，在步驟35，算出平台的位置指令。之後，在步驟36，實行與習知同樣的控制演算。之後，在步驟37，實行推力變換演算，在推力變換演算中，使用在步驟31所求出的行列G來實行(2)式的演算，求出各致動器的推力指令。之後，在步驟38，實行電流指令演算。在電流指令演算中，將各致動器的推力指令變換成電流指令。例如在步驟23所輸入之致動器種別為音圈電動機的情形，則電流指令iref是利用次式被計算。但是，將在步驟24所輸入的推力常數作為Kvcm。

i _ref ＝K _vcm f _ref

又，例如在步驟23所輸入的致動器種別為3相的線性電動機的情形，則U相、V相、W相的電流指令iUref、iVref、iWref是利用下式被計算。但是，將在步驟24所輸入的推力常數作為Klm、而在步驟26所輸入的極間距作為Pi，又在步驟27所輸入的磁極位置作為Po。

i _Uref ＝K _lm f _ref cos((x －P _o )P _i )

然後，在步驟39中將在步驟38所求出的電流指令輸出至多自由度平台控制裝置100的電流放大器。多自由度平台控制裝置100是如所接受的電流指令地來控制各致動器的電流，而利用所發生的推力進行動作。之後，回到步驟32，一定的控制週期別地重複以上的演算。

如此地，具備感測器構成輸入裝置與致動器構成輸入裝置，將推力演算器、電流指令器、位置演算器、感測器訊號變換器作為可變的構成之故，因而從複數感測器構成，致動器構成中可選擇，可提高位置演算部與致動器推力變換演算部的通用性，而可將多自由度平台控制裝置使用於各種控制對象。又，首先就難以對應於所有感測器構成或是致動器構成的情形，則主要將常被使用的感測器構成或是致動器構成的演算式僅具備於記憶體上，而在欲追加所對應的感測器構成或致動器構成的情形，則事後將另外地位置算出式或推力變換演算式給予作為函數也可以，藉由此，將重新對應的感測器構成或致動器構成，作為追加的修改功能施以網路傳訊等的使用方法也成為可能。

依據感測器或致動器資訊施以自動演算，不必個別設定之故，因而不但可成為多自由度平台控制，還可成為適用於如機械手的多關節控制。

1、2、3、4‧‧‧間隙感測器

5、6‧‧‧二維感測器

7、8、9‧‧‧變位感測器

10、11‧‧‧三維感測器

21‧‧‧移動件

22‧‧‧固定件

31、32、33、34、35、36、37‧‧‧致動器

100‧‧‧多自由度平台控制裝置

110‧‧‧多自由度平台

140‧‧‧指令生成器

150‧‧‧控制演算器

160‧‧‧推力變換演算器

170‧‧‧電流指令器

180‧‧‧位置演算器

190‧‧‧感測器訊號變換器

220‧‧‧致動器構成輸入裝置

230‧‧‧感測器構成輸入裝置

第1圖是表示本發明的第1實施例的多自由度平台控制裝置的方塊圖。

第2圖是表示本發明的多自由度平台控制裝置的動作的流程圖。

第3圖是表示習知的多自由度平台控制裝置的方塊圖。

第4圖是表示習知的多自由度平台的感測器構成圖1。

第5圖是表示習知的多自由度平台的感測器構成圖2。

第6圖是表示習知的多自由度平台的感測器構成圖3。

第7圖是表示習知的多自由度平台的感測器構成圖4。

第8圖是表示習知的多自由度平台的感測器構成圖5。

第9圖是表示習知的多自由度平台的感測器構成圖6。

第10圖是表示習知的多自由度平台的感測器構成圖7。

第11圖是表示習知的多自由度平台的感測器構成圖8。

第12圖是表示習知的多自由度平台的致動器構成圖1。

第13圖是表示習知的多自由度平台的致動器構成圖2。