CN105137982A - Single-core low-speed four-wheel mini mouse sprint controller and control method for the same - Google Patents

Abstract

The invention discloses a single-core low-speed four-wheel mini mouse sprint controller, comprising a battery device, a sensor device, a gyroscope device, a vacuum suction device and a control unit module; a pair of the four wheels of the mini mouse is positioned in the middle of the mini mouse, and the other pair is arranged on the tail part of the mini mouse; the gyroscope device comprises a gyroscope G1 and a speedometer A1; the gyroscope G1 is a three shaft gyroscope which is used for measuring movement of three rotation directions; and the speedometer A1 is a three-shaft speedometer used for measuring accelerated speed of three translational motions. The invention solves the slipping of the mini mouse during the walking through the vacuum suction device arranged under a chassis, and automatically adjusts the servo control of the vacuum suction DC motor M according to the marching speed of the mini mouse and the ground situation, so that the mini mouse is not limited to the ground situation of the maze.

Description

The slight mouse sprint controller of monokaryon low speed four-wheel and control method thereof

Technical field

The present invention relates to minisize maze spurt robot field, particularly relate to the slight mouse sprint controller of a kind of monokaryon low speed four-wheel and control method thereof.

Background technology

Micro computer mouse is a kind of Intelligent walking robot using embedded microcontroller, sensor and electromechanical movement parts to form, abroad contest nearly 30 years, and it often adopts two wheel constructions, two-wheeled micro computer mouse two-dimensional structure as shown in Figure 1.

Micro computer mouse at different " labyrinth " middle automatic Memory and selecting paths, can adopt corresponding algorithm, arrives set destination rapidly.The signal of one of its labyrinth solved as shown in Figure 2.

Along with the continuous progress of microelectric technique, Computer Control Technology, external expert solves micro computer mouse and proposes one on the technical foundation in labyrinth and have more challenging maze robot---mouse slightly: the difficulty solving labyrinth in order to strengthen labyrinth complexity and mouse, labyrinth retaining wall becomes 90mm by original 180mm, original labyrinth becomes 32*32 lattice by 16*16 lattice, and new labyrinth two-dimensional structure as shown in Figure 3.Power supply is once open, slight mouse whole process relies on self-contained sensor self-navigation completely, and solve the various complex mazes be made up of 1024 maze lattices, an optimal path arriving target setting point can be found from starting point fast, then make a spurt terminal with the fastest speed.As the self-service navigation intelligent robot of one, because labyrinth information can be inputted to controller by wireless device, slight mouse or micro computer mouse international rule refusal use wireless device, in order to can obtain slight mouse or micro computer mouse spurt, spurt after information, can only be deposited fast by algorithm and store its walking information, after finishing the work, read storage information by 232 serial ports of controller or USB serial ports.

Slight mouse will judge the environment of surrounding the moment in labyrinth spurt process, then communicates a parameter to controller, by controller repetitive control its accurate acceleration and deceleration campaign in the grid of labyrinth.An outstanding slight mouse must possess good perception, has good locomotor activity, outstanding intelligent algorithm, otherwise cannot complete spurt task.Slight mouse labyrinth spurt technological synthesis multi-subject knowledge, for promoting the manipulative ability of students, team collaboration's ability and innovation ability, promote that the scope of one's knowledge of the digestion of student classroom knowledge and expansion student is all very helpful, and slightly mouse labyrinth spurt technology carry out the talent that can cultivate large quantities of association area, and then promote technical development and the industrialization process of association area.

If think that slight mouse is the simple copy of micro computer mouse, design slight mouse according to micro computer mouse technology, then can find following problem in practice:

(1) the skid level on ground, adaptation labyrinth that can only be passive based on wheeled slight mouse, along with the raising of slight mouse speed, its skidding probability also greatly increases, and causes solving labyrinth failure.

(2) owing to solving rolling up of labyrinth number, original micro computer mouse solves maze technique cannot solve existing complex maze.

(3) due to the significantly minimizing of slight mouse size, if mouse adopts six groups of sensor technology detection labyrinths in Fig. 1 slightly, often there is the situation that sensor disturbs mutually, cause it to read labyrinth information failure.

(4) due to the employing of micro computer mouse servo-drive system is all more rudimentary algorithm, and make the spurt of slight mouse in the middle of labyrinth generally all will spend the longer time, this makes cannot win victory in real contest.

(5) owing to being subject to single-chip microcomputer capacity impact, existing slight mouse all only has two power drive wheels substantially, two wheel guide robot mode is adopted to travel, system is made to require higher to the servo of diaxon, particularly during straight dash, requirement speed and acceleration will be pursued strict consistent, otherwise straight dash will be failed, and the phenomenon causing slight mouse to occur hitting wall occurs;

(6) the slight mouse sprint system of two-wheeled when accelerating due to center of gravity after move, make mouse front portion light, even if slight mouse also can be skidded on good road surface, the phenomenon likely causing hitting wall occurs, is unfavorable for the development of slight mouse at a high speed.

(7) if two-wheeled slight mouse system design when normal traveling improper cause center of gravity before partially, the normal pressure causing driving wheel bears reduced, at this moment mouse system is more prone to skid slightly, also more easily wanders off, causes navigating unsuccessfully.

(8) if two-wheeled slight mouse system designs the improper center of gravity lateral deviation that causes by different for the normal pressure causing two driving wheels to bear when normal traveling, when starting fast, two-wheeled skid level is inconsistent, moment is with regard to off-track, during turning, the wheel that wherein normal pressure is little may skid, and causes cornering difficulties.

(9) owing to adopting two power wheels to drive, in order to meet acceleration under complex state and deceleration, make the power of single drive motor larger, the space not only taken is larger, and under the state that some relative requirements energy are lower, sometimes cause the phenomenon of " low load with strong power " to occur, be unfavorable for the saving of slight mouse body miniaturization and slight mouse system energy.

