3DOF plane motor servo controller based on two DSP
One, technical field:
The present invention is a kind of servo controller of 3DOF planar motor.
Two, background technology:
Along with the fast development of advanced manufacturing industry, the hi-Fix platform technology has obtained deep research and development, has broad application prospects in fields such as semiconductor industry, little stereolithography, nanometer workbench, high accuracy plotters.High-precision fixed bit platform need be realized minor rotation around reference axis toward contact except that need are realized the big stroke motion of horizontal X, Y-axis, for example around the rotation θ of Z axle
Z, controlling object 3DOF planar motor of the present invention is exactly a kind ofly can carry out X, Y, θ
ZThe quick pinpoint workbench of Three Degree Of Freedom.
The electromagnetic actuator of planar motor is 4 group of 3 phase linear electric motors, the planar motor controller need be realized the Synchronization Control of 4 groups of linear electric motors actuators and 3 freedoms of motion, need to gather 12 road current signals, 3 road position signallings, export 24 road pwm signals, also need realize trajectory planning, digital filtering, position servo, the Current Control scheduling algorithm, single dsp controller structure not only can't satisfy the AD sampling and PWM exports required number of active lanes, and can't in the control cycle of regulation, finish required control algolithm, so the planar motor controller architecture of two DSP and two DSP Synchronization Control strategies of 3DOF motion have been proposed; Information exchange between two DSP realize by the CAN network and communication modes such as traditional RS232 and RS485 relatively, the CAN communication speed is fast, antijamming capability is strong, is convenient to realize network joint, provides condition for planar motor enters complication system.
The three-freedom planar Electric Machine Control need realize the big travel displacement of X-axis, the big travel displacement of Y-axis, around Z axle minor rotation θ
ZNon-cpntact measurement, external locating platform often adopts the laser interference ranging system, resolution reaches Nano grade, about 60,000 U.S. dollars of price, concerning general locating platform was used, price was too expensive, and the planar motor controller uses 3 groups of laser position sensors and respective sensor equation to realize the non-cpntact measurement of 3 freedom of motion positions, resolution reaches 1 μ m, about 0.3 ten thousand dollars of price; The planar motor controller needs 12 road winding currents are carried out precision control, current PI (proportional integral) control circuit that external locating platform control system often adopts the power discharge circuit to realize, shortcoming is that analog circuit parameters is difficult to determine, and be subject to environmental interference, current sample is finished by Hall current sensor in the planar motor controller, power amplification realizes that by PWM copped wave the current PI control algolithm is realized by DSP.
Three, summary of the invention:
The invention provides a kind of 3DOF plane motor servo controller based on two DSP, the control plane motor carries out X-axis, Y-axis, θ
ZAccurately locating fast of three freedoms of motion.For reaching above-mentioned purpose, the technical solution used in the present invention is:
A kind of 3DOF plane motor servo controller based on two DSP, it is characterized in that comprising main control unit and from the control unit.Described main control unit comprises main DSP, host buffer driver module, ten two road key light coupling isolation drive modules, six mutually main full bridge power driver modules, main Hall current sensor module and be used to the sample first via, No. 1 X-axis laser position sensors of the second road planar motor X-axis position, No. 2 X-axis laser position sensors of six phase plane motor drive currents of the main control unit output that is used to sample, and main DSP comprises on ADC module on the main DSP sheet, the main DSP sheet CAN module on the PWM module and main DSP sheet; The pwm signal output of PWM module is connected with the input of host buffer driver module on the main DSP sheet, the output of host buffer driver module is connected with the input of key light coupling isolation drive module, the output of key light coupling isolation drive module is connected with the input of main full bridge power driver module, six phase output currents of main full bridge power driver module are six phase plane motor drive currents of main control unit output, six road current sampling signal outputs of main Hall current sensor module are connected with first to the 6th input of ADC module on the main DSP sheet respectively, the 7th input of ADC module is connected on the position sampling signal output part of No. 1 X-axis laser position sensors and the main DSP sheet, and the 8th input of ADC module is connected on the position sampling signal output part of No. 2 X-axis laser position sensors and the main DSP sheet.Described comprise from the control unit from DSP, from the buffering driver module, ten two the road from light-coupled isolation driver module, six from the full bridge power driver module, be used to sample from six phase plane motor drive currents of control unit output from the Hall current sensor module and No. 1 Y-axis laser position sensors of the first via planar motor Y-axis position that is used to sample, from DSP comprise from the DSP sheet the ADC module, from the DSP sheet the PWM module and from the DSP sheet the CAN module; The pwm signal output of PWM module is connected with input from the buffering driver module from the DSP sheet, be connected with input from the output of buffering driver module from the light-coupled isolation driver module, be connected with input from the output of light-coupled isolation driver module from the full bridge power driver module, from six phase output currents of full bridge power driver module is the six phase plane motor drive currents of exporting from the control unit, be connected with first to the 6th input from ADC module on the DSP sheet respectively from six road current sampling signal outputs of Hall current sensor module, the position sampling signal output part of No. 1 Y-axis laser position sensors is connected with the 7th input of ADC module from the DSP sheet.On the main DSP sheet of main control unit the CAN module by the CAN bus be connected from the CAN module from the DSP sheet of controlling the unit.
After described main DSP waits for that regularly control cycle begins, according to trajectory planning algorithm computation planar motor mover platform barycenter X-axis reference by location value X
Pr, planar motor mover platform barycenter Y-axis reference by location value Y
Pr, planar motor mover platform is around the angle position reference value θ of Z axle rotation
Zpr, send synchronous mark F by the CAN bus
SynGive from DSP, and wait for the confirmation of synchronization sign A that sends from DSP
SynWait for and receive synchronous mark F from DSP
SynAfter, send the confirmation of synchronization sign A by the CAN bus
SynGive main DSP, read the current sampling data of No. 2, No. 4 linear electric motors actuators, read the sampled value S of No. 1 Y-axis laser position sensors
Y1, sampled value is carried out digital filtering, correction, then wait for the planar motor mover platform barycenter Y-axis reference by location value Y that main DSP sends
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliMain DSP waits for and receives the confirmation of synchronization sign A
SynAfter, read the current sampling data of No. 1, No. 3 linear electric motors actuator, read the sampled value S of No. 1, No. 2 X-axis laser position sensors
X1, S
X2, sampled value is carried out digital filtering, correction, according to the sampled value S of No. 1, No. 2 X-axis laser position sensors
X1, S
X2With transducer Equation for Calculating planar motor mover platform barycenter X-axis position currency X
p, planar motor mover platform is around the angle position currency θ of Z axle rotation
Zp, planar motor mover platform barycenter Y-axis position currency correction term Y
Pcali, send planar motor mover platform barycenter Y-axis reference by location value Y by the CAN bus again
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliGive from DSP, then wait for the planar motor mover platform barycenter Y-axis position currency Y that sends from DSP
pWait for and receive planar motor mover platform barycenter Y-axis reference by location value Y from DSP
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliAfter, according to the sampled value S of No. 1 Y-axis laser position sensors
Y1, planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliWith transducer Equation for Calculating planar motor mover platform barycenter Y-axis position currency Y
p, send Y by the CAN bus again
pGive main DSP, and carry out Y-axis degree of freedom position, speed control, carry out linear electric motors actuator Current Control No. 2, No. 4, then wait for the synchronous mark F of next control cycle
SynMain DSP waits for and receives planar motor mover platform barycenter Y-axis position currency Y
pAfter, carry out X-axis and θ
ZDegree of freedom position, speed control, and carry out linear electric motors actuator Current Control No. 1, No. 3, then regularly wait for the beginning of next control cycle.
Compared with prior art, the present invention has following advantage:
May command planar motor of the present invention carries out the translational motion of horizontal X, Y-axis and around the minor rotation θ of Z axle z, stroke be 40mm * 40mm * ± 20mrad, error be ± 5 μ m * ± 5 μ m * ± 0.05mrad.
The two dsp controller structures that the present invention is adopted and the benefit analysis of pair DSP Synchronization Control strategies are as follows: the electromagnetic actuator of planar motor is 4 group of 3 phase linear electric motors actuator, the planar motor controller need be realized the Synchronization Control of 4 groups of linear electric motors actuators and 3 freedoms of motion, need to gather 12 road current signals, 3 road position signallings, export 24 road pwm signals, also need realize trajectory planning in addition, digital filtering, position servo, the Current Control scheduling algorithm, single dsp controller structure not only can't satisfy AD sampling and the required number of active lanes of PWM output, and can't in the control cycle of regulation, finish required control algolithm, the present invention proposes two dsp controller structures and two DSP Synchronization Control strategy, wherein, ADC and PWM way have satisfied the demand of planar motor control on the sheet of two DSP, parallel computation and Synchronization Control by two DSP, can realize accurately locating fast of planar motor three degree of freedom, Three Degree Of Freedom positioning experiment curve is referring to Fig. 6, wherein: the adjusting time of X-axis 10mm step response, the mean value of steady-state error was less than ± 5 μ m less than 750ms; The adjusting time of Y-axis 10mm step response, the mean value of steady-state error was less than ± 5 μ m less than 750ms; θ
ZThe adjusting time of degree of freedom 10mrad step response is less than 750ms, and the mean value of steady-state error is less than ± 0.05mrad.
The information exchange of two DSP realizes by the CAN network and traditional communication modes such as RS232, RS485 compare, and the CAN communication speed is fast, and antijamming capability is strong, is convenient to realize network joint, provides condition for planar motor enters complication system.
Planar motor control needs to realize the big travel displacement of X-axis, the big travel displacement of Y-axis, around the non-cpntact measurement of Z axle minor rotation θ z, external high-precision fixed bit platform often adopts the laser interference ranging system, resolution reaches Nano grade, about 60,000 U.S. dollars of price, concerning general application, price is too expensive, the present invention uses 3 groups of laser position sensors and corresponding sensor equation to realize the non-cpntact measurement of 3 degree of freedom positions, 3 degree of freedom resolution reach 1 μ m, 1 μ m, 0.02mrad, about 0.3 ten thousand dollars of price has significantly reduced the cost of position sensor system.
The planar motor controller needs 12 road winding currents of 4 groups of linear electric motors actuators are carried out precision control, the current PI control circuit that external locating platform control system often adopts the power discharge circuit to realize, shortcoming is that analog circuit parameters is difficult to determine, and be subject to environmental interference, current sample is finished by Hall current sensor among the present invention, power amplification is realized by PWM copped wave, the current PI control algolithm is realized by DSP, wherein: use Hall current sensor to carry out current measurement and feedback, compare and seal in feedback resistance and carry out current measurement, the feedback accuracy height, temperature is floated little; Use the PWM mode to carry out Current Control, the precision height, response is fast, and antijamming capability is strong; Use the electrical isolation between photoelectrical coupler realization control circuit and the power driving circuit.
Four, description of drawings
Fig. 1 is a structure diagram of the present invention.
Fig. 2 is a circuit block diagram of the present invention.
Fig. 3 is principal and subordinate DSP Synchronization Control strategic process figure of the present invention.
Fig. 4 is circuit theory diagrams of the present invention, and wherein Fig. 4 a is the main control unit circuit theory diagrams, and Fig. 4 b is from control element circuit schematic diagram.
Fig. 5 is a position transducer array of figure of the present invention.
Fig. 6 is of the present invention three free positioning experiment curves.
Main label in the accompanying drawing has:
1 main control unit 2 is from the control unit
11 main DSP 21 are from DSP
12 host buffer driver modules 22 are from the buffering driver module
13 key light coupling isolation drive modules 23 are from the light-coupled isolation driver module
14 main full bridge power driver modules 24 are from the full bridge power driver module
15 main Hall current sensor modules 25 are from the Hall current sensor module
26 No. 1 Y-axis laser position sensors of 16 No. 1 X-axis laser position sensors
17 No. 2 X-axis laser position sensors M planar motors
1~No. 4 linear electric motors actuator of L1~L4
D1 serial communication interface D2~D3 controller local area network communication interface
U1~U4 buffering driver U5~U16 photoelectrical coupler
U17~U28 full bridge power driver U29~U40 Hall current sensor
P1 mover platform initial position C1 mover platform barycenter initial position
Position after the translation of position C2 mover platform barycenter after the translation of P2 mover platform
P3 mover platform final position
Five, embodiment
Below in conjunction with accompanying drawing the present invention is elaborated.
Referring to Fig. 1, a kind of 3DOF plane motor servo controller based on two DSP, it is characterized in that comprising main control unit 1 and from the control unit 2.
Referring to Fig. 2,16, No. 2 X-axis laser position sensors 17 of No. 1 X-axis laser position sensors that described main control unit 1 comprises main DSP11, host buffer driver module 12, ten two road key light coupling isolation drive modules, 13, six mutually main full bridge power driver modules 14, the main Hall current sensor module 15 of six phase plane motor drive currents of the main control unit output that is used to sample and being used to is sampled the first via, the second road planar motor X-axis position, main DSP11 comprises on ADC module on the main DSP sheet, the main DSP sheet CAN module on the PWM module and main DSP sheet; The pwm signal output of PWM module is connected with the input of host buffer driver module 12 on the main DSP sheet, the output of host buffer driver module 12 is connected with the input of key light coupling isolation drive module 13, the output of key light coupling isolation drive module 13 is connected with the input of main full bridge power driver module 14, six phase output currents of main full bridge power driver module 14 are six phase plane motor drive currents of main control unit output, six road current sampling signal outputs of main Hall current sensor module 15 are connected with first to the 6th input of ADC module on the main DSP sheet respectively, the 7th input of ADC module is connected on the position sampling signal output part of No. 1 X-axis laser position sensors 16 and the main DSP sheet, and the 8th input of ADC module is connected on the position sampling signal output part of No. 2 X-axis laser position sensors 17 and the main DSP sheet.Described comprise from control unit 2 from DSP21, from buffering driver module 22, ten two the road from light-coupled isolation driver module 23, six from full bridge power driver module 24, be used to sample from six phase plane motor drive currents of control unit output from Hall current sensor module 25 and No. 1 Y-axis laser position sensors 26 of the first via planar motor Y-axis position that is used to sample, from DSP21 comprise from the DSP sheet the ADC module, from the DSP sheet the PWM module and from the DSP sheet the CAN module; The pwm signal output of PWM module is connected with input from buffering driver module 22 from the DSP sheet, be connected with input from the output of buffering driver module 22 from light-coupled isolation driver module 23, be connected with input from the output of light-coupled isolation driver module 23 from full bridge power driver module 24, from six phase output currents of full bridge power driver module 24 is the six phase plane motor drive currents of exporting from the control unit, be connected with first to the 6th input from ADC module on the DSP sheet respectively from six road current sampling signal outputs of Hall current sensor module 25, the position sampling signal output part of No. 1 Y-axis laser position sensors 26 is connected with the 7th input of ADC module from the DSP sheet.On the main DSP sheet of main control unit 1 the CAN module by the CAN bus be connected from the CAN module from the DSP sheet of controlling unit 2.
Referring to Fig. 3, the Synchronization Control strategy of principal and subordinate DSP can be described below: principal and subordinate DSP also can exchange synchronous control signal by two-port RAM by CAN bus switch synchronous control signal; After described main DSP11 waits for that regularly control cycle begins, according to trajectory planning algorithm computation planar motor mover platform barycenter X-axis reference by location value X
Pr, planar motor mover platform barycenter Y-axis reference by location value Y
Pr, planar motor mover platform is around the angle position reference value θ of Z axle rotation
Zpr, send synchronous mark F by the CAN bus
SynGive from DSP21, and wait for the confirmation of synchronization sign A that sends from DSP21
SynWait for and receive synchronous mark F from DSP21
SynAfter, send the confirmation of synchronization sign A by the CAN bus
SynGive main DSP11, read the current sampling data of No. 2, No. 4 linear electric motors actuators, read the sampled value S of No. 1 Y-axis laser position sensors 26
Y1, sampled value is carried out digital filtering, correction, then wait for the planar motor mover platform barycenter Y-axis reference by location value Y that main DSP11 sends
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliMain DSP11 waits for and receives the confirmation of synchronization sign A
SynAfter, read the current sampling data of No. 1, No. 3 linear electric motors actuator, read the sampled value S of No. 1, No. 2 X-axis laser position sensors
X1, S
X2, sampled value is carried out digital filtering, correction, according to the sampled value S of No. 1, No. 2 X-axis laser position sensors
X1, S
X2With transducer Equation for Calculating planar motor mover platform barycenter X-axis position currency X
p, planar motor mover platform is around the angle position currency θ of Z axle rotation
Zp, planar motor mover platform barycenter Y-axis position currency correction term Y
Pcali, send planar motor mover platform barycenter Y-axis reference by location value Y by the CAN bus again
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliGive from DSP21, then wait for the planar motor mover platform barycenter Y-axis position currency Y that sends from DSP21
pWait for and receive planar motor mover platform barycenter Y-axis reference by location value Y from DSP21
PrWith planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliAfter, according to the sampled value S of No. 1 Y-axis laser position sensors 26
Y1, planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliWith transducer Equation for Calculating planar motor mover platform barycenter Y-axis position currency Y
p, send Y by the CAN bus again
pGive main DSP11, and carry out Y-axis degree of freedom position, speed control, carry out linear electric motors actuator Current Control No. 2, No. 4, then wait for the synchronous mark F of next control cycle
SynMain DSP11 waits for and receives Y
pAfter, carry out X-axis and θ
ZDegree of freedom position, speed control, and carry out linear electric motors actuator Current Control No. 1, No. 3, then regularly wait for the beginning of next control cycle.The trajectory planning algorithm of mentioning can adopt the disclosed method of texas,U.S agricultural machinery university doctorate paper " DEVELOPMENT OF NOVEL HIGH-PERFORMANCESIX-AXIS MAGNETICALLY LEVITATED INSTRUMENTS FOR NANOSCALEAPPLICATIONS "; The transducer equation can adopt the disclosed method of U.S. MIT university doctorate paper " Precision six-degree-of-freedom magnetically levitated photolithography stage "; Electric current, position, speed control can adopt the disclosed method of monograph " the DSP control of motor ".
Referring to Fig. 4 a, described main control unit 1 comprises: main DSP11, model is TMS320F2812, host buffer driver module 12 is by the main first buffering driver U1, the main second buffering driver U2 forms, model is SN74LS244, the key light coupling drives isolation module 13 by the main first photoelectrical coupler U5, the main second photoelectrical coupler U6, main the 3rd photoelectrical coupler U7, main the 4th photoelectrical coupler U8, main the 5th photoelectrical coupler U9, main the 6th photoelectrical coupler U10 forms, model is HCPL0631, main full bridge power driver module 14 is by the main first full bridge power driver U17, the main second full bridge power driver U18, main the 3rd full bridge power driver U19, main the 4th full bridge power driver U20, main the 5th full bridge power driver U21, main the 6th full bridge power driver U22 forms, model is L298, main Hall current sensor module 15 is by the main first Hall current sensor U29, the main second Hall current sensor U30, main the 3rd Hall current sensor U31, main the 4th Hall current sensor U32, main the 5th Hall current sensor U33, main the 6th Hall current sensor U34 forms, model is LTSR6-NP, No. 1 X-axis laser position sensors 16, No. 2 X-axis laser position sensors 17, model are LG10A65PI.Wherein: the pwm signal output " PWM1~PWM6 " of PWM module is connected with input " 1A1~1A4 ", 2A1, the 2A2 of the master first buffering driver U1 of host buffer driver module 12 respectively on the sheet of main DSP11, and the pwm signal output " PWM7~PWM12 " of PWM module is connected with input " 1A1~1A4 ", 2A1, the 2A2 of the master second buffering driver U2 of host buffer driver module 12 respectively on the main DSP11 sheet; The output " 1Y1~1Y4 " of the master first buffering driver U1 of host buffer driver module 12,2Y1,2Y2 drives the master first photoelectrical coupler U5 of isolation module 13 respectively with the key light coupling, the main second photoelectrical coupler U6, the input VF1-of main the 3rd photoelectrical coupler U7, VF2-connects, the output " 1Y1~1Y4 " of the master second buffering driver U2 of host buffer driver module 12,2Y1,2Y2 drives master the 4th photoelectrical coupler U8 of isolation module 13 respectively with the key light coupling, main the 5th photoelectrical coupler U9, the input VF1-of main the 6th photoelectrical coupler U10, VF2-connects; The key light coupling drives the master first photoelectrical coupler U5 of isolation module 13, the main second photoelectrical coupler U6, main the 3rd photoelectrical coupler U7, main the 4th photoelectrical coupler U8, main the 5th photoelectrical coupler U9, the output end vo 1 of main the 6th photoelectrical coupler U10, Vo2 respectively with the master first full bridge power driver U17 of main full bridge power driver module 14, the main second full bridge power driver U18, main the 3rd full bridge power driver U19, main the 4th full bridge power driver U20, main the 5th full bridge power driver U21, the input IN1 of main the 6th full bridge power driver U22, IN2 connects; The master first full bridge power driver U17 of main full bridge power driver module 14, the main second full bridge power driver U18, main the 3rd full bridge power driver U19, main the 4th full bridge power driver U20, main the 5th full bridge power driver U21, the output OUT1 of main the 6th full bridge power driver U22 respectively with the master first Hall current sensor U29 of main Hall current sensor module 15, the main second Hall current sensor U30, main the 3rd Hall current sensor U31, main the 4th Hall current sensor U32, main the 5th Hall current sensor U33, the input 1 of main the 6th Hall current sensor U34 connects, the master first full bridge power driver U17 of main full bridge power driver module 14, the main second full bridge power driver U18, the output OUT2 of main the 3rd full bridge power driver U19 is connected master the 4th full bridge power driver U20 of main full bridge power driver module 14 with the input of No. 1 linear electric motors actuator L1 " IN1~-IN3 " respectively, main the 5th full bridge power driver U21, the output OUT2 of main the 6th full bridge power driver U22 is connected with the input of No. 3 linear electric motors actuator L3 " IN1~-IN3 " respectively; The master first Hall current sensor U29 of main Hall current sensor module 15, the main second Hall current sensor U30, main the 3rd Hall current sensor U31, main the 4th Hall current sensor U32, main the 5th Hall current sensor U33, No. 3 pin of main the 6th Hall current sensor U34 are connected with separately No. 5 pin respectively, No. 2 pin are connected with separately No. 6 pin respectively, output OUT is connected with the input " ADCINA0~ADCINA5 " of ADC module on the sheet of main DSP11 respectively, the master first Hall current sensor U29 of main Hall current sensor module 15, the main second Hall current sensor U30, the output 4 of main the 3rd Hall current sensor U31 is connected master the 4th Hall current sensor U32 of main Hall current sensor module 15 with the input of No. 1 linear electric motors actuator L1 "+IN1~+ IN3 " respectively, main the 5th Hall current sensor U33, the output 4 of main the 6th Hall current sensor U34 is connected with the input of No. 3 linear electric motors actuator L3 "+IN1~+ IN3 " respectively; The output Wh of 16, No. 2 X-axis laser position sensors 17 of No. 1 X-axis laser position sensors is connected respectively to input ADCINA6, the ADCINA7 of ADC module on the sheet of main DSP11; The CAN module is connected to master controller local area network (LAN) communication interface D2 on the sheet of main DSP11.
Referring to Fig. 4 b, described from the control unit 2 comprise: from DSP21, model is TMS320F2812, from buffering driver module 22 by from the first buffering driver U3, form from the second buffering driver U4, model is SN74LS244, drive isolation module 23 by from the first photoelectrical coupler U11 from optocoupler, from the second photoelectrical coupler U12, from the 3rd photoelectrical coupler U13, from the 4th photoelectrical coupler U14, from the 5th photoelectrical coupler U15, form from the 6th photoelectrical coupler U16, model is HCPL0631, from full bridge power driver module 24 by from the first full bridge power driver U23, from the second full bridge power driver U24, from the 3rd full bridge power driver U25, from the 4th full bridge power driver U26, from the 5th full bridge power driver U27, form from the 6th full bridge power driver U28, model is L298, from Hall current sensor module 25 by from the first Hall current sensor U35, from the second Hall current sensor U36, from the 3rd Hall current sensor U37, from the 4th Hall current sensor U38, from the 5th Hall current sensor U39, form from the 6th Hall current sensor U40, model is LTSR6-NP, No. 1 Y-axis laser position sensors 26, model are LG10A65PI.Wherein: the pwm signal output " PWM1~PWM6 " of PWM module is connected with the input from the first buffering driver U3 " 1A1~1A4 ", 2A1,2A2 from buffering driver module 22 respectively from the sheet of DSP21, and the pwm signal output " PWM7~PWM12 " of PWM module is connected respectively with from the input from the second buffering driver U4 " 1A1~1A4 ", 2A1, the 2A2 that cushions driver module 22 from the sheet of DSP21; The output from the first buffering driver U3 " 1Y1~1Y4 " from buffering driver module 22,2Y1,2Y2 respectively with drive from optocoupler isolation module 23 from the first photoelectrical coupler U11, from the second photoelectrical coupler U12, input VF1-from the 3rd photoelectrical coupler U13, VF2-connects, from the output from the second buffering driver U4 " 1Y1~1Y4 " of buffering driver module 22,2Y1,2Y2 respectively with drive from optocoupler isolation module 23 from the 4th photoelectrical coupler U14, from the 5th photoelectrical coupler U15, input VF1-from the 6th photoelectrical coupler U16, VF2-connects; From optocoupler drive isolation module 23 from the first photoelectrical coupler U11, from the second photoelectrical coupler U12, from the 3rd photoelectrical coupler U13, from the 4th photoelectrical coupler U14, from the 5th photoelectrical coupler U15, output end vo 1 from the 6th photoelectrical coupler U16, Vo2 respectively with from full bridge power driver module 24 from the first full bridge power driver U23, from the second full bridge power driver U24, from the 3rd full bridge power driver U25, from the 4th full bridge power driver U26, from the 5th full bridge power driver U27, input IN1 from the 6th full bridge power driver U28, IN2 connects; From full bridge power driver module 24 from the first full bridge power driver U23, from the second full bridge power driver U24, from the 3rd full bridge power driver U25, from the 4th full bridge power driver U26, from the 5th full bridge power driver U27, from the output OUT1 of the 6th full bridge power driver U28 respectively with from Hall current sensor module 25 from the first Hall current sensor U35, from the second Hall current sensor U36, from the 3rd Hall current sensor U37, from the 4th Hall current sensor U38, from the 5th Hall current sensor U39, connect from the input 1 of the 6th Hall current sensor U40, from full bridge power driver module 24 from the first full bridge power driver U23, from the second full bridge power driver U24, be connected with the input of No. 2 linear electric motors actuator L2 " IN1~-IN3 " respectively from the output OUT2 of the 3rd full bridge power driver U25, from full bridge power driver module 24 from the 4th full bridge power driver U26, from the 5th full bridge power driver U27, be connected with the input of No. 4 linear electric motors actuator L4 " IN1~-IN3 " respectively from the output OUT2 of the 6th full bridge power driver U28; From Hall current sensor module 25 from the first Hall current sensor U35, from the second Hall current sensor U36, from the 3rd Hall current sensor U37, from the 4th Hall current sensor U38, from the 5th Hall current sensor U39, be connected with separately No. 5 pin respectively from No. 3 pin of the 6th Hall current sensor U40, No. 2 pin are connected with separately No. 6 pin respectively, output OUT is connected with the input " ADCINA0~ADCINA5 " of ADC module from the sheet of DSP21 respectively, from Hall current sensor module 25 from the first Hall current sensor U35, from the second Hall current sensor U36, be connected with the input of No. 2 linear electric motors actuator L2 "+IN1~+ IN3 " respectively from the output 4 of the 3rd Hall current sensor U37, from Hall current sensor module 25 from the 4th Hall current sensor U38, from the 5th Hall current sensor U39, be connected with the input of No. 4 linear electric motors actuator L4 "+IN1~+ IN3 " respectively from the output 4 of the 6th Hall current sensor U40; The output Wh of No. 1 Y-axis laser position sensors 26 is connected to ADC module input ADCINA6 from the sheet of DSP21; The CAN module is connected to slave controller local area network (LAN) communication interface D3 from the sheet of DSP21; Master controller local area network (LAN) communication interface D2 and slave controller local area network (LAN) communication interface D3 interconnect by twisted-pair feeder.
Referring to Fig. 5,17, No. 1 Y-axis laser position sensors of 16, No. 2 X-axis laser position sensors of described No. 1 X-axis laser position sensors 26 constitutes the position transducer array; The sampled value of No. 1 X-axis laser position sensors 16 is S
X1, the sampled value of No. 2 X-axis laser position sensors 17 is S
X2, the sampled value of No. 1 Y-axis laser position sensors 26 is S
Y1The initial point of XYZ coordinate system is consolidated in space O point, and when planar motor mover platform was in initial position P1, mover platform barycenter initial position C1 and O point overlapped, at this moment S
X1=S
X2=X
0, S
Y1=Y
0, the distance of laser position sensors laser beam and mover platform barycenter initial position C1 is l
S1, l
S2, l
S3The motion of mover platform can and rotatablely move synthetic by translational motion, the mover platform at first moves to position P2 after the translation, mover platform barycenter moves to position C2 after the translation, walk around C2 then and parallel with Z axle axle rotates to final position P3, this moment, mover platform barycenter X-axis, Y-axis position currency were X
p, Y
p, be θ around the angle position currency of Z axle rotation
ZpWith S
X1, S
X2Substitution transducer equation can get X
p, θ
Zp, planar motor mover platform barycenter Y-axis position currency correction term Y
PcaliWith S
Y1, Y
PcaliSubstitution transducer equation can get Y
pThe transducer equation of laser position sensors array is: