CN201378823Y - Servo controller for 3-DOF (three-degree-of-freedom) planar motor based on double DSPs (digital signal processor) - Google Patents

Abstract

A servo controller for 3-DOF (three-degree-of-freedom) planar motor based on double DSPs (digital signal processor) comprises a main control unit and a slave control unit, wherein the main control unit and the slave control unit comprise DSP chips, optocoupler drive and isolation modules, full bridge power driver modules, current sensor modules, and position sensor modules, wherein the DSP chip of the main control unit sends a PWM signal, after optocoupler isolation, the DSP chip controls the full bridge power driver module to output the drive current of a six-phase planar motor, and gathers the position of a two-path planar motor on an X axle; the DSP chip of the slave control unit sends a PWM signal, after optocoupler isolation, the DSP chip controls the full bridge power driver module to output the drive current of the six-phase planar motor, and gathers the position of a one-path planar motor on a Y axle; and the DSP chip of the main control unit and the slave control unit exchanges synchronous control signals in a real-time manner through a CAN bus. The three-degree-of-freedom synchronous control strategy is used to control the planar motor to achieve the fast and accurate positioning of the X axle, Y axle and the Theta Z degree of freedom.

Description

3DOF plane motor servo controller based on two DSP

One, technical field:

The utility model 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 utility model 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, utility model content:

The utility model provides a kind of 3DOF plane motor servo controller based on two DSP, and 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 adopted in the utility model 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 utlity model has following advantage:

The utility model may command planar motor 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 utility model 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 utility model 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 Z axle minor rotation θ _ZNon-cpntact measurement, 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 utility model 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, and 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 in the utility model, 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 utility model.

Fig. 2 is a circuit block diagram of the present utility model.

Fig. 3 is principal and subordinate DSP Synchronization Control strategic process figure of the present utility model.

Fig. 4 is circuit theory diagrams of the present utility model, 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 utility model.

Fig. 6 is of the present utility model 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 utility model 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 ₀, S _Y1=Y ₀, 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:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_{\mathrm{Zp}}=(S_{X2}-S_{X1})/({\mathrm{<figure-callout\; id="2"\; label="l\; S"\; filenames="Y2009200419240022H1.png"\; state="\{\{state\}\}">l</figure-callout>}}_S+l_{S1})\\ X_p=X_0-(l_{S2}{S}_{X1}+l_{S1}{S}_{X2})/(l_{\mathrm{<figure-callout\; id="2"\; label="S"\; filenames="Y2009200419240022H1.png"\; state="\{\{state\}\}">S</figure-callout>}2}+l_{S1})\\ Y_{\mathrm{pcali}}=(l_{S3}-X_p)\&times;{\mathrm{\&theta;}}_{\mathrm{Zp}}\\ Y_p=-Y_0+S_{Y1}-Y_{\mathrm{pcali}}\end{array}\right..$

Claims (2)

1. the 3DOF plane motor servo controller based on two DSP is characterized in that, comprising: main control unit (1) and from the control unit (2),

Described main control unit (1) comprises main DSP (11), host buffer driver module (12), ten two road key light coupling isolation drive modules (13), six mutually main full bridge power driver modules (14), be used to sample main control unit output six phase plane motor drive currents main Hall current sensor module (15) and be used to the first via of sampling, No. 1 X-axis laser position sensors (16) of the second road planar motor X-axis position, No. 2 X-axis laser position sensors (17), main DSP (11) comprises ADC module on the main DSP sheet, CAN module on PWM module and the main DSP sheet on the 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, 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 from the control unit (2) comprise from DSP (21), 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, comprise ADC module from the DSP sheet from DSP (21), 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 respectively from six road current sampling signal outputs of Hall current sensor module (25) from ADC module on the DSP sheet, 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),

After described main DSP (11) regularly waits for that 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 (21), and wait for the confirmation of synchronization sign A that sends from DSP (21) _SynWait for and receive synchronous mark F from DSP (21) _SynAfter, send the confirmation of synchronization sign A by the CAN bus _SynGive main DSP (11), 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 DSP (11) sends _PrWith planar motor mover platform barycenter Y-axis position currency correction term Y _PcaliMain DSP (11) 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 (21), then wait for the planar motor mover platform barycenter Y-axis position currency Y that sends from DSP (21) _pWait for and receive planar motor mover platform barycenter Y-axis reference by location value Y from DSP (21) _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 DSP (11), 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 (11) 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.

