CN105974923A - High-speed picomouse exploration controller with double-core three-axis four-wheel variable structure - Google Patents

Abstract

The invention discloses a high-speed picomouse exploration controller with a double-core three-axis four-wheel variable structure. The high-speed picomouse exploration controller comprises a picomouse shell, wheels, a first infrared sensor, a second infrared sensor, a third infrared sensor, a fourth infrared sensor, a fifth 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 control panel, wherein the control panel adopts a double-core controller and comprises an ARM and an FPGA in communication connection. By adopting the high-speed picomouse exploration controller, the stability of a double-core picomouse all-digital servo system is improved, ground slippage of a picomouse during high-speed maze exploration is effectively prevented, the phenomenon that the picomouse further deviates from the central position is avoided, meanwhile, the speed and the shift accuracy of the picomouse are improved, and the accuracy that the picomouse explores a maze is guaranteed.

Description

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

Technical field

The present invention relates to a kind of double-core three axle four-wheel structure changes Mus (PICOMOUSE) the most slightly automatically control in exploration labyrinth System, belongs to minisize maze robot 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, the walking in whole labyrinth of the slight Mus 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 slight Mus exploration solves labyrinth and occupies non-the whole motion of slight Mus The most important position, once Mus explores labyrinth failure slightly, and the whole function of slight Mus the most just cannot realize.

Mus slightly to judge the environment of surrounding in labyrinth the moment in heuristic process, then communicates a parameter to controller, by Controller repetitive control its accurate acceleration and retarded motion in the grid of labyrinth.One outstanding slight Mus is explored labyrinth and becomes Merit solves labyrinth must possess good perception, has good locomotor activity, outstanding intelligent algorithm, otherwise will be unable to One-tenth task.If using existing simple algorithm and structure to realize slight Mus explore labyrinth, 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 uses six groups of sensor technologies in Fig. 1 to explore the most slightly Miscellaneous labyrinth, at some to exploring in the international rule having time requirement, is affected by single core processor processing speed, is often occurred The phenomenon generation that the exploration time is longer, ultimately results in slight Mus and competes unsuccessfully；

(3) some simple Mus model machine monokaryon servosystem slightly use the chip than lower level and algorithm so that Mus is fan slightly Exploration in the middle of palace typically will spend the longer time, not only consumes the energy of a large amount of battery, and in real contest In also cannot win victory；

(4) due to the minimizing of labyrinth retaining wall size so that Mus list lattice slightly explore the distance minimizing run, and slight Mus was being explored Frequently braking in journey and startup have increased the weight of the workload of monokaryon controller, and monokaryon controller cannot meet slight Mus rapid discovery The requirement started and stop；

(5) for the slight Mus of two-wheel drive, general two the motor PWM control signals requiring to drive its motion to synchronize, Limited monokaryon servosystem by computing capability and be difficult to meet this condition, can not accurately be gone when slight Mus travels on straight way Walk on center line, be easy to bump against labyrinth retaining wall when walking at a high speed, cause mission failure；

(6) owing to being affected by monokaryon controller capacity and algorithm, slight Mus cannot store labyrinth information, when running into power-down conditions All of information will disappear, and this makes whole heuristic process restart；

(7) monokaryon controls slight Mus when walking in labyrinth, it is easy to by external interference, due to compensate the most in time cause micro- Micro-Mus collision labyrinth retaining wall, finally cannot complete task；

(8) move after two-wheeled slight Mus exploration servosystem is when accelerating to explore due to center of gravity so that mouse front portion is light, even if On good road surface, 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 exploration servo-control system designs improper inclined before causing center of gravity, will cause bearing on driving wheel Normal pressure reduce, at this moment slightly Mus system is more prone to skid, and is also easier to wander off, causes navigating unsuccessfully；

(10) if the slight Mus of two-wheeled is explored servosystem and designs and improper cause center of gravity lateral deviation two driving wheels will to be caused to bear Normal pressure is different, and when quickly starting, two-wheeled skid level is inconsistent, and moment deflects away from track, and during turning, wherein normal pressure is little Wheel may skid, cause cornering difficulties；

(11) explore servosystem many employings photoelectric encoder due to traditional slight Mus and realize the speed of mouse and the anti-of position Feedback, owing to the volume of photoelectric encoder is bigger so that the volume of Mus slightly is relatively large, is unfavorable for that the miniaturization of 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 running is easy to skid, cause the labyrinth information errors of search, final Mus slightly to complete Exploration task；

(13) exploring the integrated drive chips volume of servosystem employing relatively greatly due to the slight Mus of tradition, the volume of slight Mus cannot Miniaturization and weight are relatively big, cannot obtain sufficiently large acceleration, adding of system under equal-wattage DC servo motor drives Speed performance is more weak；

(14) the slight Mus MPU Controlled All Digital Servo System controlled based on monokaryon should process various photo-sensor signal and labyrinth letter Breath, and the multiple-axis servo also processing slight Mus controls so that the workload of processor is relatively big, very big have impact on slight Mus The raising of speed and stability.

Slightly Mus is explored and solves labyrinth 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 structure changes Mus exploration labyrinth the most slightly of a kind of double-core three axle four wheel belt vac sorb and watch Take controller.

