CN105116893A - Double-core high-speed two-wheeled picomouse exploration controller and control method thereof - Google Patents
Double-core high-speed two-wheeled picomouse exploration controller and control method thereof Download PDFInfo
- Publication number
- CN105116893A CN105116893A CN201510521077.8A CN201510521077A CN105116893A CN 105116893 A CN105116893 A CN 105116893A CN 201510521077 A CN201510521077 A CN 201510521077A CN 105116893 A CN105116893 A CN 105116893A
- Authority
- CN
- China
- Prior art keywords
- stm32f407
- sensor
- slight mouse
- speed
- motor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Abstract
The invention discloses a double-core high-speed two-wheeled picomouse exploration controller, comprising a sensor device, a gyroscope device G, a vacuum suction device and a control unit module; the sensor device and the gyroscope device are both in signal connection with the control unit module; the control unit module is respectively in signal connection with motors X, Y and M. The controller solves the problem of skidding of a picomouse in walking through the vacuum suction device arranged under a chassis, and meanwhile automatically regulates the servo control of a vacuum suction direct current motor M according to the walking speed of the picomouse and ground conditions, thereby preventing the picomouse from being bound to labyrinth ground road conditions.
Description
Technical field
The present invention relates to minisize maze exploring robot field, particularly relate to the slight mouse of a kind of double-core high-speed two-wheeled and explore controller 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, explore in order to slight mouse or micro computer mouse can be obtained, information after spurt, 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 process in labyrinth exploring, then communicate 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 exploration task.Slight mouse is explored maze technique and combines 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 what slightly mouse explored maze technique carries out the talent that can cultivate large quantities of association area, and then promotes 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 slight mouse explores the raising of 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 searching system is all more rudimentary algorithm, and make the exploration of slight mouse in the middle of labyrinth generally all will spend the longer time, this makes cannot win victory in real contest.
(5) due to the minimizing of labyrinth retaining wall size, the distance that the single lattice of slight mouse are run reduces, and slight mouse is braked frequently and starts the workload having increased the weight of single-chip microcomputer, and single single-chip microcomputer cannot meet the requirement that slight mouse is explored.
(6) for two-wheel drive slight mouse General Requirements two motor PWM control signals driving it to move want synchronous, the single single-chip microcomputer servo-drive system of restriction by computing power is difficult to meet this condition, can not accurately walk on center line when slight mouse travels on straight way, being easy to bump against labyrinth retaining wall when exploring at a high speed, causing mission failure.
(7) owing to affecting by single-chip microcomputer capacity and algorithm, slight mouse cannot store labyrinth information, and the information all when running into power-down conditions will disappear, and this makes whole heuristic process to restart.
(8) mouse, when exploring labyrinth, is easy to be subject to external interference slightly, causes slight mouse to collide labyrinth retaining wall, finally cannot complete exploration task owing to not carrying out compensating in time.
(9) slightly mouse, in the process of exploration labyrinth, is hit wall situation all motor rotation blockage situation can occur once run into, cause motor immediate current excessive, burn out motor time serious.
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 technology, causes the domestic unit also not researching and developing this robot.Therefore, need design a kind of meet elementary person learn slight mouse solve labyrinth high speed explore controller.
Summary of the invention
The object of the invention is to solve the deficiencies in the prior art, provide the slight mouse of a kind of double-core high-speed two-wheeled to explore controller, to solve the problems such as slight mouse is skidded in heuristic process, sensor disturbs mutually, the processing time is slow.
The technical solution used in the present invention is: based on the exploration controller of STM32F407+FPGA chip, its flow chart as shown in Figure 4.Slight mouse two-wheeled exploration controller for core with FPGA microprocessor, gives full play to FPGA data processing speed feature faster, makes it give sb. his head the various algorithms of three axle DC servomechanisms, STM32F407 is freed from the Data processing of complexity.Wherein the set-point of position, speed, electric current is generated according to the navigation value of sensor S1, S2, S5, S6 by microprocessor STM32F407, the detection electric current of photoelectric encoder feedback and motor as the value of feedback of three Close loop servo control, generates the PWM ripple of control three spindle motor after the decoding of FPGA internal algorithm after the inner PID of FPGA regulates.STM32F407 realizes the signal processing algorithm (direct current generator compensated torque etc.) of part and the steering logic of FPGA simultaneously, and response is interrupted, and realizes data communication therebetween and stores live signal.
The slight mouse of double-core high-speed two-wheeled explores controller, comprises sensor device, gyroscope equipment G, vacuum suction apparatus and control module module; 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 and M; Described motor X and motor Y is positioned at both sides, and motor M is positioned at afterbody, and described gyroscope equipment G is positioned at center; 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 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 FPGA processor, described FPGA processor comprises diaxon walking servo control unit and single axle vacuum absorption servo control unit, described STM32F407 processor is electrically connected FPGA processor, and described diaxon walking servo control unit signal connects 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 and C2, described current sensor C1 is connected STM32F407 processor with C2 signal.
Further improve as of the present invention, described FPGA processor is by I/O port and the real-time communication of STM32F407 processor, and STM32F407 processor control FPGA processor turns on and off.
As a further improvement on the present invention, described motor X and Y is 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 exploration state of slight mouse is being monitored and computing based on the STM32F407+FPGA controller moment, avoid the generation of big current, so fundamentally 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, by walking servocontrol and the vacuum suction servocontrol of the slight mouse searching system of FPGA process, give full play to speciality and the program portable function of FPGA control aspect, 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.
3, the present invention realizes full SMD components material substantially, achieves veneer and controls, not only save control panel and take up room, and be conducive to alleviating of volume and weight, be conducive to the stability and the dynamic property that improve slight mouse searching system.
4, the data adopting the slight mouse of FPGA process two-wheeled digital three axle searching systems a large amount of due to this controller and algorithm, STM32F407 is freed from hard work amount, effectively prevent " race flies " of program, the slight mouse of three axle two-wheeleds is explored servo-drive system antijamming capability and greatly strengthens.
6, PWM modulation signal and direction signal is exported by FPGA, can Direct driver direct current generator by driving circuit, not only alleviate the burden of STM32F407, simplify interface circuit, and eliminate STM32F407 internal composition position, speeds control program, and the trouble of various pid algorithm, make the debugging of system simple.
7, by regulating motor M can effectively regulate vacuum cup to the absorption affinity on ground, the generation of slight mouse skidding when exploring has been stopped.
8, in slight mouse heuristic process, STM32F407 can carry out on-line identification to the torque of direct current generator X and motor Y and utilize the relation of direct current generator moment and electric current to compensate, and decreases controller is explored in motor torque shake impact on slight mouse.
9, in the controlling, according to the value of feedback of sensor S1, S2, S5, S6, STM32F407 determines that slight mouse departs from the side-play amount of center in heuristic process, then the pid parameter of FPGA inside is adjusted in real time according to different side-play amounts, easily realize segmentation P, PD, PID to control and nonlinear PID controller, make system have certain self-adaptation.
10, the interruptive command of FPGA can well solve slight mouse run in heuristic process hit wall situation occur motor rotation blockage, utilize interruptive command can detect current of electric export exceed setting value time send request, then STM32F407 blocks the PWM output of FPGA immediately, release High-speed DC motor X and motor Y, thus efficiently solve stall problem.
11, owing to having memory function, can transfer the labyrinth information explored easily after this makes slight mouse power down, the time that secondary is explored and path reduce greatly.
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 that slight mouse servo-control system explores director demon block diagram.
Fig. 5 is schematic diagram of the present invention.
Fig. 6 is theory diagram of the present invention.
Fig. 7 is the catenation principle figure of STM32F407 processor of the present invention and FPGA.
Fig. 8 is that slight mouse is explored the way forward schematic diagram.
Fig. 9 is that slight mouse explores the schematic diagram that oppositely advances.
Figure 10 is that slight mouse explores right-hand rotation schematic diagram.
Figure 11 is that slight mouse explores left-hand rotation schematic diagram.
Embodiment
Below in conjunction with Fig. 1 to Figure 11, 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), F1 system class (78MHz), 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 more suitable for the signal transacting of slight mouse diaxon servo-drive system relative to other STM32F system class.
FPGA is the abbreviation of English FieldProgrammableGateArray, i.e. field programmable gate array, is the product further developed on the basis of the programming devices such as PAL, GAL, EPLD.It occurs as a kind of semi-custom circuit in special IC (ASIC) field, namely solves the deficiency of custom circuit, overcomes again the shortcoming that original programming device gate circuit number is limited.
FPGA makes user according to the design needs of oneself, can reconfigure connection, design the special IC of oneself within the shortest time by specific placement-and-routing instrument to its inside, so just reduces cost, shortens the construction cycle.Because FPGA adopts the design philosophy of software implementation to realize the design of hardware circuit, so just make to have good reusable and amendment property based on FPGA designed system, the Multi-asis servo system being particularly suitable for High-speed Control uses.
As shown in the figure, the slight mouse of double-core high-speed two-wheeled explores controller, comprises sensor device, gyroscope equipment G, vacuum suction apparatus and control module module; 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 and M; Described motor X and motor Y is positioned at both sides, and motor M is positioned at afterbody, and described gyroscope equipment G is positioned at center; 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 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 FPGA processor, described FPGA processor comprises diaxon walking servo control unit and single axle vacuum absorption servo control unit, described STM32F407 processor is electrically connected FPGA processor, and described diaxon walking servo control unit signal connects 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 exploration controller of the present invention accurately control the advance of slight mouse and turn to further, also include current sensor C1 and C2, described current sensor C1 is connected STM32F407 processor with C2 signal.
For expanding the processing power of two core processors further, accomplish that the division of labor is clear and definite, work is interference-free, and described FPGA processor is by I/O port and the real-time communication of STM32F407 processor, and STM32F407 processor control FPGA processor turns on and off.
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.
Slight mouse is placed on labyrinth starting point, under power-on state, slight mouse is introduced into self-locking state, then mouse mouse relies on front slightly, the infrared sensor S1 that keeps in obscurity in left and right side, S2, S5, S6 according to actual navigational environment transformation parameter to the STM32F407 in dual-core controller, then STM32F407 is converted into actual navigational environment the position of slight mouse diaxon motor servo system control, speed and acceleration parameter command value, then with FPGA communication, FPGA is again in conjunction with the feedback of photoelectric encoder and current of electric, synchronous servo through its inner servo control algorithm process two individual motor controls, and process data communication to STM32F407.In whole motion process, there is gyroscope G1 to carry out position detection and second compensation in real time, ensure the stability of the slight mouse high speed detector of diaxon.
The slight mouse of double-core high-speed two-wheeled explores control method, comprises the steps:
1) enabling signal confirms, waits at starting point coordinate (0,0) the exploration order that controller sends, and after receiving exploration task, can start to explore to terminal (F, F), (F, 10), (10, F), (10,10) along starting point, 2) obstacle judges, sensor S1, the S6 in front and can judge 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, the process if the interrupt response of STM32F407 is not able to do in time, and the X motor of slight mouse and Y motor are by original place self-locking, then STM32F407 judges that front information is determined in labyrinth, prevents information from judging by accident for bis-times, 3) track search, travel forward along XY axle slight mouse, if determine do not have barricade to enter the range of movement in front in any one side's center of a lattice, then mouse will store its coordinate (X slightly, and the location parameter of the lattice that travel forward is transferred to STM32F407 Y), 4) path judges, when slight mouse arrives (F, F), (F, 10), (10, F), when the terminal or in (10,10) prepares return exploration, controller can recall its labyrinth stored, then calculate the optimal path that may exist according to quick labyrinth algorithm, return starts to enter one that wherein thinks optimum, 5) make a return voyage, slight mouse enter labyrinth normally make a return voyage run time, and the sensor S1 of its navigation, S2, S5, S6 is by work, and give STM32F407 the photosignal reflected, FPGA is given after STM32F407 judges, communication is carried out with STM32F407 by after FPGA computing, then control signal is sent to determine to the motor X of navigation and motor Y by controller: 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 (0, 0), if words put exploration of making a return voyage and be masked as 0.
For improving exploring the way forward of slight mouse of the present invention further, when sensor S2 and S5 can judge the barricade of left and right, and record stores current labyrinth retaining wall information, slight mouse enters two wall navigation mode, single wall navigation mode or inertial navigation pattern according to the labyrinth information of working direction left and right barricade.
The present invention is following work:
The slight mouse of double-core two-wheeled is explored at a high speed controller and 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, data storage, I/O control of slight mouse system, the two-axis synchronization servo control unit that wherein workload is maximum and single axle vacuum absorption servo control unit give FPGA process, remaining comprises completing of master system and gives STM32F407 and complete, so just achieve the division of labor of STM32F407 and FPGA, therebetween also can carry out communication simultaneously, carry out exchanges data in real time and call.
1) before slight mouse does not receive exploration order, it generally can at starting point coordinate (0,0) the exploration order that controller sends is waited for, if all are normal, first controller opens vacuum draw motor M, is first aspirated micro vacuum sucker 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.Once after receiving exploration task, can start to explore to terminal (F, F), (F, 10), (10, F), (10,10) along starting point.
2) mouse is placed on starting point coordinate (0 slightly, 0), after receiving task its front sensor S1, S6 and can judge 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, the process if the interrupt response of STM32F407 is not able to do in time, the X motor of slight mouse and Y motor are by original place self-locking, then STM32F407 judges that front information is determined in labyrinth, prevents information from judging by accident for bis-times;
3) travel forward along Y-axis slight mouse, if determine do not have barricade to enter the range of movement in front in any one side's center of a lattice, then mouse will store its coordinate (X slightly, Y), and the location parameter of the lattice that travel forward is transferred to STM32F407, according to the requirement exploring controller speed and acceleration, generate the instruction set-point of diaxon direct current generator three closed loop servo system by STM32F407 and be transferred to FPGA, then FPGA is again in conjunction with photoelectric encoder and current sensor C1, the feedback of C2 generates the PWM ripple driving direct current generator motion, PWM ripple is promoted slight mouse after being amplified by drive axle and travels forward.In slight mouse forward movement, 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 the process of labyrinth is explored slight mouse, when slight mouse rapid discovery has departed from setting center, within the new sampling period, STM32F407 regenerates the instruction set-point of three closed loop servo systems according to the state of current S2, S5 and is transferred to FPGA, then FPGA is again in conjunction with the PWM ripple input of the feedback fine setting motor of photoelectric encoder and current sensor C1, C2, diaxon servo-drive system starts to carry out real-Time Compensation to adjust the attitude of slight mouse, makes it come back to setting center.When slight mouse moves a lattice distance new address of arrival under the control of gyroscope G1, microprocessor will upgrade its coordinate for (X, Y+1), under the prerequisite of Y+1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10,10) one of them, if not continuation being explored and upgrading its coordinate, if it is notification controller searches target, then put exploration of making a return voyage and be masked as 1, slight mouse prepares return and explores;
4) if judge that front has barricade to enter range of movement slight mouse along sensor S1 and S6 in Y-axis forward movement, and now sensor S2, when having barricade about S5 judges respectively, slight mouse will store now coordinate (X, Y), controller calculates the location parameter travelling forward and stop according to the feedback of sensor S1 and S6, according to the requirement exploring controller speed and acceleration, generate the instruction set-point of direct current generator closed loop servo system by STM32F407 and be transferred to FPGA, FPGA is again in conjunction with photoelectric encoder and current sensor C1, the feedback of C2 generates the PWM ripple of new driving diaxon direct current generator, PWM ripple moves before driving two independent direct current motor X and motor Y-direction.In forward movement, sensor S2, S5 judge the barricade of left and right in real time, and feed back current labyrinth retaining wall information, and slight mouse enters two wall navigation deceleration mode, and under gyrostatic help, slight mouse realizes arranging stop parking.The enable FPGA of STM32F407 the PWM ripple adjusting slight mouse two motors export, and make two permanent magnet DC motor direction of motion contrary, slight mouse original place realizes 180 degree, accurate original place and turns under gyrostatic control, and starts counter motion along Y-axis.
Slight mouse oppositely explores principle and slight mouse along Y-axis, and to explore principle along Y-axis forward similar, in slight mouse forward movement, sensor S2 and S5 can judge the barricade 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 pattern, two wall navigation mode or inertial navigation pattern, the instantaneous acceleration of the slight mouse of gyroscope real time record, speed and positional information.When slight mouse moves a lattice distance new address of arrival under gyrostatic control, microprocessor will upgrade its coordinate for (X, Y-1), under the prerequisite of Y-1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10,10) one of them, if not continuation explored going forward is upgraded its coordinate, if it is notification controller searches 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 the now sensor S2 of left and right, S5 judge that there is barricade left and right does not have a barricade time, slight mouse will store now coordinate (X, Y), the location parameter travelling forward and stop is calculated according to the feedback of sensor S1 and S6, according to the requirement exploring controller speed and acceleration, generate the instruction set-point of diaxon direct current generator three closed loop servo system by STM32F407 and be transferred to FPGA, FPGA is again in conjunction with photoelectric encoder and current sensor C1, the feedback of C2 generates the PWM ripple driving direct current generator motion, PWM ripple is promoted slight mouse after being amplified by drive axle and travels forward.In slight mouse forward movement, sensor S2 can judge left barricade, and record stores current labyrinth retaining wall information, slight mouse is determined to enter single wall navigation mode according to the labyrinth information of left barricade, then the instantaneous acceleration of the slight mouse of gyroscope real time record, speed and positional information.When slight mouse rapid discovery has departed from setting center, the value of sensor S2 will change, within the new sampling period, the instruction set-point that STM32F407 regenerates three closed loop servo systems according to current state is transferred to FPGA, then FPGA is again in conjunction with the PWM ripple input of the feedback fine setting motor of photoelectric encoder and current sensor C1, C2, diaxon servo-drive system starts to carry out real-Time Compensation to adjust the attitude of slight mouse, it is made to come back to setting center, until mouse arrives central point accurate stopping slightly.The enable FPGA of STM32F407 the PWM ripple adjusting slight mouse two motors export, and make two permanent magnet DC motor direction of motion contrary, slight mouse is original place 90-degree rotation to the right under gyroscope accurately controls, then along X-axis positive movement.
Slight mouse along in X-axis positive movement process, controller is transferred to STM32F407 the location parameter of the lattice that travel forward, according to the requirement exploring controller speed and acceleration, generate the instruction set-point of diaxon direct current generator three closed loop servo system by STM32F407 and be transferred to FPGA, FPGA generates in conjunction with the feedback of photoelectric encoder and current sensor C1, C2 the PWM ripple driving direct current generator motion again, and PWM ripple is promoted slight mouse after being amplified by drive axle and travels forward.In slight mouse forward movement, sensor S2 and S5 can judge the barricade 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 pattern, two wall navigation mode or inertial navigation pattern.The instantaneous acceleration of the slight mouse of gyroscope real time record, speed and positional information.When slight mouse rapid discovery has departed from setting center, within the new sampling period, the instruction set-point that STM32F407 regenerates three closed loop servo systems according to the state of current S2, S5 is transferred to FPGA, FPGA is again in conjunction with the PWM ripple input of the feedback fine setting motor of photoelectric encoder and current sensor C1, C2, diaxon servo-drive system starts to carry out real-Time Compensation to adjust the attitude of slight mouse, makes it come back to setting center.When slight mouse moves a lattice distance new address of arrival under gyrostatic control, microprocessor will upgrade its coordinate for (X+1, Y), under the prerequisite of X+1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10,10) one of them, if not by continuation upgrade its coordinate, if words notification controller search target, then put exploration of making a return voyage and be masked as 1, slight mouse prepares return and explores;
6) if having barricade to enter the range of movement in front slight mouse in Y-axis forward movement, and when now sensor S2, S5 of left and right judge that right has barricade and left not have a barricade, slight mouse will store now coordinate (X, Y), slight mouse enters left-hand rotation exploration state, and slight mouse turns left to explore principle and slight mouse and turns right that to explore principle similar.When slight mouse completes the new address of arrival under gyrostatic control, microprocessor will upgrade its coordinate for (X-1, Y), under the prerequisite of X-1<1F, judge that its coordinate is (F, F), (F, 10), (10, F), (10,10) one of them, if not by continuation upgrade its coordinate, if words notification controller search target, then put exploration of making a return voyage and be masked as 1, slight mouse prepares return and explores;
7) when slight mouse arrives (F, F), (F, 10), (10, F), (10,10) prepare return explore time, controller can recall its labyrinth stored, then calculate the optimal path that may exist according to quick labyrinth algorithm, return starts to enter one that wherein thinks optimum.
8) slight mouse enter labyrinth normally make a return voyage run time, and the sensor S1 of its navigation, S2, S5, S6 is by work, and give STM32F407 the photosignal reflected, FPGA is given after STM32F407 judges, communication is carried out with STM32F407 by after FPGA computing, then control signal is sent to determine to the motor X of navigation and motor Y by controller: 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 (0, 0), 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.
9) in order to slight mouse coordinate computing function accurately can be realized, in heuristic process, 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 transition, then illustrate that slight mouse enters the interface point of labyrinth retaining wall and pillar, mouse prepares to exit current labyrinth grid, STM32F407 can carry out fine compensation according to the distance of the current operation of slight mouse, the method thoroughly can be eliminated slight mouse and explore in labyrinth and travel accumulative error, lay a good foundation for slight mouse accurately solves.
10) in slight mouse operational process, STM32F407 can carry out on-line identification to the torque of motor X and motor Y, when the torque of motor is subject to the larger shake of external interference appearance, compensation time controller can utilize the relation of motor torque and electric current to carry out, decrease the impact of motor torque shake on slight mouse rapid discovery, ensure that the accuracy in labyrinth during exploration.
11) when slight pindone becomes whole heuristic process to get back to starting point (0,0), control FPGA makes slight mouse central point stop by STM32F407, and the PWM ripple then readjusting FPGA exports, motor X and motor move with contrary direction, and under gyrostatic control, turnback is revolved in slight mouse original place, then stop 1 second, labyrinth information transferred by controller, finally calculate optimum spurt path according to algorithm, then put spurt and be masked as 1, system enters the sound lunge stage.
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 of double-core high-speed two-wheeled explores controller, it is characterized in that comprising sensor device, gyroscope equipment G, vacuum suction apparatus and control module module;
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 and M;
Described motor X and motor Y is positioned at both sides, and motor M is positioned at afterbody, and described gyroscope equipment G is positioned at center;
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 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 FPGA processor, described FPGA processor comprises diaxon walking servo control unit and single axle vacuum absorption servo control unit, described STM32F407 processor is electrically connected FPGA processor, and described diaxon walking servo control unit signal connects single axle vacuum absorption servo control unit.
2. the slight mouse of double-core high-speed two-wheeled according to claim 1 explores controller, it 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 of double-core high-speed two-wheeled according to claim 1 explores controller, it 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 of double-core high-speed two-wheeled according to claim 1 explores controller, and it is characterized in that also including current sensor C1 and C2, described current sensor C1 is connected STM32F407 processor with C2 signal.
5. the slight mouse of double-core high-speed two-wheeled as claimed in any of claims 1 to 4 explores controller, it is characterized in that described FPGA processor is by I/O port and the real-time communication of STM32F407 processor, STM32F407 processor control FPGA processor turns on and off.
6. the slight mouse of double-core high-speed two-wheeled according to claim 5 explores controller, and it is characterized in that described motor X and Y is high-speed permanent magnetic direct current generator, described motor M is DC micromotor.
7. the slight mouse of double-core high-speed two-wheeled as claimed in any of claims 1 to 4 explores controller, 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. the slight mouse of double-core high-speed two-wheeled as claimed in claim 1 explores a control method, it is characterized in that comprising the steps:
1) enabling signal confirms, waits at starting point coordinate (0,0) the exploration order that controller sends, and after receiving exploration task, can start to explore to terminal (F, F), (F, 10), (10, F), (10,10) along starting point;
2) obstacle judges, sensor S1, the S6 in front and can judge 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, the process if the interrupt response of STM32F407 is not able to do in time, and the X motor of slight mouse and Y motor are by original place self-locking, then STM32F407 judges that front information is determined in labyrinth, prevents information from judging by accident for bis-times;
3) track search, travel forward along XY axle slight mouse, if determine do not have barricade to enter the range of movement in front in any one side's center of a lattice, then mouse will store its coordinate (X slightly, and the location parameter of the lattice that travel forward is transferred to STM32F407 Y);
4) path judges, when slight mouse arrives (F, F), (F, 10), (10, F), when the terminal or in (10,10) prepares return exploration, controller can recall its labyrinth stored, then calculate the optimal path that may exist according to quick labyrinth algorithm, return starts to enter one that wherein thinks optimum;
5) make a return voyage, slight mouse enter labyrinth normally make a return voyage run time, and the sensor S1 of its navigation, S2, S5, S6 is by work, and give STM32F407 the photosignal reflected, FPGA is given after STM32F407 judges, communication is carried out with STM32F407 by after FPGA computing, then control signal is sent to determine to the motor X of navigation and motor Y by controller: 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 (0, 0), if words put exploration of making a return voyage and be masked as 0.
9. the slight mouse of double-core high-speed two-wheeled as claimed in claim 8 explores control method, it is characterized in that in described step 3 track search, when sensor S2 and S5 can judge the barricade of left and right, and record stores current labyrinth retaining wall information, slight mouse enters two wall navigation mode, single wall navigation mode or inertial navigation pattern according to the labyrinth information of working direction left and right barricade.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510521077.8A CN105116893A (en) | 2015-08-24 | 2015-08-24 | Double-core high-speed two-wheeled picomouse exploration controller and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510521077.8A CN105116893A (en) | 2015-08-24 | 2015-08-24 | Double-core high-speed two-wheeled picomouse exploration controller and control method thereof |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN105116893A true CN105116893A (en) | 2015-12-02 |
Family
ID=54664911
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510521077.8A Pending CN105116893A (en) | 2015-08-24 | 2015-08-24 | Double-core high-speed two-wheeled picomouse exploration controller and control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105116893A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105953026A (en) * | 2016-06-08 | 2016-09-21 | 江苏若博机器人科技有限公司 | Three-core two-shaft crawler type rapid natural gas pipeline robot control system |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20060238156A1 (en) * | 2005-04-25 | 2006-10-26 | Lg Electronics Inc. | Self-moving robot capable of correcting movement errors and method for correcting movement errors of the same |
| CN201886351U (en) * | 2010-10-25 | 2011-06-29 | 陕西科技大学 | Micro-mouse for labyrinth sprint |
| CN103472830A (en) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | Ultra-fast exploring controller of two-wheel micro-mouse based on dual processors |
| CN204086988U (en) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | A kind of labyrinth machine mouse system resolved for labyrinth |
-
2015
- 2015-08-24 CN CN201510521077.8A patent/CN105116893A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20060238156A1 (en) * | 2005-04-25 | 2006-10-26 | Lg Electronics Inc. | Self-moving robot capable of correcting movement errors and method for correcting movement errors of the same |
| CN201886351U (en) * | 2010-10-25 | 2011-06-29 | 陕西科技大学 | Micro-mouse for labyrinth sprint |
| CN103472830A (en) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | Ultra-fast exploring controller of two-wheel micro-mouse based on dual processors |
| CN204086988U (en) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | A kind of labyrinth machine mouse system resolved for labyrinth |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105953026A (en) * | 2016-06-08 | 2016-09-21 | 江苏若博机器人科技有限公司 | Three-core two-shaft crawler type rapid natural gas pipeline robot control system |
| CN105953026B (en) * | 2016-06-08 | 2019-04-09 | 江苏若博机器人科技有限公司 | A kind of quick natural gas line robot control system of three core, two axis crawler type |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105334853A (en) | Double-core high-speed four-wheel miniature micro-mouse sprint controller | |
| CN102841618A (en) | Novel quick-exploration automatic control system for microcomputer rat | |
| CN105005249A (en) | Fully automatic four-wheel two-core high speed fire extinguishing robot servo controller | |
| CN105159325A (en) | STM32F407 and FPGA-based two-wheel high-speed fire-extinguishing robot servo controller | |
| CN105137983A (en) | Double-core high speed two-wheel mini mouse spurting controller and control method | |
| CN105045289A (en) | Single-wheel full-automatic high-speed fire-fighting robot servo control system | |
| CN105045288A (en) | Single-wheel full-automatic middle-speed fire-fighting robot servo control system | |
| CN105116897A (en) | Double-core high-speed four-wheeled picomouse full digital navigation servo controller | |
| CN105137974A (en) | Six-wheel double-core automatic intermediate speed fire extinguishing robot servo controller | |
| CN105169610A (en) | Dual-core servo control system based on two-wheeled high-speed fully automatic fire extinguishing robot | |
| CN103472840A (en) | Probe controller based on ARM9 four-wheeled microcomputer mouse | |
| CN105320146A (en) | Single-wheel dual-core middle-speed fire extinguishing robot servo control system | |
| CN105116891A (en) | Double-core middle-speed two-wheeled picomouse sprint controller and control method thereof | |
| CN105204502A (en) | Dual-core medium-speed six-wheel little mouse exploration controller and control method thereof | |
| CN103472835A (en) | Fast sprint controller of four-wheel micro-mouse based on dual processors | |
| CN105137979A (en) | Double-core intermediate speed six-wheel mini mouse spurting controller and control method | |
| CN105005308A (en) | Six-wheel fire-fighting robot servo controller based on STM32F407 | |
| CN105137988A (en) | Single-core low-speed two-wheel mini mouse full digital navigation servo controller and control method | |
| CN105116898A (en) | Single-core low-speed two-wheeled picomouse sprint controller | |
| CN105159323A (en) | Six-wheel single-core fully automated type low-speed fire-extinguishing robot servo controller | |
| CN105137991A (en) | Single-core low-speed two-wheel mini mouse exploration controller | |
| CN105068558A (en) | Single-wheeled dual-core high speed fire-fighting robot servo control system | |
| CN105159324A (en) | Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller | |
| CN105116893A (en) | Double-core high-speed two-wheeled picomouse exploration controller and control method thereof | |
| CN105137978A (en) | Double-core high-speed four-wheel mini mouse controller |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20151202 |
|
| RJ01 | Rejection of invention patent application after publication |