It is international emerging a special kind of skill that slight mouse solves labyrinth, and the complicacy of the higher and labyrinth design of the difficulty due to slight mouse labyrinth spurt technology, causes the domestic unit also not researching and developing this robot.Therefore, need that design is a kind of to be met elementary person and learn the sound lunge controller that slight mouse solves labyrinth.

Summary of the invention

The object of the invention is to solve the deficiencies in the prior art, provide a kind of monokaryon low speed four-wheel slight mouse sprint controller, to solve the problems such as slight mouse is skidded in the process of walking, sensor disturbs mutually, the processing time is slow.

The technical solution used in the present invention is: the slight mouse sprint controller of monokaryon low speed four-wheel, comprises cell apparatus, sensor device, gyroscope equipment, vacuum suction apparatus and control module module; The four-wheel of slight mouse is wherein positioned at the middle part of slight mouse for a pair, and another is to the afterbody being positioned at slight mouse; Described gyroscope equipment comprises gyroscope G1 and accelerometer A1, and described gyroscope G1 is that three-axis gyroscope moves for measuring three rotation directions, and described accelerometer A1 is that three axis accelerometer is for measuring the acceleration of three translation motions; Described sensor device comprises infrared sensor S1 and S6 being positioned at both sides, be positioned at infrared sensor S2 and S5 of front end, described sensor S1, S6 acting in conjunction judges front barricade, sensor S2 judges the existence of its left side barricade, sensor S5 judges the existence of barricade on the right of it, and S2 cooperates as rectilinear motion provides navigation foundation with S5 simultaneously; Described cell apparatus is electrically connected control module module, motor X, motor Y, motor R, motor Z and motor M respectively, described sensor device and gyroscope equipment divide level signal connection control unit module, and described control module module divides level signal to connect motor X, Y, R, Z and M; Described control module module comprises host computer procedure module and motion control program module, described host computer procedure module comprises STM32F407 processor, described motion control program module comprises two panels diaxon drived control chip L6207D, described L6207D chip process four axle walking servocontrol and single axle vacuum absorption servocontrol, described STM32F407 processor is electrically connected L6207D chip, and described four axle walking servo control unit signals connect single axle vacuum absorption servo control unit.

As a further improvement on the present invention, also include voltage sensor V1, described voltage sensor V1 is electrically connected cell apparatus, the STM32F407 processor of signal connection control unit module.

As a further improvement on the present invention, also comprise light compensation sensor L1, the STM32F407 processor of described smooth compensation sensor L1 signal connection control unit module.

As a further improvement on the present invention, also include current sensor C1, C2, C3 and C4, described current sensor C1, C2, C3 are connected STM32F407 processor with C4 signal.

Further improve as of the present invention, described STM32F407 connects L6207D chip by I/O mouth, thus enable control two panels diaxon drived control chip L6207D.

As a further improvement on the present invention, described motor X, Y, Z and R are high-speed permanent magnetic direct current generator, and described motor M is DC micromotor.

As a further improvement on the present invention, the angle between described infrared sensor S1 and S2 signal transmit direction is 75 ° ~ 90 ° angles, and the angle between described infrared sensor S5 and S6 signal transmit direction is 75 ° ~ 90 ° angles.

The beneficial effect that the present invention adopts is: 1: in motion process, take into full account battery effect in this system, all the running status of slight mouse is being monitored and computing based on the STM32F407+L6207D controller moment, because L6207D inside is integrated with current collection circuit, moment can gather the electric current of motor, fundamentally avoid the generation of big current, so solve the impact of big current to lithium ion battery, avoid the generation of the lithium ion battery overaging phenomenon caused due to heavy-current discharge.

2: in order to fully improve stability and the driveability of slight mouse system, and take into account the mid-advantage turned to of two-wheeled, the present invention adopts four-wheel drive structure: the power of mid-driving is larger, two each and every one powers of motor of rearmounted driving are less, only just start when power demand is higher, play power-assisted effect.Owing to adopting four-wheel drive technology, slight mouse front and back wheel is all dynamic, can be different with ambient conditions and be distributed on all wheels in front and back by demand torque by different proportion, to improve the driveability of slight mouse by ground, labyrinth.

3: realize time sharing as required.Under normal running environment, slight mouse generally can adopt release trailing wheel, adopts the mode of mid-wheel drive; And once run into the more or accelerated condition of road dust, STM32407 automatically can detect to exist side by side and distribute to rearmounted two power-assisted driving wheels by slight mouse demand torque, the servocontrol of motor M is changed with Time Controller, slight mouse system is switched to four-wheel drive state naturally, enhances the adhesion of slight mouse and handling.

4: in adopting, drive+compound four-wheel drive the mode of rear-guard, when needs give it the gun, power is assigned to four motors, once power wheel because ground, physical construction etc. cause ground away from keyboard, STM32407 can redistribute moment of torsion, more torque distribution on the driving wheel of non-stall, makes system hightail non-steady state, come back to four axle dynamic equilibrium states, make slight mouse have better straight line moving function.

5: when mouse turns to slightly, in order to ensure the stability rotated, adopting two mid-driving wheels to realize turning, and discharging two rearmounted power-assisted driving wheels, and utilizing accelerometer to measure the instantaneous acceleration of slight mouse in real time, for slight mouse precise turns provides feedback.

6: if four-wheel slight mouse system design when normal traveling improper cause center of gravity before partially, reduce causing the normal pressure that rear side driving wheel bears, the power that STM32F407 can adjust rear side automatically distributes, and makes system be in a kind of new equilibrium state, prevents slight mouse from skidding.

7: if four-wheel slight mouse system designs when normal traveling and improperly causes center of gravity lateral deviation, the normal pressure causing a side drive wheel is born is reduced, the power that STM32F407 can adjust this side automatically distributes, and makes system be in a kind of new equilibrium state, prevents slight mouse from skidding.

8: by the independent servocontrol of five direct current generators of the slight mouse of STM32F407 process, make control fairly simple, substantially increase arithmetic speed, solve scm software and run slower bottleneck, shorten the construction cycle short, and program transportability ability is strong.

9: the present invention realizes full SMD components material substantially, not only saves control panel and takes up room, and is conducive to alleviating of volume and weight, be conducive to the stability and the dynamic property that improve slight mouse.

10: adopt four groups of sensors to solve maze technique and substitute original six groups of sensors and solve the interference that maze technique not only reduces each sensor group in system, and improve labyrinth retaining wall frequency acquisition, be conducive to the arithmetic speed improving slight mouse servo controller.

11: because this controller adopts the reading of STM32F407 process labyrinth, spurt algorithm, effectively prevent " race flies " of program, antijamming capability strengthens greatly.

12: in slight mouse spurt process, controller can carry out on-line identification to the torque of four spindle motors and utilize the relation of motor torque and electric current to compensate, and decreases the impact of motor torque shake on slight mouse fast sprint dynamic property.

13: in the controlling, STM32F407 according to the pid parameter of labyrinth situation adjustment controller inside around reality, easily can realize segmentation P, PD, PID and control and nonlinear PID controller, make system have certain self-adaptation.

The Enable Pin of 14:L6207D can well solve slight mouse in spurt process, run into the motor rotation blockage hitting wall situation and occur, utilize interruptive command can when output exceeds setting value, the current collection circuit of L6207D sends immediately forbids enable request, the static Enable Pin of STM32F407, release direct current generator X, motor Y, motor Z and motor R, thus efficiently solve stall problem.

15: by regulating motor M can effectively regulate vacuum cup to the absorption affinity on ground, effectively increasing the friction factor on slight mouse and ground, effectively reducing the generation of slight mouse skidding when sound lunge.

Accompanying drawing explanation

Fig. 1 former two takes turns six micro computer mouse X-Y schemes.

Fig. 2 is micro computer mouse 16*16 labyrinth schematic diagram.

Fig. 3 is slight mouse 32*32 labyrinth schematic diagram

Fig. 4 is slight mouse sprint controller flow chart.

Fig. 5 is schematic diagram of the present invention.

Fig. 6 is theory diagram of the present invention.

Fig. 7 is the encapsulation schematic diagram of L6207D of the present invention.

Fig. 8 is slight mouse advance schematic diagram.

Fig. 9 is that slight mouse oppositely advances schematic diagram.

Figure 10 is slight mouse speed curve diagram of the present invention.

Figure 11 is slight mouse right-hand rotation dash speed time diagram of the present invention.

Figure 12 is slight mouse right-hand rotation schematic diagram.

Figure 13 is slight mouse left-hand rotation schematic diagram.

Embodiment

Below in conjunction with Fig. 1 to Figure 13, the present invention is described further.

STM32F4 series is except pin and the high performance F2 series of softwarecompatible, the dominant frequency (168MHz) of F4 is higher than F2 series (120MHz), and support the peripheral hardware that monocycle DSP instruction and floating point unit, larger SRAM capacity (192KB, F2 are 128KB), the embedded flash memory of 512KB-1MB and image, network interface and data encryption etc. are more advanced.STM32F4 series, based on up-to-date ARMCortexM4 kernel, has increased signal processing function newly, and has improve travelling speed in existing outstanding STM32 microcontroller products combination; STM32F405x is integrated with timer, 3 ADC, 2 DAC, serial line interface, external memory interface, real-time clock, CRC computing unit and simulates real randomizers at the advanced peripheral hardware of interior the whole series.STM32F407 adds multiple advanced peripheral hardware on STM32F405 product basis.These performances make F4 series can be easier to meet the Digital Signals demand of control and signal processing function mixing.The combination of the low energy consumption of efficient signal processing function and Cortex-M4 processor family, low cost and wieldy advantage, makes it can provide flexible solution for multiaxis Motor Control.These features make STM32F407 be particularly suitable for the signal transacting of slight mouse four axle servo-drive system.

L6207D chip adopts two DMOS full-bridge design; and achieve isolation DMOS power transistor, CMOS and bipolar circuit technology on the same chip; operating voltage is 8 ~ 52V; output peak point current is 5.6A; frequency of operation, up to 100KHz, has the defencive functions such as non-dissipation overcurrent protection, Thermal shutdown, under-voltage locking.In Motor control Application, two direct current generators can be driven simultaneously.Its conventional encapsulation is SO24, as shown in Figure 4.

Wherein 23 pin of a slice L6207D and 14 pin EnA, EnB is enable control end, controls the stall of permanent magnet DC motor X and permanent magnet DC motor Y respectively, in addition 23 pin of a slice L6207D and 14 pin EnA, the enable control end of EnB, controls the stall of permanent magnet DC motor Z and permanent magnet DC motor R respectively.1,2,11,12 pin of L6207D connect the input pin of STM32F407, and by STM32F407 control OUT1A, the level of OUT2A and OUT1B, OUT2B, controls the rotating of motor X and motor Y or motor Z and motor R by different level combinations.Simultaneously current sensor is input to SENSEA and SENSEB and carries out Current limited Control detecting electric current to motor.

Find in an experiment, due to the significantly reduction of slight mouse volume, in Fig. 1, sensor S2 and S3 and S4 and S5 often produces mutual interference; All need the regular hour owing to often organizing sensor collection labyrinth retaining wall parameter simultaneously, increased the weight of the sampling period, caused sample frequency to reduce, extended the processing time of processor STM32F407.The present invention is the defect overcoming six groups of sensor detection labyrinths, independent research is based on the two-wheeled slight mouse sprint controller in four groups of sensors detection labyrinths, the slight mouse two-dimensional structure invented is as shown in Figure 5: sensor S1, S6 acting in conjunction judges front barricade, sensor S2 judges the existence of its left side barricade, sensor S5 judges the existence of barricade on the right of it, and S2 cooperates with S5 as slight mouse rectilinear motion provides navigation foundation simultaneously.Labyrinth retaining wall information is calculated in order to use intelligent algorithm, at slight mouse calibration phase, slight mouse is placed on different set position, labyrinth, the infrared light of infrared sensor S1, S2, S5, S6 can be received by the infrared remote receiver TSL262 of correspondence after side barricade feedback, then the reception value via controller of TSL262 calculate after as the setting threshold values of current location, then mouse obtains its labyrinth information by comparing with these threshold values in the process of walking slightly.In same position, in Fig. 1, the calculating threshold values of sensor S2 is obviously accurate than S3, the calculating threshold values of sensor S5 is obviously accurate than S4, and S2 and S5 can also accurately measure labyrinth from have barricade to without barricade and without barricade to the change having barricade, the sensor signal change of this position can be captured by controller, then fine compensation can be carried out to slight mouse in this position, this calculates most important for complex maze, if do not have this intelligent compensation, the cumulative errors of slight mouse in complex maze are enough to make to solve labyrinth failure.Therefore this controller adopts the structure in Fig. 6 to solve labyrinth, to guarantee the accuracy in labyrinth.

Slight mouse is placed on labyrinth starting point, and under power-on state, slight mouse is introduced into self-locking state, and transfers the spurt labyrinth information optimized.First this Time Controller opens vacuum draw motor M, the friction factor on slight mouse and ground is adjusted by the absorption affinity increasing itself and ground, then mouse relies on front slightly, the infrared sensor S1 that keeps in obscurity in left and right side, S2, S5, S6 passes through four infrared receiver sensor TSL262 transformation parameters to the STM32F407 in controller according to actual navigational environment, again in conjunction with photoelectric sensor after STM32F407 process, the feedback of current sensor C1 ~ C4 generates four road PWM ripples, rear drive four individual motor are amplified and servocontrol when realizing its fast sprint through drive axle, finally processing data communication to STM32F407, the follow-up running status of process is continued by STM32F407.

As shown in the figure, the slight mouse sprint controller of monokaryon low speed four-wheel, comprises cell apparatus, sensor device, gyroscope equipment, vacuum suction apparatus and control module module; The four-wheel of slight mouse is wherein positioned at the middle part of slight mouse for a pair, and another is to the afterbody being positioned at slight mouse; Described gyroscope equipment comprises gyroscope G1 and accelerometer A1, and described gyroscope G1 is that three-axis gyroscope moves for measuring three rotation directions, and described accelerometer A1 is that three axis accelerometer is for measuring the acceleration of three translation motions; Described sensor device comprises infrared sensor S1 and S6 being positioned at both sides, be positioned at infrared sensor S2 and S5 of front end, described sensor S1, S6 acting in conjunction judges front barricade, sensor S2 judges the existence of its left side barricade, sensor S5 judges the existence of barricade on the right of it, and S2 cooperates as rectilinear motion provides navigation foundation with S5 simultaneously; Described cell apparatus is electrically connected control module module, motor X, motor Y, motor R, motor Z and motor M respectively, described sensor device and gyroscope equipment divide level signal connection control unit module, and described control module module divides level signal to connect motor X, Y, R, Z and M; Described control module module comprises host computer procedure module and motion control program module, described host computer procedure module comprises STM32F407 processor, described motion control program module comprises two panels diaxon drived control chip L6207D, described L6207D chip process four axle walking servocontrol and single axle vacuum absorption servocontrol, described STM32F407 processor is electrically connected L6207D chip, and described four axle walking servo control unit signals connect single axle vacuum absorption servo control unit.

In order to protect big current to the impact of cell apparatus, also include voltage sensor V1, described voltage sensor V1 is electrically connected cell apparatus, the STM32F407 processor of signal connection control unit module.

In order to reduce and stop the interference of external light source to sensor, thus affect the use of whole system, also comprise light compensation sensor L1, the STM32F407 processor of described smooth compensation sensor L1 signal connection control unit module.

In order to make servo controller of the present invention accurately control the advance of slight mouse and turn to further, also include current sensor C1, C2, C3 and C4, described current sensor C1, C2, C3 are connected STM32F407 processor with C4 signal.

For expanding the processing power of processor further, accomplish that the division of labor is clear and definite, work is interference-free, and described STM32F407 connects L6207D chip by I/O mouth, thus enable control two panels diaxon drived control chip L6207D.

In order to improve the performance of slight mouse further, described motor X and Y is high-speed permanent magnetic direct current generator, and described motor M is DC micromotor.

For eliminating the interference between infrared sensor further, improve the induction precision of sensor device, angle between described infrared sensor S1 and S2 signal transmit direction is 75 ° ~ 90 ° angles, and the angle between described infrared sensor S5 and S6 signal transmit direction is 75 ° ~ 90 ° angles.

Monokaryon low speed four-wheel slight mouse spurt control method, its concrete steps are as follows: 1) detect cell voltage, cell voltage is detected, if low pressure, to forbid that two panels L6207D is enable, OUT1A, OUT2A, OUT1B and OUT2B are low level, and direct current generator X, motor Y, motor Z and motor R can not start, voltage sensor V1 is by work simultaneously, and sends alerting signal, if voltage is normal, then and recall the optimum labyrinth after exploring, 2) spurt angle detecting, infrared sensor S1, S6 and can to spurt path on front environment judge, define and enter range of movement without barricade, interrupt request will be sent to STM32F407 as there is barricade, forbid Enable Pin ENA and the ENB work of L6207D, block High-speed DC motor X, the motor Y of slight mouse, the PWM drive singal of motor Z and electrode R, make slight mouse be still in original place, then secondary judges that front information is determined in labyrinth, prevents information from judging by accident, 3) labyrinth spurt, slight mouse is along X and Y-axis fast sprint, and controller judges whether its coordinate is terminal, new spurt order is performed if not by continuing, 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 mouse prepares return and explores, when sensor S2, S5, gyroscope G1 and accelerometer A1 acting in conjunction detect slight mouse be in normal straight at the uniform velocity travel time, STM32F407 forbids that rear power-assisted wheel drive chip L6207D works, discharge rear-wheel motor Z and the motor R of slight mouse, adopt the mode of motor X and the mid-driving of motor Y, when slight mouse is in the more situation of acceleration, deceleration, road dust, STM32F407 can work by automatic enable rear power-assisted wheel drive chip L6207D, slight mouse demand torque part dispensing motor Z and motor R, slight mouse is switched to four-wheel drive state naturally, enhances the adhesion of slight mouse and handling, 4) return spurt, can prepare the return after making a spurt after reaching home explores to search more excellent path, controller can recall its labyrinth information stored, and then calculates other optimal path that may exist, and then return starts to enter one that wherein thinks optimum, 5) labyrinth return, when slight mouse enters labyrinth return exploration, the sensor S1 of its navigation, S2, S5, S6 is by work, and give STM32F407 the photosignal reflected, judge after computing, to determine present position, present labyrinth through STM32F407, then generate PWM ripple by STM32F407 according to current labyrinth and send control signal to L6207D: will advance fast if enter the region searched for, if unknown return area then adopts normal speed to search for, and the moment upgrades its coordinate (X, Y), and judge that its coordinate is starting point, if words put exploration of making a return voyage and be masked as 0, slight mouse enters the sprint stage, juxtaposition spurt is masked as 1, 6) secondary spurt, after slight pindone becomes whole spurt process to reach home, slight mouse can be put exploration and is masked as 1, slight mouse return is explored and is got back to starting point, the level controlling two panels L6207D exports and slight mouse starting point central point is stopped by STM32F407, then the OUT1A of L6207D is readjusted, OUT2A, the level of OUT1B and OUT2B, motor X and motor Y is moved with contrary direction, and under gyrostatic control, turnback is revolved in original place, then stop 1 second, secondary transfers labyrinth information, and the optimum spurt path of slight mouse exploration is calculated according to improvement ant group algorithm (Improvedantcolonyalgorithm), then put spurt and be masked as 1, system enters the fast sprint stage, then controller adjusts motor M automatically according to the size of dash speed, changes vacuum cup to the absorption affinity on ground, meets friction needs during fast sprint.

Slight mouse sprint controller is divided into two parts: master system and kinetic control system.Wherein master system completes the functions such as labyrinth reading, coordinate setting, online output; Kinetic control system completes the function such as servocontrol, single axle vacuum absorption servocontrol, data storage, I/O control of the slight mouse sprint system of four axles, the five axle servo-drive systems that wherein workload is maximum are given STM32F407 process and are generated drive singal, drive singal timesharing after L6207D amplifies drives four spindle motors, therebetween also can carry out communication, carry out exchanges data in real time and call.

As a further improvement on the present invention, the labyrinth sprint stage, sensor S2 and S5 can judge the labyrinth retaining wall of left and right, and record stores current labyrinth retaining wall information, according to the labyrinth information of left and right barricade, slight mouse determines that it enters single wall navigation mode, two wall navigation mode or inertial navigation pattern; Its instantaneous acceleration of gyroscope real time record, speed and position in slight mouse fast sprint process, external interference is subject to when having departed from setting center when slight mouse walks fast, probe value will produce certain deviation with setting value, now gyroscope G1 and accelerometer A1 records movement velocity and the position of current slight mouse, Full-digital servo controller starts to carry out real-Time Compensation and finely tunes the PWM ripple input of four spindle motors, the attitude of slight mouse can be readjusted by this mode, make it come back to setting center.

The present invention is following work:

1) before slight mouse does not receive spurt order, it generally can wait at starting point coordinate (0,0) the spurt order that controller sends, and recalls the optimum labyrinth after exploring.First controller opens vacuum draw motor M, first micro vacuum sucker is aspirated by aspirator, make vacuum cup over the ground mask have certain absorption affinity, controller also detects in real time, if ground is unclean, system can strengthen vacuum cup to the absorption affinity on ground by self-regulation motor M, increases the friction factor on when slight mouse is made a spurt and ground.Once after receiving spurt order, slight mouse to be sprinted for the line (F, F), (F, 10), (10, F), (10,10) from the off fast along optimal path.

2) mouse is placed on starting point coordinate (0 slightly, 0), spurt direction is misplaced in order to prevent after receiving task, the sensor S1 in its front, S6 and judging the environment in front, define and do not have barricade to enter range of movement, interrupt request will be sent to STM32F407 as there is barricade, STM32F407 can do very first time response to interruption, then Enable Pin ENA and the ENB work of L6207D is forbidden, block the motor X of slight mouse, motor Y, the PWM drive singal of motor Z and motor R, it is made to be still in original place, then secondary judges that front information is determined in labyrinth, prevent information from judging by accident, if do not have barricade to enter the range of movement in front, slight mouse will be made a spurt normally.