Summary of the invention

The technical problem that present invention mainly solves is to provide a kind of double-core three axle four-wheel structure changes Mus the most slightly and explores control Device processed, for overcoming monokaryon controller can not meet stability and the requirement of rapidity when slight Mus is explored at a high speed, has 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)+FPGA (A3P250) the brand-new double-core three axle slight Mus of four-wheel structure changes explore servo controller, panel with FPGA(A3P250) producing three axle servosystem PWM ripples for process core, STM32F405 converts external environment backward A3P250 sends the command value such as position, speed, acceleration, and A3P250 generates two axles in conjunction with the feedback of magnetism encoder M1 and M2 Explore the PWM ripple of servo-control system, amplify the slight Mus of rear drive through drive axle A3906SESTR-T and advance at utmost speed. 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.

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 explores controller, including slight Mus housing, wheel, the first infrared sensor, the second infrared sensor, 3rd infrared sensor, the 4th infrared sensor, the 5th infrared sensor, the 6th infrared sensor, the first High-speed DC servo Motor, the second High-speed DC servomotor, vac sorb motor, the first magnetism encoder, the second magnetism encoder and motion Sensor, four described wheels are arranged on the limit, the left and right sides of slight Mus housing, the first described infrared sensing the most two-by-two Device and the 6th infrared sensor are separately positioned on the limit, the left and right sides of slight Mus housing and are positioned at the front end of wheel, described second Infrared sensor and the 5th infrared sensor are arranged on the front end of slight Mus housing, and the 3rd described infrared sensor is angularly disposed Between the first infrared sensor and the second infrared sensor, the 4th described infrared sensor is angularly disposed in the 5th infrared biography Between sensor and the 6th infrared sensor, the first described High-speed DC servomotor and the second High-speed DC servomotor are respectively Being arranged on the right and left of slight Mus housing the position between two wheels, described vac sorb motor is arranged on Centre position above one High-speed DC servomotor and the second High-speed DC servomotor, the first described magnetism encoder and Second magnetism encoder is separately positioned on the first High-speed DC servomotor and the lower section of the second High-speed DC servomotor, described Motion sensor be arranged on the lower section of vac sorb motor, wherein, the 3rd described sensor and the 4th sensor is oblique sets Corner dimension when putting and between Y-axis is:, also include that panel, described panel are arranged on slight Mus In housing, described panel uses dual-core controller, is communicatively coupled including ARM and FPGA, described ARM with FPGA.

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 double-core three axle four-wheel structure changes Mus the most slightly explores controller Also including supply unit, described supply unit panel electric current driving described in is provided separately, described panel is respectively Send the first control signal, the second control signal and the 3rd control signal, by the first described control signal, the second control signal Control described the second High-speed DC servomotor, the first High-speed DC servomotor and vacuum respectively with the 3rd control signal to inhale The motion of slight Mus is controlled again after the signal syntheses of attached motor.

In a preferred embodiment of the present invention, described supply unit uses lithium ion battery.

In a preferred embodiment of the present invention, described double-core three axle four-wheel structure changes Mus the most slightly explores controller Being additionally provided with host computer procedure and motion control program, described host computer procedure includes that Maze Exploration, labyrinth update, labyrinth is deposited Storage and online output, described motion control program includes exploring SERVO CONTROL, seat based on the ARM+FPGA tri-slight Mus of axle four-wheel Demarcate position and I/O controls.

In a preferred embodiment of the present invention, described explores SERVO CONTROL based on the ARM+FPGA tri-slight Mus of axle four-wheel Also include interconnective watching based on structure changes two axle four-wheel slight Mus Maze Exploration SERVO CONTROL and single axle vacuum sucker suction Clothes control, and described includes exploring position mould based on structure changes based on structure changes two axle four-wheel slight Mus Maze Exploration SERVO CONTROL Block, based on structure changes explore acceleration module and based on structure changes explore acceleration module；Described single axle vacuum sucker suction is watched Take control and include 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 the most slightly explores controller, carries The high stability of double-core slight Mus MPU Controlled All Digital Servo System, effectively prevent the slight Mus ground when high speed Maze Exploration and beats Sliding, it is to avoid the generation of slight Mus off-center position phenomenon far away, improve stability during its rapid solving, improve simultaneously The speed of slight Mus and the accuracy of displacement, also ensure that slight Mus explores the accuracy in labyrinth.

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 traditional two wheel drive slight Mus X-Y scheme；

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

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

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

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

Fig. 6 is based on double-core three axle four-wheel structure changes slight Mus heuristic routine block diagram；

Fig. 7 is slight Mus advance schematic diagram；

Fig. 8 is that slight Mus retreats schematic diagram；

Fig. 9 is that meaning figure is entered on the slight Mus right side；

Figure 10 is that schematic diagram is moved back on a slight Mus left side；

