CN105911935A - High-speed slight rat sprint controller of dual-core triaxial four-wheel variable structure - Google Patents

Abstract

The invention discloses a high-speed slight rat sprint controller of a dual-core triaxial four-wheel variable structure. The high-speed slight rat sprint controller comprises a slight rat shell, wheels, a first infrared sensor, a third infrared sensor, a fourth infrared sensor, a sixth infrared sensor, a first high-speed direct current servo motor, a second high-speed direct current servo motor, a vacuum adsorption motor, a first magnetoelectric encoder, a second magnetoelectric encoder, a motion sensor and a sampling sensor as well as a control panel, wherein the control panel adopts a dual-core controller and comprises an ARM and an FPGA, and the ARM and the FPGA are in communication connection with each other. By virtue of the way, the high-speed slight rat sprint controller has the advantages that stability of a dual-core slight rat full-digital servo system is improved, a slight rat is effectively prevented from slipping on ground during high speed maze sprint, the phenomenon that the slight rat is far away from the central position is avoided, the stability of the slight rat during quick sprint is improved, and the slight rat has a better quick sprint function.

Description

A kind of double-core three axle four-wheel structure changes Mus sprint controller the most slightly

Technical field

The present invention relates to a kind of double-core three axle four-wheel structure changes Mus (PICOMOUSE) the most slightly automatically make a spurt SERVO CONTROL System, belongs to microrobot field.

Background technology

Along with microelectric technique, the continuous progress of Computer Control Technology, external expert solves labyrinth micro computer Mus Propose one on technical foundation and have more challenging maze robot---slightly Mus, its conventional two-dimensional structure such as Fig. 1 institute Show.Solve the difficulty in labyrinth for enhancing labyrinth complexity and mouse, labyrinth retaining wall is become 90mm by original 180mm, Original labyrinth is become 32*32 lattice by 16*16 lattice, and new labyrinth two-dimensional structure is as shown in Figure 2.Power supply is once opened, slightly Mus whole process fully relies on self-contained sensor self-navigation, and solves the various complicated fan being made up of 1024 maze lattices Palace, it is possible to quickly find an optimal path arriving target setting point from starting point, then makes a spurt terminal with the fastest speed.

Mus slightly, as a kind of novel maze robot technology, has many countries and regions launching this most in the world The competition of the technology of kind, and there are the different rules of contest.Mus walking in whole labyrinth slightly is divided into two parts: explores and rushes Thorn, the corresponding time spent is exploration time TS and spurt time TD and owing to adding of occurring in violation of rules and regulations penalizes time TP, slightly Final performance TIME of Mus is determined by TS, TD and TP, and the most representational is Japan, the U.S., Britain and Singapore.

Day this rule is as follows: TIME=TD；

American rule is as follows: TIME=TS/30+TD+TP, and wherein TP is adding of breaking down of slight Mus to penalize the time；

Britain's rule is as follows: TIME=TS/30+TD+TP, and wherein TP is adding of breaking down of slight Mus to penalize the time；

Singapore's rule is as follows: TIME=TS/60+TD+TP, and wherein TP is adding of breaking down of slight Mus to penalize the time.

From international rule above it can be seen that Mus solves the final goal in labyrinth slightly is exactly quickly to make a spurt from a bit Terminal, spurt occupies very important position in the whole motion of slight Mus, and once Mus labyrinth slightly is made a spurt unsuccessfully, slight Mus Whole function the most just cannot realize.

Mus slightly to judge the environment of surrounding the moment during making a spurt in labyrinth, then communicates a parameter to controller, by Controller repetitive control its accurate acceleration and retarded motion in the grid of labyrinth.It is necessary that one outstanding slight Mus solves labyrinth Possess good perception, have good locomotor activity, outstanding intelligent algorithm, otherwise will be unable to task.If adopted Realize the spurt of slight Mus labyrinth with existing simple algorithm and structure, find in practice:

(1) owing to solving the substantial increase of labyrinth number, and detection set-point, labyrinth is being not center, original labyrinth, but fan Any one lattice in palace so that original simple Mus slightly solves maze technique and cannot solve existing complex maze；

(2) being greatly decreased due to slight Mus size, if Mus slightly uses six groups of sensor technologies in Fig. 1 to complete complexity Spurt environment detection in labyrinth, makes a spurt increasing the sampling period, and causes the spurt time longer, and slight Mus finally competes mistake Lose；

(3) some simple Mus model machine servosystem slightly use the chip than lower level and algorithm so that Mus slightly is worked as in labyrinth In spurt typically will spend the longer time, not only consume the energy of a large amount of battery, and in real contest also Cannot win victory；

(4) due to the minimizing of labyrinth retaining wall size so that the distance that the spurt of Mus list lattice runs slightly reduces, and slight Mus was being explored Frequently braking in journey and startup have increased the weight of the workload of single-chip microcomputer, and single single-chip microcomputer cannot meet slight Mus fast sprint and open Requirement that is dynamic and that stop；

(5) general two motor PWMs requiring to drive its motion for the slight Mus for two-wheel drive carrys out sound lunge system Control signal to synchronize, and is limited single single-chip microcomputer servosystem by computing capability and is difficult to meet this condition, and slight Mus is directly Can not accurately walk on center line during sound lunge on road, it is easy to bump against labyrinth retaining wall, cause mission failure；

(6) owing to being affected by single-chip microcomputer capacity and algorithm, slight Mus cannot store labyrinth information, all when running into power-down conditions Information will disappear, this makes whole spurt procedure failure, it is necessary to again explore labyrinth；

(7) Mus slightly is when labyrinth sound lunge, it is easy to by external interference, cause slight Mus owing to compensating the most in time Collision labyrinth retaining wall, finally cannot complete task；

(8) move after two-wheeled slight Mus system is when accelerating spurt due to center of gravity so that mouse front portion is light, even if on good road On face, slight Mus also can be skidded, it is possible to causes the phenomenon hitting wall to occur, is unfavorable for the development of Mus the most slightly；

(9) if two-wheeled slight Mus system designs improper inclined before causing center of gravity, the normal pressure born on driving wheel will be caused to reduce, At this moment Mus system is more prone to skid slightly, is also easier to wander off, causes making a spurt unsuccessfully；

(10) if two-wheeled slight Mus system design improper cause center of gravity lateral deviation by cause two driving wheels to bear normal pressure not With, when fast sprint, two-wheeled skid level is inconsistent, and moment deflects away from track, and during turning, the wheel that wherein normal pressure is little can Can skid, cause cornering difficulties；

(11) speed of mouse and the anti-of position are realized due to traditional slight Mus spurt control system many employings photoelectric encoder Feedback, owing to the volume of photoelectric encoder is bigger so that the volume of Mus slightly is relatively large, it is impossible to the miniaturization realizing slight Mus is sent out Exhibition；

(12) relatively big due to the dust of competition area, particularly labyrinth is after repeatedly match, the dust of absorption on floor, labyrinth Bigger so that the slight Mus of fast sprint is easy to skid, and ultimately results in slight Mus and cannot complete spurt task；

(13) the integrated drive chips volume used due to the slight Mus of tradition is relatively big, and the volume of slight Mus cannot be miniaturized and weight Relatively big, sufficiently large acceleration cannot be obtained under equal-wattage DC servo motor drives, the fast sprint performance of system is relatively Weak；

(14) the slight Mus digital sound lunge servosystem controlled based on monokaryon should process various photo-sensor signal With spurt labyrinth information, the servo SERVO CONTROL and the multiaxis also processing slight Mus is made a spurt so that the workload of processor is relatively Greatly, very big speed when have impact on slight Mus sound lunge and stability.

Slightly Mus labyrinth spurt technology is international emerging a special kind of skill, owing to the difficulty of slight Mus technology is higher and The complexity of labyrinth design, causes the domestic unit the most not researching and developing this robot.The present invention is by existing Dynamic matrix control skill Art and Dynamic matrix control chip design the slight Mus labyrinth fast sprint SERVO CONTROL of a kind of double-core three axle four wheel belt vac sorb Device.