3) barricade is not had to enter the range of movement in front if shown under there are Z lattice rectilinear coordinates in front in labyrinth information in fast sprint process before Y-axis slight mouse, slight mouse is by its present coordinate (X of storage, Y), processor passes to STM32F407 the location parameter of the Z lattice that travel forward, STM32F407 requires formation speed motion ladder diagram according to sprint controller speed and acceleration, and the area that this Velocity-time graphics package contains is exactly the Z lattice distance that slight mouse motor X and motor Y will run.STM32F407 is according to the PWM ripple of this ladder diagram in conjunction with the feedback generation driving direct current generator of photoelectric encoder, current sensor C1 ~ C4, then the enable L6207D of STM32F407, move before driving two independent built-in motor X and motor Y-direction by L6207D, the photoelectric encoder of motor X and motor Y can record the distance of movement and flows to controller in the moment, slight mouse along in current maze lattice thrust forward process, controller can detect the numerical value of motor X and motor Y photoelectric encoder in real time, and strengthen vacuum cup to the absorption affinity on ground according to its velocity magnitude self-regulation motor M, then sensor S2 and S5 can judge the barricade of left and right, and record stores current labyrinth retaining wall information, slight mouse enters single wall navigation mode or two wall navigation mode according to the labyrinth information of working direction left and right barricade, then gyroscope G1 and accelerometer A1 records the transient motion acceleration of slight mouse, speed and position, when slight mouse fast sprint has departed from setting center, microprocessor starts to carry out real-Time Compensation by gyroscope G1 and accelerometer A1 according to the deviation leaving center, the PWM ripple input of fine setting motor, can the attitude of the slight mouse of accurate adjustment by this mode, it is made to come back to setting center, before not arriving target, if during mouse sound lunge, labyrinth floor-dust is more slightly, STM32F407 can regulate M to strengthen the friction on slight mouse and ground and the Enable Pin of enable rear-guard chip L6207D, open motor Z and motor R and carry out power-assisted, system enters four-wheel drive state, STM32F407 is according to time and rate request, remaining distance D1 is converted into the PWM ripple of four axle direct current generators, make motor X, motor Y, motor Z advances with identical speed with motor R, slight mouse is under 4 wheel driven state, slight mouse still advances according to original single wall navigation mode or two wall navigation mode, the acceleration of motion of gyroscope G1 and the slight mouse of accelerometer A1 real time record, speed and position, when slight mouse fast sprint has departed from setting center, microprocessor starts to carry out real-Time Compensation by gyroscope G1 and accelerometer A1 according to the deviation leaving center, the PWM ripple input of fine setting motor, can the attitude of the slight mouse of accurate adjustment by this mode, it is made to come back to setting center, when slight mouse moves the Z lattice distance new address of arrival under gyrostatic control, microprocessor will upgrade its coordinate for (X, Y+Z), under the prerequisite of Y+Z<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10,10) one of them, upgrade its coordinate if not by continuation, if words notification controller to have made a spurt target, then put exploration of making a return voyage and be masked as 1, slight mouse prepares return and explores,

4) barricade is not had to enter the range of movement in front if shown under there are Z lattice rectilinear coordinates in front in labyrinth information in the reverse fast sprint process forward of Y-axis slight mouse, slight mouse is by its present coordinate (X of storage, Y), slight mouse is similar to positive movement principle along counter motion principle, when slight mouse moves the Z lattice distance new address of arrival under gyrostatic control, it is (X that microprocessor will upgrade its coordinate, Y-Z), under the prerequisite of Y-Z<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10, 10) one of them, its coordinate is upgraded if not by continuation, if words notification controller to have made a spurt target, then put exploration of making a return voyage and be masked as 1, slight mouse prepares return and explores,

5) if having barricade to enter the range of movement in front slight mouse in Y-axis forward movement, and when now in the information of labyrinth, there is a barricade left, slight mouse will store now coordinate (X, Y), then the curvilinear motion track shown in Fig. 9 is entered, when right spurt is turned, controller the Distance Transmission of speed now and decentering point A to STM32F407, STM32F407 generates the acceleration of four axle servo-drive systems according to current speed and spurt time, speed and position signalling, and generate four axle PWM ripples in conjunction with the feedback of photoelectric encoder and current sensor C1 ~ C4, then drive and rear-guard driving chip L6207D in enable, make motor X, motor Y, motor Z slows down with identical acceleration with motor R, slight mouse along in current maze lattice thrust forward process, controller first regulates motor M to strengthen the friction on slight mouse and ground, then sensor S2 can judge left barricade, and record stores current labyrinth retaining wall information, slight mouse enters single wall navigation mode according to the labyrinth information of the left barricade of working direction, then gyroscope G1 and accelerometer A1 records the acceleration of motion of slight mouse, speed and position, when slight mouse fast sprint has departed from setting center, microprocessor starts to carry out real-Time Compensation by gyroscope G1 and accelerometer A1 according to the deviation leaving center, the PWM ripple input of fine setting motor, can the attitude of the slight mouse of accurate adjustment by this mode, it is made to come back to setting center, when slight mouse arrives labyrinth central point, sensor reference value R90_FrontWallRef starts working, prevent external interference from starting to do error compensation, ensure that slight mouse is stopped at grid center, labyrinth, now motor X, motor Y, motor Z and motor R speed reduce to 0, L6207D is driven during STM32F407 is enable, and forbid that rear-guard L6207D works, by driving L6207D pin OUT1A in adjustment, OUT2A and OUT1B, the level of OUT2B, left side wheels is rotated forward, right-hand wheel is reversed, slight mouse flicker 90 degree is made under the control of gyroscope G1 and accelerometer A1, then controller starts to prepare to make a spurt along X-axis forward, when slight mouse goes out current labyrinth under gyroscope controls, it is (X+1 that controller will upgrade its coordinate, Y), under the prerequisite of X+1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10, 10) one of them, its coordinate is upgraded if not by continuation, if words notification controller to have made a spurt target, then put exploration of making a return voyage and be masked as 1, spurt is masked as 0, slight mouse prepares the secondary return after spurt and explores, go to search more excellent maze path,