Figure 11 Velocity-time runs ladder diagram.

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 the most slightly explores controller, including slight Mus housing, wheel, the first infrared biography Sensor S1, the second infrared sensor S2, the 3rd infrared sensor S3, the 4th infrared sensor S4, the 5th infrared sensor S5, Six infrared sensor S6, the first High-speed DC servomotor Y, the second High-speed DC servomotor X, vac sorb motor M, first Magnetism encoder M1, the second magnetism encoder M2 and motion sensor G1, four described wheels are arranged on micro-the most two-by-two The limit, the left and right sides of micro-Mus housing, the first described infrared sensor S1 and the 6th infrared sensor S6 is separately positioned on slight Mus The limit, the left and right sides of housing is also positioned at the front end of wheel, and the second described infrared sensor S2 and the 5th infrared sensor S5 is arranged In the front end of slight Mus housing, the 3rd described infrared sensor S3 is angularly disposed infrared at the first infrared sensor S1 and second Between sensor S2, the 4th described infrared sensor S4 is angularly disposed at the 5th infrared sensor S5 and the 6th infrared sensor Between S6, the first described High-speed DC servomotor Y and the second High-speed DC servomotor X is separately mounted to slight Mus housing The right and left and position between two wheels, described vac sorb motor M is arranged on the first High-speed DC servo Centre position above motor Y and the second High-speed DC servomotor X, the first described magnetism encoder M1 and the second magnetoelectricity are compiled Code device M2 is separately positioned on the first High-speed DC servomotor Y and the lower section of the second High-speed DC servomotor X, described motion Sensor G1 is arranged on the lower section of vac sorb motor M.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 this enforcement, also including that panel, described panel are arranged in slight Mus housing, described panel uses Dual-core controller, 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 the most slightly is explored controller and also includes supply unit, Panel electric current driving described in being provided separately by described supply unit, described panel sends the first control letter respectively Number, the second control signal and the 3rd control signal, by the first described control signal, the second control signal and the 3rd control signal The signal controlling described the second High-speed DC servomotor, the first High-speed DC servomotor and vac sorb motor respectively closes The motion of slight Mus is controlled again after one-tenth.Wherein, described supply unit uses lithium ion battery.

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 explores the volume of servo-control system to reduce the slight Mus of double-core high-speed, has given up two traditional axles straight Flow servo 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, two axles that These characteristics makes A3906SESTR-T be particularly suitable for slight Mus explore 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 explore PWM control defeated Entering signal and exploration enables signal and comes from servo controller, controller controls input signal by adjustment exploration PWM and adjusts It is explored PWM and controls output signal, then realizes the four-quadrant motion of DC servo motor.

As shown in Figure 6, described double-core three axle four-wheel structure changes Mus the most slightly is explored controller and is additionally provided with host computer Program and motion control program, described host computer procedure includes that Maze Exploration, labyrinth update, labyrinth stores and exports online, Described motion control program includes exploring SERVO CONTROL, coordinate setting and I/O control based on the ARM+FPGA tri-slight Mus of axle four-wheel System.

Wherein, described based on the ARM+FPGA tri-slight Mus of axle four-wheel explore SERVO CONTROL also include interconnective based on Structure changes two axle four-wheel slight Mus Maze Exploration SERVO CONTROL and single axle vacuum sucker suction SERVO CONTROL, described ties based on becoming Structure two axle four-wheel slight Mus Maze Exploration SERVO CONTROL includes exploring position module based on structure changes, exploring speed based on structure changes Module and based on structure changes explore acceleration module；Described single axle vacuum sucker suction SERVO CONTROL includes sucker position mould Block, sucker acceleration module and sucker acceleration module.

Explore the stability of servo-control system in order to improve the slight Mus of double-core high-speed, increase contacting of slight Mus and ground Area, reduces the reach of slight Mus center of gravity, rear shifting or sidesway, and the present invention realizes two axle four-wheels by gear mechanism structure Function, the rotating shaft of the every spindle motor in left and right is equipped with the canine tooth turbine of a little mechanical gear, this gear and former and later two wheels Tool connects, and 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 Son all becomes power wheel.

Explore the stability of servosystem in order to improve the slight Mus of double-core high-speed further, prevent slight Mus from exploring at a high speed Time due to floor-dust more and cause walking to be skidded, the present invention slight Mus explore in servo hardware system add miniature directly Stream motor M, in slight Mus motor process, motor M does not stop to aspirate the air in micro vacuum sucker by vacuum suction apparatus, The external and internal pressure making micro vacuum sucker is different, produces certain negative pressure so that it is the ground, labyrinth with dust is produced one Fixed absorption affinity, effectively prevent the slight Mus ground when high speed Maze Exploration and skids.

In order to improve the double-core high-speed slight Mus stability when exploring labyrinth further, the present invention explores slight Mus and watches 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 exploration labyrinth 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 rapid discovery.

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, and explore at a high speed three Close loop servo control provides for slight Mus Reliable basis.

The present invention is to overcome monokaryon controller can not meet stability and the requirement of rapidity, house when slight Mus is explored at a high speed 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 The brand-new double-core three axle slight Mus of four-wheel structure changes of based on ARM (STM32F405)+FPGA (A3P250) explores SERVO CONTROL Device.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 M1 and M2 generates two axles and explores the PWM ripple of servo-control system, amplifies through drive axle A3906SESTR-T The slight Mus of rear drive advances at utmost speed.STM32F405 frees in the middle of complicated work, it is achieved the signal processing algorithm of part Response with A3P250 is interrupted, it is achieved data communication and storage live signal.

