減振控制裝置的制作方法

專利名稱：減振控制裝置的制作方法
技術領域：
本發明涉及一種用于控制機器可動部位的運動的控制裝置，特別是涉及一種能夠抑制在可動部位產生的振動的控制裝置。
背景技術：
工業機械、機床、機器人等機械的可動部位，一般地在由驅動源經由各種各樣的傳動機構進行驅動的同時，作為控制對象控制可動部位的位置與速度。在此，在驅動源與可動部位之間存在的傳動機構等機械元件具有比較低的剛性。因此，像在使可動部位以高速移動停止在目標位置的情況下，具有在可動部位容易產生振動的傾向。
作為一個例子，對機器人手臂的傳動機構進行研究。圖21表示機器人手臂的傳動機構的結構。在圖21中，機器人手臂1經由由齒輪等構成的減速機構2，由電動機3驅動。控制機器人手臂1運動的沒有圖示的控制裝置，使用設置在電動機3上的位置/速度檢測器4檢測出的機器人手臂1的位置以及速度，進行位置以及速度的反饋控制，控制機器人手臂1的位置以及速度。
減速機構2一般具有較低的剛性。因此，圖21中表示的機器人手臂驅動系統能夠由圖22所示的二慣性系模型表示，作為從電動機3的電流(輸入)到機器人1的位置以及速度(輸出)的方框線圖。此外，為了簡化研究，忽略摩擦以及粘性的影響。另外，在圖22中，i為電動機電流，Tq為電動機轉矩，ωM為電動機速度，θM電動機位置，ωL為手臂速度，θL手臂位置，JM為電動機慣性，JL為手臂慣性，Kt為電動機轉矩常數，K為低剛性部位彈性系數，s為拉普拉斯算符。
在這樣的機器人手臂驅動系統中控制機器人手臂1的位置以及速度時，一般進行如圖23所示的半閉環控制。即，反饋由位置/速度檢測器4檢測出的電動機速度ωM以及電動機位置θM，進行位置以及速度的環控制。如果進行詳細敘述的話，如圖23所示，從位置指令θR減去被反饋的電動機位置θM求取位置偏差，根據位置控制增益Kp(函數)進行位置環處理，求取速度指令。而且，從該速度指令減去被反饋的電動機位置θM求取速度偏差，根據速度控制增益Kv(函數)進行速度環處理，求取電流指令i。而且，根據該電流指令i驅動電動機3。
上述的半閉環控制因為是對電動機3的位置以及速度的反饋控制，所以能夠很好地控制電動機3的位置以及速度。但是，如同上述，因為低剛性的減速機構2等傳動機構的存在，所以具有在機器人手臂1產生振動的問題。
作為該問題的對策，在機器人手臂1等機械的可動部位設置檢測其自身的位置以及/或者速度的檢測器，反饋可動部位的位置以及/或者速度的、如圖24所示的閉環控制一般在機床等機械上進行。圖24所示的方框線圖在反饋手臂位置θL以及手臂速度ωL取代反饋電動機位置θM以及電動機速度ωM這點上與圖23的方框線圖不同。在應用該閉環控制時，需要在機器人手臂等機械的可動部位設置用于檢測該可動部位的位置以及速度的檢測器。
此外，例如特開平6-178570號公報(JP-A-06-178570)公開了具有機械驅動系統的振動抑制功能的半閉環控制的控制裝置。該控制裝置為了抑制機械的振動，除了半閉環控制還在機械上設置速度檢測器以及加速度檢測器，反饋機械的速度以及加速度(即狀態變量)的檢測值，修正對電動機的轉矩指令。
這樣，在機械的可動部位設置檢測器時，為了得到來自檢測器的反饋信號需要電纜、信號接收電路等很多的附加裝置，因此整體系統的費用可能增加。此外，還存在各個裝置的維護費用上升的可能。進而，由于部件個數的增加，存在系統可靠性下降的可能。
作為其他的對策，人們所知的控制可動部位的方法為在控制電路中設置估計可動部位的狀態變量的觀測器(即狀態變量估計部)，反饋由觀測器估計的狀態變量所表示的位置以及速度，以代替像上述那樣設置直接檢測機械可動部位的位置和速度的檢測器。關于像這樣的狀態反饋式的控制方法，對估計的狀態變量、反饋方法提出了很多的方案。
例如，特開平7-20940號公報(JP-A-07-020940)公開了在進行半閉環控制的控制裝置中，具備估計電動機與機械負載之間的扭轉角以及扭轉角速度的觀測器的控制裝置。該控制裝置通過使用由觀測器所估計的扭轉角以及扭轉角速度修正對電動機的轉矩指令，抑制機械的振動。
在JP-A-07-020940中公開的狀態反饋控制方法是主要以控制對象的穩定化為目的的控制方法，并不使狀態變量積極地追蹤于目標值。參照圖25對該方法進行說明。圖25作為使用狀態反饋控制方法的一般的控制系統的一個例子，用方框圖表示機器人手臂等機械可動部位的位置以及速度的控制系統。在該例中，控制對象為驅動可動部位的電動機的狀態。
在圖示的控制系統中，根據位置指令θR與電動機位置θM的反饋信號的偏差，通過位置控制增益KP進行位置環控制求取速度指令，根據該速度指令與電動機速度ωM的反饋信號的偏差為基礎，通過速度控制增益KV求取轉矩指令(電流指令)。而且，從作為控制對象(電動機)操作量的該轉矩指令減去由觀測器反饋的估計狀態變量的修正量，求取修正轉矩指令，將該修正轉矩指令輸入給控制對象。
此處，觀測器輸出根據輸入給控制對象的修正轉矩指令與控制對象電動機速度ωM估計的狀態變量x1、x2。此時，例如估計狀態變量x1為可動部位(機器人手臂等)的位置θL的估計值，估計狀態變量x2為可動部位的速度ωL的估計值。將狀態反饋增益K1、K2分別乘以對應的狀態變量x1、x2進行反饋，修正操作量。這樣，圖示的控制方法不是使由觀測器估計的控制對象的狀態變量積極地追蹤于目標值的控制方法。
另一方面，特開2001-136767號公報(JP-A-2001-136767)公開的控制裝置為直線電動機的控制裝置，由觀測器估計直線電動機可動部位重心處的位置以及速度，將估計的位置以及速度分別反饋給位置控制器以及速度控制器，控制可動部位。
此外，特開2002-287804號公報(JP-A-2002-287804)公開的控制方法為在由觀測器估計控制對象的狀態變量的同時，根據目標值通過標準模型求取參照狀態變量，將狀態變量估計值與參照狀態變量的差反饋給控制器，控制控制對象。
這些公報公開的狀態反饋控制方法是使由觀測器估計的狀態變量積極地追蹤于目標值的控制方法。但是，存在由于適用的機械的結構，控制對象不穩定，得不到足夠的反饋增益的問題。此外，該反饋增益雖然需要根據機械的特性來決定，但得到該反饋增益的最佳值一般來說很困難。