6) if having barricade to enter the range of movement in front slight mouse in Y-axis forward movement, and when now in the information of labyrinth, there is a barricade right, slight mouse will store now coordinate (X, Y), then the curvilinear motion track shown in Figure 11 is entered, slight mouse spurt principle of turning left is similar with the principle of making a spurt of turning right, when slight mouse goes out current labyrinth under gyroscope controls, it is (X-1 that controller will upgrade its coordinate, Y), under the prerequisite of X-1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10, 10) one of them, its coordinate is upgraded if not by continuation, if words notification controller to have made a spurt target, then put exploration of making a return voyage and be masked as 1, spurt is masked as 0, slight mouse prepares the secondary return after spurt and explores, go to search more excellent maze path,

7) when slight mouse spurt arrives (F, F), (F, 10), (10, F), (10,10) can prepare the return after making a spurt after explores to search more excellent path, controller can recall its labyrinth information stored, and then calculates other optimal path that may exist, and then return starts to enter one that wherein thinks optimum.

8) in order to the coordinate calculating accurately when making a spurt of slight mouse can be realized, sensor S2 and S5 about slight mouse can detect the labyrinth retaining wall of surrounding and pillar in the moment, if S2 or S5 finds that sensor signal there occurs the transition of bigger numerical, then illustrate that slight mouse enters from having labyrinth retaining wall to the change without labyrinth retaining wall (or from without labyrinth retaining wall to there being labyrinth retaining wall) state, STM32F407 according to slight mouse current operating conditions fine compensation, can thoroughly eliminate the error added up when slight mouse is explored in complex maze.

9) when slight mouse enters labyrinth return exploration, sensor S1, S2, S5, S6 of its navigation are by work, and give STM32F407 the photosignal reflected, present position, present labyrinth is determined after STM32F407 computing, then generate PWM ripple by STM32F407 according to current labyrinth and send control signal to L6207D: if enter the region searched for will strengthen the dutycycle of PWM ripple, slight mouse is advanced fast, reduces the time of labyrinth search; If unknown return area then adopts normal speed to search for, controller adjusts the servocontrol of motor M in real time according to current search speed, ensure the stable row of slight mouse, in search procedure, the moment upgrades its coordinate (X, Y), and judges that its coordinate is (0,0), if words put exploration of making a return voyage and be masked as 0, slight mouse enters the sprint stage, and juxtaposition spurt is masked as 1.

10) in slight mouse spurt process, if interference has appearred in system, STM32F407 can have been compensated electric current according to current state, the pid parameter of rapid adjustment electric current loop, and the system fast and stable of making gets off, and prevents the impact disturbed during sound lunge system.

11) in the whole spurt process of slight mouse, sensor L1 can the moment to external world jamming light source gather, then be transferred to STM32F407, STM32F407 according to the auto-compensation external interference of L1, can decrease the interference of external interference light source to slight mouse fast sprint servo-drive system.

12) when slight pindone becomes whole spurt process to arrive (F, F), (F, 10), (10, F), (10, 10), slight mouse can be put exploration and is masked as 1, slight mouse return is explored and is got back to starting point (0, 0), the level controlling two panels L6207D exports and slight mouse starting point central point is stopped by STM32F407, then the OUT1A of L6207D is readjusted, OUT2A, the level of OUT1B and OUT2B, motor X and motor Y is moved with contrary direction, and under gyrostatic control, turnback is revolved in original place, then stop 1 second, secondary transfers labyrinth information, and the optimum spurt path of slight mouse exploration is calculated according to improvement ant group algorithm (Improvedantcolonyalgorithm), then put spurt and be masked as 1, system enters the fast sprint stage.Then controller adjusts motor M automatically according to the size of dash speed, changes vacuum cup to the absorption affinity on ground, meets friction needs during fast sprint.

Those skilled in the art should know; protection scheme of the present invention is not limited only to the above embodiments; various permutation and combination and conversion can also be carried out on the basis of above-described embodiment; under the prerequisite without prejudice to spirit of the present invention, the various conversion that the present invention carries out all are dropped in protection scope of the present invention.

Claims (9)

1. the slight mouse sprint controller of monokaryon low speed four-wheel, is characterized in that comprising cell apparatus, sensor device, gyroscope equipment, vacuum suction apparatus and control module module; The four-wheel of slight mouse is wherein positioned at the middle part of slight mouse for a pair, and another is to the afterbody being positioned at slight mouse;

Described gyroscope equipment comprises gyroscope G1 and accelerometer A1, and described gyroscope G1 is that three-axis gyroscope moves for measuring three rotation directions, and described accelerometer A1 is that three axis accelerometer is for measuring the acceleration of three translation motions;

Described sensor device comprises infrared sensor S1 and S6 being positioned at both sides, be positioned at infrared sensor S2 and S5 of front end, described sensor S1, S6 acting in conjunction judges front barricade, sensor S2 judges the existence of its left side barricade, sensor S5 judges the existence of barricade on the right of it, and S2 cooperates as rectilinear motion provides navigation foundation with S5 simultaneously;

Described cell apparatus is electrically connected control module module, motor X, motor Y, motor R, motor Z and motor M respectively, described sensor device and gyroscope equipment divide level signal connection control unit module, and described control module module divides level signal to connect motor X, Y, R, Z and M;

Described control module module comprises host computer procedure module and motion control program module, described host computer procedure module comprises STM32F407 processor, described motion control program module comprises two panels diaxon drived control chip L6207D, described L6207D chip process four axle walking servocontrol and single axle vacuum absorption servocontrol, described STM32F407 processor is electrically connected L6207D chip, and described four axle walking servo control unit signals connect single axle vacuum absorption servo control unit.