The present invention takes techniques below scheme, in order to improve arithmetic speed, it is ensured that three axle four-wheel structure changes band vac sorbs The stability of slight Mus servosystem and reliability, the present invention introduces A3P250 in controller based on STM32F405, is formed Double-core based on STM32F405+A3P250 explores controller, and this controller takes into full account the battery effect in this system, The three axle servosystem that in control system, workload is maximum (watch by two axle slight Mus walking servosystem+single axle vacuum suction motor Clothes control) give A3P250 processs, give full play to A3P250 as the process fast feature of data speed, and man machine interface, labyrinth The control function such as exploration, labyrinth renewal, labyrinth storage, online output is given STM32F405 and is completed, and thus achieves The division of labor of STM32F405 Yu A3P250, 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.Once visit After rope start key is opened, slight Mus is opened six groups of sensors according to software design patterns and explores pattern or four groups of sensors exploration moulds Formula, slight Mus relies on front, left and right side to keep in obscurity infrared sensor S1, S2, S3, S4, S5, S6(or S1, S3, S4, S6) in real time Detect actual navigational environment, and transmit parameter to STM32F405, STM32F405 these rings by infrared remote receiver BPW85A Border parameter is converted into position, speed and the acceleration parameter command value of slight Mus motion, and then with A3P250 communication, A3P250 ties The feedback information closing outside magnetism encoder M1 and M2 generates PWM waveform and the control direction letter of two axle direct current generator servosystem Number, then STM32F405 adjusts the input signal of A3906SESTR-T, and A3906SESTR-T changes its output pin state to be completed Servomotor X and the SERVO CONTROL of motor Y, motor operating state feeds back to STM32F405 through magnetoelectricity photoelectric coder M1 and M2, by STM32F405 adjusts the pin output state of A3906SESTR-T again after processing, move before driving motor X, motor Y-direction, In motor process, the situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and watching by regulation motor M Clothes control effectively to regulate the vacuum cup absorption affinity to ground, add stability when slight Mus is explored at a high speed.

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

Slight Mus control system is divided into two parts: master system and kinetic control system.Wherein master system completes fan The functions such as palace exploration, coordinate setting, online output；Kinetic control system completes three axle SERVO CONTROL of slight Mus system, data The functions such as storage, I/O control, wherein three axle direct current generator servosystem of workload maximum give A3P250 process, remaining Including master system complete give STM32F405 and complete, thus achieve the division of labor of STM32F405 Yu A3P250, with Time can also carry out therebetween communication, carry out data exchange in real time 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) turning on the power switch, power circuit, sensor circuit, clock circuit etc. can be detected by STM32F405, determine and are System hardware without difference after, A3P250 first according to accelerate need open vacuum draw motor M, by aspirator first to miniature very Suction dish aspirates, and makes vacuum cup mask over the ground have certain absorption affinity, and controller also detects in real time, if ground is not clean, System can strengthen the vacuum cup absorption affinity to ground by self-regulation motor M；Before Mus does not receives exploration order slightly, it is general Can wait the exploration order that send of STM32F405 at starting point coordinate (0,0), after once activate switch starts, slight Mus can be along Starting point start to terminal (X1, Y1), (X2, Y2), (X3, Y3), (X4, Y4) explore, wherein (X1, Y1), (X2, Y2), (X3, Y3), the value of (X4, Y4) be arbitrarily, generally X1=X2=X3=X4, Y1=Y2=Y3=Y4, i.e. detect is appointing in labyrinth Anticipate a maze lattice (X1, Y1)；

2) slightly Mus is placed on starting point coordinate (0,0), receive explore its front after order sensor S1, S6 and can be to the ring in front Border judges, determining and enters range of movement either with or without barricade, will send interruption to STM32F405 and A3P250 as there is barricade Request, STM32F405 and A3P250 can be to interrupting doing very first time response, and then STM32F405 forbids that A3P250 works, A3P250 blocks the motor X of slight Mus, the PWM drive signal of motor Y so that it is being still in original place, then secondary judges that labyrinth is true Determine front information, prevent information from judging by accident；

3) if all gone well, exploring at micro computer and starting moment, independent red of sensor S1, S2, S3, S4, S5, S6(six The infrared light that emission pipe SFH4350 sends is received the information being converted into labyrinth around after device BPW85A accepts) judge around Environment and give STM32F405, it is to be run that STM32F405 is converted into slight Mus four-wheel all around these ambient parameters Position, speed and acceleration command value, then STM32F405 Yu A3P250 communication, by A3P250 according to these parameters in conjunction with The feedback of magnetism encoder M1 and M2 generates the PWM wave controller signal driving two axle DC servo motor X and motor Y；PWM ripple Travel before drive axle A3906SESTR-T amplifies rear drive motor X, motor Y-direction, until completing accelerator until reaching to visit Rope setting speed, in accelerating exploration walking process, the situations such as the slight Mus speed of travel, ground are entered by A3P250 controller in real time Row detection, and effectively regulate the vacuum cup absorption to ground by the SERVO CONTROL of regulation motor M, and lead to processing data Interrogate to STM32F405, STM32F405 continue with follow-up running status, then determine whether to prohibit according to exploration speed The only servo regulation of motor M；