發明內容
本發明的目的在于提供在用于控制機械可動部位的運動的控制裝置中，執行不需要在可動部位設置振動檢測器的狀態反饋式控制，在能夠有效地抑制可動部位振動的同時，通過使控制對象的狀態變量積極地追蹤于目標值，能夠確保穩定的響應性的控制裝置。
為了達到上述的目的，本發明提供的裝置，具備狀態變量估計部，其估計機械中控制對象的狀態變量，輸出估計狀態變量；標準模型，其輸出控制對象的理想控制量；補償器，其根據估計狀態變量與理想控制量的差，算出用于修正對控制對象的操作量的補償量；和反饋控制部，其根據目標指令值與估計狀態變量以及理想控制量的任意一個的偏差，求取操作量，在將根據補償器算出的補償量修正而得到的修正操作量輸入給控制對象的同時，將由反饋控制部求得的操作量輸入給狀態變量估計部以及標準模型。
在該結構中，如果控制對象是二慣性系，狀態變量可以是控制對象的負載側的加速度、速度以及位置的任意一個。
在上述控制裝置中，狀態變量可以是加速度、速度以及位置中的任意一種，補償量可以是補償電流或者補償轉矩。
補償器的傳遞函數可以具有與標準模型的傳遞函數的特性相反的逆特性。
此時，補償器的傳遞函數可以為對逆特性附加濾波的傳遞函數。
上述控制裝置在機械為機器人時，特別適用。