Summary of the invention

The technical problem that present invention mainly solves is to provide the Mus spurt control the most slightly of a kind of double-core three axle four-wheel structure changes Device processed, during for overcoming monokaryon controller can not meet slight Mus sound lunge, stability and the requirement of rapidity, given up domestic The monokaryon mode of operation that micro computer Mus is used, absorbing on the premise of external Dynamic matrix control thought, and independent research is based on ARM (STM32F405) the brand-new double-core three axle four-wheel structure changes of+FPGA (A3P250) Mus spurt servo controller the most slightly, controls Plate is with FPGA(A3P250) produce three axle servosystem PWM ripples for processing core, STM32F405 converts external environment backward A3P250 sends the command value such as position, speed, acceleration, and A3P250 generates two axles spurt in conjunction with the feedback of magnetism encoder and watches Taking the deviation signal of controller servosystem, panel processes core with A3P250 and produces two axle servosystem PWM ripples, warp Drive axle A3906SESTR-T amplifies rear drive slight Mus sound lunge and advances, and STM32F405 frees in the middle of complicated work Out, it is achieved the signal processing algorithm of part and the response of A3P250 are interrupted, it is achieved data communication and storage live signal.Micro- During micro-Mus sound lunge, magnetism encoder M1 and M2 Real-time Feedback motor X and the speed of motor Y and displacement information, A3P250 regulates the coefficient of friction of slight Mus and ground according to the SERVO CONTROL of rate request unlatching motor M.

For solving above-mentioned technical problem, the technical scheme that the present invention uses is: provide a kind of double-core three axle four-wheel Structure changes at a high speed slightly Mus sprint controller, including slight Mus housing, wheel, the first infrared sensor, the 3rd infrared sensor, 4th infrared sensor, the 6th infrared sensor, the first High-speed DC servomotor, the second High-speed DC servomotor, vacuum Absorption motor, the first magnetism encoder, the second magnetism encoder, motion sensor and sampling sensor, four described cars Wheel is arranged on the limit, the left and right sides of slight Mus housing the most two-by-two, and the first described infrared sensor and the 6th infrared sensor divide It is not arranged on the limit, the left and right sides of slight Mus housing and is positioned at the front end of wheel, the 3rd described infrared sensor and the 4th infrared Sensor is the most angularly disposed at the first infrared sensor with the inner side edge of the 6th infrared sensor, the first described High-speed DC Servomotor and the second High-speed DC servomotor be separately mounted to slight Mus housing the right and left and be positioned at two wheels it Between position, described vac sorb motor is arranged on the first High-speed DC servomotor and the second High-speed DC servomotor The centre position of side, the first described magnetism encoder and the second magnetism encoder are separately positioned on the first High-speed DC servo electricity Machine and the lower section of the second High-speed DC servomotor, described motion sensor and sampling sensor are successively set on vac sorb The lower section of motor, also includes that panel, described panel are arranged in slight Mus housing, and described panel uses double-core control Device processed, is communicatively coupled including ARM and FPGA, described ARM with FPGA.

In a preferred embodiment of the present invention, when the 3rd described sensor and the 4th sensor are angularly disposed and Y-axis Between corner dimension be:。

In a preferred embodiment of the present invention, described ARM uses STM32F405 controller, described FPGA to use A3P250 controller.

In a preferred embodiment of the present invention, the first described magnetism encoder and the second magnetism encoder all use base Encoder in magnetoelectric transducer AS5040H.

In a preferred embodiment of the present invention, described wheel is provided with vacuum cup.

In a preferred embodiment of the present invention, described panel sends the first control signal, the second control letter respectively Number and the 3rd control signal, the first described control signal, the second control signal and the 3rd control signal control described respectively The second High-speed DC servomotor, the first High-speed DC servomotor and vac sorb motor signal syntheses after control again The motion of Mus slightly.

In a preferred embodiment of the present invention, described double-core three axle four-wheel structure changes Mus sprint controller the most slightly Being additionally provided with host computer procedure and motion control program, described host computer procedure includes that path is read, labyrinth updates, labyrinth is deposited Storage and parameter export, and described motion control program includes based on ARM+FPGA tri-axle four-wheel slight Mus spurt SERVO CONTROL, fan Location, palace and direction controlling.

In a preferred embodiment of the present invention, described make a spurt SERVO CONTROL based on the ARM+FPGA tri-slight Mus of axle four-wheel Also include interconnective based on structure changes two axle four-wheel slight Mus labyrinth spurt SERVO CONTROL and single axle vacuum sucker suction watch Clothes control, and described includes based on minimum sense samples system based on structure changes two axle four-wheel slight Mus labyrinth spurt SERVO CONTROL Spurt position module, based on minimum sense samples system dash speed module with based on minimum sense samples system spurt acceleration Module；Described single axle vacuum sucker suction SERVO CONTROL includes sucker position module, sucker acceleration module and sucker acceleration Module.

The invention has the beneficial effects as follows: the double-core three axle four-wheel structure changes of present invention Mus sprint controller the most slightly, carry The high stability of double-core slight Mus MPU Controlled All Digital Servo System, effectively prevent ground when slight Mus is made a spurt in high speed labyrinth and beats Sliding, it is to avoid the generation of slight Mus off-center position phenomenon far away, improve stability during its fast sprint, add micro- The contact area on micro-Mus and ground, decreases sending out of the sound lunge stall problem that the slight Mus of two-wheeled causes due to frame for movement Raw so that Mus slightly has more preferable fast sprint function.

Accompanying drawing explanation

For the technical scheme being illustrated more clearly that in the embodiment of the present invention, in embodiment being described below required for make Accompanying drawing be briefly described, it should be apparent that, below describe in accompanying drawing be only some embodiments of the present invention, for From the point of view of those of ordinary skill in the art, on the premise of not paying creative work, it is also possible to obtain other according to these accompanying drawings Accompanying drawing, wherein:

Fig. 1 is two wheel drive slight Mus X-Y scheme；

Fig. 2 is slight Mus 32*32 labyrinth schematic diagram；

Fig. 3 is based on A3906SESTR-T two axle double-core slight Mus spurt servo-control system connection diagram；

Fig. 4 is double-core three axle four-wheel structure changes slight Mus two dimension schematic diagram；

Fig. 5 is based on double-core three axle four-wheel structure changes Mus theory diagram the most slightly；

Fig. 6 is based on double-core three axle four-wheel structure changes Mus spurt flow chart the most slightly；

Fig. 7 is slight Mus speed curve diagram；

Fig. 8 is slight Mus forward straight dash schematic diagram；

Fig. 9 is slight Mus reverse linear spurt schematic diagram；

Figure 10 is slight Mus right-hand rotation spurt schematic diagram；

Figure 11 is slight Mus left-hand rotation spurt meaning figure.

Detailed description of the invention

Technical scheme in the embodiment of the present invention will be clearly and completely described below, it is clear that described enforcement Example is only a part of embodiment of the present invention rather than whole embodiments.Based on the embodiment in the present invention, this area is common All other embodiments that technical staff is obtained under not making creative work premise, broadly fall into the model of present invention protection Enclose.

As shown in Figure 4, the embodiment of the present invention includes:

A kind of double-core three axle four-wheel structure changes Mus sprint controller the most slightly, including slight Mus housing, wheel, the first infrared biography Sensor S1, the 3rd infrared sensor S3, the 4th infrared sensor S4, the 6th infrared sensor S6, the first High-speed DC servo electricity Machine Y, the second High-speed DC servomotor X, vac sorb motor M, the first magnetism encoder M1, the second magnetism encoder M2, fortune Dynamic sensor G1 and sampling sensor L1, four described wheels are arranged on the left and right sides of slight Mus housing the most two-by-two Limit, the first described infrared sensor S1 and the 6th infrared sensor S6 is separately positioned on the limit, the left and right sides of slight Mus housing also Being positioned at the front end of wheel, the 3rd described infrared sensor S3 and the 4th infrared sensor S4 is the most angularly disposed infrared first Sensor S1 and the inner side edge of the 6th infrared sensor S6, the first described High-speed DC servomotor Y and the second High-speed DC Servomotor X is separately mounted to the right and left of slight Mus housing the position between two wheels, and described vacuum is inhaled Attached motor M is arranged on the centre position above the first High-speed DC servomotor Y and the second High-speed DC servomotor X, described The first magnetism encoder M1 and the second magnetism encoder M2 be separately positioned on the first High-speed DC servomotor Y and second at a high speed The lower section of DC servo motor X, described motion sensor G1 and sampling sensor L1 is successively set on vac sorb motor M's Lower section.Wherein, described wheel includes X wheel, Y wheel, R wheel and Z wheel.

Corner dimension in above-mentioned, when the 3rd described sensor S1 and the 4th sensor S4 is angularly disposed and between Y-axis For:。

In the present embodiment, also including that panel, described panel are arranged in slight Mus housing, described panel is adopted By dual-core controller, it is communicatively coupled including ARM and FPGA, described ARM with FPGA.Wherein, described ARM uses STM32F405 controller, described FPGA uses A3P250 controller.

Further, the first described magnetism encoder M1 and the second magnetism encoder M2 all uses based on magnetoelectric transducer The encoder of AS5040H；Being provided with vacuum cup on described wheel, absorption property is good.

As it is shown in figure 5, described double-core three axle four-wheel structure changes Mus sprint controller the most slightly also includes panel, institute The panel stated sends the first control signal, the second control signal and the 3rd control signal respectively, by the first described control letter Number, the second control signal and the 3rd control signal control described the second High-speed DC servomotor, the first High-speed DC respectively The motion of slight Mus is controlled again after the signal syntheses of servomotor and vac sorb motor.

In above-mentioned, described supply unit uses lithium ion battery；Described panel uses STM32F405 spurt servo Controlling, STM32F405 spurt SERVO CONTROL is the introducing integrated special driving chip of multiaxis in controller based on STM32F405 A3906SESTR-T, described panel is with STM32F405 for processing core.

STM32F4 series is in addition to pin and softwarecompatible high performance F2 series, and the dominant frequency (168MHz) of F4 is higher than F2 system Row (120MHz), and support monocycle DSP instruction and floating point unit, bigger SRAM capacity (192 KB, F2 are 128 KB), The more advanced peripheral hardwares such as the embedded flash memory of 512KB-1MB and image, network interface and data encryption.STM32F4 series base In up-to-date ARM Cortex M4 kernel, in existing outstanding STM32 microcontroller products combination, increase signal processing newly Function, and improve the speed of service；STM32F405x be integrated with intervalometer, 3 ADC, 2 DAC, serial line interface, external memory interface, Real-time clock, CRC computing unit and simulation real random number generator are at interior a whole set of advanced peripheral hardware.STM32F405 exists Multiple advanced peripheral hardware is added on the basis of STM32F405 product.These performances make F4 series can be easier to meet control and The Digital Signals demand of signal processing function mixing.Efficient signal processing function and Cortex-M4 processor family Low energy consumption, low cost and the combination of wieldy advantage so that it can be that slight Mus multiple sensors signal processing provides Reliable foundation.

FPGA is the abbreviation of English Field Programmable Gate Array, i.e. field programmable gate array, be The product of development further on the basis of the programming devices such as PAL, GAL, EPLD.It is to lead as special IC (ASIC) A kind of semi-custom circuit in territory and occur, i.e. solve the deficiency of custom circuit, overcome again original programming device door The shortcoming that circuit number is limited.FPGA uses the design philosophy of software implementation to realize the design of hardware circuit, thus make based on The system of FPGA design has good reusable and amendment property.A3P250 is that the one of ACTEL company design is based on non-volatile The FPGA device of property Flash technology.Device have employed fine granular framework VersaTile, has 250K system door structure, uses The Technology of 130 nm, core voltage 1.5 V, A3P250 be antifuse, radioprotective, high-low temperature resistant, low in energy consumption, fast Degree is fast, and application is relatively wide, and These characteristics makes A3P250 be particularly suitable for owing to high performance multiaxis DC servo drive controls, Three axles being particularly suitable in the present invention synchronize actuated control system, directly STM32F405 from complicated SERVO CONTROL In free.

The present invention, in order to reduce the volume of double-core high-speed slight Mus spurt servosystem, has given up two traditional axle direct currents and has watched Take motor H type drive axle L6207D, and use the two axle DC servo motor drive axles that volume is less, voltage is less A3906SESTR-T, A3906SESTR-T are a kind of single and double line DC motor Driver chips, and A3906 is intended to walk for low-voltage Enter the pulse-width controlled (PWM) of motor, single channel and two-way direct current motor, can be in the electric current of each passage output up to 1 A, work Voltage range is 2.5 to 9 V.The built-in fixing turn-off time PWM timer of A3906SESTR-T, according to chip periphery The selection of sampling resistor, arranges peak point current.Cross stream output token and reach peak value for notification controller current of electric, can For overcurrent protection, These characteristics make A3906SESTR-T be particularly suitable for slight Mus two axles spurt SERVO CONTROL system In system, A3906SESTR-T with the connection figure of slight Mus two axle DC servo motor as it is shown on figure 3, wherein spurt PWM control defeated Entering signal and spurt enables signal and comes from servo controller, controller controls input signal by adjustment spurt PWM and adjusts Its spurt PWM controls output signal, then realizes the four-quadrant motion of DC servo motor, meets slight Mus fast sprint need Want.

Full-digital servo controller flow chart such as Fig. 6 of this invention design, described double-core three axle four-wheel structure changes Mus sprint controller the most slightly is additionally provided with host computer procedure and motion control program, and described host computer procedure includes path Read, labyrinth updates, labyrinth stores and parameter output, and described motion control program includes based on ARM+FPGA tri-axle four-wheel micro- Micro-Mus spurt SERVO CONTROL, location, labyrinth and direction controlling.

Wherein, described based on ARM+FPGA tri-axle four-wheel slight Mus spurt SERVO CONTROL also include interconnective based on Structure changes two axle four-wheel slight Mus labyrinth spurt SERVO CONTROL and single axle vacuum sucker suction SERVO CONTROL, described ties based on becoming Structure two axle four-wheel slight Mus labyrinth spurt SERVO CONTROL includes based on minimum sense samples system spurt position module, based on minimum Sense samples system dash speed module and based on minimum sense samples system spurt acceleration module；Described single axle vacuum is inhaled Dish absorption SERVO CONTROL includes sucker position module, sucker acceleration module and sucker acceleration module.

In order to improve the stability of double-core high-speed slight Mus spurt servosystem, increase the contact surface of slight Mus and ground Long-pending, reduce the reach of slight Mus center of gravity, rear shifting or sidesway, the present invention realizes the merit of two axle four-wheels by gear mechanism structure Can, the rotating shaft of the every spindle motor in left and right is equipped with the gear wheel machinery of a little mechanical gear, this gear and former and later two wheels Connecting, wherein the tooth of gear wheel is weekly 60, and the tooth of little gear is weekly 15, makes each wheel by such mechanical connection All become power wheel.

In order to improve the stability of double-core high-speed slight Mus spurt servosystem further, prevent slight Mus at sound lunge Time due to floor-dust more and cause walking skid, the present invention slight Mus spurt servo hardware system in add miniature directly Stream motor M, during slight Mus sound lunge, motor M does not stop to aspirate in micro vacuum sucker by vacuum suction apparatus Air, the external and internal pressure making micro vacuum sucker is different, produces certain negative pressure so that it is produce the ground, labyrinth with dust Raw certain absorption affinity, effectively prevent the slight Mus ground when sound lunge and skids.