4) travel forward along Y-axis slight Mus, in any one side's center of a lattice if it is determined that do not have barricade to enter front Range of movement, then Mus slightly will store its coordinate (X, Y), and the location parameter of the lattice that travel forward is transferred to STM32F405, According to exploring 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 Close the feedback of magnetism encoder M1 and M2 to generate and drive 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 explored going 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, STM32F405 record stores current labyrinth retaining wall information, and slight Mus determines it according to the labyrinth information of left and right barricade Enter single wall navigation pattern, double wall navigation pattern or inertial navigation pattern；Then sensor G1 (LY3200ALH) examines in real time Instantaneous rotary speed when the micro-Mus of micrometer is explored, when slight Mus is explored at a high speed departing from when setting center, A3P250 according to The deviation size leaving center proceeds by real-Time Compensation, the PWM ripple of fine setting motor by sensor G1 (LY3200ALH) Input, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, defeated Entering after signal is amplified by drive axle A3906SESTR-T promotes slight Mus to travel forward, and can accurately adjust micro-by this mode Micro-Mus is in the position of straight way and attitude so that it is come back to set center；If in exploring motor process, slight Mus goes out When existing driving wheel stall or the more situation of dust, the SERVO CONTROL of A3P250 Secondary Control motor M at once is effectively adjusted The joint 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 lattice apart from arriving new address under dual-core controller control Time, will update its coordinate is (X, Y+1), at Y+1 < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if not inciting somebody to action Continuing to update its coordinate, if it is notification controller has searched target, then puts exploration of making a return voyage and is masked as 1, slight Mus Prepare return to explore；

5) if slight Mus along the range of movement having in Y-axis forward movement labyrinth retaining wall to enter front, and now Sensor S3, S4(or S2 of left and right, S3, S4, S5) judge about when having barricade, slight Mus will store now coordinate (X, Y), Feedback STM32F405 according to sensor S1 and S6 calculates the location parameter S1 stopped that travels forward, and STM32F405 is this Parameter according to time requirement be converted into location parameter, speed parameter and acceleration parameter be transferred to A3P250, A3P250 according to These parameters generate driving motor X and the pwm control signal of motor Y, A3P250 in conjunction with the feedback of magnetism encoder M1 and M2 With A3906SESTR-T communication input signal IN1 that adjusts A3906SESTR-T, IN2, IN3 and IN4, input signal is by driving Dynamic bridge A3906SESTR-T promotes slight Mus to stop forward after amplifying, and A3P250 is come by the SERVO CONTROL adjusting motor M simultaneously Strengthen the friction to ground, prevent slight Mus because exploration speed is too high and bump against the labyrinth retaining wall in front；Slight Mus forward In docking process, sensor S3, S4(or S2, S3, S4, S5) in real time the barricade of left and right is judged, and feed back current labyrinth Barricade information, slight Mus enters double wall navigation pattern, and slight Mus realizes arranging stop parking；Then A3P250 adjusts Input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input signal is made after being amplified by drive axle A3906SESTR-T Obtaining motor X and motor Y motion is in opposite direction, turnback is adjusted in slight Mus original place, is ready for along Y-axis adverse movement；

Reversely travel forward along Y-axis slight Mus, in any one side's center of a lattice if it is determined that do not have barricade to enter front Range of movement, then slightly Mus will store its coordinate (X, Y), and the location parameter of the lattice that travel forward is transferred to STM32F405, according to exploring 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 driving DC servo motor X and the PWM ripple of Y motion, A3P250 in conjunction with the feedback of magnetism encoder M1 and M2 With A3906SESTR-T communication input signal IN1 that adjusts A3906SESTR-T, IN2, IN3 and IN4, input signal is by driving Dynamic bridge A3906SESTR-T promotes slight Mus to travel forward after amplifying；In slight Mus forward movement, magnetoelectric transducer M1 Feed back, with the M2 moment, speed and the displacement that motor X and motor Y runs, and be transferred to A3P250；Slight Mus along current labyrinth During lattice explored going forward, the situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and by regulation motor The SERVO CONTROL of M effectively regulates the vacuum cup absorption to ground, and the labyrinth of left and right can be kept off by sensor S2, S3 and S4, S5 Wall judges, STM32F405 record stores current labyrinth retaining wall information, and slight Mus determines according to the labyrinth information of left and right barricade It enters single wall navigation pattern, double wall navigation pattern or inertial navigation pattern；Then sensor G1 (LY3200ALH) is real-time Detect instantaneous rotary speed when slight Mus is explored, when slight Mus rapid discovery is departing from setting center, A3P250 root Proceed by real-Time Compensation according to the deviation size leaving center by sensor G1 (LY3200ALH) and finely tune motor PWM ripple inputs, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 with IN4, input signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, can be accurate by this mode Adjust slight Mus in the position of straight way and attitude so that it is come back to set center；If it is in exploring motor process, micro- When driving wheel stall or dust more situation occurs in micro-Mus, the SERVO CONTROL of A3P250 Secondary Control motor M at once is come The effectively regulation vacuum cup absorption affinity to ground, STM32F405 is converted into new reference instruction value transmission remaining distance New in conjunction with feedback generation driving motor X, the motor Y of magnetism encoder M1 and M2 according to these parameters to A3P250, A3P250 PWM drive signal, 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 at three axle four wheel drivings Still advance according to original navigation pattern under dynamic state；When slight Mus moves lattice apart from arriving under dual-core controller control During new address, will update its coordinate is (X, Y-1), Y-1 < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if Not being that continuation is updated its coordinate, if it is notification controller has searched target, then puts exploration of making a return voyage and is masked as 1, Mus slightly prepares return exploration；