2. the slight mouse sprint controller of monokaryon low speed four-wheel according to claim 1, is characterized in that also including voltage sensor V1, and described voltage sensor V1 is electrically connected cell apparatus, the STM32F407 processor of signal connection control unit module.

3. the slight mouse sprint controller of monokaryon low speed four-wheel according to claim 1, is characterized in that also comprising light compensation sensor L1, the STM32F407 processor of described smooth compensation sensor L1 signal connection control unit module.

4. the slight mouse sprint controller of monokaryon low speed four-wheel according to claim 1, it is characterized in that also including current sensor C1, C2, C3 and C4, described current sensor C1, C2, C3 are connected STM32F407 processor with C4 signal.

5. the slight mouse sprint controller of monokaryon low speed four-wheel as claimed in any of claims 1 to 4, is characterized in that described STM32F407 connects L6207D chip by I/O mouth, thus enable control two panels diaxon drived control chip L6207D.

6. the slight mouse sprint controller of monokaryon low speed four-wheel according to claim 5, it is characterized in that described motor X, Y, Z and R are high-speed permanent magnetic direct current generator, described motor M is DC micromotor.

7. the slight mouse sprint controller of monokaryon low speed four-wheel as claimed in any of claims 1 to 4, it is characterized in that the angle between described infrared sensor S1 and S2 signal transmit direction is 75 ° ~ 90 ° angles, the angle between described infrared sensor S5 and S6 signal transmit direction is 75 ° ~ 90 ° angles.

8. a monokaryon low speed four-wheel as claimed in claim 1 slight mouse spurt control method, is characterized in that concrete steps are as follows:

1) cell voltage is detected, cell voltage is detected, if low pressure, to forbid that two panels L6207D is enable, OUT1A, OUT2A, OUT1B and OUT2B are low level, direct current generator X, motor Y, motor Z and motor R can not start, and voltage sensor V1 is by work simultaneously, and sends alerting signal; If voltage is normal, then and recall the optimum labyrinth after exploring;

2) spurt angle detecting, infrared sensor S1, S6 and can to spurt path on front environment judge, define and enter range of movement without barricade, interrupt request will be sent to STM32F407 as there is barricade, forbid Enable Pin ENA and the ENB work of L6207D, block High-speed DC motor X, the motor Y of slight mouse, the PWM drive singal of motor Z and electrode R, make slight mouse be still in original place, then secondary judges that front information is determined in labyrinth, prevents information from judging by accident;

3) labyrinth spurt, slight mouse is along X and Y-axis fast sprint, and controller judges whether its coordinate is terminal, new spurt order is performed if not by continuing, 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 mouse prepares return and explores; When sensor S2, S5, gyroscope G1 and accelerometer A1 acting in conjunction detect slight mouse be in normal straight at the uniform velocity travel time, STM32F407 forbids that rear power-assisted wheel drive chip L6207D works, discharge rear-wheel motor Z and the motor R of slight mouse, adopt the mode of motor X and the mid-driving of motor Y; When slight mouse is in the more situation of acceleration, deceleration, road dust, STM32F407 can work by automatic enable rear power-assisted wheel drive chip L6207D, slight mouse demand torque part dispensing motor Z and motor R, slight mouse is switched to four-wheel drive state naturally, enhances the adhesion of slight mouse and handling;

4) return spurt, can prepare the return after making a spurt after reaching home explores to search more excellent path, controller can recall its labyrinth information stored, and then calculates other optimal path that may exist, and then return starts to enter one that wherein thinks optimum;

5) labyrinth return, when slight mouse enters labyrinth return exploration, the sensor S1 of its navigation, S2, S5, S6 is by work, and give STM32F407 the photosignal reflected, judge after computing, to determine present position, present labyrinth through STM32F407, then generate PWM ripple by STM32F407 according to current labyrinth and send control signal to L6207D: will advance fast if enter the region searched for, if unknown return area then adopts normal speed to search for, and the moment upgrades its coordinate (X, Y), and judge that its coordinate is starting point, if words put exploration of making a return voyage and be masked as 0, slight mouse enters the sprint stage, juxtaposition spurt is masked as 1,

6) secondary spurt, after slight pindone becomes whole spurt process to reach home, slight mouse can be put exploration and is masked as 1, slight mouse return is explored and is got back to starting point, the level controlling two panels L6207D exports and slight mouse starting point central point is stopped by STM32F407, then the OUT1A of L6207D is readjusted, OUT2A, the level of OUT1B and OUT2B, motor X and motor Y is moved with contrary direction, and under gyrostatic control, turnback is revolved in original place, then stop 1 second, secondary transfers labyrinth information, and the optimum spurt path of slight mouse exploration is calculated according to improvement ant group algorithm (Improvedantcolonyalgorithm), then put spurt and be masked as 1, system enters the fast sprint stage, then controller adjusts motor M automatically according to the size of dash speed, changes vacuum cup to the absorption affinity on ground, meets friction needs during fast sprint.

9. monokaryon low speed four-wheel as claimed in claim 8 slight mouse spurt control method, it is characterized in that the labyrinth sprint stage, sensor S2 and S5 can judge the labyrinth retaining wall of left and right, and record stores current labyrinth retaining wall information, according to the labyrinth information of left and right barricade, slight mouse determines that it enters single wall navigation mode, two wall navigation mode or inertial navigation pattern; Its instantaneous acceleration of gyroscope real time record, speed and position in slight mouse fast sprint process, external interference is subject to when having departed from setting center when slight mouse walks fast, probe value will produce certain deviation with setting value, now gyroscope G1 and accelerometer A1 records movement velocity and the position of current slight mouse, Full-digital servo controller starts to carry out real-Time Compensation and finely tunes the PWM ripple input of four spindle motors, the attitude of slight mouse can be readjusted by this mode, make it come back to setting center.