TWI494725B - Control device, control method and compensating method of position command - Google Patents
Control device, control method and compensating method of position command Download PDFInfo
- Publication number
- TWI494725B TWI494725B TW101148149A TW101148149A TWI494725B TW I494725 B TWI494725 B TW I494725B TW 101148149 A TW101148149 A TW 101148149A TW 101148149 A TW101148149 A TW 101148149A TW I494725 B TWI494725 B TW I494725B
- Authority
- TW
- Taiwan
- Prior art keywords
- load
- pair
- transition
- command
- dynamic model
- Prior art date
Links
- 239000011159 matrix material Substances 0.000 claims description 38
- 230000001052 transient Effects 0.000 claims description 3
- 239000000969 carrier Substances 0.000 description 11
- 230000001360 synchronised Effects 0.000 description 9
- 230000001808 coupling Effects 0.000 description 6
- 238000010168 coupling process Methods 0.000 description 5
- 238000005859 coupling reaction Methods 0.000 description 5
- 230000001131 transforming Effects 0.000 description 5
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000010985 leather Substances 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 230000000875 corresponding Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011089 mechanical engineering Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006011 modification reaction Methods 0.000 description 1
- 238000005381 potential energy Methods 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Description
本發明是有關於一種適用於龍門式定位平台的控制裝置、控制方法以及位置命令補償方法。The invention relates to a control device, a control method and a position command compensation method suitable for a gantry positioning platform.
精密機械產業為我國重點發展產業之一,而精密定位技術對整個精密機械產業更有著相當的重要性。精密定位技術是製造產品、測量物體尺寸、運轉各種機器之機械工程上的重要技術之一。隨著精密工程的不斷進步,不論是半導體產業、精密機械工業、生物細胞領域、光電系統、顯微機構、表面工程、掃描探針顯微鏡等方面,皆朝微小化精密化的方向前進,因此對於奈米或微米級的定位系統需求量日增,目前在工業界已經使用很多精密定位的儀器。The precision machinery industry is one of the key development industries in China, and the precision positioning technology is of considerable importance to the entire precision machinery industry. Precision positioning technology is one of the important technologies in the mechanical engineering of manufacturing products, measuring object size and operating various machines. With the continuous advancement of precision engineering, no matter in the semiconductor industry, precision machinery industry, biological cell field, photoelectric system, micro-structure, surface engineering, scanning probe microscope, etc., all are moving in the direction of miniaturization and precision, so The demand for positioning systems in the nanometer or micron range is increasing, and many precision positioning instruments have been used in the industry.
一直以來高速度與高精確度都是工具機發展的目標,但應用於不同目的之工具機在不同場合上所需克服之問題也不盡相同,當所需生產之工件越來越大時,工具機的體積也越大,例如近年來LCD大尺寸面板的生產與大型太陽能板的製造等,所以在類似的應用場合中採用龍門式(Gantry)設計架構也越來越多。以往在X-Y平台或是多軸加工機的控制應用中,各軸僅由單組馬達所驅動,然而為了符合高加速、高推力和高剛性的需求,龍門式精密運動控制系統採用雙平行線性馬達共同驅動單軸之平行系統,即具機構耦合之雙線性伺服系統便順勢而生。在此架構 下,各組馬達之間的位置誤差,因機構耦合之故,除影響精度外,亦可能使耦合機構產生變形,造成受控系統的損壞,甚者危害工作人員的安全。因此確保雙平行線性馬達之同步運動也成為一相當重要的研究課題。High speed and high precision have always been the development goals of the machine tool, but the machine tools used for different purposes have different problems to be overcome in different occasions. When the workpieces to be produced are getting bigger and bigger, The size of the machine tool is also larger. For example, in recent years, the production of large-size LCD panels and the manufacture of large-scale solar panels have led to the increasing use of Gantry design architectures in similar applications. In the past, in the control applications of XY platform or multi-axis machine, each axis was driven by a single motor. However, in order to meet the requirements of high acceleration, high thrust and high rigidity, the gantry precision motion control system uses a double parallel linear motor. A parallel system that drives a single axis, that is, a bilinear servo system with mechanical coupling, is born. In this architecture Under the above, the position error between the motors of each group, due to the coupling of the mechanism, can affect the coupling mechanism, cause damage to the controlled system, and even endanger the safety of the staff. Therefore, ensuring the synchronous motion of the dual parallel linear motor has also become a very important research topic.
美國專利公告號US 7,531,981、美國專利公告號US 5,646,495及台灣專利公告號TW 200729673揭露了有關於雙平行線性馬達的同步運動補償。Synchronous motion compensation for dual parallel linear motors is disclosed in U.S. Patent No. 7,531,981, U.S. Patent No. 5,646,495, and Taiwan Patent Publication No. TW 200729673.
本發明提供一種控制裝置,用以確保龍門式定位平台之雙平行線性馬達的同步運動。The invention provides a control device for ensuring synchronous movement of a double parallel linear motor of a gantry positioning platform.
本發明提供一種控制方法,用以確保龍門式定位平台之雙平行線性馬達的同步運動。The present invention provides a control method for ensuring synchronized motion of a dual parallel linear motor of a gantry positioning platform.
本發明提供一種位置命令補償方法,用以補償龍門式定位平台之三軸的位置命令。The invention provides a position command compensation method for compensating a position command of a three-axis of a gantry type positioning platform.
本發明提出一種控制裝置,適用於一龍門式定位平台。龍門式定位平台具有一對分別沿著一對相互平行的承載軸線延伸的承載滑軌、兩端分別耦合於這對承載滑軌且沿著垂直於這對承載軸線的負載軸線延伸的一負載滑軌、耦合於負載滑軌的一負載滑塊、用以驅動負載滑軌在這對承載滑軌上移動的一對承載馬達、以及用以驅動負載滑塊在負載滑軌上移動的一負載馬達。控制裝置包括下列元件。一正向座標轉換器,依照一動態模型將一位置命令從一第一座標空間轉換到一第二座標空間以產生一過渡位置 命令,其中動態模型依照龍門式定位平台的物理參數來建立。一組位置控制器,將過渡位置命令轉換為一過渡速度命令。一第一逆向座標轉換器,依照動態模型將來自這對承載馬達及負載馬達的一速度迴授轉換為一過渡速度迴授,其中過渡速度迴授與過渡速度命令比較以產生一過渡速度誤差。一組速度控制器,將過渡速度誤差轉換為過渡轉矩命令。一第二逆向座標轉換器,依照動態模型將過渡轉矩命令從第二座標空間轉換到第一座標空間以產生一轉矩命令來驅動這對承載馬達及負載馬達。The invention provides a control device suitable for a gantry type positioning platform. The gantry type positioning platform has a pair of load-bearing slide rails respectively extending along a pair of mutually parallel load-bearing axes, and two ends coupled to the pair of load-bearing slide rails respectively and extending along a load axis perpendicular to the pair of load-bearing axes a rail, a load slider coupled to the load rail, a pair of load carrying motors for driving the load rail to move on the pair of load rails, and a load motor for driving the load slider to move on the load rail . The control device includes the following components. a forward coordinate converter that converts a position command from a first coordinate space to a second coordinate space in accordance with a dynamic model to generate a transition position Command, where the dynamic model is built according to the physical parameters of the gantry positioning platform. A set of position controllers that convert the transition position command into a transition speed command. A first reverse coordinate converter converts a speed feedback from the pair of load carrying motors and load motors into a transient speed feedback in accordance with a dynamic model, wherein the transition speed feedback is compared to a transition speed command to produce a transition speed error. A set of speed controllers that convert the transition speed error into a transition torque command. A second reverse coordinate converter converts the transition torque command from the second coordinate space to the first coordinate space in accordance with the dynamic model to generate a torque command to drive the pair of load bearing motors and the load motor.
本發明提出一種控制方法,適用於一龍門式定位平台。龍門式定位平台具有一對分別沿著一對相互平行的承載軸線延伸的承載滑軌、兩端分別耦合於這對承載滑軌且沿著垂直於這對承載軸線延伸的一負載滑軌、耦合於負載滑軌的一負載滑塊、一對用以驅動負載滑軌在這對承載滑軌上移動的承載馬達以及用以驅動負載滑塊在負載滑軌上移動的的一負載馬達。控制方法包括下列步驟。依照龍門式定位平台的物理參數建立一動態模型。依照動態模型將一位置命令從一第一座標空間轉換到一第二座標空間以產生一過渡位置命令。將過渡位置命令轉換為過渡速度命令。依照動態模型將來自這對承載馬達及負載馬達的一速度迴授轉換為一過渡速度迴授,其中過渡速度迴授與過渡速度命令比較以產生一過渡速度誤差。將過渡速度誤差轉換為過渡轉矩命令。依照動態模型將一過渡轉矩命令從第二座標空間轉換到第一座標空間以產生一轉矩命令。依照 轉矩命令驅動這對承載馬達及負載馬達。The invention provides a control method suitable for a gantry type positioning platform. The gantry type positioning platform has a pair of load-bearing slide rails respectively extending along a pair of mutually parallel bearing axes, and two ends respectively coupled to the pair of load-bearing slide rails and coupled along a load rail extending perpendicular to the pair of load-bearing axes A load slider for the load rail, a pair of load-bearing motors for driving the load rails to move on the pair of load-bearing rails, and a load motor for driving the load slider to move on the load rails. The control method includes the following steps. A dynamic model is established according to the physical parameters of the gantry positioning platform. A position command is converted from a first coordinate space to a second coordinate space in accordance with the dynamic model to generate a transition position command. Convert the transition position command to a transition speed command. A speed feedback from the pair of load bearing motors and load motors is converted to a transition speed feedback in accordance with a dynamic model, wherein the transition speed feedback is compared to a transition speed command to produce a transition speed error. Convert the transition speed error to a transition torque command. A transition torque command is converted from the second coordinate space to the first coordinate space in accordance with the dynamic model to generate a torque command. according to The torque command drives the pair of load bearing motors and load motors.
本發明提出一種位置命令補償方法,適用於一龍門式定位平台。龍門式定位平台具有一對分別沿著一對相互平行的承載軸線延伸的承載滑軌、兩端分別耦合於這對承載滑軌的一負載滑軌以及耦合於負載滑軌的一負載滑塊。位置命令補償方法包括下列步驟。依照龍門式定位平台的物理參數建立一動態模型。利用動態模型將一位置命令從一第一座標空間轉換到一第二座標空間以產生一過渡位置命令。將過渡位置命令轉換為過渡速度命令。依照動態模型將來自這對承載馬達及負載馬達的一速度迴授轉換為一過渡速度迴授,其中過渡速度迴授與過渡速度命令比較以產生一過渡速度誤差。將過渡速度誤差轉換為過渡轉矩命令。依照動態模型將一過渡轉矩命令從第二座標空間轉換到第一座標空間以產生一轉矩命令。The invention provides a position command compensation method, which is suitable for a gantry type positioning platform. The gantry type positioning platform has a pair of load-bearing slide rails respectively extending along a pair of mutually parallel bearing axes, a load rail coupled to the pair of load-bearing rails at both ends, and a load slider coupled to the load rail. The position command compensation method includes the following steps. A dynamic model is established according to the physical parameters of the gantry positioning platform. A dynamic model is used to convert a position command from a first coordinate space to a second coordinate space to generate a transition position command. Convert the transition position command to a transition speed command. A speed feedback from the pair of load bearing motors and load motors is converted to a transition speed feedback in accordance with a dynamic model, wherein the transition speed feedback is compared to a transition speed command to produce a transition speed error. Convert the transition speed error to a transition torque command. A transition torque command is converted from the second coordinate space to the first coordinate space in accordance with the dynamic model to generate a torque command.
基於上述,本發明依照龍門式定位平台之物理參數建立龍門式定位平台之動態模型,可增加使用者於開發控制系統上的便利性。Based on the above, the present invention establishes a dynamic model of the gantry positioning platform according to the physical parameters of the gantry positioning platform, which can increase the convenience of the user in developing the control system.
為讓本發明之上述特徵和優點能更明顯易懂,下文特舉實施例,並配合所附圖式作詳細說明如下。The above described features and advantages of the present invention will be more apparent from the following description.
圖1是本發明所適用的一種龍門式定位平台的立體圖。請參考圖1,本實施例的龍門式定位平台10具有一對承載滑軌11、一負載滑軌12及一負載滑塊13。這對承載 滑軌11分別沿著一對相互平行的承載軸線y1及y2延伸。負載滑軌12的兩端分別耦合於這對承載滑軌11以在這對承載滑軌11上同步移動。負載滑塊13耦合於負載滑軌12以在負載滑軌12上移動。1 is a perspective view of a gantry type positioning platform to which the present invention is applied. Referring to FIG. 1 , the gantry positioning platform 10 of the present embodiment has a pair of load-bearing slide rails 11 , a load slide rail 12 , and a load slide 13 . This pair of bearers The slide rails 11 extend along a pair of mutually parallel bearing axes y1 and y2, respectively. Both ends of the load rail 12 are coupled to the pair of load rails 11 to move synchronously on the pair of load rails 11, respectively. Load slider 13 is coupled to load rail 12 for movement on load rail 12.
龍門式定位平台10更具有一對承載馬達14及負載馬達15。這對承載馬達14分別設置於這對承載滑軌11與負載滑軌12的兩端之間,以驅動負載滑軌12的兩端分別沿著這對承載滑軌11移動。負載馬達15設置於負載滑軌12與負載滑塊13之間。在本實施例中,這對承載馬達11可為線性馬達而分別設置於負載滑軌12的兩端,而負載馬達15可為線性馬達而直接設置於負載滑塊13上。在另一未繪示實施例中,這對承載馬達11可分別設置於這對承載滑軌上,並藉由蝸桿或皮帶來帶動負載滑軌12的兩端,而負載馬達15可設置在負載滑軌上,並藉由皮帶來帶動負載滑塊13。基於上述,龍門式定位平台10可視為一具有機構耦合之三個自由度(承載軸線y1、y2及負載軸線x)的伺服系統。The gantry positioning platform 10 further has a pair of load bearing motors 14 and a load motor 15. The pair of load-bearing motors 14 are respectively disposed between the pair of load-bearing rails 11 and the load rails 12 to drive the two ends of the load rail 12 to move along the pair of load-bearing slide rails 11, respectively. The load motor 15 is disposed between the load rail 12 and the load slider 13. In the present embodiment, the pair of load motors 11 may be linear motors and respectively disposed at both ends of the load rail 12, and the load motor 15 may be a linear motor and directly disposed on the load slider 13. In another embodiment, the pair of load-bearing motors 11 can be respectively disposed on the pair of load-bearing slide rails, and the two ends of the load rail 12 are driven by a worm or a leather belt, and the load motor 15 can be disposed at the load. On the slide rail, the load slider 13 is driven by the leather belt. Based on the above, the gantry positioning platform 10 can be regarded as a servo system having three degrees of freedom of mechanism coupling (bearing axes y1, y2 and load axis x).
圖2是圖1的龍門式定位平台的承載軸線及負載軸的簡圖。請參考圖1及圖2,這對承載馬達14在這對承載軸線y1及y2上分別對應二個位置輸出d y1 與d y2 。當這二個位置相同(即d y1 =d y2 )時,這代表這對承載馬達14同步運動,也就是負載滑軌12的兩端在這對承載滑軌11上同步移動。然而,在實際的應用中,由於這對承載馬達14及負載馬達15在組裝上的差異性、行進間的外力干擾或耦合效 應,所以相當難達到這對承載馬達14的同步運動響應。2 is a schematic view of a bearing axis and a load shaft of the gantry positioning platform of FIG. 1. Referring to FIG. 1 and FIG. 2, the pair of carrier motors 14 output d y1 and d y2 corresponding to the two positions on the pair of bearing axes y1 and y2, respectively. When the two positions are the same (i.e., d y1 = d y2 ), this represents the synchronous movement of the pair of load-bearing motors 14, that is, both ends of the load rail 12 move synchronously on the pair of load-bearing slides 11. However, in practical applications, due to the difference in assembly of the load bearing motor 14 and the load motor 15, the external force interference or the coupling effect between the travels, it is quite difficult to achieve the synchronous motion response of the pair of loader motors 14.
在本實施例中,除了考慮了各軸的線性運動,也同時考慮了因這對承載馬達14的不同步運動造成的負載軌道14的兩端偏移,因而產生旋轉運動。當這對承載馬達14不同步運動時,如圖2所示,d y1 與d y2 之間會存在一偏差值。機構耦合使得負載滑軌12相對於負載軸線x有著一角度的偏移,這造成負載滑塊13在負載滑軌12上運動時,會以中心點C(即負載滑軌12的兩端之間的中心點)產生一轉動慣量,這導致在這對承載軸線y1上的這對承載馬達14產生時變的慣量與負載變動。In the present embodiment, in addition to considering the linear motion of the respective axes, the both ends of the load rail 14 due to the asynchronous movement of the pair of carriers 14 are simultaneously considered, thereby generating a rotational motion. When the pair of carrier motors 14 are not synchronized, as shown in Fig. 2, there is a deviation value between d y1 and d y2 . The mechanism coupling causes the load rail 12 to have an angular offset relative to the load axis x, which causes the load slider 13 to move toward the load rail 12 with a center point C (ie, between the ends of the load rail 12) The center point) produces a moment of inertia which results in a time varying inertia and load variation on the pair of load bearing motors 14 on the pair of load bearing axes y1.
因此,在本實施例中,建立一動態模型來控制這對承載馬達14及負載馬達15。除了考慮三軸的線性響應以外,同時將轉動慣量的概念給模型化到此動態模型中,也就是動態模型包含了慣性矩陣。Therefore, in the present embodiment, a dynamic model is established to control the pair of carrier motor 14 and load motor 15. In addition to considering the linear response of the three axes, the concept of moment of inertia is simultaneously modeled into this dynamic model, that is, the dynamic model contains the inertia matrix.
在推導動態模型之前,先定義圖2中的參數,其中M 1
表示為負載軸線定子(即負載滑軌12)的質量;M 2
表示為負載軸線動子(即負載滑塊13)的質量;L
為負載軸線定子(即負載滑軌12)的長度;2w
為負載軸線定子(即負載滑軌12)的寬度;I 1
與I 2
分別代表負載軸線定子(即負載滑軌12)的慣性動能與負載軸線動子(即負載滑塊13)的慣性動能,其依序表示如下:
I 2 =M 2 (d x 2 +(w +v )2 ) (2) I 2 = M 2 ( d x 2 +( w + v ) 2 ) (2)
而負載軸線定子(即負載滑軌12)的質心位置與負載軸線動子(即負載滑塊13)的質心位置則分別定義如下:
其中d y
=d y1
+(d y2
-d y1
)/2。將(3)式與(4)式分別微分,則負載軸線定子(即負載滑軌12)的質心速度 v M1
與負載軸線動子(即負載滑塊13)的質心速度 v M2
分別定義如下:
根據(5)式與(6)式,則負載軸線定子(即負載滑軌12)的質心與負載軸線動子(即負載滑塊13)的質心的總動能,包含了移動動能與轉動動能可表示如下:
則整個龍門式定位平台10的總動能可以重新表示如
下:
再將(9)式整理成矩陣型式如下:
其中X
=[d y
θd x
]T
;D
為慣性矩陣,定義如下:
根據Lagrange方程式的推導,龍門式定位平台10的控制支配方程式(Governing Equation)可以表示如下:
其中L
=K
-V
;K
為龍門式定位平台10的總動能;V
為龍門式定位平台10的總位能;U
為龍門式定位平台10的各軸馬達的轉矩矩陣;F
則為摩擦力矩陣。接著,柯氏力與離心力(Coriolis and Centrifugal Force)矩陣(即C
矩陣)
中的各項元素值可以表示如下:
其中、和分別表示d y
、θ和d x
的微分項。則克氏符號(Christoffel Symbols)cijk
可經由以下的計算得到:
其中d ij
表示為慣性矩陣D
中的第i
排第j
列的元素。再將慣性矩陣I 1
與I 2
分別代入(11)式並重新推導(14)式,則矩陣C
可得如下:
最後將上述矩陣重新整理,則龍門式定位平台的具機構耦合之龍門動態模型可以表示成DX
+CX
+BF
=BU
的型式,其中
F =[F y 1 ,F y 2 ,F x ] T (17) F = [ F y 1 , F y 2 , F x ] T (17)
U =[u y 1 ,u y 2 ,u x ] T (18) U =[ u y 1 , u y 2 , u x ] T (18)
B為座標轉換矩陣;Fy1 、Fy2 和Fx 分別為龍門式位平台12的三軸(y1、y2、x)的摩擦力;uy1 、uy2 和ux 分別為龍門式定位平台10的三軸(y1、y2、x)的轉矩。B is a coordinate transformation matrix; F y1 , F y2 and F x are the frictional forces of the three axes (y1, y2, x) of the gantry type platform 12; u y1 , u y2 and u x are respectively the gantry positioning platform 10 The torque of the three axes (y1, y2, x).
圖3是本發明的一實施例的一種控制裝置與圖1的龍門式定位平台的控制架構。請參考圖2及圖3,本實施例的控制裝置100適用於龍門式定位平台10,特別是控制龍門式定位平台10的一對承載馬達(即圖3的y1承載馬達14a及y2承載馬達14b)及一負載馬達(即圖3的x負載馬達15a)。控制裝置100利用一動態模型來計算和補償過渡命令以產生轉矩命令,且將轉矩命令傳送至這對承載馬達(即圖3的y1承載馬達14a及y2承載馬達14b)及一負載馬達(即x負載馬達15a)以驅動它們。所述動態模型是依照龍門式定位平台10的物理參數來建立。這些物理參數包括龍門式定位平台10的負載滑軌的寬度、負載滑軌的長度、負載滑軌的質量、負載滑塊的質量以及從負載滑塊的質心至負載滑軌的邊界的距離等。3 is a control architecture of a control device and the gantry positioning platform of FIG. 1 in accordance with an embodiment of the present invention. Referring to FIG. 2 and FIG. 3, the control device 100 of the present embodiment is applied to the gantry positioning platform 10, in particular, a pair of load bearing motors for controlling the gantry positioning platform 10 (ie, the y1 carrying motor 14a and the y2 carrying motor 14b of FIG. 3). And a load motor (ie x load motor 15a of Figure 3). The control device 100 utilizes a dynamic model to calculate and compensate for the transition command to generate a torque command, and transmits the torque command to the pair of carrier motors (ie, the y1 carrier motor 14a and the y2 carrier motor 14b of FIG. 3) and a load motor ( That is, the x load motor 15a) drives them. The dynamic model is established in accordance with the physical parameters of the gantry positioning platform 10. These physical parameters include the width of the load rail of the gantry positioning platform 10, the length of the load rail, the mass of the load rail, the mass of the load slider, and the distance from the center of mass of the load slider to the boundary of the load rail. .
控制裝置100包括一正向座標轉換器110a,其依照動態模型將位置命令(位置誤差)從一第一座標空間(y1,y2,x)轉換到一第二座標空間(y,θ,x)以產生過渡位置命令,其中第一座標空間(y1,y2,x)包含這對承載軸線y1及y2的位置和負載軸線x的位置,而第二座標空間(y,θ,x)包含這對承載軸線y1及y2的幾何中心位置、負載軸線x之偏移 角度θ以及負載軸線x之幾何中心位置。Control device 100 includes a forward coordinate converter 110a that converts position commands (position errors) from a first coordinate space (y1, y2, x) to a second coordinate space (y, θ, x) in accordance with a dynamic model. To generate a transition position command, wherein the first coordinate space (y1, y2, x) contains the position of the pair of bearing axes y1 and y2 and the position of the load axis x, and the second coordinate space (y, θ, x) contains the pair Geometric center position of load bearing axes y1 and y2, offset of load axis x The angle θ and the geometric center position of the load axis x.
控制裝置100包括一組位置控制器,包括y位置控制器120a、θ位置控制器120b及x位置控制器120c,用以將過渡位置命令轉換為過渡速度命令。The control device 100 includes a set of position controllers including a y position controller 120a, a θ position controller 120b, and an x position controller 120c for converting a transition position command into a transition speed command.
控制裝置100包括一第一逆向座標轉換器110b,其依照動態模型將來自y1承載馬達14a、y2承載馬達14b及x負載馬達15a的速度迴授轉換為過渡速度迴授,其中過渡速度迴授與過渡速度命令比較以產生過渡速度誤差。The control device 100 includes a first reverse coordinate converter 110b that converts the speed feedback from the y1 carrier motor 14a, the y2 carrier motor 14b and the x load motor 15a into a transient speed feedback in accordance with a dynamic model, wherein the transition speed feedback is The transition speed commands are compared to produce a transition speed error.
控制裝置100包括一組速度控制器,包括y速度控制器130a、θ速度控制器130b及x速度控制器130c,用以將過渡速度誤差轉換為過渡轉矩命令。The control device 100 includes a set of speed controllers including a y speed controller 130a, a θ speed controller 130b, and an x speed controller 130c for converting the transition speed error into a transition torque command.
控制裝置100包括一第二逆向座標轉換器110c,其依照動態模型將過渡轉矩命令從第二座標空間(y,θ,x)轉換到第一座標空間(y1,y2,x)以產生一轉矩命令來驅動y1承載馬達14a、y2承載馬達14b及x負載馬達15a。The control device 100 includes a second reverse coordinate converter 110c that converts the transition torque command from the second coordinate space (y, θ, x) to the first coordinate space (y1, y2, x) in accordance with the dynamic model to generate a The torque command drives the y1 carrying motor 14a, the y2 carrying motor 14b and the x load motor 15a.
在一實施例中,第一逆向座標轉換器110b與第二逆向座標轉換器110c可合併為單一逆向座標轉換器。In an embodiment, the first inverse coordinate converter 110b and the second inverse coordinate converter 110c may be combined into a single inverse coordinate converter.
在本實施例中,龍門式定位平台10更可包括一組位置檢測器和一組速度估測器。這組位置檢測器包括圖3的y1位置檢測器16a、y2位置檢測器16b和x位置檢測器16c,而這組速度估測器包括圖3的y1速度估測器17a、y2速度估測器17b和x速度估測器17c。In this embodiment, the gantry positioning platform 10 further includes a set of position detectors and a set of speed estimators. The set of position detectors includes the y1 position detector 16a, the y2 position detector 16b and the x position detector 16c of FIG. 3, and the set of speed estimators includes the y1 speed estimator 17a, y2 speed estimator of FIG. 17b and x speed estimator 17c.
這組位置檢測器(y1位置檢測器16a、y2位置檢測器16b和x位置檢測器16c)分別檢測y1承載馬達14a、y2承 載馬達14b及x負載馬達15a的位置,以產生位置命令。這組速度估測器(y1速度估測器17a、y2速度估測器17b和x速度估測器17c)將來自位置檢測器的位置迴授轉換成速度迴授並輸入至第二逆向座標轉換器110c。The set of position detectors (y1 position detector 16a, y2 position detector 16b and x position detector 16c) respectively detect y1 carrying motors 14a, y2 The position of the motor 14b and the x load motor 15a is generated to generate a position command. The set of speed estimators (y1 speed estimator 17a, y2 speed estimator 17b and x-speed estimator 17c) converts the position feedback from the position detector into speed feedback and input to the second reverse coordinate conversion The device 110c.
為了消除轉動慣量所造成的影響,動態模型至少包含慣性矩陣,例如是上文所揭露的慣性矩陣D 。此外,動態模型也可包含柯氏力與離心力矩陣,例如是上文所揭露的C 矩陣。另外,動態模型還可包含摩擦力矩陣,例如是上文所揭露的F 矩陣。為了將位置命令從第一座標空間(y1,y2,x)轉換至第二座標空間(y,θ,x),動態模型可包含座標轉換矩陣,例如是上文所揭露的座標轉換矩陣B 。為了獲得三軸(y1、y2及x)的轉矩,動態模型更可包含轉矩矩陣,例如是上文所揭露的龍門式定位平台10的各軸馬達的轉矩矩陣U 。In order to eliminate the effects of the moment of inertia, the dynamic model contains at least an inertia matrix, such as the inertia matrix D disclosed above. In addition, the dynamic model may also comprise a Coriolis force and centrifugal force matrix, such as the C matrix disclosed above. Additionally, the dynamic model may also include a friction matrix, such as the F matrix disclosed above. In order to position commands from a first coordinate space (y1, y2, x) into a second coordinate space (y, θ, x), the dynamic model may include coordinate transformation matrix is, for example disclosed above coordinate transformation matrix B. In order to obtain the torque of the three axes (y1, y2, and x), the dynamic model may further include a torque matrix, such as the torque matrix U of each of the shaft motors of the gantry positioning platform 10 disclosed above.
圖4是本發明的一實施例的一種控制方法。請參考圖2、圖3及圖4,本實施例的控制方法適用於圖1至圖3所揭露的龍門式定位平台。在步驟S210中,依照龍門式定位平台10的物理參數建立一動態模型。這些物理參數包括龍門式定位平台10的負載滑軌的寬度、負載滑軌的長度、負載滑軌的質量、負載滑塊的質量以及從負載滑塊的質心至負載滑軌的邊界的距離等。4 is a control method of an embodiment of the present invention. Referring to FIG. 2, FIG. 3 and FIG. 4, the control method of this embodiment is applicable to the gantry positioning platform disclosed in FIG. 1 to FIG. In step S210, a dynamic model is established according to the physical parameters of the gantry positioning platform 10. These physical parameters include the width of the load rail of the gantry positioning platform 10, the length of the load rail, the mass of the load rail, the mass of the load slider, and the distance from the center of mass of the load slider to the boundary of the load rail. .
在步驟S220中,依照動態模型將位置命令(位置誤差)從第一座標空間(y1,y2,x)轉換到第二座標空間(y,θ,x)以產生過渡位置命令。在本實施例中,第一座標空間包含 這對承載軸線y1及y2的位置和負載軸線x的位置,而第二座標空間包含這對承載軸線y1及y2的幾何中心位置、負載軸線x之偏移角度θ以及負載軸線x之幾何中心位置。這樣的動作可藉由圖3的正向座標轉換器110a來達成。In step S220, the position command (position error) is converted from the first coordinate space (y1, y2, x) to the second coordinate space (y, θ, x) in accordance with the dynamic model to generate a transition position command. In this embodiment, the first coordinate space includes The pair positions the positions of the bearing axes y1 and y2 and the load axis x, and the second coordinate space contains the geometric center position of the pair of bearing axes y1 and y2, the offset angle θ of the load axis x, and the geometric center position of the load axis x . Such an action can be achieved by the forward coordinate converter 110a of FIG.
在步驟S230中,將過渡位置命令轉換為過渡速度命令。這樣的動作可藉由圖3的這組位置控制器(y位置控制器120a、θ位置控制器120b及x位置控制器120c)來達成。In step S230, the transition position command is converted into a transition speed command. Such an action can be achieved by the set of position controllers (y position controller 120a, θ position controller 120b, and x position controller 120c) of FIG.
在步驟S240中,依照動態模型將來自y1承載馬達14a、y2承載馬達14b及x負載馬達15a的速度迴授轉換為一過渡速度迴授,而這樣的動作可藉由圖3的第一逆向座標轉換器110b來達成。過渡速度迴授與過渡速度命令比較以產生過渡速度誤差。In step S240, the speed feedback from the y1 carrier motor 14a, the y2 carrier motor 14b and the x load motor 15a is converted into a transition speed feedback according to the dynamic model, and such an action can be performed by the first inverse coordinate of FIG. The converter 110b is achieved. The transition speed feedback is compared to the transition speed command to produce a transition speed error.
在步驟S250中,將過渡速度誤差轉換為過渡轉矩命令。這樣的動作可藉由圖3的這組速度控制器(y速度控制器120a、θ速度控制器120b及x速度控制器120c)來達成。In step S250, the transition speed error is converted into a transition torque command. Such an action can be achieved by the set of speed controllers of FIG. 3 (y speed controller 120a, θ speed controller 120b, and x speed controller 120c).
在步驟S260中,依照動態模型將過渡轉矩命令從第二座標空間(y,θ,x)轉換到第一座標空間(y1、y2及x)以產生一轉矩命令。這樣的動作可藉由圖3的第二逆向座標轉換器110c來達成。In step S260, the transition torque command is converted from the second coordinate space (y, θ, x) to the first coordinate space (y1, y2, and x) in accordance with the dynamic model to generate a torque command. Such an action can be achieved by the second inverse coordinate converter 110c of FIG.
在步驟S270中,依照轉矩命令驅動y1承載馬達14a、y2承載馬達14b及x負載馬達15a。In step S270, the y1 carrying motor 14a, the y2 carrying motor 14b and the x load motor 15a are driven in accordance with the torque command.
同樣地,為了消除轉動慣量所造成的影響,動態模型至少包含慣性矩陣,例如是上文所揭露的慣性矩陣D 。此外,動態模型也可包含柯氏力與離心力矩陣,例如是上文 所揭露的C 矩陣。另外,動態模型還可包含摩擦力矩陣,例如是上文所揭露的F 矩陣。為了將位置命令從第一座標空間轉換至第二座標空間,動態模型可包含座標轉換矩陣,例如是上文所揭露的座標轉換矩陣B 。為了獲得三軸(y1、y2及x)的轉矩,動態模型更可包含轉矩矩陣,例如是上文所揭露的龍門式定位平台10的各軸馬達的轉矩矩陣U 。Likewise, in order to eliminate the influence rotational inertia caused by the dynamic model includes at least the inertia matrix, for example, disclosed hereinabove inertia matrix D. In addition, the dynamic model may also comprise a Coriolis force and centrifugal force matrix, such as the C matrix disclosed above. Additionally, the dynamic model may also include a friction matrix, such as the F matrix disclosed above. In order to convert the position command from the first coordinate space to the second coordinate space, the dynamic model may comprise a coordinate transformation matrix, such as the coordinate transformation matrix B disclosed above. In order to obtain the torque of the three axes (y1, y2, and x), the dynamic model may further include a torque matrix, such as the torque matrix U of each of the shaft motors of the gantry positioning platform 10 disclosed above.
本發明的一實施例的一種位置命令補償方法包含了圖4所揭露的控制方法的步驟S210至S260,而不包含步驟S270之驅動動作。A position command compensation method according to an embodiment of the present invention includes steps S210 to S260 of the control method disclosed in FIG. 4, and does not include the driving action of step S270.
綜上所述,本發明依照龍門式定位平台之物理參數透過全數位化與模組化的方式建立龍門式定位平台之動態模型,可增加使用者於開發控制系統上的便利性。本發明能有效模式化三軸間的同步運動響應與線性的動態響應,因此各軸的追跡誤差或是承載馬達之間的同步運動誤差皆能獲得良好的控制。In summary, the present invention establishes a dynamic model of the gantry positioning platform according to the physical parameters of the gantry positioning platform through full digitization and modularization, thereby increasing the convenience of the user in developing the control system. The invention can effectively model the synchronous motion response and the linear dynamic response between the three axes, so that the tracking error of each axis or the synchronous motion error between the bearing motors can be well controlled.
雖然本發明已以實施例揭露如上,然其並非用以限定本發明,任何所屬技術領域中具有通常知識者,在不脫離本發明之精神和範圍內,當可作些許之更動與潤飾,故本發明之保護範圍當視後附之申請專利範圍所界定者為準。Although the present invention has been disclosed in the above embodiments, it is not intended to limit the invention, and any one of ordinary skill in the art can make some modifications and refinements without departing from the spirit and scope of the invention. The scope of the invention is defined by the scope of the appended claims.
10‧‧‧龍門式定位平台10‧‧‧ gantry positioning platform
11‧‧‧承載滑軌11‧‧‧Loading rails
12‧‧‧負載滑軌12‧‧‧Load rails
13‧‧‧負載滑塊13‧‧‧Load slider
14‧‧‧承載馬達14‧‧‧Bearing motor
14a‧‧‧y1承載馬達14a‧‧‧y1 carrying motor
14b‧‧‧y2承載馬達14b‧‧‧y2 carrying motor
15‧‧‧負載馬達15‧‧‧Load motor
15a‧‧‧x負載馬達15a‧‧‧x load motor
16a‧‧‧y1位置檢測器16a‧‧‧y1 position detector
16b‧‧‧y2位置檢測器16b‧‧‧y2 position detector
16c‧‧‧x位置檢測器16c‧‧‧x position detector
17a‧‧‧y1速度估測器17a‧‧‧1 speed estimator
17b‧‧‧y2速度估測器17b‧‧‧y2 speed estimator
17c‧‧‧x速度估測器17c‧‧‧x speed estimator
100‧‧‧控制裝置100‧‧‧Control device
110a‧‧‧正向座標轉換器110a‧‧‧ forward coordinate converter
110b‧‧‧第一逆向座標轉換器110b‧‧‧First reverse coordinate converter
110c‧‧‧第二逆向座標轉換器110c‧‧‧Second reverse coordinate converter
120a‧‧‧y位置控制器120a‧‧‧y position controller
120b‧‧‧θ位置控制器120b‧‧ θ position controller
120c‧‧‧x位置控制器120c‧‧‧x position controller
130a‧‧‧y速度控制器130a‧‧‧y speed controller
130b‧‧‧θ速度控制器130b‧‧ θ speed controller
130c‧‧‧x速度控制器130c‧‧‧x speed controller
圖1是本發明所適用的一種龍門式定位平台的立體圖。1 is a perspective view of a gantry type positioning platform to which the present invention is applied.
圖2是圖1的龍門式定位平台的承載軸線及負載軸的簡圖。2 is a schematic view of a bearing axis and a load shaft of the gantry positioning platform of FIG. 1.
圖3是本發明的一實施例的一種控制裝置與圖1的龍門式定位平台的控制架構。3 is a control architecture of a control device and the gantry positioning platform of FIG. 1 in accordance with an embodiment of the present invention.
圖4是本發明的一實施例的一種控制方法。4 is a control method of an embodiment of the present invention.
14a‧‧‧y1承載馬達14a‧‧‧y1 carrying motor
14b‧‧‧y2承載馬達14b‧‧‧y2 carrying motor
15a‧‧‧x負載馬達15a‧‧‧x load motor
16a‧‧‧y1位置檢測器16a‧‧‧y1 position detector
16b‧‧‧y2位置檢測器16b‧‧‧y2 position detector
16c‧‧‧x位置檢測器16c‧‧‧x position detector
17a‧‧‧y1速度估測器17a‧‧‧1 speed estimator
17b‧‧‧y2速度估測器17b‧‧‧y2 speed estimator
17c‧‧‧x速度估測器17c‧‧‧x speed estimator
100‧‧‧控制裝置100‧‧‧Control device
110a‧‧‧正向座標轉換器110a‧‧‧ forward coordinate converter
110b‧‧‧第一逆向座標轉換器110b‧‧‧First reverse coordinate converter
110c‧‧‧第二逆向座標轉換器110c‧‧‧Second reverse coordinate converter
120a‧‧‧y位置控制器120a‧‧‧y position controller
120b‧‧‧θ位置控制器120b‧‧ θ position controller
120c‧‧‧x位置控制器120c‧‧‧x position controller
130a‧‧‧y速度控制器130a‧‧‧y speed controller
130b‧‧‧θ速度控制器130b‧‧ θ speed controller
130c‧‧‧x速度控制器130c‧‧‧x speed controller
Claims (13)
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW101148149A TWI494725B (en) | 2012-12-18 | 2012-12-18 | Control device, control method and compensating method of position command |
CN201310045870.6A CN103869749B (en) | 2012-12-18 | 2013-02-05 | Control device, control method and position command compensation method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
TW101148149A TWI494725B (en) | 2012-12-18 | 2012-12-18 | Control device, control method and compensating method of position command |
Publications (2)
Publication Number | Publication Date |
---|---|
TW201426229A TW201426229A (en) | 2014-07-01 |
TWI494725B true TWI494725B (en) | 2015-08-01 |
Family
ID=50908395
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
TW101148149A TWI494725B (en) | 2012-12-18 | 2012-12-18 | Control device, control method and compensating method of position command |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN103869749B (en) |
TW (1) | TWI494725B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11331762B2 (en) | 2019-11-22 | 2022-05-17 | Industrial Technology Research Institute | Method for synchronous control of gantry mechanism with online inertia matching |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105527796B (en) * | 2014-09-28 | 2018-03-13 | 上海微电子装备(集团)股份有限公司 | Planer-type equipment and control method |
CN104908030A (en) * | 2015-06-23 | 2015-09-16 | 江南大学 | SCARA robot accurate positioning method based on granularity model |
TWI558089B (en) | 2015-08-06 | 2016-11-11 | 財團法人工業技術研究院 | Synchronous control system for multi-axis motors and method thereof |
JP6885436B2 (en) * | 2019-09-11 | 2021-06-16 | 富士電機株式会社 | Servo amplifier and servo system |
TWI728757B (en) * | 2020-03-23 | 2021-05-21 | 微正股份有限公司 | Direct pose feedback control method and direct pose feedback controlled machine |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2007082779A1 (en) * | 2006-01-19 | 2007-07-26 | Carl Zeiss Industrielle Messtechnik Gmbh | Coordinate measuring machine and method for operating a coordinate measuring machine |
US7333862B2 (en) * | 2004-07-14 | 2008-02-19 | Sumitomo Heavy Industries, Ltd. | Position control device of moving body, stage device using the position control device, and position control method of moving body |
JP2008210028A (en) * | 2007-02-23 | 2008-09-11 | Yokogawa Electric Corp | Xy stage |
TW201205216A (en) * | 2010-01-12 | 2012-02-01 | Yaskawa Denki Seisakusho Kk | Synchronous control apparatus |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3595357B2 (en) * | 1994-06-30 | 2004-12-02 | ファナック株式会社 | Tandem control method using digital servo |
TWI313094B (en) * | 2006-01-27 | 2009-08-01 | ||
JP4235210B2 (en) * | 2006-02-16 | 2009-03-11 | ファナック株式会社 | Servo motor control device |
-
2012
- 2012-12-18 TW TW101148149A patent/TWI494725B/en active
-
2013
- 2013-02-05 CN CN201310045870.6A patent/CN103869749B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7333862B2 (en) * | 2004-07-14 | 2008-02-19 | Sumitomo Heavy Industries, Ltd. | Position control device of moving body, stage device using the position control device, and position control method of moving body |
WO2007082779A1 (en) * | 2006-01-19 | 2007-07-26 | Carl Zeiss Industrielle Messtechnik Gmbh | Coordinate measuring machine and method for operating a coordinate measuring machine |
JP2008210028A (en) * | 2007-02-23 | 2008-09-11 | Yokogawa Electric Corp | Xy stage |
TW201205216A (en) * | 2010-01-12 | 2012-02-01 | Yaskawa Denki Seisakusho Kk | Synchronous control apparatus |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US11331762B2 (en) | 2019-11-22 | 2022-05-17 | Industrial Technology Research Institute | Method for synchronous control of gantry mechanism with online inertia matching |
Also Published As
Publication number | Publication date |
---|---|
CN103869749A (en) | 2014-06-18 |
CN103869749B (en) | 2016-08-03 |
TW201426229A (en) | 2014-07-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
TWI494725B (en) | Control device, control method and compensating method of position command | |
Wu et al. | A control strategy of a two degrees-of-freedom heavy duty parallel manipulator | |
Fang et al. | Motion control of a tendon-based parallel manipulator using optimal tension distribution | |
CN109605371B (en) | Mobile hybrid robot processing integrated system | |
Son et al. | Design and dynamic analysis of an arch-type desktop reconfigurable machine | |
CN108161896A (en) | 6-PSS parallel institutions and forward position analysis method | |
CN109657282B (en) | H-shaped motion platform modeling method based on Lagrangian dynamics | |
CN110549151B (en) | Track guide rail driving micro-feeding servo system and synchronous control method | |
CN103017726A (en) | Robot pose error measuring system and method in Cartesian coordinate mode | |
Xie et al. | Parameter optimization for the driving system of a 5 degrees-of-freedom parallel machining robot with planar kinematic chains | |
CN202994132U (en) | Robot pose error measuring system based on rectangular coordinate mode | |
CN206105840U (en) | Three flexible degree of freedom parallel mechanism vibration detection controlling means | |
CN109495026A (en) | Double drive gantry platform drive system, method, equipment and computer-readable memory | |
Hsieh | An experimental study on cam-controlled planetary gear trains | |
Zi et al. | Integrated mechanism design and control for completely restrained hybrid-driven based cable parallel manipulators | |
Yuen et al. | Trajectory generation and control of a 9 axis CNC micromachining center | |
CN112799304B (en) | Dual-motor synchronous control method and device based on time-varying friction compensation | |
CN206416155U (en) | A kind of parallel institution device driven based on linear motion unit | |
Zhou et al. | A study on the dynamic characteristics of the drive at center of gravity (DCG) feed drives | |
Krishnamurthy et al. | Control design and implementation for Sawyer motors used in manufacturing systems | |
He et al. | Design and performance analysis of a novel parallel servo press with redundant actuation | |
CN112904741A (en) | High-precision synchronous control method and system for double-drive gantry truss system | |
CN208992704U (en) | A kind of plane parallel mechanism device of double five-rod drivings | |
CN109495025A (en) | Double drive gantry platform drive system, method, equipment and computer-readable memory | |
Wu et al. | Dynamic model and force control of the redundantly actuated parallel manipulator of a 5-DOF hybrid machine tool |