6) if slight Mus along the range of movement having in Y-axis forward movement labyrinth retaining wall to enter front, and now Sensor S3, S4(or S2 of left and right, S3, S4, S5) judge when there is barricade on a left side, slight Mus will store now coordinate (X, Y), according to The feedback STM32F405 of sensor S1 and S6 calculates the location parameter S2, STM32F405 stopped that travel forward this parameter Being converted into location parameter, speed parameter and acceleration parameter according to time requirement and be transferred to A3P250, A3P250 is according to these Parameter generates in conjunction with the feedback of magnetism encoder M1 and M2 and drives motor X and the pwm control signal of motor Y, 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 stop forward after amplifying, and A3P250 is added by the SERVO CONTROL adjusting motor M simultaneously The big friction to ground, prevents slight Mus because exploration speed is too high and bumps against the labyrinth retaining wall in front；Stop forward slight Mus During car, sensor S3(or S2, S3) in real time left barricade is judged, and feed back current labyrinth retaining wall information, slight Mus Entering single left wall navigation pattern, slight Mus realizes arranging stop parking；Then A3P250 adjusts the defeated of A3906SESTR-T Entering signal IN1, IN2, IN3 and IN4, input signal makes motor X and motor Y transport after being amplified by drive axle A3906SESTR-T Dynamic in opposite direction, slight Mus original place is adjusted to the right and is turn 90 degrees, and is ready for along X-axis positive movement；

Travel forward along X-axis slight Mus, in any one side's center of a lattice if it is determined that do not have barricade to enter the fortune in front Dynamic scope, then Mus slightly will store its coordinate (X, Y), and the location parameter of the lattice that travel forward is transferred to STM32F405, root According to exploring controller speed and the requirement of acceleration, by the internal three closed loop servo system Program Generating two axle direct currents of STM32F405 The movement instruction set-point of servomotor X and Y, then STM32F405 Yu A3P250 communication transmit data, A3P250 in conjunction with The feedback of magnetism encoder M1 and M2 generates and drives DC servo motor X and the PWM ripple of Y motion, A3P250 and A3906SESTR- T communication also adjusts input signal IN1 of A3906SESTR-T, and IN2, IN3 and IN4, input signal passes through drive axle A3906SESTR-T promotes slight Mus to travel forward after amplifying；In slight Mus forward movement, magnetoelectric transducer M1 and M2 The speed of moment feedback motor X and motor Y operation and displacement, and it is transferred to A3P250；Slight Mus along current maze lattice to In front heuristic process, the situations such as the slight Mus speed of travel, ground are detected by A3P250 in real time, and by regulation motor M's SERVO CONTROL effectively regulates the vacuum cup absorption to ground, and the labyrinth retaining wall of left and right can be entered by sensor S2, S3 and S4, S5 Row judges, STM32F405 record stores current labyrinth retaining wall information, according to the labyrinth information of left and right barricade, slight Mus determines that it enters Enter single wall navigation pattern, double wall navigation pattern or inertial navigation pattern；Then sensor G1 (LY3200ALH) detects in real time Instantaneous rotary speed when slightly Mus is explored, when slight Mus rapid discovery 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 Mus is in the position of straight way and attitude so that it is come back to set center；If in exploring 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 M1 and M2 feedback generate drive motor X, the new PWM of motor Y Driving signal, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 with IN4, input signal promotes slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, and slight Mus is at three axle four wheel drivings Still advance according to original navigation pattern under dynamic state；When slight Mus moves lattice apart from arriving under dual-core controller control During new address, will update its coordinate is (X+1, Y), X+1 < on the premise of 1F, it is judged that its coordinate is (X1, Y1), if Not being that continuation is updated its coordinate, if it is notification controller has searched target, then puts exploration of making a return voyage and is masked as 1, Mus slightly prepares return exploration；