2. the 3DOF plane motor servo controller based on two DSP according to claim 1, it is characterized in that described main control unit (1): main DSP (11) model is TMS320F2812, host buffer driver module (12) is by main first buffering driver (U1), main second buffering driver (U2) is formed, model is SN74LS244, the key light coupling drives isolation module (13) by main first photoelectrical coupler (U5), 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) is formed, 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) is formed, model is L298, main Hall current sensor module (15) is by main first Hall current sensor (U29), 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) is formed, model is LTSR6-NP, No. 1 X-axis laser displacement sensor (16), No. 2 X-axis laser displacement sensors (17), model is LG10A65PI

The pwm signal output " PWM1～PWM6 " of PWM module is connected with input " 1A1～1A4 ", 2A1, the 2A2 of master's first buffering driver (U1) of host buffer driver module (12) respectively on the sheet of main DSP (11), the pwm signal output " PWM7～PWM12 " of PWM module is connected with input " 1A1～1A4 ", 2A1, the 2A2 of master's second buffering driver (U2) of host buffer driver module (12) respectively on main DSP (11) sheet

The output " 1Y1～1Y4 " of master's first buffering driver (U1) of host buffer driver module (12), 2Y1,2Y2 drives master's first photoelectrical coupler (U5) of isolation module (13) respectively with the key light coupling, main second photoelectrical coupler (U6), the input VF1-of main the 3rd photoelectrical coupler (U7), VF2-connects, the output " 1Y1～1Y4 " of master's second buffering driver (U2) of host buffer driver module (12), 2Y1,2Y2 drives master's 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 master's first photoelectrical coupler (U5) of isolation module (13), 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 master's 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

Master's 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 master's first Hall current sensor (U29) of main Hall current sensor module (15), 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, master's 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 with the input of No. 1 linear electric motors actuator (L1) " IN1～-IN3 " respectively, master's the 4th full bridge power driver (U20) of main full bridge power driver module (14), 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 actuators (L3) " IN1～-IN3 " respectively

Master's first Hall current sensor (U29) of main Hall current sensor module (15), 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 DSP (11) respectively, master's first Hall current sensor (U29) of main Hall current sensor module (15), main second Hall current sensor (U30), the output 4 of main the 3rd Hall current sensor (U31) is connected with the input of No. 1 linear electric motors actuator (L1) "+IN1～+ IN3 " respectively, master's the 4th Hall current sensor (U32) of main Hall current sensor module (15), 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 actuators (L3) "+IN1～+ IN3 " respectively

The output Wh of No. 1 X-axis laser position sensors (16), No. 2 X-axis laser position sensors (17) is connected respectively to input ADCINA6, the ADCINA7 of ADC module on the sheet of main DSP (11),

The CAN module is connected to master controller local area network (LAN) communication interface (D2) on the sheet of main DSP (11), described from the control unit (2) comprising: from DSP (21) model is TMS320F2812, from buffering driver module (22) by from first buffering driver (U3), form from second buffering driver (U4), model is SN74LS244, drive isolation module (23) by from first photoelectrical coupler (U11) from optocoupler, from 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 first Hall current sensor (U35), from 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 displacement sensor (26), model is LG10A65PI

The pwm signal output " PWM1～PWM6 " of PWM module is connected with the input from first buffering driver (U3) " 1A1～1A4 ", 2A1,2A2 from buffering driver module (22) respectively from the sheet of DSP (21), the pwm signal output " PWM7～PWM12 " of PWM module is connected with the input from second buffering driver (U4) " 1A1～1A4 ", 2A1,2A2 from buffering driver module (22) respectively from the sheet of DSP (21)

The output from first buffering driver (U3) " 1Y1～1Y4 " from buffering driver module (22), 2Y1,2Y2 respectively with drive from optocoupler isolation module (23) from first photoelectrical coupler (U11), from second photoelectrical coupler (U12), input VF1-from the 3rd photoelectrical coupler (U13), VF2-connects, the output from second buffering driver (U4) " 1Y1～1Y4 " from 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 first photoelectrical coupler (U11), from 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 first Hall current sensor (U35), from 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 actuators (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 actuators (L4) " IN1～-IN3 " respectively from the output OUT2 of the 6th full bridge power driver (U28)

From Hall current sensor module (25) from first Hall current sensor (U35), from 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 DSP (21) respectively, from Hall current sensor module (25) from first Hall current sensor (U35), from second Hall current sensor (U36), be connected with the input of No. 2 linear electric motors actuators (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 actuators (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 DSP (21),

The CAN module is connected to slave controller local area network (LAN) communication interface (D3) from the sheet of DSP (21),

Master controller local area network (LAN) communication interface (D2) and slave controller local area network (LAN) communication interface (D3) interconnect by twisted-pair feeder.

Images