In order to improve stability when the slight Mus of double-core high-speed is made a spurt in labyrinth further, the present invention watches in the spurt of slight Mus Taking and add high-performance MEMS motion sensor LY3200ALH in hardware system, LY3200ALH can measure slight Mus Yaw rate, the LY3200ALH parameter moment is by STM32F405 controller record and calculates, when slight Mus sends out in labyrinth spurt attitude Raw when varying more than setting threshold values, at a new sampling period controller the most immediately to its position compensation, it is to avoid slight Mus The generation of off-center position phenomenon far away, improves stability during its sound lunge.

In order to preferably gather labyrinth information and reduce the area that infrared sensor takies, the present invention uses infrared sensor SFH4350 instead of the OPE9954A of tradition use, and the infrared light of infrared sensor S1, S2, S3, S4, S5, S6 is through side barricade Can be received by corresponding infrared remote receiver BPW85A after feedback, as current after then the value of feedback via controller of BPW85A calculates The feedback of position, controller adjusts the attitude of slight Mus by these values of feedback.

Find in an experiment, use four groups of sensors can improve the sample frequency of sensor, be conducive to improving slight Mus Speed, but if four groups of sensors compensate do bad it would be possible to that cause that slight Mus solves is a wrong fan Palace；If using the unknown labyrinths of six groups of sensors detection, solving labyrinth and typically not havinging a mistake, but too much sensor group Conjunction have impact on sample frequency, is unfavorable for the raising of slight Mus speed；In order to take into account different international rules and Maze Exploration Accuracy, independent research of the present invention brand-new control models based on six groups of sensor independent assortment structure changes detection labyrinths, institute Invention three axle four-wheels slight Mus two-dimensional structure as shown in Figure 4, in the diagram, in order to preferably detect labyrinth, sensor S3 and S4 And the corner dimension between Y-axis is:, interval at this, sensor cooperating state is optimal.For exploring nothing The international rule of time requirement, opens six groups of sensors by software and explores pattern, and sensor S1, S6 jointly act on and judge front Barricade, sensor S2, S3 judge that the existence of its left side barricade, sensor S4, S5 judge the existence of barricade, simultaneously S2 on the right of it, S3 and S4, S5 cooperation provides navigation foundation for slight Mus linear motion；For exploring the international rule having time requirement, by soft Part opens four groups of mode sensors, and sensor S1, S6 jointly act on and judge that front barricade, sensor S3 judge its left side barricade Existing, sensor S4 judges the existence of barricade on the right of it, and S3 cooperates with S4 to provide navigation foundation for slight Mus linear motion simultaneously. The infrared light of infrared sensor S1, S3, S4, S6 can be received by corresponding infrared remote receiver BPW85A after side barricade feeds back, Then as the feedback of current location after the value of feedback via controller of BPW85A calculates, then controller is adjusted by these values of feedback The attitude of whole slight Mus.

In order to reduce the volume that photoelectric encoder takies, and reducing the dust impact on photoelectric encoder, the present invention uses Encoder M1, M2 of based on magnetoelectric transducer AS5040H instead of traditional photoelectric encoder C1 and C2, and this sensor can have Effect measures speed when two axle DC servo motors move and displacement, provides for slight Mus sound lunge three Close loop servo control Reliable basis.

Stability and the requirement of rapidity when the present invention is to overcome monokaryon controller can not meet slight Mus sound lunge, house Abandon the monokaryon mode of operation that domestic micro computer Mus is used, on the premise of absorbing external Dynamic matrix control thought, independent research Brand-new double-core three axle four-wheel structure changes of based on ARM (STM32F405)+FPGA (A3P250) Mus spurt servo control the most slightly Device processed.Its principle is as shown in Figure 5: panel is with FPGA(A3P250) produce three axle servosystem PWM ripples for processing core, STM32F405 sends the command value such as position, speed, acceleration to A3P250 after external environment is converted, and A3P250 is in conjunction with magnetic The feedback of photoelectric coder generates the deviation signal of two axle spurt servo controller servosystem, and panel processes core with A3P250 The heart produces two axle servosystem PWM ripples, amplifies rear drive slight Mus sound lunge through drive axle A3906SESTR-T and advances. STM32F405 frees in the middle of complicated work, it is achieved the signal processing algorithm of part and the response of A3P250 are interrupted, real Existing data communication and storage live signal.During slight Mus sound lunge, magnetism encoder M1 and M2 Real-time Feedback motor X With speed and the displacement information of motor Y, the SERVO CONTROL that A3P250 opens motor M according to rate request regulates slight Mus and ground The coefficient of friction in face.

For reaching above-mentioned purpose, the present invention takes techniques below scheme, in order to improve arithmetic speed, it is ensured that three axle four-wheels become The stability during spurt of structure high-speed slight Mus servosystem and reliability, the present invention is in control based on ARM (STM32F405) Device introduces FPGA (A3P250), forms brand-new dual-core controller based on ARM (STM32F405)+FPGA (A3P250), this control Device processed is given FPGA (A3P250) the three axle direct current generator synchronous servo systems that workload in control system is maximum and is processed, fully Play FPGA (A3P250) data processing speed feature faster, and labyrinth is read and renewal, online output, data storage, I/O The functions such as control are given ARM (STM32F405) and are completed, and thus achieve dividing of ARM (STM32F405) and FPGA (A3P250) Work, can also carry out therebetween communication simultaneously, carry out data exchange in real time and call.

For the dual-core controller designed herein, under power-on state, slight Mus is introduced into self-locking state, STM32F405 automatically transfers the labyrinth information of storage and calculates spurt path.Controller is then turned on vacuum draw motor M, The coefficient of friction of itself and ground is adjusted by the absorption affinity increasing slight Mus and ground.Mus relies on front slightly, left and right sides is flushing Keep in obscurity outward sensor S1, S3, S4, S6 detection labyrinth information, and STM32F405 feeds back according to four infrared receiver sensor BPW85A Actual navigational environment, STM32F405 is raw in conjunction with the feedback of magnetoelectric transducer M1, M2 with A3P250 communication, A3P250 after processing Becoming two-way PWM ripple, then A3P250 adjusts the input signal of A3906SESTR-T, drives signal through drive axle A3906SESTR-T Amplify two servomotor X and Y of rear drive to advance, it is achieved the SERVO CONTROL of Mus sound lunge slightly；Magnetoelectricity during spurt The speed of sensor M1, M2 Real-time Feedback motor and displacement signal to STM32F405, STM32F405 after continuing with A3P250 communication, secondary adjusts follow-up running status.

With reference to Fig. 6, being embodied as step is:

Slight Mus spurt servo-control system is divided into two parts: master system and kinetic control system.Wherein host computer system Labyrinth of having united is read and the function such as optimization, coordinate setting, online output；Kinetic control system completes slight Mus three axle servo system SERVO CONTROL during system spurt, data storage, the function such as I/O control, wherein three axle servosystem of workload maximum are given FPGA process, remaining includes, and path is read, labyrinth updates, labyrinth storage and data output etc. are given STM32F405 and completed, this Sample is achieved that the division of labor of STM32F405 Yu FPGA, can also carry out therebetween communication simultaneously, carry out in real time data exchange and Call.

With reference to Fig. 4, Fig. 5, Fig. 6, Fig. 7, Fig. 8, Fig. 9, Figure 10 and Figure 11, its concrete functional realiey is as follows:

1) before slight Mus does not receives spurt order, after holding electricity, controller can detect battery supply, if low pressure will report Alert prompting charging；If voltage the most typically can wait, at starting point coordinate (0,0), the spurt order that controller sends, and recalls Optimum labyrinth after exploring；Controller first turns on vacuum draw motor M, is first taken out micro vacuum sucker by aspirator Inhaling, make vacuum cup mask over the ground have certain absorption affinity, controller also detects in real time, if ground is not clean, system can be certainly Dynamic regulation motor M strengthens the vacuum cup absorption affinity to ground, increases the coefficient of friction with ground when slight Mus is made a spurt；Once connect After spurt order, slight Mus can the most quickly sprint for the line (X1, Y1) along optimal path；