7) if slight Mus along the range of movement having in Y-axis forward movement labyrinth retaining wall to enter front, and now Sensor S3, S4(or S2 of left and right, S3, S4, S5) judge when there is barricade on the right side, slight Mus will store now coordinate (X, Y), according to The feedback STM32F405 of sensor S1 and S6 calculates the location parameter S3, STM32F405 stopped that travel forward this parameter Being converted into location parameter, speed parameter and acceleration parameter according to time requirement and be transferred to A3P250, A3P250 is according to these Parameter generates in conjunction with the feedback of magnetism encoder and drives motor X and the pwm control signal of motor Y, 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 stop forward after amplifying, and A3P250 is added by the SERVO CONTROL adjusting motor M simultaneously The big friction to ground, prevents slight Mus because exploring excessive velocities and bumps against the labyrinth retaining wall in front；Stop forward slight Mus During car, sensor S4(or S4, S5) in real time right barricade is judged, and feed back current labyrinth retaining wall information, slight Mus Entering single right wall navigation pattern, slight Mus realizes arranging stop parking；Then A3P250 adjusts the defeated of A3906SESTR-T Entering signal IN1, IN2, IN3 and IN4, input signal makes motor X and motor Y transport after being amplified by drive axle A3906SESTR-T Dynamic in opposite direction, slight Mus original place is adjusted to the left and is turn 90 degrees, and is ready for along X-axis adverse movement；

Reversely travel forward along X-axis slight Mus, in any one side's center of a lattice if it is determined that do not have barricade to enter front Range of movement, then slightly Mus will store its coordinate (X, Y), and the location parameter of the lattice that travel forward is transferred to STM32F405, according to exploring 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 explored going 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 when Mus is explored, when slight Mus rapid discovery 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 exploring motor process, driving occurs in slight Mus When wheel stall or the more situation of dust, the SERVO CONTROL of A3P250 Secondary Control motor M at once effectively regulates vacuum The sucker 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 drive signal of motor Y, A3P250 Yu A3906SESTR-T communication also finely tunes input signal IN1 of A3906SESTR-T, IN2, IN3 and IN4, input signal Promoting slight Mus to travel forward after being amplified by drive axle A3906SESTR-T, slight Mus remains unchanged under three axle four-wheel drive states Advance according to original navigation pattern；When slight Mus moves a lattice distance new address of arrival under dual-core controller control, will Updating its coordinate is (X-1, Y), 0 < X-1 < 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；

8) when slight Mus arrives (X1, Y1) preparation return exploration, controller can recall the labyrinth that it is the most stored, then basis Quickly labyrinth algorithm calculates optimal path that may be present, and return initially enters 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 slight Mus explore time accurately coordinate calculate, sensor S3(or S2 about slight Mus, S3) With S4(or S4, S5) can the moment labyrinth retaining wall and the pillar of surrounding be detected, if S3(or S2, S3) or S4(or S4, S5) find that sensor signal there occurs larger value of transition, then illustrate that slight Mus enters from having labyrinth retaining wall to without labyrinth The change of barricade (or from without labyrinth retaining wall to there being labyrinth retaining wall) state, STM32F405 currently can run according to slight Mus The feedback of combinations of states magnetism encoder M1 and M2 is accurately compensated by A3P250, thoroughly eliminates slight Mus in complex maze The error added up during exploration；

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

12) Mus can detect cell voltage at running in the moment slightly, and when low pressure occurs in system, sensor V1 will open concurrently Go out alarm, be effectively protected lithium ion battery；

13) in slight Mus running, A3P250 can carry out on-line identification to the torque of motor, when the torque of motor is by outward When there is bigger shake in boundary's interference, compensation time controller can utilize the relation of motor torque and electric current to carry out, decrease motor The torque shake impact on slight Mus rapid discovery, it is ensured that the accuracy in labyrinth during exploration；

14) returning to starting point (0,0) when micro computer completes whole heuristic process, the interruption of response A3P250 is rung by STM32F405 Two spindle motor X and Y should be controlled slow down simultaneously and make micro computer central point stop, then readjust the PWM ripple output of A3P250, so Rear A3P250 adjusts the input signal of A3906SESTR-T, and A3906SESTR-T drives navigation motor X, motor Y with contrary side To motion, slight Mus rotates in place 180 degree, then stops 1 second；STM32F405 transfers labyrinth information, then according to improving flood Algorithm for Solving goes out optimal path, then puts spurt and is masked as 1, and system enters the sound lunge stage.

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 micro- The volume of micro-Mus, the miniaturization of the most slight Mus；

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, reducing of the most slight Mus volume, and BGA package is more conducive to the heat radiation of chip when slight Mus is explored at a high speed；

3: in order to fully improve the stability of double-core slight Mus exploration servo-control system, and take into account the advantage of two-wheel drive, this Original many power real-time 4 wheel driven structure has been given up in invention, realizes two axle four-wheel functions by gear mechanism structure, both decreased Mus controller slightly drives the number of power motor, achieves the function of many wheels further through gear, is effectively increased slight Mus Explore driveability；

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；In slight Mus high speed heuristic process, once run into road dust more or accelerate During situation, A3P250 can open watching of absorption motor M immediately according to the feedback of magnetism encoder M1 and M2 of two spindle motor X and Y Clothes control, and slight Mus system is switched to three axle four-wheel drive states naturally, enhance the adhesive force of slight Mus and handling；

6: owing to using two axle four-wheel drive structures, add the contact area of slight Mus and ground, decrease the slight Mus of two-wheeled Generation due to the stall problem that frame for movement causes so that Mus slightly has exploration walking function the most at a high speed；