通過與附圖相關的以下恰當的實施方式的說明，本發明的上述以及其他的目的、特征以及優點會變得更加清楚。在該附圖中，圖1為表示涉及本發明的控制裝置的結構的方框圖。
圖2為表示包含本發明第1實施方式的控制裝置的機器人手臂控制系統的結構的方框圖。
圖3為表示模型參照型控制的一例的方框圖。
圖4為表示模型參照型控制的其他例的方框圖。
圖5為二慣性系模型中的從電流指令到手臂前端加速度的傳遞函數的伯德線圖(Bode diagram)。
圖6是表示在圖3的模型參照型控制中裝入觀測器的、圖2的控制裝置的第1控制例的結構的方框線圖。
圖7為圖6方框線圖中的從電流指令到手臂加速度的傳遞函數的伯德線圖。
圖8為用1型模型表示第1控制例中的從加速度指令到手臂前端加速度的伺服環的方框線圖。
圖9為表示對圖6的方框線圖附加加速度控制環的結構的方框線圖。
圖10為圖9的方框線圖中的從加速度指令到手臂加速度的傳遞函數的伯德線圖。
圖11為表示對圖9的方框線圖附加了位置以及速度控制環的第1控制例的整體結構的方框線圖。
圖12為圖11的方框圖中的從位置指令到手臂前端位置的傳遞函數的伯德線圖。
圖13為現有半閉環控制中的從位置指令到手臂前端位置的傳遞函數的伯德線圖。
圖14A為表示在圖11表示的控制系統中，針對位置指令階躍輸入的手臂前端位置的響應的圖。
圖14B為表示在現有半閉環控制中，針對位置指令階躍輸入的手臂前端位置的響應的圖。
圖15為圖2的控制裝置的第2控制例中的從速度指令到手臂前端速度的伺服環的簡易模型的方框線圖。
圖16為表示第2控制例中的從速度指令到手臂前端速度的結構的方框線圖。
圖17為表示對圖16的方框線圖附加了位置控制環的第2控制例的整體結構的方框線圖。
圖18為表示在圖17表示的控制系統中，針對位置指令階躍輸入的手臂前端速度的響應的圖。
圖19為圖2的控制裝置的第3控制例中的從位置指令到手臂前端位置的伺服環的簡易模型的方框線圖。
圖20為表示第3控制例中的從位置指令到手臂前端位置的結構的方框線圖。
圖21為簡略地表示機器人手臂的傳動裝置的結構的圖。
圖22為表示機器人手臂驅動系統中的從電動機電流(輸入)到位置以及速度(輸出)的結構的方框線圖。
圖23為現有半閉環控制的機器人手臂的位置以及速度控制的方框線圖。
圖24為現有閉環控制的機器人手臂的位置以及速度控制的方框線圖。
圖25為使用狀態反饋式控制方法的現有控制系統的方框圖。
具體實施例方式
下面，參照附圖詳細地說明本發明的實施方式。在附圖中，對相同或者類似的構成要素附以共有的參照符號。
在參照附圖時，圖1用方框圖表示涉及本發明的控制裝置的結構。涉及本發明的控制裝置具備觀測器(即狀態變量估計部)12，其估計表示控制對象10內部狀態的狀態變量α(實際無法測定)，輸出狀態估計變量α～，控制對象10是機械中實際要控制的特定部位；標準模型14，其輸出作為控制對象10的理想響應值的理想控制量α*；補償器16，其根據估計狀態變量α～與理想控制量α*的差，算出用于修正針對控制對象10的操作量u的補償量；和控制器(即反饋控制部)18，其根據目標指令值αR與估計狀態變量α～以及理想控制量α*的任意一方的偏差，求取操作量u。
在第1實施方式中，對控制器18輸入用于控制控制對象10的目標指令值αR與由觀測器12估計的估計狀態變量α～的偏差(用實線表示)。而且，將從作為控制器18的輸出的操作量u減去作為補償器16的輸出的補償量而得到的修正操作量v作為操作輸入，輸入給控制對象10。將修正操作量v與控制對象10的控制量y輸入給觀測器12，由這些修正操作量v與控制量y，觀測器12求取控制對象10的估計狀態變量α～。此外，對標準模型14輸入修正操作量v，通過該輸入求取控制對象10的理想控制量α*。補償器16根據從作為觀測器12的輸出的估計狀態變量α～減去作為標準模型14的輸出的理想控制量α*而得到的差，求取操作量u的修正值。根據該修正值，修正操作量u得到修正操作量v。
根據具有上述結構的本發明的控制裝置，執行使作為控制對象10的理想響應值的理想控制量α*與由觀測器12估計的控制對象10的估計狀態變量α～相互一致的控制。進而，反饋作為觀測器12的輸出的估計狀態變量α～，通過將控制對象10的目標指令值αR與估計狀態變量α～的偏差輸入給控制器18，執行使控制對象10追蹤于目標指令值αR的控制。結果，因為觀測器12所估計的控制對象10的估計狀態變量α～收斂于控制對象10的實際的狀態變量α，所以能夠使控制對象10的狀態變量α高精度地追蹤于目標指令值αR。
在上述結構中，選擇加速度作為狀態變量α時，能夠使控制對象10的加速度高精度地追蹤于目標指令值αR。同樣，如果選擇位置或者速度作為狀態變量α，能夠使控制對象10的速度或者位置高精度地追蹤于目標指令值αR。在所有這些情況時，作為補償器16的輸出的補償量為補償轉矩。此外，因為沒有必要為了控制機械可動部位(例如機器人手臂前端)的位置、速度或者加速度而在機械的可動部位設置檢測這些變量的檢測器，所以能夠解決費用升高和可靠性下降的問題，而且，通過使控制對象10的狀態變量α積極地追蹤于目標指令值αR，能夠抑制機械可動部位振動的發生，同時確保按照位置、速度或者加速度的指令值的穩定的響應性。
在圖1表示的結構中，補償器16的傳遞函數可以具有與標準模型14的傳遞函數的特性相反的逆特性。根據該結構，能夠使針對修正操作量v的控制對象10的狀態變量α的響應特性與標準模型14的響應特性相同。但是，作為標準模型14因為通常選擇特有的函數，所以其逆特性的函數分子的階數比分母的階數大。因此如同后面敘述的那樣，希望補償器16除上述逆特性之外，還具備具有該相對階數以上的階數的濾波。
此外，在圖1中也可以采用將向控制對象10的目標指令值αR與作為標準模型14的輸出的理想控制量α*的偏差輸入給控制器18的結構(由虛線表示)，來取代將向控制對象10的目標指令值αR與作為觀測器12的輸出的估計狀態變量α～的偏差輸入給控制器18的結構(由實線表示)。這是因為被控制成使由觀測器12所估計的控制對象10的狀態變量α與由標準模型14輸出的控制對象10的理想控制量α*一致。
然后，參照圖2至圖20，對適用于機器人手臂控制的涉及本發明第1實施方式的控制裝置的結構進行說明。
如圖2所示，機器人中的控制對象10為設置在機器人上的多個控制軸的每一個，例如參照圖21被構成為具備已敘述的機器人手臂1、減速機構2、電動機3以及位置/速度檢測器4。此外，內部含有涉及本發明第1實施方式的控制裝置的機器人控制裝置20具有將控制整體系統的主CPU22與ROM24、ROM26、接口28、輸入輸出電路30以及公共存儲器32經由總線34相互連接的結構。將采用觸摸懸吊等方式形成的顯示/操作器36與接口28連接，將安裝在機器人手臂1的前端的機械手等外部裝置38與輸入輸出電路30連接。此外，將用于控制驅動機器人手臂1的電動機3的位置以及速度的伺服CPU40與公共存儲器32連接，將控制對象10中的電動機3與伺服放大器42連接。
主CPU22根據在ROM24中存儲的系統程序控制圖示的機器人控制系統整體，同時根據在ROM26中存儲的示范程序，將向機器人的各個控制軸的運動指令經由公共存儲器32向伺服CPU40輸出。伺服CPU40根據該運動指令和來自設置在電動機3上的位置/速度檢測器4的位置以及速度的反饋信號，進行位置以及速度的反饋控制，向伺服放大器42輸出電流指令。而且，服放大器42執行上述的涉及本發明的狀態反饋式控制，以使實際流入電動機3的電流成為遵從該電流指令的電流。
然后，作為通過伺服放大器42進行控制的第1例，對控制對象10的狀態變量α(圖1)為機器人手臂1前端部位的加速度的情況進行說明。
包括機器人手臂1的控制對象10的模型為參照圖22已敘述的二慣性系模型。該控制對象10能夠通過下面的狀態方程式(1)來表示。
x1′x2′x3′=00-JLJM001KJL-KJM0·x1x2x3+KtJM00·i---(1)]]>y=100·x1x2x3]]>在方程式(1)中，x1、x2、x3全部為狀態變量，x1′、x2′、x3′為各個狀態變量的微分值。此外，參照圖22，x1＝ωM(電動機速度)、x2＝ωL(手臂速度)、x3＝aL(手臂加速度)。此外，y為控制對象10的控制量(圖1)，y＝x1＝ωM。此外，圖1中的修正操作量v為狀態方程式(1)的電動機電流i(圖22)(即v＝i)。
觀測器12能夠由下面的方程式(2)表示。
x1~′x2~′x3~′=-K10-JLnJMn-K201KnJLn-K3-KnJMn0·x1~x2~x3~+KtnJMn00·i+K1K2K3·y---(2)]]>在方程式(2)中，x1～、x2～、x3～；為狀態變量x1、x2、x3的估計值(估計狀態變量)，x1～′、x2～′、x3～′為其微分值。此外，JMn為電動機慣性JM的標稱值，JLn為手臂慣性JL的標稱值，Kn為低剛性部彈性系數K的標稱值，Ktn為電動機轉矩常數Kt的標稱值，K1、K2、K3為觀測器參數。此外，因為關于觀測器理論廣為人知，所以省略詳細的說明。此外，如果將干擾加在狀態變量中能夠期待估計精度更加提高，但是在此處為了簡單地進行說明，采用不考慮干擾的觀測器。
而且，我們知道如果選擇觀測器參數K1、K2、K3使觀測器系統穩定，那么估計狀態變量x～收斂于實際的狀態變量x。此外在該例中，因為控制對象10的狀態變量為手臂加速度，所以觀測器使用表示手臂加速度aL的狀態變量x3的估計值x3～作為觀測器12(圖1)的輸出。
此處，對于在本發明中所采用的模型參照型控制說明其概要。
在圖3表示的模型參照型控制的方框線圖中，從操作量u到控制量y的傳遞函數由下面的式(3)表示。
yu=GP(s)1+GP(s)·H(s)-Gm(s)·H(s)---(3)]]>在圖3以及式(3)中，u為操作量，y為控制量，Gp(s)為控制對象10的傳遞函數，Gm(s)為標準模型14(圖1)的傳遞函數，H(s)為補償器16(圖1)的傳遞函數。
在式(3)中，如果使補償器16的傳遞函數H(s)為標準模型14的傳遞函數Gm(s)的反函數，即H(s)＝1/Gm(s)，那么從操作量u到控制量y的傳遞函數成為下面的式(4)，成為與標準模型14的傳遞函數Gm(s)相等的函數。
yu=GP(s)1+GP(s)Gm(s)-Gm(s)Gm(s)=Gm(s)---(4)]]>此外，在圖4表示的模型參照型控制的方框線圖中，從操作量u到控制量y的傳遞函數由下面的式(5)表示。
yu=Gp(s)·1+Gm(s)·H(s)1+GP(s)·H(s)---(5)]]>此處，如果采用足夠大的增益函數作為補償器16的傳遞函數H(s)，如式(6)所示，y/u約等于標準模型14的傳遞函數Gm(s)。
yu≈GP(s)·Gm(s)·H(s)GP(s)·H(s)=Gm(s)---(6)]]>
因此，無論進行圖3以及圖4的哪個模型參照型控制，都能夠達成本發明的目的，但在此處說明的控制例是進行圖3所示的模型參照型控制的例子。
在圖22所示的機器人手臂控制的二慣性系模型中，從電流指令i(對應操作量u)到手臂前端加速度αL的傳遞函數由下面的式(7)表示。
aLi=Kt·KJL·JMS2+(JL+Jm)·K=KtJL+Jm·1JL·JM(JL+Jm)·K·s2+1---(7)]]>此處，設電動機轉矩常數Kt＝5[kgf·cm/A]、電動機慣性JM＝0.01[kgf·cm/s2]、手臂慣性JL＝0.1[kgf·cm/s2]、彈性系數K＝200[kgf/rad]，由式(7)表示的傳遞函數的伯德線圖成為在圖5表示的線圖。
如圖5的伯德線圖所示，該傳遞函數(7)的響應不穩定。因此，為了使響應穩定采用具有標準2階系統的傳遞函數的標準模型14。標準模型14的傳遞函數Gm(s)由下面的式(8)表示。
Gm(s)=KtmJm·11ωn2·s2+2·ζωn·s+1---(8)]]>在式(8)中，Jm為標準模型慣性，Ktm為標準模型轉矩常數，ωn為模型的共振角頻率，ζ為阻尼系數。
表示標準模型14的逆特性的補償器16的傳遞函數，即標準模型14的傳遞函數Gm(s)的逆傳遞函數Gm(s)-1由下面的式(9)表示。
Gms-1=JmKtm·[1ωn2·s2+2·ζωn·s+1]---(9)]]>但是，由上式(9)表示的逆傳遞函數Gm(s)-1為非特有，具有微分要素非常強的特性，難以處理。因此，作為補償器16的傳遞函數H(s)，采用由下面式(10)表示的函數。
H(s)=Gm(s)-1·F(s)=JmKtm·1ωn2·s2+2·ζωn·s+11ωf2·s2+2·ζfωf·s+1---(10)]]>在式(10)中，F(s)為濾波的傳遞函數，由下面的式(11)表示。此外，ωf、ζf為濾波參數。
F(s)=ωf2s2+2·ζf·ωf·s+ωf2---(11)]]>這樣，將標準模型14的傳遞函數Gm(s)作為式(8)的函數，將補償器16的傳遞函數Gm(s)-1作為式(10)的函數時的，將由之前的狀態方程式(2)表示的、裝入觀測器12的控制裝置的方框線圖在圖6中表示。
此處，確定觀測器參數K1、K2、K3，以使使用控制對象10的標稱值、觀測器極配置成為120Hz的巴特沃斯(Butterworth)模式的極配置，關于標準模型14設定為參照轉矩常數Ktm＝5，標準模型慣性Jm＝0.1，模型的共振角頻率ωn＝2·π·20，阻尼系數ζ＝0.7，關于補償器16，在設定濾波的參數ωf＝2·π·100[Hz]，ζf＝1時，在圖6所示的方框線圖中，從作為操作量u的電流指令i′(i′＝u)到作為控制量的手臂加速度aL的傳遞函數的伯德線圖成為圖7所示的線圖。此外，如圖6所示，通過補償器16求得的補償量為補償電流(或者補償轉矩)τ，從電流指令i′減去補償電流τ而求得的新的電流指令i成為修正操作量v(i＝v)。
如同圖7清楚地表示的那樣，已顯著地將控制對象10穩定化。
因此，將從電流指令i′(＝u)到手臂加速度aL(控制量)的傳遞函數作為新的控制對象10，構筑用于確保針對加速度指令的手臂前端加速度的指令追蹤性的伺服環。此處，為了進行簡單地說明，采用圖8所示的1型伺服系統。該伺服系統的傳遞函數由下面的式(12)表示。此外，在圖8以及式(12)中，aR為加速度指令，Ka為加速度環增益。
aLaR=Ka·KtmJm·s+Ka·Ktm=1JmKa·Ktm·s+1---(12)]]>式(12)中，如果Ka＝Jm/(T·Ktm)，那么該傳遞函數成為下面的式(13)。
aLaR=1T·s+1---(13)]]>即，傳遞函數成為時間常數T(s)的一次響應，其結果為不發生振動，對沒有穩定偏差的追蹤性實現良好的加速度控制環。
將在圖6的模型參照型控制環的外側設置的上述加速度控制環的系統用方框線圖在圖9中表示。
圖10表示裝入加速度控制環的圖9方框線圖中的從加速度aR到手臂加速度aL的傳遞函數的伯德線圖。此時，選定加速度環增益Ka＝Jm/Ktm(1/2/π/10)。
如圖10所示，實現極其穩定的控制響應。此處，如果只使觀測器12符合控制對象10的特性，那么標準模型14、補償器16以及加速度環增益與控制對象10的特性沒有關系，能夠廣泛地使用。
圖11通過方框線圖表示在圖9的方框線圖附加了位置以及速度環的、第1例的機器人手臂控制的整體結構。圖12表示在圖11的方框線圖中，從位置指令θR到將手臂加速度aL進行2重積分得到的手臂前端位置θL的傳遞函數的伯德線圖。此外為了進行比較，將圖23半閉環控制中的從位置指令θR到手臂前端位置θL的傳遞函數的伯德線圖在圖13中表示。此外，在圖12以及圖13中，將位置環帶寬設為2Hz，將速度環帶寬設為5Hz(因為機械共振頻率為7Hz)。
如同根據圖12與圖13的比較理解的那樣，通過本發明能夠實現穩定的控制。
在此，在圖14A中表示由位置指令θR進行了階越輸入時的手臂前端位置θL的響應。與此相對，在圖14B中表示對于圖23的半閉環控制中的階越輸入的響應。如同根據圖14A與圖14B的比較理解的那樣，對于現有的半閉環控制中發生振動，通過本發明能夠進行不發生振動、穩定的控制。
然后，作為通過增益放大器42(圖2)的控制的第2例，將控制對象10的狀態變量α(圖1)為機器人手臂1(圖2)的前端部位的速度的情況進行說明。對于包含機器人手臂1的控制對象10，從指令電流i到手臂前端速度ωL的傳遞函數由下面的式(14)表示。
ωLi=Kt(JL+JM)·s·1JL·JM(JL+JM)·K·s2+1---(14)]]>因此，標準模型14的傳遞函數Gm(s)由下面的式(15)表示。
Gm(s)=KtmJm·s·11ωn2·s2+2·ζωn·s+1---(15)]]>因為該標準模型14的傳遞函數Gm(s)的相對階數為3次，所以作為補償器16的傳遞函數H(s)，采用由下面的式(16)表示的函數。
H(s)=Gm(s)-1·F(s)=Jm·sKtm·1ωn2·s2+2·ζωn·s+1a3·s3+·a2·s2+·a1·s+1---(16)]]>選定圖(16)中的濾波參數a1～a3以使系統穩定。此外，觀測器12(圖1)使用與上述第1例相同的觀測器，作為與標準模型14的輸出相比較的觀測器輸出，使用與估計手臂速度ωL～對應的估計狀態變量x2～。
此處，如果標準模型14的固有角頻率ωn對于速度控制環的響應頻率足夠高，那么作為設計速度環時的控制對象10，可以使用由下面的式(17)表示的傳遞函數。
G(s)=KtmJm·s---(17)]]>
將上述傳遞函數Gm(s)作為控制對象10，使用由圖15所示的方框線圖表示的簡易模型來決定速度控制增益。此外，ωR為速度指令，Kv為速度環增益。
圖15的簡易模型的傳遞函數由下面式(18)表示。
ωLωR=Kv·KtmJm·s+Kv·Ktm=1JmKv·Ktm·s+1---(18)]]>在式(18)中，如果速度環增益Kv＝Jm/(Ktm·T)，那么式(18)的傳遞函數成為下面的式(19)，能夠得到沒有時間常數T(s)的振動的穩定的響應。
ωLωR=1T·s+1---(19)]]>圖16表示使控制對象10的狀態變量為手臂前端速度ωL時的，從速度指令ωR到手臂前端速度ωL的方框線圖。此外，圖17表示在圖16的方框線圖中裝入位置環的結構。
在圖17的裝入了位置環的控制系統中，根據位置指令θR已進行階躍輸入時的手臂前端速度的響應在圖18中表示。由圖可知，得到沒有振動的穩定的控制系統。
此外，在上述第2例中，對控制對象10、觀測器12和標準模型14內的標準2階系統參數、Ktm以及Jm使用與之前在第1例中使用的相同的值。而且，對于濾波參數a1至a3選用滿足由下面的式(20)表示的條件的參數。此外，在式(20)中，δ＝1/ωf，設ωf＝2·π·100[Hz]。并且，設速度控制增益Kv＝Jm/Ktm/(1/2/π/5[Hz])，位置控制增益Kp＝2·π·2[Hz]。
α3·s3+α2·s2+α1·s+1＝0.15σ3·s3+0.5σ2·s2+σ·s+1 (20)然后，作為通過伺服放大器42(圖2)進行控制的第3例，對控制對象10的狀態變量α(圖1)為機器人手臂1(圖2)的前端部位的位置的情況進行說明。
其中，如果使手臂前端加速度包括在狀態函數內，那么控制對象10用狀態方程式(21)表示。在狀態方程式(21)中，狀態變量x1為電動機速度ωM，x2為電動機位置θM，x3為手臂速度ωL，X4為手臂位置θL。而且，為了提高觀測性，將控制量y設為電動機位置x2(＝θM)。
x1′x2′x3′x4′=0-KJM0KJM10000KJL0-KJL0010·x1x2x3x4KtJM000·i---(21)]]>y=0100·x1x2x3x4]]>觀測器12(圖1)由下面的狀態方程式(22)表示。
x1~′x2~′x3~′x4~′=-K1-KnJMn0KnJMn1-K2000-K3KnJLn0-KnJLn-K4010·x1~x2~x3~x4~+KtnJMn000·i+K1K2K3K4·y---(22)]]>在狀態方程式(22)中，x1～、x2～、x3～、x4～為狀態變量x1、x2、x3、x4的估計值，x1～’、x2～’、x3～’、x4～’為其微分值。此外，JMn為電動機慣性JM的標稱值，JLn為手臂慣性JL的標稱值，Kn為低剛性部位彈性系數K的標稱值，Ktn為電動機轉矩常數Kt的標稱值，K1、K2、K3以及K4為觀測器參數。
對于控制對象10的從電流指令i到手臂前端位置θL的傳遞函數由下面的式(23)表示。
θLi=Kt(JL+JM)·s2·1JL·JM(JL+JM)·K·s2+1---(23)]]>因此，標準模型14的傳遞函數Gm(s)由下面的式(24)表示。
Gm(s)=KtmJm·s2·11ωn2·s2+2·ζωn·s+1---(24)]]>因為該標準模型14的傳遞函數Gm(s)的相對階數為4次，所以作為補償器16的傳遞函數H(s)，采用由下面的式(25)表示的函數。在補償器16的濾波參數中，選擇系統穩定的參數。
H(s)=Gms-1·F(s)=Jm·s2Ktm·1ωn2·s2+2·ζωn·s+1a4·s4+a3·s3+·a2·s2+·a1·s+1---(25)]]>此處，標準模型14的固有角頻率ωn如果對于位置控制環的響應頻率足夠高，那么作為設計位置環時的控制對象10，可以使用由下面的式(26)表示的傳遞函數。
G(s)=KtmJm·s2---(26)]]>將上述傳遞函數Gm(s)作為控制對象10，使用由圖19中的方框線圖表示的簡易模型(使用P-D控制器，以使容易穩定環傳遞函數)決定位置控制增益。此外，θR為指令位置，Kp1為位置環比例增益，Kp2為位置環微分增益。
圖19的簡易模型的傳遞函數由下面的式(27)表示。
θLθR=Kv1·KtmJm·s2+Kv2·Ktm·s+Kv1·Ktm=1JmKv1·Ktm·s2+Kv2Kv1·s+1---(27)]]>在式(27)中，如果確定位置環比例增益Kp1由式(28)表示，位置環微分增益Kp2由式(29)表示，那么式(27)的傳遞函數成為下面的式(30)，能夠得到任意穩定的響應。此外，在式(28)至(30)中，ωc為位置控制響應的固有角頻率，ζc為阻尼系數。
Kv1=ωc2·JmKtm---(28)]]>Kv2=2·ζcωc·JmKtm---(29)]]>θLθR=11ωc2s2+2·ζcωc·s+1---(30)]]>圖20表示將控制對象10的狀態變量作為手臂前端位置θL時的、從指令位置θR到手臂前端位置θL的方框線圖。
如同上述說明那樣，通過本發明能夠按照指令控制控制對象，得到沒有振動的穩定的響應性。此外，因為能夠使控制對象的響應與標準模型的響應相等，所以能夠將標準模型視作控制對象設計反饋環，因此，可以容易地設計、調整增益。另外，因為沒有必要在控制對象上直接設置振動檢測用的檢測器，所以通常，本發明在沒有在手臂前端設置傳感器的機器人手臂的控制中特別有效。
以上結合適當的實施方式對本發明進行了說明，但在不脫離后面記述的權利要求的范圍以及精神進行各種各樣的修正以及變更，應該會被本行業人員所理解。
權利要求
1.一種控制裝置，其特征在于，具備狀態變量估計部(12)，其估計機器中的控制對象(10)的狀態變量(α)，輸出估計狀態變量(α～)；標準模型(14)，其輸出控制對象的理想控制量(α*)；補償器(16)，其根據上述估計狀態變量與上述理想控制量的差，算出用于修正針對控制對象的操作量(u)的補償量；和反饋控制部(18)，其根據目標指令值(αR)與上述估計狀態變量以及上述理想控制量的任意一方的偏差，求取上述操作量，將通過上述補償器算出的上述補償量修正得到的修正操作量(v)輸入給控制對象，同時將由上述反饋控制部求得的上述操作量輸入給上述狀態變量估計部以及上述標準模型。
2.根據權利要求1所述的控制裝置，其特征在于，上述狀態變量為加速度、速度以及位置中的任意一種，上述補償量為補償電流或者補償轉矩。
3.根據權利要求2所述的控制裝置，其特征在于，上述控制對象為二慣性系，上述狀態變量為該控制對象的負載側的加速度、速度以及位置中的任意一種。
4.根據權利要求1所述的控制裝置，其特征在于，上述補償器的傳遞函數具有與上述標準模型的傳遞函數的特性相反的逆特性。
5.根據權利要求4所述的控制裝置，其特征在于，上述補償器的傳遞函數為對上述逆特性附加濾波的函數。
6.根據權利要求1至5中任意一項所述的控制裝置，其特征在于，上述機器為機器人。
全文摘要
抑制在可動部位產生的振動，同時控制機械可動部位運動的控制裝置。該控制裝置具備狀態變量估計部，其估計機械中控制對象的狀態變量，輸出估計狀態變量；標準模型，其輸出控制對象的理想控制量；補償器，其根據估計狀態變量與理想控制量的差，算出用于修正對控制對象的操作量的補償量；和反饋控制部，其根據目標指令值與估計狀態變量以及理想控制量的任意一方的偏差，求取操作量。而且，將通過補償器算出的補償量修正得到的修正操作量輸入給控制對象，同時將由反饋控制部求得的操作量輸入給狀態變量估計部以及標準模型。例如，狀態變量是加速度、速度以及位置中的任意一種，補償量是補償轉矩。
文檔編號G05B13/02GK1680893SQ200510063280
公開日2005年10月12日 申請日期2005年4月7日 優先權日2004年4月8日
發明者二瓶亮, 加藤哲朗, 有田創一 申請人:發那科株式會社