2) Mus slightly is placed on starting point coordinate (0,0), in order to prevent from misplacing spurt direction, the sensor in its front after receiving task S1, S6 and can judge the environment in front, determine and enter range of movement either with or without barricade, will be to as there is barricade STM32F405 sends interrupt requests, to interrupting doing very first time response, then STM32F405 can forbid that A3P250 works, A3P250 adjusts the input signal of A3906SESTR-T, drives signal to be blocked, and the motor X of slight Mus, motor Y cannot start, Mus slightly is still in original place, and then STM32F405 judges that labyrinth determines front information, prevents information from judging by accident for bis-times；Without Barricade enters the range of movement in front, and slight Mus will be opened reset circuit, and be transferred normal labyrinth information, is ready for normal Spurt；

3) starting spurt moment slight Mus, infrared transmitting tube SFH4350 independent for sensor S1, S2, S5, S6(tetra-sends Infrared light be received after device BPW85A accepts be converted into the information in labyrinth around) judge environment around and give STM32F405, then by STM32F405 according to the instruction set-point of spurt labyrinth information formation speed-time motion ladder diagram, This trapezoidal area comprised is exactly slight Mus two motor X, motor Y distance to be run；Then STM32F405 enables A3P250, by A3P250 according to these parameters in conjunction with magnetism encoder feedback generate drive two axle direct current generators PWM ripple； PWM ripple, through two individual motor X of drive axle A3906SESTR-T rear drive and Y, completes whole accelerator until reaching spurt Setting speed, the situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and by regulating the servo of motor M Control effectively to regulate the vacuum cup absorption affinity to ground, with the coefficient of friction on ground when increasing slight Mus sound lunge, anti- Only slight Mus sound lunge skids；

4) travelling forward along Y-axis slight Mus, labyrinth spurt information shows currently has Z lattice distance not have barricade to enter front Range of movement, then Mus slightly will store its coordinate (X, Y), and the location parameter of the Z lattice that travel forward is transferred to STM32F405, According to sprint controller speed and the requirement of acceleration, straight by internal three closed loop servo system Program Generating two axles of STM32F405 The movement instruction set-point of flow servo motor X and Y, then STM32F405 Yu A3P250 communication transmit data, A3P250 ties again The feedback closing magnetism encoder generates driving DC servo motor X and the PWM ripple of Y motion, A3P250 Yu A3906SESTR-T leads to Interrogating and adjust input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input signal passes through drive axle A3906SESTR-T Slight Mus is promoted to travel forward after amplification；In slight Mus forward movement, magnetoelectric transducer M1 and M2 moment feed back motor X The speed run with motor Y and displacement, and it is transferred to A3P250；Slight Mus along during current maze lattice thrust forward, The situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and are come effectively by the SERVO CONTROL of regulation motor M The regulation vacuum cup absorption to ground, the labyrinth retaining wall of left and right can be judged by sensor S3 and S4, STM32F405 record Store current labyrinth retaining wall information, according to the labyrinth information of left and right barricade, slight Mus determines that it enters single wall navigation pattern, Shuan Qiang Navigation pattern or inertial navigation pattern；Then instantaneous when sensor G1 (LY3200ALH) detects in real time the spurt of slight Mus Rotary speed, when slight Mus fast sprint is departing from when setting center, and A3P250 is big according to the deviation leaving center Little proceed by real-Time Compensation by sensor G1 (LY3200ALH), the PWM ripple input of fine setting motor, A3P250 with A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, and input signal is by driving Bridge A3906SESTR-T promotes slight Mus to travel forward after amplifying, and can accurately adjust slight Mus at straight way at a high speed by this mode The position of spurt and attitude so that it is come back to set center；If in sound lunge motor process, slight Mus occurs When driving wheel stall or the more situation of dust, the SERVO CONTROL of A3P250 Secondary Control motor M at once effectively regulates The vacuum cup absorption affinity to ground, STM32F405 is converted into new reference instruction value remaining distance and is transferred to A3P250, A3P250 according to these parameters in conjunction with magnetism encoder feedback generate drive motor X, the new PWM of motor Y drive letter Number, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input Signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, and slight Mus is under three axle four-wheel drive states Still advance according to original navigation pattern；When slight Mus moves the Z lattice distance new address of arrival under dual-core controller control, To update its coordinate is (X, Y+Z), at Y+Z < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if not continuing Updating its coordinate, if it is notification controller has searched target, then puts exploration of making a return voyage and is masked as 1, and slight Mus prepares Return is explored；

5) reversely travel forward along Y-axis slight Mus, labyrinth spurt information show currently have Z lattice distance do not have barricade entrance before The range of movement of side, then Mus slightly will store its coordinate (X, Y), and the location parameter of the Z lattice that travel forward is transferred to STM32F405, according to sprint controller speed and the requirement of acceleration, by the internal three closed loop servo system programs of STM32F405 Generate the movement instruction set-point of two axle DC servo motor X and Y, then STM32F405 Yu A3P250 communication transmit data, A3P250 generates in conjunction with the feedback of magnetism encoder and drives DC servo motor X and the PWM ripple of Y motion, A3P250 with A3906SESTR-T communication also adjusts input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, and input signal is by driving Bridge A3906SESTR-T promotes slight Mus to travel forward after amplifying；In slight Mus forward movement, magnetoelectric transducer M1 and The M2 moment feeds back speed and the displacement that motor X and motor Y runs, and is transferred to A3P250；Slight Mus along current maze lattice During thrust forward, the situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and by regulation motor M SERVO CONTROL effectively regulate the vacuum cup absorption to ground, sensor S2, S3 and S4, S5 can be to the labyrinth retaining walls of left and right Judging, and record the current labyrinth retaining wall information of storage, according to the labyrinth information of left and right barricade, slight Mus determines that it enters single Wall navigation pattern, double wall navigation pattern or inertial navigation pattern；Then sensor G1 (LY3200ALH) detection in real time is slightly Instantaneous rotary speed during Mus spurt, when slight Mus fast sprint is departing from setting center, during A3P250 is according to leaving The deviation size of heart position proceeds by real-Time Compensation by sensor G1 (LY3200ALH), the PWM ripple input of fine setting motor, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input signal Promote slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, can accurately adjust slight Mus by this mode and exist The position of straight way and attitude so that it is come back to set center；If in sound lunge motor process, slight Mus occurs When driving wheel stall or the more situation of dust, the SERVO CONTROL of A3P250 Secondary Control motor M at once effectively regulates The vacuum cup absorption affinity to ground, STM32F405 is converted into new reference instruction value remaining distance and is transferred to A3P250, A3P250 according to these parameters in conjunction with magnetism encoder feedback generate drive motor X, the new PWM of motor Y drive letter Number, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input Signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, and slight Mus is under three axle four-wheel drive states Still advance according to original navigation pattern；When slight Mus moves the Z lattice distance new address of arrival under dual-core controller control, To update its coordinate is (X, Y-Z), 0 < Y-Z < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if not continuing Continuous its coordinate of renewal, if it is notification controller has searched target, then puts exploration of making a return voyage and is masked as 1, and slight Mus is accurate Standby return is explored；

6) if slight Mus along the range of movement having in Y-axis forward movement labyrinth retaining wall to enter front, and now In the information of labyrinth left have barricade and right without barricade constantly, slight Mus will store now coordinate (X, Y), subsequently into Figure 10 institute The curvilinear motion track shown: in order to make turning more steady, system uses three-stage process to realize turning: correction distance before spurt R90_Leading, radian turning ARC, post-equalization distance R90_Passing of making a spurt；