7: at this, slight Mus introduces high-performance MEMS motion sensor LY3200ALH in exploring servosystem, it is achieved that slightly The detection of the Mus instantaneous rotary speed when Maze Exploration, and utilize feedback to realize the real time correction of whole process navigation, be conducive to carrying The high slightly stability that Mus is explored at a high speed and dynamic property；

8: when Mus turns to the most slightly, in order to ensure the stability and the accuracy that rotate, by sensor G1(LY3200ALH) Real-time Feedback, the exploration servo controller turning real time correction to slight Mus based on A3P250, improve labyrinth information and explore Correctness；

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 in labyrinth, the arithmetic speed of beneficially raising system；

11: owing to using the traditional photoelectric encoder technology of magnetism encoder technical substitution so that the volume of Mus slightly is permissible 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 the slight Mus high speed speed of travel 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 various algorithms when slight Mus is explored at a high speed, be effectively prevented " race flies " of program, capacity of resisting disturbance is greatly enhanced；

14: in slight Mus high speed heuristic process, 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 that Mus is explored at a high speed；

15: can effectively regulate the vacuum cup absorption affinity to ground by regulation motor M, eliminate slight Mus and exploring at a high speed 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: in slight Mus high speed heuristic process, 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 Ability；

18: in slight Mus high speed heuristic process, taken into full account battery effect in this system, based on STM32F405+ FPGA(A3P250) running status of slight Mus is all being monitored and computing by the controller moment, it is to avoid the product of big electric current Raw, so fundamentally solving the impact to lithium ion battery of the big electric current, it is to avoid the lithium caused due to heavy-current discharge The generation of ion battery overaging phenomenon；

The current acquisition function of 19: drive axle A3906SESTR-T can well solve slight Mus and meet in high speed heuristic process 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, DC servo motor X, the driving signal of DC servo 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 the most slightly explores controller, improves double-core micro- The stability of micro-Mus MPU Controlled All Digital Servo System, effectively prevent the slight Mus ground when high speed Maze Exploration and skids, it is to avoid The generation of Mus off-center position phenomenon far away, improves stability during its rapid solving, improves slight Mus simultaneously slightly Speed and the accuracy of displacement, also ensure that slight Mus explore labyrinth accuracy.

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 the most slightly explores controller, it is characterised in that include slight Mus housing, car Wheel, the first infrared sensor, the second infrared sensor, the 3rd infrared sensor, the 4th infrared sensor, the 5th infrared sensing Device, the 6th infrared sensor, the first High-speed DC servomotor, the second High-speed DC servomotor, vac sorb motor, first Magnetism encoder, the second magnetism encoder and motion sensor, four described wheels are arranged on slight Mus shell the most two-by-two The limit, the left and right sides of body, the first described infrared sensor and the 6th infrared sensor are separately positioned on the left and right of slight Mus housing Dual-side is also positioned at the front end of wheel, and the second described infrared sensor and the 5th infrared sensor are arranged on slight Mus housing Front end, the 3rd described infrared sensor is angularly disposed between the first infrared sensor and the second infrared sensor, described 4th infrared sensor is angularly disposed between the 5th infrared sensor and 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 is arranged on the lower section of vac sorb motor, wherein, Corner dimension when the 3rd described sensor and the 4th sensor are angularly disposed and between Y-axis is:, also wrap Including panel, described panel is arranged in slight Mus housing, and described panel uses dual-core controller, including ARM and FPGA, described ARM with FPGA are communicatively coupled.

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 the most slightly explores controller, 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 the most slightly explores controller, 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 the most slightly explores controller, it is characterised in that institute The double-core three axle four-wheel structure changes stated Mus the most slightly is explored controller and also includes supply unit, independent by described supply unit Thering is provided the panel described in electric current driving, described panel sends the first control signal, the second control signal and the 3rd respectively Control signal, is controlled described second respectively by the first described control signal, the second control signal and the 3rd control signal high Slight Mus is controlled again after the signal syntheses of speed DC servo motor, the first High-speed DC servomotor and vac sorb motor Motion.

Double-core three axle four-wheel structure changes the most according to claim 4 Mus the most slightly explores controller, it is characterised in that institute The supply unit stated uses lithium ion battery.

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 double-core three axle four-wheel structure changes stated Mus the most slightly is explored controller and is additionally provided with host computer procedure and motion control program, institute The host computer procedure stated includes that Maze Exploration, labyrinth update, labyrinth stores and exports online, and described motion control program includes Explore SERVO CONTROL, coordinate setting and I/O based on the ARM+FPGA tri-slight Mus of axle four-wheel to control.

Double-core three axle four-wheel structure changes the most according to claim 7 Mus the most slightly explores controller, 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 exploration SERVO CONTROL Mus Maze Exploration SERVO CONTROL and single axle vacuum sucker suction SERVO CONTROL slightly, described is slight based on structure changes two axle four-wheel Mus Maze Exploration SERVO CONTROL includes exploring position module based on structure changes, exploring acceleration module based on structure changes and based on becoming knot Structure explores acceleration module；Described single axle vacuum sucker suction SERVO CONTROL includes sucker position module, sucker acceleration module With sucker acceleration module.