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
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)
- Control Of Position Or Direction (AREA)
- External Artificial Organs (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
Technical field：The present invention provides a kind of triaxial movement platform modified cross-coupling control device and method, belongs to
Fields of numeric control technique.
Background technology：In modern digital-control processing system, the control of two axle XY platform contours can not meet people to complicated member
The processing request of part, therefore triaxial movement platform profile control technology is introduced, to realize the precision to space three-dimensional parts profile
Processing.Triaxial movement platform is directly driven by permanent magnetic linear synchronous motor, it is to avoid the intermediate transmission ring of " ball+leading screw "
Section, improves the processing efficiency of system.
The content of the invention：
Goal of the invention：The present invention provides a kind of triaxial movement platform modified cross-coupling control device and method, its mesh
Be to solve conventional mode to do the problem of existing.
Technical scheme：The present invention is achieved by the following technical solutions：
A kind of triaxial movement platform modified cross-coupling control device, it is characterised in that：The device includes main circuit, control
Circuit processed and the part of control object three；Main circuit includes AC voltage adjusting module, rectification filtering module and IPM inversion modules；Control
Circuit includes DSP Processor, current sampling circuit, rotor position sample circuit, voltage-regulating circuit, IPM isolated drive circuits
With IPM protection circuits；Control object is three-phase permanent linear synchronous generator, and fuselage is equipped with grating scale；Current sampling circuit, mover
Position sample circuit, voltage-regulating circuit, IPM isolated drive circuits and IPM protection circuits are connected with DSP Processor, IPM every
It is connected from drive circuit and IPM protection circuits with IPM inversion modules, current sampling circuit is connected to three-phase by Hall sensor
Permanent magnet linear synchronous motor, voltage-regulating circuit connection AC voltage adjusting module, AC voltage adjusting module connection rectification filtering module is whole
Filtration module connection IPM inversion modules are flowed, IPM inversion modules connection three-phase permanent linear synchronous generator, three-phase permanent straight line is same
Grating scale on step motor is connected with rotor position sample circuit.
The triaxial movement platform modified implemented using above-mentioned triaxial movement platform modified cross-coupling control device
Cross-coupling control method, it is characterised in that：This method uses a kind of profile errors estimation algorithm, to set up triaxial movement platform
Profile errors model, and uniaxiality tracking control is combined with three axle cross-coupling controls, improve conventional cross-couplings control
Structure processed, so as to ensure that system uniaxiality tracking precision and contour accuracy level off to zero.
Uniaxiality tracking is controlled, and uniaxiality tracking control uses position-speed ring double circle controling mode, uniaxiality tracking control
System design.
Speed ring uses the Pseudo-derivative- feedback controller with feedforward, i.e. PDFF controllers, and its control algolithm is expressed as：
Wherein kfFor feedforward compensation gain, kiFor storage gain, kpFor proportional gain；Speed ring control input vd(s) it is and real
Border output speed function va(s) relation between is：
Disturbance input ξ (s) and reality output velocity function va(s) relation between is：
Controlled device uses permanent magnetic linear synchronous motor, and its transmission function is
Wherein, G0(s)=1/ (Ms+B) is actual controlled device, KfFor electromagnetic push coefficient.
Position ring adoption rate controller, coefficient is kx, therefore the transmission function of whole uniaxiality tracking control system can table
It is shown as：
By setting fixed disturbance ξ, it is able to verify that system has stronger antijamming capability and very fast responding ability.
The step of this method, is as follows：
The present invention includes step in detail below：
Step 1：Set up triaxial movement platform profile errors model：
It is the synchronous electricity of permanent-magnet linear by permanent magnetic linear synchronous motor perpendicular to each other (PMLSM) that triaxial movement platform, which is used,
Mechanical equation formula is：
In formula, Fe：Electromagnetic push；M：The load-carrying gross mass of mover and mover institute of permanent-magnetism linear motor；iqFor mover q
Shaft current；Kf：Electromagnetic push coefficient；B：Viscous friction coefficient；F：Total perturbed force suffered by system.V is mover speed；It is
Sub- acceleration；
Choose x (t) and v (t) rewritable is for system state variables, i.e. PMLSM state equation
Wherein, v (t) is electric mover speed；U=iqRepresent the control input amount of motor；X (t) is then linear electric motors
Position is exported.
Therefore, direct drive triaxial movement platform can be made up of three 2 rank differential equations：
The form for being expressed as state space is：
Wherein, z1(t)=[x1(t) x2(t) x3(t)]T,U=[u1 u2 u3
]T, ρ=[F1 F2 F3]T, A11=0, A12=I, A21=0, A22=diag (- Bi/Mi), i=x, y, z are 3 × 3 matrixes；
Step 2：Triaxial movement platform profile errors model is set up：
In triaxial movement platform, the precision of profile errors model estimation directly affects profile control performance.Assuming that three axles
In motion platformFor command position, P is physical location, and position error vector isProfile errors vector isR0、R1For life
2 points on position are made, R is designated as respectively0(x0,y0,z0), R1(x1,y1,z1)；Q points are command position vectorOn a bit, sit
Labeled as Q (x, y, z).Point P to point R1Distance be position error vectorThe form for being expressed as relationship is：
VectorFor
By R0、R1It is with this 3 points release command position linear equations of Q：
Assuming that physical location P is to command positionBeeline for vectorTherefore it is vectorialFor
VectorWith vectorIt is mutually perpendicular to, inner product is zero；I.e.Obtain parameter t and be updated to equation
(12) coordinate Q can be obtained after, coordinate Q can further obtain profile errors value after obtaining, finally release profile errorsFor
Profile errors are understood by formula (14)In the component of x-axis, y-axis and z-axis；
Step 3：The design of Compensator of profile errors
In order to reduce profile errors, it is desirable to which physical location P can be to command position vectorAmendment, except correction position is missed
Difference vectorIn each axis component Ex, Ey, EzOutside, profile errors vector need to be compensated in additionTherefore, vector is chosenIt is used as reality
Position is to profile errors between command positionCompensation, compensation rate number depend on λ size.Therefore,It is used as whole system
The compensation rate of system, the compensation relationship formula of physical location to desired locations is：
By formula (15), true location point P can have both been compensated to expectation location point R1Tracking error, two can be compensated again
Profile errors between point, make it level off to command position.And then obtain whole compensation rateIn the component of each axle：
Composite vector may be such that by formula (16)Level off to command position path, wherein λ is cross-couplings yield value, shadow
Ring the erection rate of profile errors.By composite vectorGeometrical relationship understand λ value it is bigger,More order path is inclined to, is corrected
Profile errors vectorAmount will be big；
Step 4：Uniaxiality tracking controller design
In order to ensure the contour accuracy of three axles, uniaxiality tracking control is also essential, uniaxiality tracking control in the present invention
The control mode that system is combined using speed ring controller and position ring controller, speed ring controller uses PDFF controlling parties
Case, position ring controller kxAdoption rate control mode；
Step 5：Profile control is designed
By the profile errors estimation technique noted earlier, it is known that profile errorsOnly with command positionHave with physical location P
Close, belong to the geometrical relationship of position, therefore designed cross-coupling controller is located at the position loop part of control system, changes
Conventional cross-coupling control structure is entered.
The input of cross-coupling controller is the given position R of triaxial movement platformx、RyAnd RzWith the tracking error of every axle
Ex、EyAnd Ez。ex、eyAnd ezIt is the profile errors component of each axle of cross-coupling controller output.
The final control program by embedded DSP Processor of the inventive method realizes that its control process is held according to the following steps
OK：
Step 1 system initialization；
Step 2 allows TN1, TN2 to interrupt；
Step 3 starts T1 underflows and interrupted；
Step 4 routine data is initialized；
Step 5 opens total interruption；
Step 6 interrupt latency；
The sub- control program of step 7 TN1 interrupt processings；
Step 8 terminates.
The sub- control program of T1 interrupt processings is according to the following steps wherein in step 7：
Step 1 T1 interrupts sub- control program；
Step 2 keeps the scene intact；
Step 3 judges whether initial alignment；It is to enter step 4, otherwise into step 10；
Step 4 current sample, CLARK conversion, PARK conversion；
Step 5 judges whether to need position adjustments；Otherwise step 7 is entered；
The sub- control program of step 6 position adjustments interrupt processing；
Step 7 d q shaft currents are adjusted；
Step 8 PARK inverse transformations；
Step 9 calculates CMPPx and PWM outputs；
Sample step 10 position；
Step 11 initial alignment program；
Step 12 restoring scene；
Step 13, which is interrupted, to be returned.
The sub- control program of position adjustments interrupt processing is according to the following steps wherein in step 6：
Step 1 position adjustments interrupt sub- control program；
Step 2 reads encoder values；
Step 3 judges angle；
Step 4 calculates distance；
Step 5 execution position controller；
The order of step 6 calculating current is simultaneously exported；
Step 7, which is interrupted, to be returned.
Advantage and effect：The present invention provides a kind of triaxial movement platform modified cross-coupling control device and method, with
The increase that people are required complex components, the control of multiaxial motion platform Precise outline is compared to two in the past representative axles
For the control of XY platform contours, the Precise outline motion control research of multiaxial motion platform high-performance contour machining is with important
Realistic meaning and wide application prospect.And multiaxial motion platform uses multiple permanent magnetic linear synchronous motor direct drive sides
Formula, it is to avoid the intermediate transmission link of " ball+leading screw " so that load only by the direct Thrust of linear electric motors, eliminates biography
The problem that system transmission mechanism.The zero clearance transmission from linear electric motors to controlled device is realized, turns into linear electric motors
At a high speed, the main type of drive of High-precision servo control system.
For in existing control technology, the problem of existing for the profile control accuracies of complex components, the present invention is in three axles
Coordinate to use a kind of profile errors estimation algorithm in control, set up three axle profile errors models, improve the knot of cross-coupling control
Structure, designs three-dimensional figure error controller.
Controller designed by the present invention is applied to the digital control platform perpendicular to each other that a linear electric motors drive X-Y-Z axles
In.Experimental system is as shown in Figure 2.The position of the platform is connected to the linear encoder of each drive shaft, linear encoder
Sensor resolution is 0.1 micron.The speed of each drive shaft by position measurement it is reverse it is poor calculate, this sampling period
For 2 milliseconds.
The present invention includes triaxial movement platform profile errors model and set up so that system can complete the profile traces in space
Tracing task；Uniaxiality tracking controller design, it is ensured that per axle tracking error in less scope；The design of profile control,
The profile errors of reduction system；Profile errors model geometric relation, as shown in Figure 3；Profile errors compensation rate geometrical relationship, such as schemes
Shown in 4；Uniaxiality tracking controller design, as shown in Figure 1；Three axle profile controls are designed, as shown in Figure 5.
It is of the invention main using triaxial movement platform as research object, by controlling the overall profile errors of three axles to ensure zero
Part machining accuracy.To improve contour machining precision, many scholars are directed to studying various feedforwards, feedback control strategy to improve list
Axle tracking accuracy, so as to improve contour motion control accuracy indirectly.Such as feedforward controller, zero phase error tracking control device,
PID control, Self Adaptive Control, the method such as robust control can reduce uniaxiality tracking error.But the reduction of uniaxiality tracking error
Overall contour accuracy can not be ensured.So it is influence triaxial movement platform system that uniaxiality tracking control coordinates control between centers
Two key factors of system contour accuracy.The control method that uniaxiality tracking control is combined using position ring and speed ring, position
Ring controls for ratio, and speed ring controls for PDFF, ensure that the faster response speed of single shaft and tracking accuracy.In order to improve axle
Between harmony, the control of between centers profile is general to be coordinated using cross-coupling controller (CCC) caused by parameter mismatch
Dynamic property difference, reduce system profile errors.For this problem, the present invention uses a kind of profile errors estimation algorithm,
Set up the profile errors model of three between centers.And traditional cross-coupling control structure is improved on this basis, devise three axles
Cross-coupling controller, by verifying that this method can effectively improve the contour accuracy of three between centers.
Brief description of the drawings：
Fig. 1 uniaxiality tracking control system block diagrams
The experimental system that Fig. 2 designs for the present invention
Fig. 3 is outline of straight line error vector geometrical relationship figure
Fig. 4 is that profile errors compensate geometrical relationship figure
Fig. 5 triaxial movement platform cross-coupling control block diagrams
Fig. 6 is the vector control system for permanent magnet linear synchronous motor hardware configuration hardware block diagram designed by the realization present invention
Fig. 7 is vector control system program flow diagram in the inventive method
Fig. 8 is the sub- control program flow chart of the inventive method position adjustments interrupt processing
Fig. 9 is realization control system schematic diagram of the invention
(a) electric machine control system main circuit schematic diagram
(b) A, B phase current sampling circuit schematic diagram
(c) grating scale signal sample circuit schematic diagram
(d) IPM hardware drivings circuit theory diagrams.
Embodiment：The present invention is described further below in conjunction with the accompanying drawings：
As shown in figure 1, the present invention provides a kind of triaxial movement platform modified cross-coupling control device and method, (one)
System hardware structure
Realize that shown in control system main circuit such as Fig. 9 (a) of the present invention, regulating circuit uses reverse voltage regulating module EUV-25A-
II, can be achieved 0~220V isolation pressure regulation.Rectification filtering unit uses the uncontrollable rectification of bridge-type, and bulky capacitor filtering coordinates suitably
Resistance capaciting absorpting circuit, can obtain the constant DC voltage needed for IPM work.IPM is using company of Fuji 6MBP50RA060 intelligence
Power model, pressure-resistant 600V, maximum current 50A, maximum operating frequency 20kHz.IPM is supplied with the 15V driving power supplies of four groups of independence
Electricity.Main power source input terminal (P, N), lead-out terminal (U, V, W), main terminal is fixed with the screw carried, and electric current transmission can be achieved.
P, N are the main power source input terminal after the rectifying conversion smothing filtering of frequency converter, and P is anode, and N is negative terminal, inverter output
Three-phase alternating current is connected to motor by lead-out terminal U, V, W.
The core for controlling circuit of the present invention is TMS320F2812 processors, and it is read-only that its supporting development board includes target
Memory, analog interface, eCAN interfaces, serial boot ROM, user lamp, reset circuit, it can be configured to RS232/RS422/
The RS485 outer 256K*16 RAM of asynchronous serial port, SPI synchronous serial interfaces and piece.
Current sample uses LEM companies Hall current sensor LT58-S7 in actual control system.By two Hall currents
Sensor detects A, B phase current, obtains current signal, by current sampling circuit, is converted into 0~3.3V voltage signal, most
The binary number of 12 precision is converted into by TMS320LF2812 A/D modular converters afterwards, and is stored in numerical register.A、
Shown in the current sampling circuit of B phases such as Fig. 9 (b).Adjustable resistance VR2 Regulate signal amplitudes, the skew of adjustable resistance VR1 Regulate signals
Signal, by the regulation to the two resistance, can be adjusted to 0~3.3V, then be sent to DSP AD0, AD1 pin by amount.
Voltage-stabiliser tube in figure be in order to prevent send into DSP signal more than 3.3V, cause DSP to be damaged by high pressure.Operational amplifier is used
OP27, power supply connects positive and negative 15V voltages, in the indirect decoupling capacitor of voltage and ground.Circuit input end connects capacitor filtering, to remove high frequency
Signal is disturbed, and improves sampling precision.
The A phases and B phase pulse signals of grating scale output will be isolated by rapid light coupling 6N137 to signal, Ran Houjing
Cross bleeder circuit and signal level be converted into 3.3V by 5V, be eventually connected to DSP two-way quadrature coding pulse interface QEP1 and
QEP2.Shown in circuit theory such as Fig. 9 (c).Fig. 9 (d) gives the schematic diagram of six road isolated drive circuits.It is to be noted that
IPM error protection signal pins pair are non-duplicate transient faults, are achieved by the following measures in the present system：IPM failure
Output signal is by being optically coupled to DSP'sPin, DSP is in time by all events during ensuring that IPM breaks down
Manager output pin puts high-impedance state.
(2) system software is realized
Vector control system program flow diagram is as shown in Figure 7 in the inventive method.Fig. 8 is in the inventive method position adjustments
The disconnected sub- control program flow chart of processing.The main program of software includes system initialization；Open INT1, INT2 interruption；Allow timer
Interrupt；Timer interruption handles subprogram.Wherein initialization program is included at the beginning of closing all interruptions, dsp system initialization, variable
Beginningization, task manager initialization, AD initialization and quadrature coding pulse QEP initialization.Interrupt service subroutine includes protection
Interruption subroutine and T1 underflow interrupt service subroutines.Other parts such as mover initialization positioning, PID regulations, vector etc.
Performed all in timer TI underflow Interrupt Subroutines.
The protection interrupt response that IPM protection signals are produced belongs to external interrupt, and INT1 interrupt priority levels are higher than timer T1.
IPM can send protection signal automatically in abnormal conditions such as excessively stream, overvoltages, the converted power drive for being connected to DSP of this signal
Protection pinOnce have abnormal conditions, DSP can enter protection interruption subroutine, forbid first it is all in
It is disconnected, then block PWM outputs and motor is stalled at once, play a part of protection motor and IPM.
The smooth startup of vector control system, can be to the dynamic of motor using software, it is necessary to know the initial position of mover
Son leads to the direct current of a constant amplitude, stator is produced a constant magnetic field, this magnetic field and the stationary magnetic field phase of rotor
Interaction, makes electric mover move to the position that two magnetic linkages are overlapped.And mover initial alignment, the reading of AD sampled values, motor
The calculating of rotor position, coordinate transform, PID regulations, the generation of SVPWM waveform comparison values are all in T1 underflow interrupt service subroutines
It is middle to complete.
Describe in detail as follows：
As shown in fig. 6, the device includes main circuit, control circuit and the part of control object three；Main circuit includes exchange and adjusted
Die block, rectification filtering module and IPM inversion modules；Circuit is controlled to include DSP Processor, current sampling circuit, rotor position
Sample circuit, voltage-regulating circuit, IPM isolated drive circuits and IPM protection circuits；Control object is three-phase permanent Linear Synchronous
Motor, fuselage is equipped with grating scale；Current sampling circuit, rotor position sample circuit, voltage-regulating circuit, IPM isolation drives electricity
Road and IPM protection circuits are connected with DSP Processor, and IPM isolated drive circuits and IPM protection circuits connect with IPM inversion modules
Connect, current sampling circuit is connected to three-phase permanent linear synchronous generator, voltage-regulating circuit connection exchange by Hall sensor
Voltage regulating module, AC voltage adjusting module connection rectification filtering module, rectification filtering module connection IPM inversion modules, IPM inversion modules
The grating scale connected on three-phase permanent linear synchronous generator, three-phase permanent linear synchronous generator connects with rotor position sample circuit
Connect.
Triaxial movement platform modified cross-coupling control method, this method uses a kind of profile errors estimation algorithm, to build
The profile errors model of vertical triaxial movement platform, and uniaxiality tracking control is combined with three axle cross-coupling controls, improve
Conventional cross-coupling control structure, so as to ensure that system uniaxiality tracking precision and contour accuracy level off to zero.
Uniaxiality tracking is controlled, the control mode being combined using position ring and speed ring, uniaxiality tracking Control System Design
Such as Fig. 1,1/ (Ms+B) is actual controlled device, KfFor electromagnetic push coefficient, xrFor the reference instruction of input, xpFor reality output
Position.The control mode that uniaxiality tracking control is combined using position-speed ring, position ring adoption rate control, speed ring is adopted
Use PDFF controllers, kxFor position ring proportional gain；K in speed ringfFor feedforward compensation gain, kiFor storage gain, kpFor ratio
Gain；ξ is external disturbance, by setting fixed disturbance, is able to verify that system is rung with stronger antijamming capability and comparatively fast
Should be able to power.
In traditional contour machining, normally only it is directed to X/Y plane and carries out contour accuracy control, be difficult to extend to three-dimensional space
Between, there is significant limitation for actual digital control processing in this.Therefore, using a kind of profile errors estimation algorithm, establish
Triaxial movement platform space profiles error model.And according to claim, using improved cross-coupling control method
To improve Contour extraction performance, contour accuracy is improved.
The step of this method, is as follows：
The present invention includes step in detail below：
Step 1：Set up triaxial movement platform profile errors model：
It is the synchronous electricity of permanent-magnet linear by permanent magnetic linear synchronous motor perpendicular to each other (PMLSM) that triaxial movement platform, which is used,
Mechanical equation formula is：
In formula, Fe：Electromagnetic push；M：The load-carrying gross mass of mover and mover institute of permanent-magnetism linear motor；iqFor mover q
Shaft current；Kf：Electromagnetic push coefficient；B：Viscous friction coefficient；F：Total perturbed force suffered by system.V is mover speed；It is
Sub- acceleration；
Choose x (t) and v (t) rewritable is for system state variables, i.e. PMLSM state equation
Wherein, v (t) is electric mover speed；U=iqRepresent the control input amount of motor；X (t) is then linear electric motors
Position is exported.
Therefore, direct drive triaxial movement platform can be made up of three 2 rank differential equations：
The form for being expressed as state space is：
Wherein, z1(t)=[x1(t) x2(t) x3(t)]T,U=[u1 u2 u3
]T, ρ=[F1 F2 F3]T, A11=0, A12=I, A21=0, A22=diag (- Bi/Mi), i=x, y, z are 3 × 3 matrixes；
Step 2：Triaxial movement platform profile errors model is set up：
In triaxial movement platform, the precision of profile errors model estimation directly affects profile control performance.Fig. 3 is straight line
Profile errors vector geometry graph of a relation.Wherein,For command position, P is physical location, and position error vector isProfile is missed
Difference vector isR0、R1For 2 points on command position, R is designated as respectively0(x0,y0,z0), R1(x1,y1,z1)；Physical location P is arrived
Command position R beeline is vectorThe profile errors of as physical location to reference position are vectorialQ point coordinates is remembered
For Q (x, y, z).Point P to point R1Distance be position error vector
By R0、R1It is with this 3 points release command position linear equations of Q：
As shown in Figure 4, it is vectorialWith vectorIt is mutually perpendicular to, inner product is zero；I.e.Obtain parameter t substitutions
Coordinate Q can be obtained after to equation (6), coordinate Q can further obtain profile errors value after obtaining, finally release profile errorsFor
Profile errors are understood by formula (6)In the component of x-axis, y-axis and z-axis；
Step 3：The design of Compensator of profile errors
According to Fig. 4, in order to reduce profile errors, it is desirable to which physical location P can be to command position vectorAmendment, except repairing
Positive position error vectorIn each axis component Ex, Ey, EzOutside, profile errors vector need to be compensated in additionBy vector geometry addition and subtraction
Understand, choose vectorAs physical location to the compensation between command position, it is set to level off to command position.Whole compensation rateIt can be expressed as in the component of each axle：
Composite vector may be such that by formula (7)Level off to command position path, wherein λ is cross-couplings yield value, shadow
Ring the erection rate of profile errors.By composite vectorGeometrical relationship understand λ value it is bigger,More order path is inclined to, is corrected
Profile errors vectorAmount will be big；
Step 4：Uniaxiality tracking controller design
In order to ensure the contour accuracy of three axles, uniaxiality tracking control is also essential, uniaxiality tracking control in the present invention
The control mode that system is combined using speed ring controller and position ring controller, speed ring controller uses PDFF controlling parties
Case, position ring controller kxAdoption rate control mode；
Step 5：Profile control is designed
By the profile errors estimation technique noted earlier, it is known that profile errors e only has with command position R and physical location P
Close, belong to the geometrical relationship of position, therefore designed cross-coupling controller is located at the position loop part of control system, changes
Conventional cross-coupling control structure, structured flowchart such as Fig. 5 are entered.
The input of cross-coupling controller is the given position R of triaxial movement platformx、RyAnd RzWith the tracking error of every axle
Ex、EyAnd Ez。ex、eyAnd ezIt is the profile errors component of each axle of cross-coupling controller output.And by institute in the present invention
Three axle cross-coupling controller structured flowcharts of design are compared with the structured flowchart used in the past, and profile errors of the invention are mended
Repay and just completed before position loop controller.From profile errors compensation rate geometrical relationship, when the control of adjustment position loop
Yield value K in device processedpWhen, profile errors compensation rate can be had influence on simultaneouslyIts effect is equal to adjustmentSize, rather than
Direction, but direction now is determined by cross-couplings yield value λ size.Therefore KpAdjustment with λ be each it is independent,
Respectively size and Orientation.And conventional cross-coupling controller structure is then that compensation rate is placed in after controller, work as adjustment
KpWhen, its effect is equal to only to be adjusted in figure 3Size.Therefore, the method proposed in the present invention mends profile errors
The amount of repayingDirection and size simultaneously change, add position loop gain K in the structure chartpBetween cross-couplings gain λ
Do the matching problem most suitably adjusted.
The input of cross-coupling controller is the given position R of triaxial movement platformx、RyAnd RzWith the tracking error of every axle
Ex、EyAnd Ez。ex、eyAnd ezIt is the profile errors component of each axle of cross-coupling controller output.
The final control program by embedded DSP Processor of the inventive method realizes that its control process is held according to the following steps
OK：
Step 1 system initialization；
Step 2 allows TN1, TN2 to interrupt；
Step 3 starts T1 underflows and interrupted；
Step 4 routine data is initialized；
Step 5 opens total interruption；
Step 6 interrupt latency；
The sub- control program of step 7 TN1 interrupt processings；
Step 8 terminates.
The sub- control program of T1 interrupt processings is according to the following steps wherein in step 7：
Step 1 T1 interrupts sub- control program；
Step 2 keeps the scene intact；
Step 3 judges whether initial alignment；It is to enter step 4, otherwise into step 10；
Step 4 current sample, CLARK conversion, PARK conversion；
Step 5 judges whether to need position adjustments；Otherwise step 7 is entered；
The sub- control program of step 6 position adjustments interrupt processing；
Step 7 d q shaft currents are adjusted；
Step 8 PARK inverse transformations；
Step 9 calculates CMPPx and PWM outputs；
Sample step 10 position；
Step 11 initial alignment program；
Step 12 restoring scene；
Step 13, which is interrupted, to be returned.
The sub- control program of position adjustments interrupt processing is according to the following steps wherein in step 6：
Step 1 position adjustments interrupt sub- control program；
Step 2 reads encoder values；
Step 3 judges angle；
Step 4 calculates distance；
Step 5 execution position controller；
The order of step 6 calculating current is simultaneously exported；
Step 7, which is interrupted, to be returned.
The present invention is for direct drive triaxial movement platform, and advantages of the present invention, which is essentially consisted in, establishes three-dimensional figure
Error model and the method being controlled to space profiles error.Solve in modern system of processing, people are to complicated member
The demand of part is continuously increased, and but it is impossible to meet the problem of complex components machining accuracy.Present invention is generally directed to reduce single shaft
Tracking error and profile errors.Uniaxiality tracking error make use of the control that position ring controller is combined with speed ring controller
Mode, it is ensured that uniaxiality tracking error is in good accuracy rating.The control present invention of three between centers profile errors is mainly proposed
A kind of new profile errors estimate model to estimate profile errors, and apply it in three axle cross-couplings profile controls,
Improve the control structure of three axle cross-coupling controllers.By above-mentioned two-part combination, finally cause triaxial movement platform
The profile errors of system level off to zero.
Claims (6)
1. a kind of triaxial movement platform modified cross-coupling control method, it is characterised in that：This method is real using following device
Apply：The device includes main circuit, control circuit and the part of control object three；Main circuit includes AC voltage adjusting module, rectifying and wave-filtering
Module and IPM inversion modules；Control circuit to include DSP Processor, current sampling circuit, rotor position sample circuit, voltage to adjust
Whole circuit, IPM isolated drive circuits and IPM protection circuits；Control object is three-phase permanent linear synchronous generator, and fuselage is equipped with light
Grid chi；Current sampling circuit, rotor position sample circuit, voltage-regulating circuit, IPM isolated drive circuits and IPM protection circuits
It is connected with DSP Processor, IPM isolated drive circuits and IPM protection circuits are connected with IPM inversion modules, current sampling circuit
Three-phase permanent linear synchronous generator is connected to by Hall sensor, voltage-regulating circuit connection AC voltage adjusting module, exchange is adjusted
Die block connects rectification filtering module, and rectification filtering module connection IPM inversion modules, IPM inversion modules connection three-phase permanent is straight
Grating scale on line locking motor, three-phase permanent linear synchronous generator is connected with rotor position sample circuit；
This method uses a kind of profile errors estimation algorithm, to set up the profile errors model of triaxial movement platform, and by single shaft with
Track control is combined with three axle cross-coupling controls, conventional cross-coupling control structure is improved, so as to ensure that system list
Axle tracking accuracy and contour accuracy level off to zero；
Uniaxiality tracking is controlled, and uniaxiality tracking control uses position-speed ring double circle controling mode, uniaxiality tracking control system
Design；
Speed ring uses the Pseudo-derivative- feedback controller with feedforward, i.e. PDFF controllers, and its control algolithm 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 kfFor feedforward compensation gain, kiFor storage gain, kpFor proportional gain；Speed ring control input vd(s) it is defeated with reality
Go out velocity function va(s) relation between 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 reality output velocity function va(s) relation between 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>
Controlled device uses permanent magnetic linear synchronous motor, and its transmission function is
<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, G0(s)=1/ (Ms+B) is actual controlled device, KfFor electromagnetic push coefficient；
Position ring adoption rate controller, coefficient is kx, therefore entirely the transmission function of uniaxiality tracking control system is represented by：
<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 fixed disturbance ξ, it is able to verify that system has stronger antijamming capability and very fast responding ability.
2. triaxial movement platform modified cross-coupling control method according to claim 1, it is characterised in that：This method
The step of it is as follows：
The present invention includes step in detail below：
Step 1：Set up triaxial movement platform profile errors model：
It is by permanent magnetic linear synchronous motor PMLSM perpendicular to each other, permanent magnet linear synchronous motor tool side that triaxial movement platform, which is used,
Formula is：
<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 formula, Fe：Electromagnetic push；M：The load-carrying gross mass of mover and mover institute of permanent-magnetism linear motor；iqFor mover q axles electricity
Stream；Kf：Electromagnetic push coefficient；B：Viscous friction coefficient；F：Total perturbed force suffered by system；V is mover speed；For mover plus
Speed；
Choose x (t) and v (t) rewritable is for system state variables, i.e. PMLSM state equation：
<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 electric mover speed；U=iqRepresent the control input amount of motor；X (t) is then defeated for the position of linear electric motors
Go out；
Therefore, direct drive triaxial movement platform can be made up of three 2 rank 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>
The form for being expressed as state space is：
<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, z1(t)=[x1(t) x2(t) x3(t)]T,U=[u1 u2 u3]T, ρ=
[F1 F2 F3]T, A11=0, A12=I, A21=0, A22=diag (- Bi/Mi), i=x, y, z are 3 × 3 matrixes；
Step 2：Triaxial movement platform profile errors model is set up：
In triaxial movement platform, the precision of profile errors model estimation directly affects profile control performance；Assuming that three-axis moving
In platformFor command position, P is physical location, and position error vector isProfile errors vector isR0、R1For command bit
2 points put, are designated as R respectively0(x0,y0,z0), R1(x1,y1,z1)；Q points are command position vectorOn a bit, coordinate note
For Q (x, y, z)；Point P to point R1Distance be position error vectorThe form for being expressed as relationship is：
<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>
VectorFor
<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>
By R0、R1It is with this 3 points release command position linear equations of Q：
<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 that physical location P is to command positionBeeline for vectorTherefore it is vectorialFor
<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>
VectorWith vectorIt is mutually perpendicular to, inner product is zero；I.e.Obtaining that parameter t is updated to can after equation (12)
To obtain coordinate Q, coordinate Q can further obtain profile errors value after obtaining, finally release profile errorsFor
Profile errors are understood by formula (14)In the component of x-axis, y-axis and z-axis；
Step 3：The design of Compensator of profile errors
In order to reduce profile errors, it is desirable to which physical location P can be to command position vectorAmendment, except correction position error to
AmountIn each axis component Ex, Ey, EzOutside, profile errors vector need to be compensated in additionTherefore, vector is chosenArrived as physical location
Profile errors between command positionCompensation, compensation rate number depend on λ size；Therefore,It is used as the benefit of whole system
The amount of repaying, the compensation relationship formula of physical location to desired locations is：
<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 formula (15), true location point P can have both been compensated to expectation location point R1Tracking error, point-to-point transmission can be compensated again
Profile errors, it is leveled off to command position；And then obtain whole compensation rateIn the component of each axle：
<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>
Composite vector may be such that by formula (16)Level off to command position path, wherein λ is cross-couplings yield value, influence wheel
The erection rate of wide error；By composite vectorGeometrical relationship understand λ value it is bigger,More order path is inclined to, modified profile is missed
Difference vectorAmount will be big；
Step 4：Uniaxiality tracking controller design
In order to ensure the contour accuracy of three axles, uniaxiality tracking control is also essential, and uniaxiality tracking control is adopted in the present invention
The control mode being combined with speed ring controller and position ring controller, speed ring controller uses PDFF control programs, position
Put ring controller kxAdoption rate control mode；
Step 5：Profile control is designed
By the profile errors estimation technique noted earlier, it is known that profile errorsOnly with command positionIt is relevant with physical location P, category
Geometrical relationship in position, therefore designed cross-coupling controller is located at the position loop part of control system, improves
Conventional cross-coupling control structure.
3. triaxial movement platform modified cross-coupling control method according to claim 2, it is characterised in that：Intersect coupling
The input of hop controller is the given position R of triaxial movement platformx、RyAnd RzWith the tracking error E of every axlex、EyAnd Ez；ex、eyWith
ezIt is the profile errors component of each axle of cross-coupling controller output.
4. triaxial movement platform modified cross-coupling control method according to claim 2, it is characterised in that：The present invention
The final control program by embedded DSP Processor of method realizes that its control process is performed according to the following steps：
Step 1 system initialization；
Step 2 allows TN1, TN2 to interrupt；
Step 3 starts T1 underflows and interrupted；
Step 4 routine data is initialized；
Step 5 opens total interruption；
Step 6 interrupt latency；
The sub- control program of step 7 TN1 interrupt processings；
Step 8 terminates.
5. triaxial movement platform modified cross-coupling control method according to claim 4, it is characterised in that：Wherein walk
The sub- control program of T1 interrupt processings is according to the following steps in rapid 7：
Step 1 T1 interrupts sub- control program；
Step 2 keeps the scene intact；
Step 3 judges whether initial alignment；It is to enter step 4, otherwise into step 10；
Step 4 current sample, CLARK conversion, PARK conversion；
Step 5 judges whether to need position adjustments；Otherwise step 7 is entered；
The sub- control program of step 6 position adjustments interrupt processing；
Step 7 d q shaft currents are adjusted；
Step 8 PARK inverse transformations；
Step 9 calculates CMPPx and PWM outputs；
Sample step 10 position；
Step 11 initial alignment program；
Step 12 restoring scene；
Step 13, which is interrupted, to be returned.
6. triaxial movement platform modified cross-coupling control method according to claim 5, it is characterised in that：Wherein walk
The sub- control program of position adjustments interrupt processing is according to the following steps in rapid 6：
Step 1 position adjustments interrupt sub- control program；
Step 2 reads encoder values；
Step 3 judges angle；
Step 4 calculates distance；
Step 5 execution position controller；
The order of step 6 calculating current is simultaneously exported；
Step 7, which is interrupted, to be returned.
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 | |
| CN107132759A (en) | A kind of linear electric motors driving FTS is based on ESO sliding formworks and improves repetitive control | |
| Lin et al. | Robust fuzzy neural network sliding-mode control for two-axis motion control system | |
| Su et al. | Contouring accuracy improvement using cross-coupled control and position error compensator | |
| CN103401501B (en) | A kind of PMSM servo system control method based on fuzzy active disturbance rejection | |
| CN101834554B (en) | Method for improving processing precision by using load disturbance compensator and optimizing setting of load disturbance compensator | |
| CN104460518A (en) | Direct-drive XY platform profile control device and method based on fuzzy disturbance compensation | |
| CN102637011B (en) | Robust control method and system for directly driving numerical control platform based on coordinate transformation and parameter adjustment | |
| CN110032142A (en) | Modified profile errors precompensation method is minimized based on Machining Path | |
| CN104917436A (en) | Adaptive second-order terminal sliding-mode control system and method of permanent magnet linear synchronous motor | |
| CN108712131A (en) | A kind of current feed-forward method and device adjusted based on proportion integral modulus | |
| CN103414419A (en) | Double-linear-motor contour compensation device and method based on fuzzy RBF network sliding mode | |
| CN106330038A (en) | Sensorless control method for PMLSM (permanent magnet synchronous linear motor) based on self-adaptive gain sliding mode observer | |
| CN111262491B (en) | Incremental direct prediction speed control method suitable for permanent magnet motor system | |
| CN104485864B (en) | Second-order sliding mode control system of direct drive servo system and control method of second-order sliding mode control system | |
| Zhong et al. | Design of dual-drive vertical lift servo system and synchronous control performance analysis | |
| CN108923711A (en) | A kind of absolute value encoder adjusting zero method of servo-system zero point check and correction | |
| CN110757882B (en) | Full closed-loop control system and method based on sensorless servo press | |
| CN102790580B (en) | The building method of induction-type bearingless motor SVMs inverse decoupling controller | |
| CN104834219A (en) | PMLSM driven XY platform control method based on empirical mode decomposition, and system thereof | |
| Li et al. | Cross-coupling control method of the two-axis linear motor based on second-order terminal sliding mode | |
| Ren et al. | Trajectory tracking control for a 3-DOF planar parallel manipulator using the convex synchronized control method | |
| CN109143869B (en) | Synchronous control system of recursive wavelet neural network compensator of H-shaped platform | |
| CN204790506U (en) | Triaxial motion platform improved generation cross coupling controlling means | |
| CN111130411B (en) | Device and method for improving synchronous control precision of double-shaft direct-drive platform servo system |
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 |