When right spurt is turned, STM32F405 first walking the shortest distance R90_Leading of straight line according to various spurt The requirement that condition is different be converted into speed parameter and acceleration parameter then with A3P250 communication, and this set command value pass It is defeated by A3P250, A3P250 and generates what driving was all around taken turns according to these parameters in conjunction with feeding back of magnetism encoder M1 and M2 PWM waveform and direction, then A3P250 adjusts input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, and input signal is led to The bridge A3906SESTR-T that overdrives controls rapid movement before motor X, motor Y-direction after amplifying；Slight Mus along current maze lattice During thrust forward, A3P250 effectively regulates vacuum according to peripheral sensor by the SERVO CONTROL of regulation motor M in real time The sucker absorbability to ground, left barricade can be judged by sensor S3, and records the current labyrinth retaining wall information of storage, micro- Micro-Mus enters single wall navigation pattern according to the labyrinth information of the left barricade of direction of advance；Sensor G1 (LY3200ALH) detects in real time Slightly instantaneous rotary speed during Mus spurt, when slight Mus fast sprint is departing from when setting center, A3P250 according to from The deviation size opening center proceeds by real-Time Compensation by sensor G1 (LY3200ALH), and the PWM ripple of fine setting motor is defeated Entering, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input Signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, can accurately be adjusted slightly by this mode In position and the attitude of straight way during Mus sound lunge so that it is come back to set center；When arriving set objective, sensing Device reference value R90_FrontWallRef is started working, and prevents external interference from starting to do error compensation；

First STM32F405 is converted into speed parameter and acceleration radian ARC according to the requirement that various spurt conditions are different Parameter is transferred to A3P250, the A3P250 PWM in conjunction with the feedback generation driving direct current generator motion of magnetism encoder M1 and M2 Ripple and direction, A3P250 adjusts input signal IN1 of A3906SESTR-T, and IN2, IN3 and IN4, input signal passes through drive axle A3906SESTR-T controls motor X, motor Y after amplifying and completes turning motion；Spurt is turned to along current maze lattice slight Mus During, A3P250 effectively regulates vacuum cup over the ground according to peripheral sensor by the SERVO CONTROL of regulation motor M in real time The absorbability in face；During slight Mus turns spurt, sensor S1, S2, S5, S6 close, and system enters based on sensor The inertial navigation pattern of G1 (LY3200ALH)；Sensor G1 (LY3200ALH) real time record during slight Mus fast sprint The angular velocity turned, M1 and the M2 record speed of motor and positional information, and after STM32F405 processes with A3P250 communication, When slight Mus fast sprint is departing from setting position, within the new sampling period, A3P250 finely tunes motor according to deviation size PWM ripple input, A3P250 fine setting A3906SESTR-T input signal IN1, IN2, IN3 and IN4, input signal by drive Bridge A3906SESTR-T controls motor X, motor Y and completes compensation motion of turning after amplifying, make system complete the motion of radian ARC1 Track；

After arriving set objective, system turn on sensor S3, STM32F405 is first distance R90_ the shortest for walking straight line Passing is converted into speed parameter according to the requirement that various spurt conditions are different and then acceleration parameter leads to A3P250 News, and this is set command value be transferred to A3P250, A3P250 according to anti-in conjunction with magnetism encoder M1 and M2 of these parameters Feedback generates and drives the PWM waveform and direction all around taken turns, and then A3P250 adjusts input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input signal controls rapid movement before motor X, motor Y-direction after being amplified by drive axle A3906SESTR-T； Slight Mus along during current maze lattice thrust forward, A3P250 in real time according to E peripheral sensor by regulation motor M's SERVO CONTROL effectively regulates the vacuum cup absorbability to ground, and left barricade can be judged by sensor S3, and record Storing current labyrinth retaining wall information, slight Mus enters single wall navigation pattern according to the labyrinth information of the left barricade of direction of advance；Sensing Device G1 (LY3200ALH) detects the instantaneous rotary speed during spurt of slight Mus in real time, when slight Mus fast sprint is departing from setting During center, A3P250 proceeds by reality according to the deviation size leaving center by sensor G1 (LY3200ALH) Time compensate, fine setting motor PWM ripple input, A3P250 Yu A3906SESTR-T communication and finely tune A3906SESTR-T input believe Number IN1, IN2, IN3 and IN4, input signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, logical Cross when this mode can accurately adjust slight Mus sound lunge in the position of straight way and attitude so that it is come back to set centre bit Put；The geometric locus motion of whole right-hand bend is completed after arriving set objective；Now will update its coordinate is (X+1, Y), at X + 1 < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if not continuing executing with new spurt order, if it is Notification controller has been made a spurt terminal, then puts exploration of making a return voyage and is masked as 1, and slight Mus prepares return and explores；

7) if slight Mus along the range of movement having in Y-axis forward movement labyrinth retaining wall to enter front, and now In the information of labyrinth, without barricade, there is barricade right constantly in left, and slight Mus will store now coordinate (X, Y), subsequently into Figure 11 institute The curvilinear motion track shown: in order to make turning more steady, system uses three-stage process to realize turning: correction distance before spurt L90_Leading, radian turning ARC, post-equalization distance L90_Passing of making a spurt；

When left spurt is turned, STM32F405 first walking the shortest distance L90_Leading of straight line according to various spurt The requirement that condition is different be converted into speed parameter and acceleration parameter then with A3P250 communication, and this set command value pass It is defeated by A3P250, A3P250 and generates what driving was all around taken turns according to these parameters in conjunction with feeding back of magnetism encoder M1 and M2 PWM waveform and direction, then A3P250 adjusts input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, and input signal is led to The bridge A3906SESTR-T that overdrives controls rapid movement before motor X, motor Y-direction after amplifying；Slight Mus along current maze lattice During thrust forward, A3P250 effectively regulates vacuum according to peripheral sensor by the SERVO CONTROL of regulation motor M in real time The sucker absorbability to ground, left barricade can be judged by sensor S4, and records the current labyrinth retaining wall information of storage, micro- Micro-Mus enters single wall navigation pattern according to the labyrinth information of the left barricade of direction of advance；Sensor G1 (LY3200ALH) detects in real time Slightly instantaneous rotary speed during Mus spurt, when slight Mus fast sprint is departing from when setting center, A3P250 according to from The deviation size opening center proceeds by real-Time Compensation by sensor G1 (LY3200ALH), and the PWM ripple of fine setting motor is defeated Entering, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input Signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, can accurately be adjusted slightly by this mode In position and the attitude of straight way during Mus sound lunge so that it is come back to set center；When arriving set objective, sensing Device reference value L90_FrontWallRef is started working, and prevents external interference from starting to do error compensation；

First STM32F405 is converted into speed parameter and acceleration radian ARC according to the requirement that various spurt conditions are different Parameter is transferred to A3P250, the A3P250 PWM in conjunction with the feedback generation driving direct current generator motion of magnetism encoder M1 and M2 Ripple and direction, A3P250 adjusts input signal IN1 of A3906SESTR-T, and IN2, IN3 and IN4, input signal passes through drive axle A3906SESTR-T controls motor X, motor Y after amplifying and completes turning motion；Spurt is turned to along current maze lattice slight Mus During, A3P250 effectively regulates vacuum cup over the ground according to peripheral sensor by the SERVO CONTROL of regulation motor M in real time The absorbability in face；During slight Mus turns spurt, sensor S1, S2, S5, S6 close, and system enters based on sensor The inertial navigation pattern of G1 (LY3200ALH)；Sensor G1 (LY3200ALH) real time record during slight Mus fast sprint The angular velocity turned, M1 and the M2 record speed of motor and positional information, and after STM32F405 processes with A3P250 communication, When slight Mus fast sprint is departing from setting position, within the new sampling period, A3P250 finely tunes motor according to deviation size PWM ripple input, A3P250 fine setting A3906SESTR-T input signal IN1, IN2, IN3 and IN4, input signal by drive Bridge A3906SESTR-T controls motor X, motor Y and completes compensation motion of turning after amplifying, make system complete the motion rail of radian ARC Mark；

After arriving set objective, system turn on sensor S4, STM32F405 is first distance L90_ the shortest for walking straight line Passing according to the requirement that various spurt conditions are different be converted into speed parameter and acceleration parameter then with FPGA communication, And it is raw in conjunction with the feedback of magnetism encoder M1 and M2 according to these parameters that this setting command value is transferred to A3P250, A3P250 Become to drive the PWM waveform and direction all around taken turns, then input signal IN1 of A3P250 adjustment A3906SESTR-T, IN2, IN3 and IN4, input signal controls rapid movement before motor X, motor Y-direction after being amplified by drive axle A3906SESTR-T；Micro- Micro-Mus along during current maze lattice thrust forward, A3P250 in real time according to peripheral sensor by regulating the servo of motor M Controlling effectively to regulate the vacuum cup absorbability to ground, left barricade can be judged by sensor S4, and records storage Current labyrinth retaining wall information, slight Mus enters single wall navigation pattern according to the labyrinth information of the left barricade of direction of advance；Sensor G1 (LY3200ALH) the instantaneous rotary speed during spurt of slight Mus is detected in real time, when slight Mus fast sprint is departing from the center of setting During position, A3P250 proceeds by benefit in real time according to the deviation size leaving center by sensor G1 (LY3200ALH) Repaying, the PWM ripple input of fine setting motor, A3P250 Yu A3906SESTR-T communication also finely tunes the input signal of A3906SESTR-T IN1, IN2, IN3 and IN4, input signal is promoted slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, passes through In the position of straight way and attitude when this mode can accurately adjust slight Mus sound lunge so that it is come back to set centre bit Put；The geometric locus motion of whole right-hand bend is completed after arriving set objective；Now will update its coordinate is (X-1, Y), 0 < X-1 < on the premise of 1F, it is judged that its coordinate whether (X1, Y1), if not continuing executing with new spurt order, if Being that notification controller has been made a spurt terminal, then put exploration of making a return voyage and be masked as 1, slight Mus prepares return and explores；

8) (return after spurt being prepared after (X1, Y1) explore to search more excellent path when the spurt of slight Mus arrives, controlling Device can recall the labyrinth information that it is the most stored, then calculates other optimal path that may be present, then return start into Enter wherein think optimum one；

9) when normally making a return voyage and run in slight Mus entrance labyrinth, and sensor S1, S2, S3, S4, S5, S6(or S1 of its navigation, S3, S4, S6) by work, the photosignal reflected passes a parameter to STM32F405, warp after BPW85A absorbs With A3P250 communication after STM32F405 process, A3P250 generating the pwm signal driving two spindle motors, then A3P250 adjusts The input signal of A3906SESTR-T, A3906SESTR-T drives navigation motor X, motor Y: if entering the region searched for To fast forward through, if unknown return area then uses normal speed to search for, controller can detect motor X, electricity in real time The numerical value of machine Y magnetism encoder, and strengthen according to its velocity magnitude self-regulation motor M or reduce vacuum cup to ground Absorption affinity, and the moment update its coordinate (X, Y), and judge its coordinate whether (0,0), if if put the exploration mark that makes a return voyage Being 0, slight Mus enters the sprint stage, and juxtaposition spurt is masked as 1；

10) in order to realize the coordinate calculating accurately when spurt of slight Mus, when sensor S3 and S4 about slight Mus understands Carve labyrinth retaining wall and pillar to surrounding to detect, if S3 or S4 finds that sensor signal there occurs larger value of jumping Become, then illustrate that slight Mus enters from having labyrinth retaining wall to without labyrinth retaining wall (or from without labyrinth retaining wall to there being labyrinth retaining wall) The change of state, STM32F405 can according to slight Mus current operating conditions combine the feedback of magnetism encoder M1 and M2 by A3P250 accurately compensates, and thoroughly eliminates the error that slight Mus has added up when complex maze high speed is made a spurt；

11) if Mus runs into the wheel stall caused because of frame for movement during sound lunge or misreads labyrinth letter slightly During breath, it sometimes appear that hit the phenomenon of wall, now the electric current of motor will increase, when the electric current preset value of A3906SESTR-T exceedes During setting value, now STM32F405 can control A3P250 immediately and quits work, and not only reduces efficiently solving stall problem, And decrease the destruction to system hardware；

12) during slight Mus is made a spurt, A3P250 can carry out on-line identification to the torque of direct current generator, when the torque of motor is received During to external interference appearance bigger shake, compensation time controller can utilize the relation of motor torque and electric current to carry out, decrease The motor torque shake impact on slight Mus fast sprint；

13) in order to reduce the interference that slight Mus is made a spurt by light source, present invention adds photoelectric sensor L1, this sensor meeting In the slight Mus sprint stage, the abnormal light source of surrounding is read out, and automatically gives controller and do real-Time Compensation, outside eliminating The interference to spurt of boundary's light source；

14) becoming whole spurt process to arrive (X1, Y1) when slight pindone, slight Mus can be put return exploration and is masked as 1, and slight Mus returns Journey is explored and is returned to starting point (0,0), and STM32F405 will control the PWM ripple output of A3P250, and A3P250 adjusts A3906SESTR-T Input signal, A3906SESTR-T drive motor X, motor Y make slight Mus starting point central point stop, then A3P250 weight The new PWM ripple output adjusting motor so that motor X and motor Y moves in a reverse direction, and at sensor G1 (LY3200ALH) under control, rotating in place 180 degree, then stop 1 second, secondary transfers labyrinth information, then according to improving flood Water Algorithm for Solving optimum spurt path, then puts spurt and is masked as 1, and system enters the secondary fast sprint stage, then according to punching Thorn----explore---spurt, complete spurt repeatedly, to reach the purpose of fast sprint.

The invention have the advantages that:

1: resistance and electric capacity in the present invention all use 0402 encapsulation to instead of original 0603 encapsulation, can preferably reduce height The volume of speed Mus spurt servosystem slightly, the high speed of the most slight Mus and miniaturization；

2: STM32F405 of the present invention uses BGA package to instead of original LQFP176 encapsulation so that the volume that chip occupies is more Little, be conducive to reducing of the most slight Mus servo-control system volume, and BGA package is more conducive to chip during slight Mus sound lunge Heat radiation；

3: in order to fully improve the stability of double-core high-speed slight Mus spurt servo-control system, and take into account the excellent of two-wheel drive Point, the present invention has given up original many power real-time 4 wheel driven structure, realizes two axle four-wheel functions by gear mechanism structure, both subtracted Lack slight Mus controller and driven the number of power motor, achieved the function of many wheels further through gear, be effectively increased slightly The spurt driveability of Mus；

4: owing to STM32F405 is integrated with new DSP and FPU instruction, the high speed processing performance of 168MHz improves digital signal The execution speed of controller and code efficiency so that controller processes sensor signal real-time performance to be increased；

5: realize adsorption function as required；During slight Mus sound lunge, once run into road dust more or accelerate During situation, A3P250 can open the servo control of absorption motor M immediately according to the feedback of magnetism encoder M1 and M2 of two spindle motors System, slight Mus system is switched to three axle four-wheel drive states naturally, adhesive force when enhancing slight Mus sound lunge and manipulation Property；

6: owing to using two axle four-wheel drive structures, add Mus and the contact area on ground the most slightly, decrease two-wheeled micro- The generation of the stall problem that micro-Mus is caused due to frame for movement so that Mus slightly has more preferable sound lunge walking function；

7: at this, slight Mus sound lunge servosystem introduces high-performance MEMS motion sensor LY3200ALH, it is achieved that The detection of the Mus instantaneous rotary speed when labyrinth sound lunge slightly, and utilize feedback to realize the real time correction of whole process navigation, Stability when being conducive to improving slight Mus sound lunge and dynamic property；

8: when Mus sound lunge turns to slightly, in order to ensure the stability and the accuracy that rotate, by sensor G1 (LY3200ALH) Real-time Feedback, the sound lunge servo controller based on the A3P250 turning real time correction to slight Mus, carry The correctness that high labyrinth information is explored；

9: by FPGA(A3P250) output PWM modulation signal and direction signal, can directly drive dynamic triaxial by drive circuit straight Flow servo motor, not only alleviates the burden of STM32F405, simplifies interface circuit, and eliminates the internal volume of STM32F405 Writing position, speed controlling program, and the trouble of various pid algorithm so that the debugging of system is simple；

10: according to the difference of international rule, the change that controller can realize four groups of sensors and six groups of sensors by software is tied Structure switches, and improves slight Mus and solves the technology of labyrinth and sound lunge, the arithmetic speed of beneficially raising system；

11: owing to using the traditional photoelectric encoder technology of magnetism encoder technical substitution so that the spurt of Mus the most slightly Servo-control system volume can be less, the development of the most slight Mus miniaturization；

12: owing to using magnetism encoder to instead of traditional photoelectric encoder so that the dust data acquisition shadow to encoder Ring and be substantially reduced, improve slight Mus sound lunge speed and the accuracy of displacement, also ensure that slight Mus solves the standard in labyrinth Really property；

13: owing to this controller uses FPGA(A3P250) process the various algorithms of slight Mus spurt, effectively prevent program " race flies ", capacity of resisting disturbance is greatly enhanced；

14: during slight Mus sound lunge, FPGA(A3P250) can be to High-speed DC servomotor X, motor Y and motor M Torque carry out on-line identification and utilize motor torque to compensate with the relation of electric current, decrease motor torque shake to slightly The impact of Mus sound lunge；

15: can effectively regulate the vacuum cup absorption affinity to ground by regulation motor M, eliminate slight Mus at sound lunge Time skidding generation；

16: owing to having storage function, slight Mus can the most simply store the labyrinth information explored, and makes secondary explore Time and path are substantially reduced；

17: during slight Mus sound lunge, FPGA(A3P250) controller can be adjusted according to labyrinth situation around reality Internal pid parameter, easily realizes segmentation P, PD, PID and controls and nonlinear PID controller, make system have certain self adaptation Function；

18: in motor process, take into full account battery effect in this system, based on ARM (STM32F405)+FPGA (A3P250) running status of slight Mus is all being monitored and computing by the controller moment, it is to avoid the generation of big electric current, institute Fundamentally solving the impact to lithium ion battery of the big electric current, it is to avoid the lithium-ion electric caused due to heavy-current discharge The generation of pond overaging phenomenon；

The current acquisition function of 19: drive axle A3906SESTR-T can well solve slight Mus and meet during sound lunge To hitting the motor rotation blockage that the situations such as wall occur, when output is beyond setting value, the current collection circuit of A3906SESTR-T is immediately Work, direct current generator X, the driving signal of motor Y are pulled low, thus efficiently solve stall problem.

In sum, the double-core three axle four-wheel structure changes of present invention Mus sprint controller the most slightly, improve double-core micro- The stability of micro-Mus MPU Controlled All Digital Servo System, effectively prevent ground when slight Mus is made a spurt in high speed labyrinth and skids, it is to avoid The generation of Mus off-center position phenomenon far away, improves stability during its fast sprint slightly, adds slight Mus and ground The contact area in face, decreases the generation of the sound lunge stall problem that the slight Mus of two-wheeled is caused due to frame for movement so that micro- Micro-Mus has more preferable fast sprint function.

The foregoing is only embodiments of the invention, not thereby limit the scope of the claims of the present invention, every utilize this Equivalent structure or equivalence flow process that bright description is made convert, or are directly or indirectly used in other relevant technology neck Territory, is the most in like manner included in the scope of patent protection of the present invention.

Claims (8)

1. a double-core three axle four-wheel structure changes Mus sprint controller the most slightly, it is characterised in that include slight Mus housing, car Wheel, the first infrared sensor, the 3rd infrared sensor, the 4th infrared sensor, the 6th infrared sensor, the first High-speed DC are watched Take motor, the second High-speed DC servomotor, vac sorb motor, the first magnetism encoder, the second magnetism encoder, motion biography Sensor and sampling sensor, four described wheels are arranged on the limit, the left and right sides of slight Mus housing the most two-by-two, described Before first infrared sensor and the 6th infrared sensor are separately positioned on the limit, the left and right sides of slight Mus housing and are positioned at wheel End, the 3rd described infrared sensor and the 4th infrared sensor are the most angularly disposed infrared at the first infrared sensor and the 6th The inner side edge of sensor, the first described High-speed DC servomotor and the second High-speed DC servomotor are separately mounted to slightly The right and left of Mus housing the position between two wheels, described vac sorb motor is arranged on the first High-speed DC Centre position above servomotor and the second High-speed DC servomotor, the first described magnetism encoder and the second magnetoelectricity are compiled Code device is separately positioned on the first High-speed DC servomotor and the lower section of the second High-speed DC servomotor, described motion-sensing Device and sampling sensor are successively set on the lower section of vac sorb motor, also include that panel, described panel are arranged on micro- In micro-Mus housing, described panel uses dual-core controller, communicates even including ARM and FPGA, described ARM Yu FPGA Connect.

Double-core three axle four-wheel structure changes the most according to claim 1 Mus sprint controller the most slightly, it is characterised in that institute Corner dimension when the 3rd sensor stated and the 4th sensor are angularly disposed and between Y-axis is:。

Double-core three axle four-wheel structure changes the most according to claim 1 Mus the most slightly explores controller, it is characterised in that institute The ARM stated uses STM32F405 controller, described FPGA to use A3P250 controller.

Double-core three axle four-wheel structure changes the most according to claim 1 Mus sprint controller the most slightly, it is characterised in that institute The first magnetism encoder stated and the second magnetism encoder all use encoder based on magnetoelectric transducer AS5040H.

Double-core three axle four-wheel structure changes the most according to claim 1 Mus sprint controller the most slightly, it is characterised in that institute It is provided with vacuum cup on the wheel stated.

Double-core three axle four-wheel structure changes the most according to claim 1 Mus sprint controller the most slightly, it is characterised in that institute The panel stated sends the first control signal, the second control signal and the 3rd control signal respectively, by the first described control letter Number, the second control signal and the 3rd control signal control described the second High-speed DC servomotor, the first High-speed DC respectively The motion of slight Mus is controlled again after the signal syntheses of servomotor and vac sorb motor.

Double-core three axle four-wheel structure changes the most according to claim 1 Mus sprint controller the most slightly, it is characterised in that institute The double-core three axle four-wheel structure changes stated Mus sprint controller the most slightly is additionally provided with host computer procedure and motion control program, institute The host computer procedure stated includes that path is read, labyrinth updates, labyrinth stores and parameter output, and described motion control program includes Based on ARM+FPGA tri-axle four-wheel slight Mus spurt SERVO CONTROL, location, labyrinth and direction controlling.

Double-core three axle four-wheel structure changes the most according to claim 7 Mus sprint controller the most slightly, it is characterised in that institute That states also includes interconnective based on structure changes two axle four-wheel based on ARM+FPGA tri-axle four-wheel slight Mus spurt SERVO CONTROL Mus labyrinth spurt SERVO CONTROL and single axle vacuum sucker suction SERVO CONTROL slightly, described is slight based on structure changes two axle four-wheel Mus labyrinth spurt SERVO CONTROL includes based on minimum sense samples system spurt position module, based on minimum sense samples system punching Sting acceleration module and based on minimum sense samples system spurt acceleration module；Described single axle vacuum sucker suction SERVO CONTROL Including sucker position module, sucker acceleration module and sucker acceleration module.