CN104977901B - Triaxial movement platform modified cross-coupling control device and method - Google Patents
Triaxial movement platform modified cross-coupling control device and method Download PDFInfo
- Publication number
- CN104977901B CN104977901B CN201510404800.4A CN201510404800A CN104977901B CN 104977901 B CN104977901 B CN 104977901B CN 201510404800 A CN201510404800 A CN 201510404800A CN 104977901 B CN104977901 B CN 104977901B
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- mtr
- mtd
- mfrac
- Prior art date
- Legal status (The legal status 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 status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000006880 cross-coupling reaction Methods 0.000 title claims abstract description 47
- 238000013461 design Methods 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims abstract description 8
- 239000013598 vector Substances 0.000 claims description 51
- 238000005070 sampling Methods 0.000 claims description 32
- 230000001360 synchronised effect Effects 0.000 claims description 23
- 238000002955 isolation Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 11
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 230000001105 regulatory effect Effects 0.000 claims description 9
- 230000006870 function Effects 0.000 claims description 8
- 238000013459 approach Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 230000004044 response Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 238000012546 transfer Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 239000003054 catalyst Substances 0.000 claims description 3
- 239000002131 composite material Substances 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 14
- 238000003754 machining Methods 0.000 description 7
- 230000005540 biological transmission Effects 0.000 description 5
- 230000001276 controlling effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 102100024061 Integrator complex subunit 1 Human genes 0.000 description 2
- 101710092857 Integrator complex subunit 1 Proteins 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 2
- 239000003990 capacitor Substances 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 102100028043 Fibroblast growth factor 3 Human genes 0.000 description 1
- 108050002021 Integrator complex subunit 2 Proteins 0.000 description 1
- 235000010724 Wisteria floribunda Nutrition 0.000 description 1
- 238000010521 absorption reaction Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000003252 repetitive effect Effects 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
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/414—Structure of the control system, e.g. common controller or multiprocessor systems, interface to servo, programmable interface controller
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- External Artificial Organs (AREA)
- Control Of Position Or Direction (AREA)
Abstract
A kind of triaxial movement platform modified cross-coupling control device and method, it is characterised in that：The device includes main circuit, control circuit and the part of control object three；Main circuit includes AC voltage adjusting module, rectification filtering module and IPM inversion modules；The present invention uses a kind of profile errors estimation algorithm in three axles coordinate control, sets up three axle profile errors models, improves the structure of cross-coupling control, design three-dimensional figure error controller.
Description
The technical field is as follows: the invention provides an improved cross-coupling control device and method for a three-axis motion platform, and belongs to the technical field of numerical control.
Background art: in a modern numerical control machining system, the contour control of a two-axis XY platform cannot meet the machining requirements of people on complex elements, so that a three-axis motion platform contour control technology is introduced to realize the precision machining of the contour of a space three-dimensional part. The three-axis motion platform is directly driven by the permanent magnet synchronous linear motor, so that the intermediate transmission link of ball and screw is avoided, and the processing efficiency of the system is improved.
The invention content is as follows:
the purpose of the invention is as follows: the invention provides an improved cross coupling control device and method for a three-axis motion platform, and aims to solve the problems in the prior art.
The technical scheme is as follows: the invention is realized by the following technical scheme:
the utility model provides a triaxial motion platform improved generation cross coupling controlling means which characterized in that: the device comprises a main circuit, a control circuit and a control object; the main circuit comprises an alternating current voltage regulating module, a rectifying and filtering module and an IPM inversion module; the control circuit comprises a DSP processor, a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation drive circuit and an IPM protection circuit; the control object is a three-phase permanent magnet linear synchronous motor, and a grating ruler is arranged on the machine body; the intelligent power supply comprises a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation driving circuit and an IPM protection circuit which are connected with a DSP processor, the IPM isolation driving circuit and the IPM protection circuit are connected with an IPM inversion module, the current sampling circuit is connected to a three-phase permanent magnet linear synchronous motor through a Hall sensor, the voltage adjusting circuit is connected with an alternating current voltage regulating module, the alternating current voltage regulating module is connected with a rectification filter module, the rectification filter module is connected with the IPM inversion module, the IPM inversion module is connected with the three-phase permanent magnet linear synchronous motor, and a grating ruler on the three-phase permanent magnet linear synchronous motor is connected with the rotor position sampling circuit.
The improved cross-coupling control method of the three-axis motion platform implemented by the improved cross-coupling control device of the three-axis motion platform is characterized by comprising the following steps of: the method adopts a contour error estimation algorithm to establish a contour error model of the three-axis motion platform, combines single-axis tracking control with three-axis cross coupling control, and improves the prior cross coupling control structure, thereby ensuring that the single-axis tracking precision and the contour precision of the system approach to zero.
And single-axis tracking control, wherein the single-axis tracking control adopts a position-speed loop double closed loop control mode and a single-axis tracking control system design.
The speed loop employs a pseudo-differential feedback controller with feedforward, i.e., a PDFF controller, whose control algorithm is expressed as:
wherein k isfFor feed-forward compensation of gain, kiTo integrate the gain, kpIs a proportional gain; speed loop control input vd(s) function of actual output velocity vaThe relationship between(s) is:
disturbance input ξ(s) and actual output velocity function vaThe relationship between(s) is:
the controlled object adopts a permanent magnet synchronous linear motor with a transfer function of
Wherein G is0(s) < 1/(Ms + B) is the actual controlled object, KfIs the electromagnetic thrust coefficient.
The position ring adopts a proportional controller with the coefficient of kxTherefore, the transfer function of the entire single-axis tracking control system can be expressed as:
by setting the fixed disturbance xi, the system can be verified to have stronger anti-interference capability and quicker response capability.
The method comprises the following steps:
the invention comprises the following specific steps:
step 1: establishing a three-axis motion platform contour error model:
the triaxial motion platform adopts a permanent magnet synchronous linear motor (PMLSM) which is vertical to each other, and the permanent magnet linear synchronous electromechanical equation is as follows:
in the formula, Fe: electromagnetic thrust; m: the rotor of the permanent magnet linear motor and the total mass of loads carried by the rotor; i.e. iqIs the q-axis current of the rotor; kf: an electromagnetic thrust coefficient; b: a coefficient of viscous friction; f: the total disturbance force to which the system is subjected. v is the mover speed;the rotor acceleration is obtained;
choosing x (t) and v (t) as system state variables, i.e. the state equation of PMLSM can be rewritten as
Wherein v (t) is motor mover speed; u-iqRepresenting a control input quantity of the motor; x (t) is the position output of the linear motor.
Therefore, the direct drive three-axis motion platform can be composed of three 2 nd order differential equations:
i.e. expressed as a state space, of the form:
wherein z is1(t)＝[x1(t) x2(t) x3(t)]T，u＝[u1u2u3]T，ρ＝[F1F2F3]T，A11＝0，A12＝I，A21＝0，A22＝diag(-Bi/Mi) X, y, z, which are 3 × 3 matrixes;
step 2: establishing a three-axis motion platform contour error model:
in a three-axis motion platform, the accuracy of contour error model estimation directly affects contour control performance. In a hypothetical three-axis motion platformFor commanded position, P is actual position, and position error vector isThe profile error vector isR0、R1Are two points in the command position, respectively denoted as R0(x0,y0,z0)，R1(x1,y1,z1) (ii) a Point Q is the command position vectorThe coordinate of the point (c) is denoted as Q (x, y, z). Point P to point R1Is a position error vectorExpressed in mathematical relation:
vector quantityIs composed of
From R0、R1And Q, deriving a linear equation for the command position as:
assuming an actual position P to a commanded positionIs a vectorThus vectorIs composed of
Vector quantityAnd vectorMutually vertical, the inner product is zero; namely, it isThe obtained parameter t is substituted into equation (12) to obtain coordinate Q, the obtained coordinate Q can be used for further obtaining contour error value, and finally the contour error is deducedIs composed of
From equation (14), the profile error is knownComponents in the x-axis, y-axis, and z-axis;
step three: compensator design for contour error
To reduce profile errors, it is desirable that the actual position P be vectorized to the commanded positionCorrection, other than correcting position error vectorIn each axial component Ex，Ey，EzIn addition, the contour error vector needs to be compensatedThus, a vector is selectedAs profile error from actual position to commanded positionDepending on the size of λ. Thus, the handleAs an integral wholeThe compensation quantity of each system and the compensation relation between the actual position and the expected position are as follows:
by the equation (15), it is possible to compensate the actual position point P to the desired position point R1The tracking error of (2) can compensate the contour error between two points, so that the contour error approaches to the command position. Thereby obtaining the whole compensationComponent at each axis:
the resultant vector can be obtained by the equation (16)Approaching the commanded position path, where λ is the cross-coupling gain value, affecting the speed of correction of the profile error. From a composite vectorThe larger the lambda value is, the larger the geometrical relationship of (a),correcting the profile error vector for more biased command pathsThe amount of the catalyst is large;
and 4, step 4: single axis tracking controller design
In order to ensure the profile accuracy of three axes, single-axis tracking control is also necessary, the single-axis tracking control adopts a control mode of combining a speed loop controller and a position loop controller, and the speed loop controller adopts PDFF controlScheme, position loop controller kxA proportional control mode is adopted;
and 5: contour controller design
From the contour error estimation method mentioned above, the contour error can be knownOnly with the command positionThe cross coupling controller is designed to be positioned in a position loop part of a control system because of the geometrical relation of the position relative to the actual position P, and the prior cross coupling control structure is improved.
The input of the cross coupling controller is a given position R of the three-axis motion platformx、RyAnd RzAnd tracking error per axis Ex、EyAnd Ez。ex、eyAnd ezIs the profile error component for each axis of the cross-coupled controller output.
The method is finally realized by a control program embedded in a DSP processor, and the control process is executed according to the following steps:
step 1, initializing a system;
step 2, allowing TN1 and TN2 to be interrupted;
step 3 starts a T1 underflow interrupt;
step 4, initializing program data;
5, opening total interruption;
step 6, interrupt waiting;
step 7, TN1 interrupts the process of the sub-control program;
and 8, finishing the step.
Wherein the T1 interrupt processing sub-control program in step 7 comprises the following steps:
step 1T 1 interrupts the sub-control program;
step 2, protecting the site;
step 3, judging whether the initial positioning is carried out or not; if yes, entering step 4, otherwise entering step 10;
step 4, current sampling, CLARK conversion and PARK conversion;
step 5, judging whether position adjustment is needed; otherwise, entering step 7;
step 6, the position is adjusted to interrupt the sub-control program;
step 7 d q axis current adjustment;
step 8, inverse PARK transformation;
step 9, calculating CMPPx and PWM output;
step 10, sampling the position;
step 11, an initial positioning program;
step 12, restoring the site;
step 13 interrupts the return.
Wherein, the position adjusting interrupt processing sub-control program in the step 6 comprises the following steps:
step 1, position adjustment interruption sub-control program;
step 2, reading an encoder value;
step 3, judging an angle;
step 4, calculating the distance traveled;
step 5, executing the position controller;
step 6, calculating and outputting a current command;
step 7 interrupts the return.
The advantages and effects are as follows: the invention provides an improved cross-coupling control device and method for a three-axis motion platform, and along with the increase of the requirements of people on complex elements, compared with the traditional representative two-axis XY platform profile control, the precision profile motion control research of the high-performance profile processing of the multi-axis motion platform has important practical significance and wide application prospect. And the multi-axis motion platform adopts a direct driving mode of a plurality of permanent magnet synchronous linear motors, so that the intermediate transmission link of ball and screw is avoided, the load is only directly pushed by the linear motors, and the problem generated by the traditional transmission mechanism is eliminated. The zero-clearance transmission from the linear motor to a controlled object is realized, so that the linear motor becomes a main driving mode of a high-speed and high-precision servo control system.
Aiming at the problems of profile control precision of complex elements in the prior control technology, the invention adopts a profile error estimation algorithm in three-axis coordination control to establish a three-axis profile error model, improves the structure of cross coupling control and designs a three-dimensional spatial profile error controller.
The controller designed by the invention is applied to a numerical control platform which is driven by a linear motor and is vertical to each other in X-Y-Z axes. The experimental system is shown in fig. 2. The position of the stage is connected to a linear encoder for each drive shaft, the sensor resolution of which is 0.1 micron. The velocity of each drive shaft is calculated from the inverse difference of the position measurements, this sample period being 2 milliseconds.
The method comprises the steps of establishing a three-axis motion platform contour error model, so that a system can complete a spatial contour track tracking task; the single-axis tracking controller is designed to ensure that the tracking error of each axis is in a smaller range; the design of a contour controller reduces the contour error of the system; geometric relations of the contour error model, as shown in fig. 3; geometric relationship of the profile error compensation quantity, as shown in fig. 4; single axis tracking controller design, as shown in FIG. 1; a three axis profile controller design as shown in fig. 5.
The invention mainly takes a three-axis motion platform as a research object and ensures the processing precision of parts by controlling the overall contour error of three axes. In order to improve the contour machining precision, many scholars are dedicated to research various feedforward and feedback control strategies to improve the single-axis tracking precision, so as to indirectly improve the contour motion control precision. The single-axis tracking error can be reduced by methods such as a feedforward controller, a zero-phase error tracking controller, PID control, self-adaptive control, robust control and the like. But the reduction of the single axis tracking error cannot guarantee the overall profile accuracy. Therefore, single-axis tracking control and inter-axis coordination control are two important factors influencing the contour accuracy of the three-axis motion platform system. The single-axis tracking control adopts a control method combining a position ring and a speed ring, the position ring is in proportion control, the speed ring is in PDFF control, and the quick response speed and tracking precision of the single axis can be guaranteed. In order to improve the inter-axis coordination, the inter-axis contour control generally adopts a cross-coupled controller (CCC) to coordinate the dynamic performance difference caused by parameter mismatch, so as to reduce the contour error of the system. Aiming at the problem, the method adopts a contour error estimation algorithm to establish a contour error model between three axes. On the basis, the traditional cross coupling control structure is improved, a three-axis cross coupling controller is designed, and the method can effectively improve the contour precision among three axes.
Description of the drawings:
FIG. 1 Single-axis tracking control System Block diagram
FIG. 2 is an experimental system designed by the present invention
FIG. 3 is a geometric relationship diagram of error vectors of straight line profile
FIG. 4 is a geometric relationship diagram for contour error compensation
FIG. 5 is a block diagram of cross-coupling control for a three-axis motion platform
FIG. 6 is a hardware block diagram of a vector control system of a permanent magnet linear synchronous motor designed to implement the present invention
FIG. 7 is a flowchart of a vector control system routine in the method of the present invention
FIG. 8 is a flowchart of a position adjustment interrupt handling sub-control procedure of the method of the present invention
FIG. 9 is a control system schematic implementing the present invention
(a) Principle diagram of main circuit of motor control system
(b) A, B phase current sampling circuit schematic diagram
(c) Grating ruler signal sampling circuit schematic diagram
(d) IPM hardware drive circuit schematic.
The specific implementation mode is as follows: the invention is further described below with reference to the accompanying drawings:
as shown in FIG. 1, the present invention provides an improved cross-coupling control device and method for a three-axis motion platform, a system hardware structure
A main circuit of the control system of the invention is shown in fig. 9(a), and the voltage regulating circuit adopts a reverse voltage regulating module EUV-25A-II, so that 0-220V isolation voltage regulation can be realized. The rectification filtering unit adopts bridge type uncontrollable rectification and large-capacitance filtering and is matched with a proper resistance-capacitance absorption circuit, so that constant direct-current voltage required by IPM operation can be obtained. The IPM adopts a Fuji 6MBP50RA060 intelligent power module, the withstand voltage is 600V, the maximum current is 50A, and the maximum working frequency is 20 kHz. The IPM is powered by four independent 15V driving power supplies. The main power input terminals (P, N), the output terminals (U, V, W) and the main terminals are fixed by screws, so that current transmission can be realized. P, N is the input terminal of the main power supply after rectification, conversion, smoothing and filtering of the frequency converter, P is the positive terminal, N is the negative terminal, the three-phase alternating current output by the inverter is connected to the motor through the output terminal U, V, W.
The core of the control circuit is a TMS320F2812 processor, and a matched development board comprises a target read-only memory, an analog interface, an eCAN interface, a serial boot ROM, a user indicator lamp, a reset circuit, an asynchronous serial port which can be configured as RS232/RS422/RS485, an SPI synchronous serial port and an off-chip 256K 16-bit RAM.
Current sampling in a practical control system adopts LEM Hall current sensors LT 58-S7. A, B phase current is detected by two Hall current sensors to obtain a current signal, the current signal is converted into a voltage signal of 0-3.3V through a current sampling circuit, and finally the voltage signal is converted into a binary number with 12-bit precision through an A/D conversion module of TMS320LF2812 and stored in a numerical value register. A. The B-phase current sampling circuit is shown in fig. 9 (B). The adjustable resistor VR2 adjusts the amplitude of the signal, the adjustable resistor VR1 adjusts the offset of the signal, the signal can be adjusted to 0-3.3V by adjusting the two resistors, and then the signal is sent to the AD0 and AD1 pins of the DSP. The voltage regulator tube is used for preventing the signal sent into the DSP from exceeding 3.3V, so that the DSP is damaged by high voltage. The operational amplifier adopts OP27, and the power is connected with positive and negative 15V voltage, and the capacitor is indirectly decoupled between the voltage and the ground. The input end of the circuit is connected with the capacitor for filtering so as to remove the interference of high-frequency signals and improve the sampling precision.
The A-phase pulse signal and the B-phase pulse signal output by the grating ruler are isolated by a quick optical coupler 6N137, the signal level is converted from 5V to 3.3V through a voltage division circuit, and finally the signals are connected to two paths of orthogonal coding pulse interfaces QEP1 and QEP2 of a DSP. The circuit principle is shown in fig. 9 (c). Fig. 9(d) shows a schematic diagram of a six-way isolated drive circuit. It should be noted that the IPM fault protection signal is directed to non-repetitive transient faults, and is implemented in the present system by the following measures: fault output signal of IPM optically coupled to DSPAnd pins are used for ensuring that the DSP sets all output pins of the event manager to be in a high impedance state in time when the IPM fails.
(II) System software implementation
A flowchart of the vector control system routine of the method of the present invention is shown in fig. 7. FIG. 8 is a flowchart of a position adjustment interrupt handling sub-control routine of the method of the present invention. The main program of the software comprises system initialization; INT1, INT2 interrupt; allowing the timer to interrupt; the timer interrupts the process routine. The initialization program comprises the steps of closing all interrupts, initializing a DSP system, initializing variables, initializing an event manager, initializing AD and initializing orthogonal code pulses QEP. The interrupt service routine includes a protect interrupt routine and a T1 underflow interrupt service routine. Other parts such as rotor initialization positioning, PID adjustment, vector transformation and the like are all executed in the timer TI underflow interrupt processing subprogram.
The IPM protection signal generates a protection interrupt response to the external interrupt, and INT1 interrupt has higher priority than the timer T1. The IPM can automatically send out a protection signal under abnormal conditions of overcurrent, overvoltage and the like, and the signal is connected to a power drive protection pin of the DSP through conversionOnce an abnormal condition occurs, the DSP enters a protection interruption subprogram, all interruption is forbidden firstly, and then PWM output is blocked so that the motor can be stopped at once, and the motor and IPM are protected.
When the vector control system is started successfully, the initial position of the rotor needs to be known, the direct current with constant amplitude can be supplied to the rotor of the motor by using software, so that the stator generates a constant magnetic field, and the magnetic field interacts with the constant magnetic field of the rotor to enable the rotor of the motor to move to the position where the two magnetic chains are overlapped. And the initial positioning of the rotor, the reading of the AD sampling value, the calculation of the position of the motor rotor, the coordinate transformation, the PID regulation and the generation of the SVPWM waveform comparison value are all completed in the T1 underflow interrupt service subprogram.
The detailed description is as follows:
as shown in fig. 6, the device comprises a main circuit, a control circuit and a control object; the main circuit comprises an alternating current voltage regulating module, a rectifying and filtering module and an IPM inversion module; the control circuit comprises a DSP processor, a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation drive circuit and an IPM protection circuit; the control object is a three-phase permanent magnet linear synchronous motor, and a grating ruler is arranged on the machine body; the intelligent power supply comprises a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation driving circuit and an IPM protection circuit which are connected with a DSP processor, the IPM isolation driving circuit and the IPM protection circuit are connected with an IPM inversion module, the current sampling circuit is connected to a three-phase permanent magnet linear synchronous motor through a Hall sensor, the voltage adjusting circuit is connected with an alternating current voltage regulating module, the alternating current voltage regulating module is connected with a rectification filter module, the rectification filter module is connected with the IPM inversion module, the IPM inversion module is connected with the three-phase permanent magnet linear synchronous motor, and a grating ruler on the three-phase permanent magnet linear synchronous motor is connected with the rotor position sampling circuit.
The method adopts a contour error estimation algorithm to establish a contour error model of the triaxial motion platform, combines single-axis tracking control with triaxial cross-coupling control, and improves the prior cross-coupling control structure, thereby ensuring that the single-axis tracking precision and the contour precision of the system approach to zero.
Single-axis tracking control, adopting a control mode of combining a position ring and a speed ring, wherein a single-axis tracking control system is designed as shown in figure 1, 1/(Ms + B) is an actual controlled object, and KfIs the electromagnetic thrust coefficient, xrFor an input reference instruction, xpIs the actual output position. The single-axis tracking control adopts a control mode combining a position loop and a speed loop, the position loop adopts proportional control, the speed loop adopts a PDFF controller, kxIs the position loop proportional gain; velocity ring kfFor feed-forward compensation of gain, kiTo integrate the gain, kpξ is external disturbance, and the system can be verified to have stronger anti-interference capability and quicker response capability by setting fixed disturbance.
In the traditional contour machining, contour precision control is generally performed only on an XY plane, and the control is difficult to extend to a three-dimensional space, so that the practical numerical control machining has great limitation. Therefore, a three-axis motion platform space contour error model is established by adopting a contour error estimation method. And as described in the claims, improved cross-coupling control methods are used to improve the profile tracking performance and improve the profile accuracy.
The method comprises the following steps:
the invention comprises the following specific steps:
step 1: establishing a three-axis motion platform contour error model:
the triaxial motion platform adopts a permanent magnet synchronous linear motor (PMLSM) which is vertical to each other, and the permanent magnet linear synchronous electromechanical equation is as follows:
in the formula, Fe: electromagnetic thrust; m: the rotor of the permanent magnet linear motor and the total mass of loads carried by the rotor; i.e. iqIs the q-axis current of the rotor; kf: an electromagnetic thrust coefficient; b: a coefficient of viscous friction; f: the total disturbance force to which the system is subjected. v is the mover speed;the rotor acceleration is obtained;
choosing x (t) and v (t) as system state variables, i.e. the state equation of PMLSM can be rewritten as
Wherein v (t) is motor mover speed; u-iqRepresenting a control input quantity of the motor; x (t) is the position output of the linear motor.
Therefore, the direct drive three-axis motion platform can be composed of three 2 nd order differential equations:
i.e. expressed as a state space, of the form:
wherein z is1(t)＝[x1(t) x2(t) x3(t)]T，u＝[u1u2u3]T，ρ＝[F1F2F3]T，A11＝0，A12＝I，A21＝0，A22＝diag(-Bi/Mi) X, y, z, which are 3 × 3 matrixes;
step 2: establishing a three-axis motion platform contour error model:
in a three-axis motion platform, the accuracy of contour error model estimation directly affects contour control performance. FIG. 3 is a geometric relationship diagram of error vectors of straight line profiles. Wherein,for commanded position, P is actual position, and position error vector isThe profile error vector isR0、R1Are two points in the command position, respectively denoted as R0(x0,y0,z0)，R1(x1,y1,z1) (ii) a The shortest distance from the actual position P to the command position R is a vectorI.e. the profile error vector from the actual position to the reference positionThe coordinate of the point Q is denoted as Q (x, y, z). Point P to point R1Is a position error vector
From R0、R1And Q, deriving a linear equation for the command position as:
from FIG. 4, the vectorsAnd vectorMutually vertical, the inner product is zero; namely, it isThe obtained parameter t is substituted into equation (6) to obtain coordinate Q, the obtained coordinate Q can be used for further obtaining contour error value, and finally the contour error is deducedIs composed of
From equation (6), the profile error is knownComponents in the x-axis, y-axis, and z-axis;
step three: compensator design for contour error
According to fig. 4, in order to reduce profile errors, it is desirable that the actual position P can be vectored to the commanded positionCorrection, other than correcting position error vectorIn each axial component Ex，Ey，EzIn addition, the contour error vector needs to be compensatedThe vector is selected by vector geometric addition and subtractionAs compensation from the actual position to the commanded position, it approaches the commanded position. Total compensation amountThe components in each axis can be expressed as:
the resultant vector can be obtained by the formula (7)Approaching the commanded position path, where λ is the cross-coupling gain value, affecting the speed of correction of the profile error. From a composite vectorThe larger the lambda value is, the larger the geometrical relationship of (a),correcting the profile error vector for more biased command pathsThe amount of the catalyst is large;
and 4, step 4: single axis tracking controller design
In order to ensure the profile accuracy of three axes, single-axis tracking control is also necessary, the single-axis tracking control adopts a control mode of combining a speed loop controller and a position loop controller, the speed loop controller adopts a PDFF control scheme, and the position loop controller k adopts a PDFF control schemexA proportional control mode is adopted;
and 5: contour controller design
From the above-mentioned contour error estimation method, it can be known that the contour error e is only related to the command position R and the actual position P, and belongs to the geometric relationship of positions, so the designed cross-coupling controller is located in the position loop part of the control system, and the prior cross-coupling control structure is improved, and the structural block diagram is shown in fig. 5.
The input of the cross coupling controller is a given position R of the three-axis motion platformx、RyAnd RzAnd tracking error per axis Ex、EyAnd Ez。ex、eyAnd ezIs the profile error component for each axis of the cross-coupled controller output. And comparing the structural block diagram of the three-axis cross coupling controller designed in the invention with the structural block diagram adopted in the past, the contour error compensation of the invention is completed before the position loop controller. From the geometric relationship of the profile error compensation, when adjusting the gain K in the position loop controllerpWhile the profile error compensation quantity is simultaneously influencedThe effect is equal to the adjustmentBut not the direction, but the direction is determined by the magnitude of the cross-coupling gain value λ. Thus KpThe adjustment of λ is independent of the adjustment of λ, i.e. magnitude and direction. The prior cross-coupling controller structure is to place the compensation quantity after the controller and adjust KpThe effect is equivalent to adjusting only in fig. 3The size of (2). Therefore, the method proposed in the present invention compensates the contour error by the amountBoth the direction and the magnitude of the position loop gain K are changed simultaneously, so that the position loop gain K in the structure diagram is increasedpThe matching problem with the most appropriate adjustment between the cross-coupling gain lambda.
The input of the cross coupling controller is a given position R of the three-axis motion platformx、RyAnd RzAnd tracking error per axis Ex、EyAnd Ez。ex、eyAnd ezIs the profile error component for each axis of the cross-coupled controller output.
The method is finally realized by a control program embedded in a DSP processor, and the control process is executed according to the following steps:
step 1, initializing a system;
step 2, allowing TN1 and TN2 to be interrupted;
step 3 starts a T1 underflow interrupt;
step 4, initializing program data;
5, opening total interruption;
step 6, interrupt waiting;
step 7, TN1 interrupts the process of the sub-control program;
and 8, finishing the step.
Wherein the T1 interrupt processing sub-control program in step 7 comprises the following steps:
step 1T 1 interrupts the sub-control program;
step 2, protecting the site;
step 3, judging whether the initial positioning is carried out or not; if yes, entering step 4, otherwise entering step 10;
step 4, current sampling, CLARK conversion and PARK conversion;
step 5, judging whether position adjustment is needed; otherwise, entering step 7;
step 6, the position is adjusted to interrupt the sub-control program;
step 7 d q axis current adjustment;
step 8, inverse PARK transformation;
step 9, calculating CMPPx and PWM output;
step 10, sampling the position;
step 11, an initial positioning program;
step 12, restoring the site;
step 13 interrupts the return.
Wherein, the position adjusting interrupt processing sub-control program in the step 6 comprises the following steps:
step 1, position adjustment interruption sub-control program;
step 2, reading an encoder value;
step 3, judging an angle;
step 4, calculating the distance traveled;
step 5, executing the position controller;
step 6, calculating and outputting a current command;
step 7 interrupts the return.
The invention aims at a direct-drive three-axis motion platform, and has the advantages of establishing a three-dimensional space contour error model and a method for controlling the space contour error. The problem of in modern processing system, people's demand to complicated component constantly increases, but can not satisfy complicated component machining precision is solved. The invention is mainly directed to reducing single axis tracking errors and contour errors. The single-axis tracking error utilizes a control mode of combining a position ring controller and a speed ring controller, and the single-axis tracking error is guaranteed within a good precision range. The invention mainly provides a novel contour error estimation model for estimating contour errors, and the contour errors are applied to a triaxial cross-coupling contour controller, so that the control structure of the triaxial cross-coupling controller is improved. Through the combination of the two parts, the contour error of the three-axis motion platform system is finally enabled to approach zero.
Claims (6)
1. An improved cross coupling control method of a three-axis motion platform is characterized in that: the method is implemented by adopting the following devices: the device comprises a main circuit, a control circuit and a control object; the main circuit comprises an alternating current voltage regulating module, a rectifying and filtering module and an IPM inversion module; the control circuit comprises a DSP processor, a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation drive circuit and an IPM protection circuit; the control object is a three-phase permanent magnet linear synchronous motor, and a grating ruler is arranged on the machine body; the intelligent power supply comprises a current sampling circuit, a rotor position sampling circuit, a voltage adjusting circuit, an IPM isolation driving circuit and an IPM protection circuit, wherein the current sampling circuit, the rotor position sampling circuit, the voltage adjusting circuit, the IPM isolation driving circuit and the IPM protection circuit are all connected with a DSP (digital signal processor), the IPM isolation driving circuit and the IPM protection circuit are connected with an IPM (intelligent power management) inversion module, the current sampling circuit is connected to a three-phase permanent magnet linear synchronous motor through a Hall sensor, the voltage adjusting circuit is connected with an alternating current voltage adjusting module, the alternating current voltage adjusting module is connected with a rectification filter module, the rectification filter module is connected with the IPM inversion module, the IPM;
the method adopts a contour error estimation algorithm to establish a contour error model of the three-axis motion platform, combines single-axis tracking control with three-axis cross coupling control, and improves the prior cross coupling control structure, thereby ensuring that the single-axis tracking precision and the contour precision of the system approach to zero;
single-axis tracking control, wherein the single-axis tracking control adopts a position-speed loop double closed loop control mode and a single-axis tracking control system is designed;
the speed loop employs a pseudo-differential feedback controller with feedforward, i.e., a PDFF controller, whose control algorithm is expressed as:
<mrow> <mi>u</mi> <mo>=</mo> <msub> <mi>k</mi> <mi>i</mi> </msub> <msubsup> <mo>&Integral;</mo> <mn>0</mn> <mi>t</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>v</mi> <mi>d</mi> </msub> <mo>-</mo> <msub> <mi>v</mi> <mi>a</mi> </msub> <mo>)</mo> </mrow> <mi>d</mi> <mi>t</mi> <mo>+</mo> <msub> <mi>k</mi> <mi>f</mi> </msub> <msub> <mi>v</mi> <mi>d</mi> </msub> <mo>-</mo> <msub> <mi>k</mi> <mi>p</mi> </msub> <msub> <mi>v</mi> <mi>a</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
wherein k isfFor feed-forward compensation of gain, kiTo integrate the gain, kpIs a proportional gain; speed loop control input vd(s) function of actual output velocity vaThe relationship between(s) is:
<mrow> <mfrac> <mrow> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>v</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msub> <mi>G</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>k</mi> <mi>f</mi> </msub> <mo>+</mo> <msub> <mi>k</mi> <mi>i</mi> </msub> <mo>/</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>G</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>k</mi> <mi>p</mi> </msub> <mo>+</mo> <msub> <mi>k</mi> <mi>i</mi> </msub> <mo>/</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
disturbance input ξ(s) and actual output velocity function vaThe relationship between(s) is:
<mrow> <mfrac> <mrow> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mi>&xi;</mi> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msub> <mi>G</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>G</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>k</mi> <mi>p</mi> </msub> <mo>+</mo> <msub> <mi>k</mi> <mi>i</mi> </msub> <mo>/</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
the controlled object adopts a permanent magnet synchronous linear motor with a transfer function of
<mrow> <msub> <mi>G</mi> <mi>p</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <msub> <mi>K</mi> <mi>f</mi> </msub> <msub> <mi>G</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mi>s</mi> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
Wherein G is0(s) < 1/(Ms + B) is the actual controlled object, KfIs the electromagnetic thrust coefficient;
the position ring adopts a proportional controller with the coefficient of kxTherefore, the transfer function of the entire single-axis tracking control system can be expressed as:
<mrow> <mfrac> <mrow> <msub> <mi>x</mi> <mi>p</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>x</mi> <mi>r</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msub> <mi>k</mi> <mi>x</mi> </msub> <mo>&CenterDot;</mo> <mfrac> <mrow> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>v</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mn>1</mn> <mi>s</mi> </mfrac> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>k</mi> <mi>x</mi> </msub> <mo>&CenterDot;</mo> <mfrac> <mrow> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>v</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>&CenterDot;</mo> <mfrac> <mn>1</mn> <mi>s</mi> </mfrac> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <msub> <mi>k</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>v</mi> <mi>d</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> <mi>s</mi> <mo>+</mo> <msub> <mi>k</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>a</mi> </msub> <mrow> <mo>(</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
by setting the fixed disturbance xi, the system can be verified to have stronger anti-interference capability and quicker response capability.
2. The improved cross-coupling control method for the three-axis motion platform according to claim 1, wherein: the method comprises the following steps:
the invention comprises the following specific steps:
step 1: establishing a three-axis motion platform contour error model:
the three-axis motion platform adopts a PMLSM (permanent magnet synchronous linear motor) with two vertical permanent magnet synchronous motors, and the permanent magnet linear synchronous electromechanical equation is as follows:
<mrow> <msub> <mi>F</mi> <mi>e</mi> </msub> <mo>=</mo> <msub> <mi>K</mi> <mi>f</mi> </msub> <msub> <mi>i</mi> <mi>q</mi> </msub> <mo>=</mo> <mi>M</mi> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>B</mi> <mi>v</mi> <mo>+</mo> <mi>F</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
in the formula, Fe: electromagnetic thrust; m: the rotor of the permanent magnet linear motor and the total mass of loads carried by the rotor; i.e. iqIs the q-axis current of the rotor; kf: an electromagnetic thrust coefficient; b: a coefficient of viscous friction; f: the total disturbance force applied to the system; v is the mover speed;the rotor acceleration is obtained;
choosing x (t) and v (t) as system state variables, i.e. the state equation of PMLSM can be rewritten as:
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>v</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>v</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <mfrac> <mi>B</mi> <mi>M</mi> </mfrac> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <msub> <mi>K</mi> <mi>f</mi> </msub> <mi>M</mi> </mfrac> <mi>u</mi> <mo>+</mo> <mfrac> <mi>F</mi> <mi>M</mi> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
wherein v (t) is motor mover speed; u-iqRepresenting a control input quantity of the motor; x (t) is the position output of the linear motor;
therefore, the direct drive three-axis motion platform can be composed of three 2 nd order differential equations:
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <mfrac> <mi>B</mi> <mi>M</mi> </mfrac> <msub> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mn>1</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <msub> <mi>K</mi> <mrow> <mi>f</mi> <mn>1</mn> </mrow> </msub> <mi>M</mi> </mfrac> <msub> <mi>u</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>F</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <mfrac> <mi>B</mi> <mi>M</mi> </mfrac> <msub> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mn>2</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <msub> <mi>K</mi> <mrow> <mi>f</mi> <mn>2</mn> </mrow> </msub> <mi>M</mi> </mfrac> <msub> <mi>u</mi> <mn>2</mn> </msub> <mo>+</mo> <msub> <mi>F</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <mfrac> <mi>B</mi> <mi>M</mi> </mfrac> <msub> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mn>3</mn> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mfrac> <msub> <mi>K</mi> <mrow> <mi>f</mi> <mn>3</mn> </mrow> </msub> <mi>M</mi> </mfrac> <msub> <mi>u</mi> <mn>3</mn> </msub> <mo>+</mo> <msub> <mi>F</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
i.e. expressed as a state space, of the form:
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>z</mi> <mo>&CenterDot;</mo> </mover> <mn>1</mn> </msub> <mo>=</mo> <msub> <mi>A</mi> <mn>11</mn> </msub> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>A</mi> <mn>12</mn> </msub> <msub> <mi>z</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>z</mi> <mo>&CenterDot;</mo> </mover> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>A</mi> <mn>21</mn> </msub> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>A</mi> <mn>22</mn> </msub> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>+</mo> <mi>C</mi> <mi>u</mi> <mo>+</mo> <mi>&rho;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
wherein z is1(t)＝[x1(t) x2(t) x3(t)]T，u＝[u1u2u3]T，ρ＝[F1F2F3]T，A11＝0，A12＝I，A21＝0，A22＝diag(-Bi/Mi) X, y, z, which are 3 × 3 matrixes;
step 2: establishing a three-axis motion platform contour error model:
in a three-axis motion platform, the accuracy of contour error model estimation directly affects contour control performance; in a hypothetical three-axis motion platformFor commanded position, P is actual position, and position error vector isThe profile error vector isR0、R1Are two points in the command position, respectively denoted as R0(x0,y0,z0)，R1(x1,y1,z1) (ii) a Point Q is the command position vectorCoordinate Q (x, y, z); point P to point R1Is a position error vectorExpressed in mathematical relation:
<mrow> <mover> <mi>E</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msub> <mi>R</mi> <mn>1</mn> </msub> <mo>-</mo> <mi>P</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>-</mo> <mi>a</mi> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mi>b</mi> </mtd> </mtr> <mtr> <mtd> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mi>c</mi> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
vector quantityIs composed of
<mrow> <mover> <mi>R</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msub> <mi>R</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>R</mi> <mn>0</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>0</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
From R0、R1And Q, deriving a linear equation for the command position as:
<mrow> <mi>L</mi> <mo>:</mo> <mfrac> <mrow> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> </mrow> <mrow> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> </mrow> <mrow> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>0</mn> </msub> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mi>z</mi> <mo>-</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> </mrow> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>0</mn> </msub> </mrow> </mfrac> <mo>=</mo> <mi>t</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
assuming an actual position P to a commanded positionIs a vectorThus vectorIs composed of
<mrow> <mover> <mrow> <mi>P</mi> <mi>Q</mi> </mrow> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>t</mi> <mo>-</mo> <mi>a</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>t</mi> <mo>-</mo> <mi>b</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>t</mi> <mo>-</mo> <mi>c</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>x</mi> </msub> <mi>t</mi> <mo>-</mo> <mi>a</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>y</mi> </msub> <mi>t</mi> <mo>-</mo> <mi>b</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>z</mi> </msub> <mi>t</mi> <mo>-</mo> <mi>c</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>E</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>x</mi> </msub> <mi>t</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>E</mi> <mi>y</mi> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>y</mi> </msub> <mi>t</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>E</mi> <mi>z</mi> </msub> <mo>+</mo> <msub> <mi>&Delta;R</mi> <mi>z</mi> </msub> <mi>t</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
Vector quantityAnd vectorMutually vertical, the inner product is zero; namely, it isThe obtained parameter t is substituted into equation (12) to obtain coordinate Q, the obtained coordinate Q can be used for further obtaining contour error value, and finally the contour error is deducedIs composed of
From equation (14), the profile error is knownComponents in the x-axis, y-axis, and z-axis;
step three: compensator design for contour error
To reduce profile errors, it is desirable that the actual position P be vectorized to the commanded positionCorrection, other than correcting position error vectorIn each axial component Ex，Ey，EzIn addition, the contour error vector needs to be compensatedThus, a vector is selectedAs profile error from actual position to commanded positionDepending on the size of λ; thus, the handleAs the compensation amount of the whole system, the compensation relation between the actual position and the expected position is as follows:
<mrow> <mover> <mi>C</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <mover> <mi>E</mi> <mo>&RightArrow;</mo> </mover> <mo>+</mo> <mi>&lambda;</mi> <mover> <mi>e</mi> <mo>&RightArrow;</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
by the equation (15), it is possible to compensate the actual position point P to the desired position point R1Can also be usedCompensating the contour error between the two points to make the contour error approach to the command position; thereby obtaining the whole compensationComponent at each axis:
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>E</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>&lambda;e</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>E</mi> <mi>y</mi> </msub> <mo>+</mo> <msub> <mi>&lambda;e</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>E</mi> <mi>z</mi> </msub> <mo>+</mo> <msub> <mi>&lambda;e</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
the resultant vector can be obtained by the equation (16)Approaching to the command position path, wherein lambda is the cross coupling gain value and influences the correction speed of the profile error; from a composite vectorThe larger the lambda value is, the larger the geometrical relationship of (a),correcting the profile error vector for more biased command pathsThe amount of the catalyst is large;
and 4, step 4: single axis tracking controller design
In order to ensure the profile accuracy of three axes, single-axis tracking control is also necessary, the single-axis tracking control adopts a control mode of combining a speed loop controller and a position loop controller, the speed loop controller adopts a PDFF control scheme, and the position loop controller k adopts a PDFF control schemexA proportional control mode is adopted;
and 5: contour controller design
From the contour error estimation method mentioned above, the contour error can be knownOnly with the command positionThe cross coupling controller is designed to be positioned in a position loop part of a control system because of the geometrical relation of the position relative to the actual position P, and the prior cross coupling control structure is improved.
3. The improved cross-coupling control method for the three-axis motion platform according to claim 2, wherein: the input of the cross-coupling controller is the input of a three-axis motion platformSet position Rx、RyAnd RzAnd tracking error per axis Ex、EyAnd Ez；ex、eyAnd ezIs the profile error component for each axis of the cross-coupled controller output.
4. The improved cross-coupling control method for the three-axis motion platform according to claim 2, wherein: the method is finally realized by a control program embedded in a DSP processor, and the control process is executed according to the following steps:
step 1, initializing a system;
step 2, allowing TN1 and TN2 to be interrupted;
step 3 starts a T1 underflow interrupt;
step 4, initializing program data;
5, opening total interruption;
step 6, interrupt waiting;
step 7, TN1 interrupts the process of the sub-control program;
and 8, finishing the step.
5. The improved cross-coupling control method for the three-axis motion platform of claim 4, wherein: wherein the T1 interrupt processing sub-control program in step 7 comprises the following steps:
step 1T 1 interrupts the sub-control program;
step 2, protecting the site;
step 3, judging whether the initial positioning is carried out or not; if yes, entering step 4, otherwise entering step 10;
step 4, current sampling, CLARK conversion and PARK conversion;
step 5, judging whether position adjustment is needed; otherwise, entering step 7;
step 6, the position is adjusted to interrupt the sub-control program;
step 7 d q axis current adjustment;
step 8, inverse PARK transformation;
step 9, calculating CMPPx and PWM output;
step 10, sampling the position;
step 11, an initial positioning program;
step 12, restoring the site;
step 13 interrupts the return.
6. The improved cross-coupling control method for the three-axis motion platform of claim 5, wherein: wherein, the position adjusting interrupt processing sub-control program in the step 6 comprises the following steps:
step 1, position adjustment interruption sub-control program;
step 2, reading an encoder value;
step 3, judging an angle;
step 4, calculating the distance traveled;
step 5, executing the position controller;
step 6, calculating and outputting a current command;
step 7 interrupts the return.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510404800.4A CN104977901B (en) | 2015-07-10 | 2015-07-10 | Triaxial movement platform modified cross-coupling control device and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510404800.4A CN104977901B (en) | 2015-07-10 | 2015-07-10 | Triaxial movement platform modified cross-coupling control device and method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104977901A CN104977901A (en) | 2015-10-14 |
| CN104977901B true CN104977901B (en) | 2017-11-07 |
Family
ID=54274500
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510404800.4A Active CN104977901B (en) | 2015-07-10 | 2015-07-10 | Triaxial movement platform modified cross-coupling control device and method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104977901B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106125674B (en) * | 2016-08-03 | 2018-07-13 | 大连理工大学 | A kind of high-precision real time profile error estimation |
| CN106961231B (en) * | 2017-03-13 | 2019-04-30 | 江苏大学 | A kind of permanent magnet linear motor Direct Thrust Control Strategy based on anti-saturation PI controller and duty ratio modulation |
| CN107941244B (en) * | 2017-12-21 | 2020-01-17 | 中国电子科技集团公司第二十六研究所 | Cross coupling debugging device and method for triaxial inertial system |
| TWI695280B (en) * | 2018-10-08 | 2020-06-01 | 財團法人資訊工業策進會 | Apparatus and method for determining a target adjustment route for a preset control condition set of a production line |
| CN109491285B (en) * | 2018-10-31 | 2020-11-06 | 深圳中广核工程设计有限公司 | Control method and system for offline downloading of automatic processing controller in nuclear power joint debugging stage |
| CN110488749B (en) * | 2019-09-11 | 2020-08-21 | 台州学院 | Contour error controller of multi-axis motion system and control method thereof |
| CN111030536B (en) * | 2019-11-28 | 2023-03-07 | 深圳市睿阳精视科技有限公司 | Control system of permanent magnet synchronous motor speed ring |
| CN112000011B (en) * | 2020-08-12 | 2022-05-24 | 深圳市烨嘉为技术有限公司 | Electromechanical coupling analysis and optimization method for small gantry numerical control machining center |
| CN112596389B (en) * | 2020-12-18 | 2022-05-17 | 杭州电子科技大学 | Crystal grinding control method and system based on closed-loop cross-coupling iterative learning |
| CN114102612B (en) * | 2022-01-24 | 2022-05-03 | 河北工业大学 | Robot tail end path contour error control method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102854840A (en) * | 2012-09-24 | 2013-01-02 | 沈阳工业大学 | Direct-driven XY table profile control method based on predictive control and cross coupling |
| CN104460518A (en) * | 2014-11-16 | 2015-03-25 | 沈阳工业大学 | Direct-drive XY platform profile control device and method based on fuzzy disturbance compensation |
| CN104483897A (en) * | 2014-08-27 | 2015-04-01 | 渤海大学 | Direct-drive gantry type motion platform contour control device and method |
| CN204790506U (en) * | 2015-07-10 | 2015-11-18 | 沈阳工业大学 | Triaxial motion platform improved generation cross coupling controlling means |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8473103B2 (en) * | 2009-01-27 | 2013-06-25 | Fanuc Robotics America, Inc. | Secondary position feedback control of a robot |
-
2015
- 2015-07-10 CN CN201510404800.4A patent/CN104977901B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102854840A (en) * | 2012-09-24 | 2013-01-02 | 沈阳工业大学 | Direct-driven XY table profile control method based on predictive control and cross coupling |
| CN104483897A (en) * | 2014-08-27 | 2015-04-01 | 渤海大学 | Direct-drive gantry type motion platform contour control device and method |
| CN104460518A (en) * | 2014-11-16 | 2015-03-25 | 沈阳工业大学 | Direct-drive XY platform profile control device and method based on fuzzy disturbance compensation |
| CN204790506U (en) * | 2015-07-10 | 2015-11-18 | 沈阳工业大学 | Triaxial motion platform improved generation cross coupling controlling means |
Non-Patent Citations (1)
| Title |
|---|
| 三轴数控系统的轮廓误差补偿方法研究与实现;周延松;《中国优秀硕士学位论文全文数据库工程科技Ⅰ辑》;20140315(第03期);正文第21-39页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104977901A (en) | 2015-10-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104977901B (en) | Triaxial movement platform modified cross-coupling control device and method | |
| CN107132759B (en) | Improved repetitive control method for FTS (fiber to the Home) driven by linear motor based on ESO (electronic stability operating) sliding mode | |
| CN101834554B (en) | Method for improving machining precision by using load disturbance compensator and optimizing setting of load disturbance compensator | |
| CN102033508B (en) | Method for improving machining precision of contour of directly driven XY platform | |
| CN104460518A (en) | Direct-drive XY platform profile control device and method based on fuzzy disturbance compensation | |
| Mannava et al. | Optimal tracking control of motion systems | |
| CN107070336A (en) | The two patterns paste fractional order System with Sliding Mode Controller and method of permanent magnet linear synchronous motor | |
| CN103414419A (en) | Double-linear-motor contour compensation device and method based on fuzzy RBF network sliding mode | |
| CN102637011B (en) | Robust control method and system for directly driving numerical control platform based on coordinate transformation and parameter adjustment | |
| CN102385342A (en) | Self-adaptation dynamic sliding mode controlling method controlled by virtual axis lathe parallel connection mechanism motion | |
| CN108712131A (en) | A kind of current feed-forward method and device adjusted based on proportion integral modulus | |
| CN107370431A (en) | A kind of industrial robot obscures Auto-disturbance-rejection Control with permagnetic synchronous motor | |
| CN112234874B (en) | Underwater robot multi-motor propulsion system and control method | |
| CN108123648B (en) | Linear servo position tracking control based on linear matrix inequality and sliding mode control | |
| CN102707666B (en) | Motor-driven XY platform contour processing control device and method based on direction field | |
| Zhong et al. | Design of dual-drive vertical lift servo system and synchronous control performance analysis | |
| CN105676780A (en) | XY motion platform contour control method and device on the basis of fuzzy cerebellum model joint controller | |
| CN104834219A (en) | PMLSM driven XY platform control method based on empirical mode decomposition, and system thereof | |
| CN104485864B (en) | Second-order sliding mode control system of direct drive servo system and control method of second-order sliding mode control system | |
| CN110928241B (en) | Control method of motion control system of numerical control machine tool | |
| CN204790506U (en) | Triaxial motion platform improved generation cross coupling controlling means | |
| CN203896241U (en) | Dual linear motor contour compensation device based on fuzzy RBF network integral sliding-mode | |
| CN109143869B (en) | Synchronous control system of recursive wavelet neural network compensator of H-shaped platform | |
| CN110829918B (en) | Constraint back-stepping control algorithm of servo system with variable constraint coefficients | |
| CN112737442B (en) | Construction method of permanent magnet motor composite controller for electric automobile EPS |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |