CN105159324A - Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller - Google Patents
Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller Download PDFInfo
- Publication number
- CN105159324A CN105159324A CN201510519930.2A CN201510519930A CN105159324A CN 105159324 A CN105159324 A CN 105159324A CN 201510519930 A CN201510519930 A CN 201510519930A CN 105159324 A CN105159324 A CN 105159324A
- Authority
- CN
- China
- Prior art keywords
- motor
- brushless motor
- control signal
- firefighting robot
- sensor
- 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
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller. A first motor, a second motor, a third motor, a fifth motor, a sixth motor and a seventh motor are in the form of a DC brushless motor respectively. A fourth motor is in the form of a DC motor. A processor is in the form of a dual-core processor and is composed of an STM32F407 and an MC58113. Due to the adoption of the above arrangement, the MC58113 is adopted as the processing core of a control panel in the STM 32F407 and the MC58113 combined six-wheel and dual-core brand-new control mode, and the seven-axis hybrid servo control over the digital signal real-time processing process is realized by a six-axis DC brushless motor and a single-axis DC motor. Based on the vacuum adsorption technique, the slipping phenomenon of a robot during the walking process can be completely eliminated. The accuracy of the robot in searching the location of a room is effectively enhanced.
Description
Technical field
The present invention relate to multi-axis robot etc. field, relating to a kind of six takes turns firefighting robot automatic control system, particularly relates to a kind of six and takes turns double-core middling speed firefighting robot servo controller.
Background technology
Firefighting robot is that in one, in simulating reality life, the mankind find harmful burning things which may cause a fire disaster and can a kind of Intelligent Robot of automatic distinguishing burning things which may cause a fire disaster.Generally, match type firefighting robot can move in a planar structure house model, finds a candle representing burning things which may cause a fire disaster and it extinguished under working rule instructs with the shortest time.The process of robot process fire alarm in simulating reality family.Candle represents the burning things which may cause a fire disaster that family is lighted, and robot must find and extinguish burning things which may cause a fire disaster.The bottom of candle flame is by overhead 15-20cm is high.Candle is the Chinese wax candle of diameter 1-2cm.The exact height of candle flame and size are uncertain, changes, and are determined by the environment of candle condition and surrounding.Candle will be placed in a room of competition area randomly, regardless of flame specifically what size after match starts, all requires that robot can find candle.
In real match, in order to strengthen competition difficulty, competition area are divided into the mode standard of n*n lattice, and what the most often adopt is the uniform pattern of 8*8 lattice, and its competition area two-dimensional structure as shown in Figure 1, will find burning things which may cause a fire disaster and extinguish in 64 lattice rooms by firefighting robot.Search in burning things which may cause a fire disaster map in the two dimension of Fig. 1, the material of wall is wooden general and can be reflective, and the length of every block barricade is that 60cm is long, and height is at 27-34cm.Ground, competition area is smooth, and the floor in place is black.Black is all brushed in any gap on place.The gap in place is no more than 5mm.Some robots may use foam, and powder or other material carry out the flame of extinguishing candles.Because after the match of each robot, the quality in cleaning place directly has influence on surface state, face, old place does not ensure all to keep absolute black during the games whole.Once start, firefighting robot oneself must control navigation under the intervention of nobody, but not manual control, searching the stability in burning things which may cause a fire disaster process to test firefighting robot, it cannot collide or contact wall, otherwise will be caught a packet.
A complete firefighting robot is roughly divided into following components:
1) motor: actuating motor is the power source of firefighting robot, it performs according to the instruction of microprocessor the relevant action that firefighting robot walks on two dimensional surface.
2) algorithm: algorithm is the soul of firefighting robot.Firefighting robot must adopt certain intelligent algorithm could arrive the motion in an other lattice room quickly and accurately from room lattice, then finds burning things which may cause a fire disaster, and opens self-contained dry ice controller, put out burning things which may cause a fire disaster.
3) microprocessor: microprocessor is the core of firefighting robot is the brain of firefighting robot.The information that firefighting robot is all, comprises room wall information, fire location information, and motor status information etc. all needs through microprocessor processes and makes corresponding judgement.
Firefighting robot combines multi-subject knowledge, for promoting the manipulative ability of students, team collaboration's ability and innovation ability, promotes that the scope of one's knowledge of the digestion of student classroom knowledge and expansion student is all very helpful.The unit of domestic this robot of research and development is more, but the robot of research and development is relatively backward, and the firefighting robot structure of research and development is as Fig. 2, and long-play finds to there is a lot of safety problem, that is:
(1) mostly what adopt as the topworks of firefighting robot is stepper motor, often can run into pulse-losing causes motor desynchronizing phenomenon to occur, cause occurring mistake to the memory of position, firefighting robot cannot seek burning things which may cause a fire disaster, or after fire extinguishing, starting point cannot be got back to by robot;
(2) owing to adopting stepper motor, make organism fever relatively more serious, need sometimes to carry out installing heat abstractor additional, robot overall weight is increased;
(3) owing to adopting stepper motor, the mechanical noise that system is operated increases greatly, is unfavorable for environmental protection;
(4) owing to adopting stepper motor, its motor body is all generally heterogeneous structure, and control circuit needs to adopt multiple power tube, makes control circuit relatively complicated, and adds controller price;
(5) owing to adopting stepper motor, the occasion that system is generally not suitable for speed is higher is run, and easily produces vibration, sometimes may contact wall during high-speed motion, causes finding burning things which may cause a fire disaster failure;
(6) because firefighting robot will brake frequently and start, increased the weight of the workload of single-chip microcomputer, single single-chip microcomputer cannot meet the requirement that firefighting robot starts fast and stops;
(7) what relatively adopt is all the plug-in components that some volume ratios are larger, and make firefighting robot control system take larger space, weight is relatively all heavier;
(8) owing to disturbing by surrounding environment labile factor, singlechip controller often there will be exception, and cause firefighting robot out of control, antijamming capability is poor;
(9) burning things which may cause a fire disaster process is found for two-wheeled firefighting robot, the pwm control signal of two motor of General Requirements wants synchronous, due to the restriction by single-chip microcomputer computing power, single single-chip microcomputer servo-drive system is difficult to meet this condition, firefighting robot walking navigation is made to be difficult to control, particularly for worse off cake when walking fast;
(10) owing to adopting two power wheels to drive, acceleration during in order to meet fast searching burning things which may cause a fire disaster 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 the firefighting robot system energy;
(11) based on the firefighting robot that monokaryon controls, particularly for the firefighting robot that many wheels control, because the algorithm of single core processor process is more, arithmetic speed is not very fast, is unfavorable for that robot runs up.
Therefore, need to redesign based on monolithic processor controlled two-wheeled firefighting robot controller existing, seek a kind of economic and practical can use in reality six take turns firefighting robot servo-drive system.
Summary of the invention
The technical matters that the present invention mainly solves is to provide a kind of six and takes turns double-core middling speed firefighting robot servo controller, better can improve the adaptive faculty of firefighting robot to complex environment, six wheel constructions are adopted to instead of original two-wheeled and four wheel constructions, take into account the mid-advantage turned to of two-wheeled, the present invention adopt forerunner+in drive+six wheel drive structures of rear-guard, two DC brushless motor power of mid-driving are larger, four DC brushless motor power of preposition and rearmounted driving are less, only just start when power demand is higher, play power-assisted effect, owing to adopting six wheel drive technology, take turns all dynamic in before and after firefighting robot, can be different with ambient conditions and demand torque is distributed on all wheel in front and back by different proportion by exploring ground, to improve the driveability of firefighting robot.
For solving the problems of the technologies described above, the technical scheme that the present invention adopts is: provide a kind of six and take turns double-core middling speed firefighting robot servo controller, comprise battery, processor, first motor, second motor, 3rd motor, 4th motor, 5th motor, 6th motor, 7th motor and firefighting robot, described battery provides separately the processor described in electric current driving, and described processor sends the first control signal respectively, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal, by the first described control signal, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal control the first described motor respectively, second motor, 3rd motor, 4th motor, 5th motor, the motion of firefighting robot is controlled again, the first described motor after the signal syntheses of the 6th motor and the 7th motor, second motor, 3rd motor, 5th motor, 6th motor and the 7th motor adopt DC brushless motor, and the 4th described motor adopts direct current generator, and wherein, described processor adopts dual core processor, comprises STM32F407 and MC58113.
In a preferred embodiment of the present invention, described battery adopts lithium ion battery.
In a preferred embodiment of the present invention, the first described control signal, the second control signal, the 3rd control signal, the 4th control signal, the 5th control signal, the 6th control signal and the 7th control signal are PWM wave control signal.
In a preferred embodiment of the present invention, the inside of described processor is also provided with master system and kinetic control system, described master system comprises room and explores module, room storage module, path read module, human-computer interface module and online output module, described kinetic control system comprises based on MC58113 seven axle synchronized mixes servo control module, coordinate setting module and I/O control module, wherein, described comprises six axle DC brushless motor firefighting robots search servo control module and single axle vacuum sucker suction servo control module based on MC58113 seven axle synchronized mixes servo control module.
In a preferred embodiment of the present invention, described six take turns firefighting robot servo controller also comprises ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope, and described ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope are connected with processor communication.
In a preferred embodiment of the present invention, the quantity of described ultrasonic sensor is 6, the quantity of current sensor is 7, photoelectric sensor, voltage sensor, acceierometer sensor and gyrostatic quantity are 1.
In a preferred embodiment of the present invention, described six take turns firefighting robot servo controller also comprises photoelectric encoder, and described photoelectric encoder is arranged on the first motor, the second motor, the 3rd motor, the 4th motor, the 5th motor, the 6th motor and the 7th motor respectively.
The invention has the beneficial effects as follows: of the present invention six take turns double-core middling speed firefighting robot servo controller, the stability of multiaxis firefighting robot walking and the requirement of rapidity can not be met for overcoming single-chip microcomputer, give up the single single-chip microcomputer mode of operation that domestic firefighting robot adopts, under the prerequisite absorbing external Dynamic matrix control thought, autonomous invention takes turns the brand-new control model of double-core based on six of STM32F407+MC58113, control panel take MC58113 as process core, seven axles realizing six axle DC brushless motors and single shaft direct current generator mix servo-controlled digital signal and process in real time, and respond various interruption, realize the real-time storage of data-signal, dual core processor frees STM32F407 from seven axle servo control algorithm of complexity, realize the signal processing algorithm of part and the steering logic of MC58113, and response is interrupted, realize data communication and store live signal.Meanwhile, the introducing of vacuum suction technology completely eliminates the generation of firefighting robot walking skidding, effectively improves the accuracy that firefighting robot searches room location.
Accompanying drawing explanation
In order to be illustrated more clearly in the technical scheme in the embodiment of the present invention, below the accompanying drawing used required in describing embodiment is briefly described, apparently, accompanying drawing in the following describes is only some embodiments of the present invention, for those of ordinary skill in the art, under the prerequisite not paying creative work, other accompanying drawing can also be obtained according to these accompanying drawings, wherein:
Fig. 1 is firefighting robot room schematic diagram;
Fig. 2 is based on monolithic processor controlled two-wheeled firefighting robot schematic diagram;
Fig. 3 is for take turns firefighting robot structural drawing based on STM32F407 six;
Fig. 4 is for take turns firefighting robot schematic diagram based on STM32F407+MC58113 six;
Fig. 5 is for take turns firefighting robot servo programe block diagram based on STM32F407+MC58113 six;
Fig. 6 is firefighting robot traffic direction schematic diagram;
Fig. 7 is right-hand rotation schematic diagram;
Fig. 8 is left-hand rotation schematic diagram.
Embodiment
Be clearly and completely described to the technical scheme in the embodiment of the present invention below, obviously, described embodiment is only a part of embodiment of the present invention, instead of whole embodiments.Based on the embodiment in the present invention, those of ordinary skill in the art, not making other embodiments all obtained under creative work prerequisite, belong to the scope of protection of the invention.
As shown in Figure 4, the present embodiment comprises:
A kind of six take turns double-core middling speed firefighting robot servo controller, comprise battery, processor, first motor, second motor, 3rd motor, 4th motor, 5th motor, 6th motor, 7th motor and firefighting robot, described battery provides separately the processor described in electric current driving, and described processor sends the first control signal respectively, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal, by the first described control signal, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal control the first described motor respectively, second motor, 3rd motor, 4th motor, 5th motor, the motion of firefighting robot is controlled again, the first described motor after the signal syntheses of the 6th motor and the 7th motor, second motor, 3rd motor, 5th motor, 6th motor and the 7th motor adopt DC brushless motor, and the 4th described motor adopts direct current generator, and wherein, described processor adopts dual core processor, comprises STM32F407 and MC58113.In the present embodiment, the first described motor, the second motor, the 3rd motor, the 5th motor, the 6th motor and the 7th motor respectively correspondence markings are DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, and the 4th motor is labeled as direct current generator M.
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 multiaxis firefighting robot servo-drive system.
MC5X000 movement sequence control chip is the third generation servocontrol chip that PMD company develops based on High Performance Motion Control, has been widely used in the application of medical treatment, science, Automated condtrol and robot.Can realize single shaft control and multi-axle motor control in single-chip, Mc5X000 family chip is programming device, can control for the position of direct current generator, DC brushless motor and stepper motor, speed.MC5X000 family chip can pass through SPI(Serial Peripheral Interface (SPI)), CAN2.0B, or RS232/485 serial line interface establishes communication with main.Its inside comprises S shape and trapezoidal generator.According to dissimilar Electric Machine Control requirement, MC58000 family chip can realize servocontrol, speed regulates or for the pulse/direction controlling of stepper motor, integrated chip overcurrent protection simultaneously, overvoltage/under-voltage protection and overheat protector etc.The MC5X000 movement sequence control chip of all PMD provides one to be carried out initialization flexibly and arranges and controlled motion axle with powerful instruction set, and has supervision performance, and adds the synchronous function of each axle, and this point is superior to LM629 far away.
MC5X113 movement sequence control chip has four sections: wherein MC51113 realizes the control of direct current generator, MC53113 realizes the control of three-phase brushless dc motor, MC54113 realizes the control of two-phase stepping motor, and MC58113 can adopt software design patterns to control to select direct current generator, three-phase direct-current brushless motor and two-phase stepping motor motor according to actual needs.This software programming of MC58113 selects motor type mode to be applicable to very much the control of the different motor of multiaxis.Comprehensive needs of the present invention, select two panels MC58113 as servocontrol regulator, and carry out communication and data call by serial port and STM32F407.
As shown in Figure 3, described six take turns firefighting robot servo controller also comprises ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope, and described ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope are all connected with processor communication.Wherein, the quantity of described ultrasonic sensor is 6, and in the present embodiment, ultrasonic sensor is labeled as S1, S2, S3, S4, S5 and S6; The quantity of current sensor is 7, and in the present embodiment, current sensor is labeled as C1, C2, C3, C4, C5, C6 and C7; Photoelectric sensor, voltage sensor, acceierometer sensor and gyrostatic quantity are 1, and in the present embodiment, photoelectric sensor is labeled as S7, and voltage sensor is labeled as V1, and the equal acceierometer sensor of the gyrostatic G1 of being labeled as is labeled as A1.
As shown in Figure 5, the inside of described processor is also provided with master system and kinetic control system, described master system comprises room and explores module, room storage module, path read module, human-computer interface module and online output module, described kinetic control system comprises based on MC58113 seven axle synchronized mixes servo control module, coordinate setting module and I/O control module, wherein, described comprises six axle DC brushless motor firefighting robots search servo control module and single axle vacuum sucker suction servo control module based on MC58113 seven axle synchronized mixes servo control module.
In order to the adaptive faculty of firefighting robot to complex environment better can be improved, the present invention adopts six wheel constructions to instead of original four wheel constructions: in order to take into account the mid-advantage turned to of two-wheeled, the present invention adopt forerunner+in drive+six wheel drive structures of rear-guard: two DC brushless motor power of mid-driving are larger, four DC brushless motor power of preposition and rearmounted driving are less, only just start when power demand is higher, play power-assisted effect.Owing to adopting six wheel drive technology, take turns all dynamic in before and after firefighting robot, can be different with ambient conditions and demand torque is distributed on all wheel in front and back by different proportion, to improve the driveability of firefighting robot by exploring ground.
Particularly:
In order to accurately room can be sought, the present invention adopts six groups of sensor detection room mode, the firefighting robot structure invented is as shown in Figure 3: ultrasonic sensor S1, S6 acting in conjunction judges front barricade, ultrasonic sensor S2 and S3 coacts and judges the existence of its left side barricade, ultrasonic sensor S4 and S5 coacts and judges the existence of barricade on the right of it, and ultrasonic sensor S2, S3, S4, S5 cooperate to provide navigation foundation for firefighting robot rectilinear motion simultaneously.Due at in-situ match, sunshine likely direct projection is come in, and such infrared sensor just can not in use, and the present invention adopts ultrasonic sensor to instead of infrared sensor.In this vibrational power flow, ultrasonic sensor S2 and S3 can diverse location accurately measure on the left of room from have barricade to without barricade or without barricade to the change having barricade, ultrasonic sensor S4 and S5 can diverse location accurately measure on the right side of room from have barricade to without barricade or without barricade to the change having barricade, the sensor signal Spline smoothing of this position can be captured by controller, then fine compensation can be carried out to firefighting robot in this position, this for firefighting robot solve room find burning things which may cause a fire disaster and turn back to starting point calculate most important, if there is no this intelligent compensation, the cumulative errors of firefighting robot likely in complicated room are enough to make it cannot solve this room map, cause getting back to room starting point.
Take turns firefighting robot in the stability finding walking navigation in burning things which may cause a fire disaster process to improve six, the present invention adds acceierometer sensor A1 and the gyroscope G1 of three axles in firefighting robot servo hardware system.During firefighting robot walking room, whole process opens acceleration and the speed that acceierometer sensor A1 and gyroscope G1, acceierometer sensor A1 and gyroscope G1 are used for measuring firefighting robot three working direction.Controller obtains its anglec of rotation according to the acceleration recorded and rate signal by integral approach.When the attitude of firefighting robot change exceed setting threshold values time, at a new sampling period controller just immediately to its position compensation, avoid firefighting robot to depart from center far away and occur hitting the generation of wall phenomenon, improve the stability of its quick walking navigation.
In order to improve the stability that six take turns firefighting robot MPU Controlled All Digital Servo System, prevent firefighting robot from skidding when walking at a high speed and cause firefighting robot room information mistake, the present invention adds direct current generator M in firefighting robot servo hardware system, in firefighting robot motion process, direct current generator M does not stop to aspirate the air in micro vacuum sucker by vacuum suction apparatus, make the external and internal pressure of micro vacuum sucker different, produce certain negative pressure, it is made to produce certain adsorptive power to room floors, even if the destruction that room floor receives one group of player creates certain change, firefighting robot also can not be affected, effectively prevent the ground of firefighting robot when walking at a high speed to skid.
The present invention overcomes single-chip microcomputer can not meet the stability of diaxon firefighting robot walking and the requirement of rapidity, give up the single single-chip microcomputer mode of operation that domestic firefighting robot adopts, under the prerequisite absorbing external Dynamic matrix control thought, independently invent and taken turns the brand-new control model of double-core based on six of STM32F407+MC58113.Control panel take MC58113 as process core, and seven axles realizing six axle DC brushless motors and single shaft direct current generator mix servo-controlled digital signal and process in real time, and respond various interruption, realize the real-time storage of data-signal.Dual-core controller frees STM32F407 from seven axle servo control algorithm of complexity, realize the signal processing algorithm of part and the steering logic of MC58113, and response is interrupted, and realizes data communication and stores live signal.
For reaching above-mentioned purpose, the present invention takes following technical scheme, in order to improve arithmetic speed, ensure stability and the reliability of four-wheel firefighting robot system, the present invention introduces precise flange special chip MC58113 in based on the controller of STM32F407, form the brand-new dual-core controller based on STM32F407+ special sport control chip, introduce vacuum suction technology from dual-core controller, introduce acceierometer sensor simultaneously and carry out secondary attitude correction.This controller takes into full account the effect of battery in this system, MC58113 process is given seven maximum for workload in control system axle servo-drive systems, give full play to MC58113 as the advantage of servo controller and the comparatively faster feature of STM32F407 data processing speed, STM32F407 is freed from seven axle servocontrol of complexity, realizes the simple functions such as man-machine interface, room reading, room storage, coordinate setting.
Under power-on state, firefighting robot is introduced into self-locking state.Firefighting robot is by ultrasonic sensor S1, S6 judges forward environment, and actual navigational environment is converted into controling parameters and is transferred to STM32F407, and STM32F407 is converted into firefighting robot DC brushless motor X these environmental parameters, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, the distance that DC brushless motor W will run, speed and acceleration parameter command value, STM32F407 then with MC58113 communication, MC58113 generates in conjunction with the feedback of photoelectric encoder and controls DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, the pwm control signal of DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W travels forward, acceierometer sensor A1, gyroscope G1 and photoelectric encoder in real time the signal feedback recorded to STM32F407, by microprocessor by MC58113 bis-times adjustment DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, the PWM wave control signal of DC brushless motor W, thus the attitude of secondary correction firefighting robot.Firefighting robot is in motion process, STM32F407 passes through the enable direct current generator M of MC58113 according to firefighting robot movement velocity, direct current generator M regulates the adsorptive power to ground by aspiration vacuum device, increase effectively friction, prevent firefighting robot to walk fast skidding, STM32F407 stores room information in real time.Automatically dry-ice fire extinguisher solenoid valve is opened after controller finds burning things which may cause a fire disaster, by spraying dry ice fire extinguishing, after fire extinguishing, controller recalls the room information that firefighting robot has stored immediately, find out return shortest path by Flood Fill algorithm, and open six and take turns aero mode and get back to rapidly starting point and wait for that bar seeks burning things which may cause a fire disaster order.
With reference to Fig. 4, Fig. 5, Fig. 6, Fig. 7, Fig. 8, its concrete functional realiey is as follows:
1) firefighting robot mission failure is caused in order to six can be driven to take turns firefighting robot motion and prevent ground from skidding, this control system introduces two panels MC58113, but enter real-time communication by I/O mouth and STM32F407, the four axle DC brushless motors with rear-guard are driven in a slice MC58113 being controlled by the Initialize installation of STM32F407, the diaxon brshless DC motor of a slice MC58113 process forerunner and single shaft direct current generator in addition, controls turning on and off of two panels MC58113 by STM32F407;
2) opening power moment, STM32F407 can detect cell voltage, if low pressure, STM32F407 will block the PWM wave control signal of DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W by MC58113, DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W can not start, voltage sensor V1 is by work simultaneously, and sends alerting signal.If system voltage is normal, first controller opens vacuum draw direct current generator M by MC58113, first micro vacuum sucker is aspirated by aspirator, make vacuum cup over the ground mask have certain adsorptive power, controller also detects in real time, if ground is unclean or run into accelerated condition, system can regulate direct current generator M to strengthen vacuum cup to the adsorptive power on ground automatically, prevents ground from skidding;
3) before firefighting robot does not receive exploration order, its generally can wait at starting point coordinate (0,0) exploration order that controller send, once after receiving task, meeting start to carry out the exploration of full palace to find burning things which may cause a fire disaster along starting point;
4) firefighting robot is placed on starting point coordinate (0, 0), generally, firefighting robot is placed according to the direction (computer programming code is 0) in north in Fig. 6, receive the ultrasonic sensor S1 in its front after task, S6 and judging the environment in front, define and do not have barricade to enter range of movement, memory command will be sent to STM32F407 as there is barricade, STM32F407 can do very first time response to interruption, then by the PWM wave control signal work on four tunnels before and after the input inhibit of adjustment MC58113 and the work of the two-way PWM wave control signal of enable Road, and control DC brushless motor X and rotate forward, DC brushless motor Y reverses, firefighting robot is at acceierometer sensor A1, 90-degree rotation to the right under the control of gyroscope G1, firefighting robot is first along the X-axis forward (direction in east, computer programming code is 2) search burning things which may cause a fire disaster,
5) in firefighting robot motion process, be contained in DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, photoelectric encoder on direct current generator M can export its position signalling A and position signalling B and feed back to MC58113, position signalling A pulse and the B pulsed logic state of photoelectric encoder often change once, the location register of MC58113 can according to DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, the traffic direction of direct current generator M adds 1 or subtract 1,
6) in firefighting robot motion process, when being contained in the position signalling A pulse of the photoelectric encoder on DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, direct current generator M and B pulse and Z pulse simultaneously for low level, just produce an INDEX signal to MC58113 register, the absolute position of record motor, is then converted into firefighting robot particular location in a room;
7) in order to firefighting robot coordinate computing function accurately can be realized, ultrasonic sensor S2 about firefighting robot, S3 and ultrasonic sensor S4, S5 can detect the room barricade about direction of motion and pillar in the moment, if ultrasonic sensor S2, S3 or ultrasonic sensor S4, S5 finds that sensor signal there occurs transition, then illustrate that firefighting robot enters from having room barricade to the change without room barricade (or from without room barricade to there being room barricade) state, STM32F407 can according to firefighting robot current operating conditions fine compensation, the error that thorough elimination firefighting robot has added up when seeking burning things which may cause a fire disaster in complicated room,
8) travel forward along any one direction at firefighting robot, if determine do not have barricade to enter the range of movement in front in any one side's center of a lattice, then firefighting robot will store its coordinate (X, Y), and giving STM32F407 the location parameter of the lattice that travel forward, STM32F407 is being converted into firefighting robot DC brushless motor X to last lattice parameter, DC brushless motor Y, DC brushless motor Z, the position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 generates in conjunction with the feedback of photoelectric encoder and controls DC brushless motor X, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward, at firefighting robot along in current room lattice explored going forward process, and ultrasonic sensor S2, S3 and S4, S5 can judge the barricade of left and right, and record stores current search room barricade information, firefighting robot enters single wall navigation mode or two wall navigation mode according to the room information of working direction left and right barricade, and then combine the left and right barricade navigation threshold values of setting, acceierometer sensor A1, the acceleration of gyroscope G1 real time record firefighting robot, rate signal also gives controller, and STM32F407 obtains instantaneous angular information by integration, and STM32F407 record stores the instantaneous acceleration of firefighting robot, speed and positional information, when firefighting robot rapid discovery has departed from setting center, microprocessor has been converted into firefighting robot DC brushless motor X according to the deviation leaving center by STM32F407, DC brushless motor Y, DC brushless motor Z, the new position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward, can the attitude of accurate adjustment firefighting robot by this mode, it is made to come back to setting center, if firefighting robot is searched in burning things which may cause a fire disaster process occur stall or the more situation of room floors dust, STM32F407 can regulate direct current M to strengthen the friction on firefighting robot and ground and enable forerunner, in drive, six of rear-guard takes turns PWM wave control signal, and DC brushless motor X opened by controller, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W, system enters six wheel drive states, and STM32F407, according to time and rate request, is converted into firefighting robot DC brushless motor X remaining distance D, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the new position that DC brushless motor U and DC brushless motor W will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113, again in conjunction with the feedback of photoelectric encoder, generates DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W travels forward, and firefighting robot still advances according to original navigation mode under driving state six, acceierometer sensor A1, the acceleration of gyroscope G1 real time record firefighting robot, rate signal also gives controller, and STM32F407 obtains instantaneous angular information by integration, and STM32F407 record stores the instantaneous acceleration of firefighting robot, speed and positional information, when firefighting robot six take turns exploration departed from setting center time, microprocessor is converted into firefighting robot DC brushless motor X according to the deviation leaving center by STM32F407, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the new position that DC brushless motor U and DC brushless motor W will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal is amplified before rear drive motor X and motor Y-direction through drive axle and is moved, can the attitude of accurate adjustment firefighting robot by this mode, makes it come back to setting center, when a lattice distance of moving under the control of firefighting robot at acceierometer sensor A1 arrives new address, microprocessor will upgrade its coordinate information,
If the direction when coordinate (X, Y) is north, be (X, Y+1) at its coordinate of renewal, new coordinate direction is still north; If the direction when coordinate (X, Y) is east, be (X+1, Y) at its coordinate of renewal, new coordinate direction is still east; If the direction when coordinate (X, Y) is south, be (X, Y-1) at its coordinate of renewal, new coordinate direction is still south; If the direction when coordinate (X, Y) is west, be (X-1, Y) at its coordinate of renewal, new coordinate direction is still west;
Photoelectric sensor S7 starts working, light source under new coordinate is judged, if photoelectric sensor S7 captures signal and notification controller is found target, controller can allow photoelectric sensor S7 forbid work work in 2 seconds, photoelectric sensor is reopened after 2 seconds, if again capture photosignal, controller can open the solenoid valve of the dry ice container carried, start to carry out sprinkling dry ice to candle until light source disappears, then controller recalls the path that firefighting robot has been searched for, and give up the target of not searching, the optimal path searching room is found out by Flood Fill algorithm, then firefighting robot gets back to search starting point fast according to this path,
If photoelectric sensor S7 does not capture the light source under new coordinate, firefighting robot will leave current room lattice, continuation be searched and upgrade its coordinate;
9) if judge that front has barricade to enter range of movement at firefighting robot along ultrasonic sensor S1 and S6 in the forward movement of current direction, and now ultrasonic sensor S2, S3, S4, when having barricade about S5 judges respectively, firefighting robot will store now coordinate (X, Y), calculate the location parameter YS1 travelling forward and stop according to the feedback of ultrasonic sensor S1 and S6, required stopping distance parameter to be forward converted into firefighting robot DC brushless motor X according to exploration controller speed and acceleration by STM32F407, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the position that DC brushless motor U and DC brushless motor W will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 generates in conjunction with the feedback of photoelectric encoder and controls DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W travels forward, at firefighting robot along in current room lattice explored going forward process, and ultrasonic sensor S2, S3 and S4 and S5 judges left and right barricade, and record stores current search room barricade information, firefighting robot enters two wall navigation mode according to the room information of working direction left and right barricade, and then combines the left and right barricade navigation threshold values of setting, acceierometer sensor A1, the acceleration of gyroscope G1 real time record firefighting robot, rate signal also gives controller, and STM32F407 obtains instantaneous angular information by integration, and STM32F407 record stores the instantaneous acceleration of firefighting robot, speed and positional information, when firefighting robot stop fast departed from setting center time, microprocessor is converted into firefighting robot DC brushless motor X according to the deviation leaving center by STM32F407, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the new position that DC brushless motor U and DC brushless motor W will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W travels forward, can the attitude of accurate adjustment firefighting robot by this mode, it is made to come back to setting center, firefighting robot realizes arranging stop parking, the PWM ripple that STM32F407 readjusts firefighting robot six motors by MC58113 exports, first four road PWM wave control signal efforts of front and back wheel are forbidden, the two-way PWM wave control signal work of driving in simultaneously enable, contrary with DC brushless motor Y direction of motion by MC58113 adjustment DC brushless motor X, firefighting robot original place is at accelerometer sensor A1, realize 180 degree, accurate original place under the control of gyroscope G1 to turn to, then firefighting robot moves along originally contrary direction,
If the direction when coordinate (X, Y) is north, then upgrade its coordinate for (X, Y), new coordinate direction is south; If the direction when coordinate (X, Y) is east, then upgrade its coordinate for (X1, Y), new coordinate direction is west; If the direction when coordinate (X, Y) is south, then upgrade its coordinate for (X, Y), new coordinate direction is north; If the direction when coordinate (X, Y) is west, then upgrade its coordinate for (X, Y), new coordinate direction is east;
10) if having barricade to enter the range of movement in front at firefighting robot in the forward movement of current direction, and now ultrasonic sensor S2 and S3 of left and right judges that there is barricade on the left side, and ultrasonic sensor S4, S5 are when judging that right does not have a barricade, firefighting robot will store now coordinate (X, Y), then firefighting robot by the curved path walking according to Fig. 7;
When turning right, first STM32F407 requires distance R90_Leading very short for walking straight line to generate firefighting robot DC brushless motor X according to the different search speed of controller and acceleration, DC brushless motor Y, DC brushless motor Z, the position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 generates in conjunction with the feedback of photoelectric encoder and controls DC brushless motor X, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward, at firefighting robot along in current room lattice explored going forward process, and ultrasonic sensor S2, S3 can judge left barricade, and record stores current search room barricade information, and firefighting robot enters Dan Zuoqiang navigation mode according to the room information of the left barricade of working direction, and then combines the left barricade navigation threshold values of setting, acceierometer sensor A1, the acceleration of gyroscope G1 real time record firefighting robot, rate signal also gives controller, and STM32F407 obtains instantaneous angular information by integration, and STM32F407 record stores the instantaneous acceleration of firefighting robot, speed and positional information, when firefighting robot rapid discovery has departed from setting center, microprocessor has been converted into firefighting robot DC brushless motor X according to the deviation leaving center by STM32F407, DC brushless motor Y, DC brushless motor Z, the new position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward, can the attitude of accurate adjustment firefighting robot by this mode, makes it come back to setting center, if there is stall or the more situation of room floors dust in the correction position stage in firefighting robot, direct current generator M can be regulated to strengthen the friction on firefighting robot and ground for STM32F407 and six of enable front middle rear-guard take turns PWM wave control signal, and DC brushless motor X opened by controller, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W, system enters six wheel drive states, and STM32F407, according to time and rate request, is converted into firefighting robot DC brushless motor X remaining distance D, DC brushless motor Y, DC brushless motor Z, the new position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W travels forward, can the attitude of accurate adjustment firefighting robot by this mode, makes it come back to setting center,
When arriving set objective, sensor reference value R90_FrontWallRef starts working, prevent external interference from starting to do error compensation, after error compensation terminates, controller is by four DC brushless motors of 4 wheel driven before and after MC58113 release, by in two DC brushless motors driving turn, controller starts to adjust DC brushless motor X and DC brushless motor Y speed makes it complete the curve movement of radian ARC, first STM32F407 requires to be converted into according to the different search speed of controller and acceleration the position that firefighting robot DC brushless motor X and DC brushless motor Y will run radian ARC, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 generates in conjunction with the feedback of photoelectric encoder the pwm control signal controlling motor X and motor Y again, PWM wave control signal is promoted firefighting robot after being amplified by drive axle and completes search of turning.In firefighting robot turning search process, ultrasonic sensor S2, S3, S4, S5 cannot provide reference by location for system, and system relies on gyroscope G1 to carry out position correction.Its instantaneous velocity of gyroscope real time record in firefighting robot fast searching turning process, then controller obtains its positional information by integration, and by the angle contrast with desired location, when firefighting robot fast searching has departed from desired location, within the new sampling period, STM32F407 is converted into deviation size the new position that firefighting robot DC brushless motor X and DC brushless motor Y will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 is again in conjunction with the feedback of photoelectric encoder, the pwm control signal of fine setting DC brushless motor X and DC brushless motor Y, diaxon servo-drive system starts to carry out real-Time Compensation to adjust the attitude of firefighting robot, it is made to complete radian ARC,
When after arrival set objective, system relies on ultrasonic sensor S2, S3 starts navigation, controller requires distance R90_Passing very short for straight line moving to generate firefighting robot DC brushless motor X according to the different search speed of controller and acceleration, DC brushless motor Y, DC brushless motor Z, the position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 generates control DC brushless motor X in conjunction with the feedback of photoelectric encoder again, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward.At firefighting robot along in current room lattice explored going forward process, ultrasonic sensor S2, S3 can judge left barricade, and record stores current search room barricade information, and firefighting robot enters Dan Zuoqiang navigation mode according to the room information of the left barricade of working direction, and then combines the left barricade navigation threshold values of setting, acceierometer sensor A1, the acceleration of gyroscope G1 real time record firefighting robot, rate signal also gives controller, and STM32F407 obtains instantaneous angular information by integration, and STM32F407 record stores the instantaneous acceleration of firefighting robot, speed and positional information, when firefighting robot rapid discovery has departed from setting center, microprocessor has been converted into firefighting robot DC brushless motor X according to the deviation leaving center by STM32F407, DC brushless motor Y, DC brushless motor Z, the new position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, the pwm control signal of DC brushless motor R, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R travels forward.Can the attitude of accurate adjustment firefighting robot by this mode, make it come back to setting center, if there is stall or the more situation of room floors dust in the correction position stage in firefighting robot, direct current generator M can be regulated to strengthen the friction on firefighting robot and ground for STM32F407 and six of enable front middle rear-guard take turns PWM wave control signal, and DC brushless motor X opened by controller, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W, system enters six wheel drive states, and STM32F407, according to time and rate request, is converted into firefighting robot DC brushless motor X remaining distance D, DC brushless motor Y, DC brushless motor Z, the new position that DC brushless motor R will run, speed and acceleration command value, STM32F407 then with MC58113 communication, MC58113 again in conjunction with the feedback of photoelectric encoder, fine setting DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, the pwm control signal of DC brushless motor U and DC brushless motor W, control signal amplifies rear drive DC brushless motor X through drive axle, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U and DC brushless motor W travels forward.Can the attitude of accurate adjustment firefighting robot by this mode, make it come back to setting center; After arriving set objective, complete the geometric locus motion of whole right-hand bend, then control it and start to upgrade its coordinate and direction;
If the direction when coordinate (X, Y) is north, then upgrade its coordinate for (X+1, Y), new coordinate direction is east; If the direction when coordinate (X, Y) is east, then upgrade its coordinate for (X, Y-1), new coordinate direction is south; If the direction when coordinate (X, Y) is south, then upgrade its coordinate for (X-1, Y), new coordinate direction is west; If the direction when coordinate (X, Y) is west, then upgrade its coordinate for (X, Y+1), new coordinate direction is north;
Photoelectric sensor S7 starts working, light source under new coordinate is judged, if photoelectric sensor S7 captures signal and notification controller is found target, controller can allow photoelectric sensor S7 forbid work work in 2 seconds, photoelectric sensor S7 is reopened after 2 seconds, if again capture photosignal, controller can open the solenoid valve of the dry ice container that firefighting robot carries, and starts to carry out sprinkling dry ice to candle until light source disappears; If photoelectric sensor S7 does not capture the light source under new coordinate, firefighting robot will leave current room lattice, continuation be searched and upgrade its coordinate;
11) if having barricade to enter the range of movement in front at firefighting robot in the forward movement of current direction, and now ultrasonic sensor S2 and S3 of left and right judges that the left side is without barricade, and ultrasonic sensor S4, S5 are when judging that there is a barricade right, firefighting robot will store now coordinate (X, Y), then firefighting robot by the curved path walking according to Fig. 8;
When turning left, it is similar with right-hand rotation that controller controls motor walking rule, and after arriving set objective by three-stage process, complete the geometric locus motion of whole left-hand bend, controller starts to upgrade its coordinate and direction according to arranging of Fig. 6;
If the direction when coordinate (X, Y) is north, then upgrade its coordinate for (X-1, Y), new coordinate direction is west; If the direction when coordinate (X, Y) is east, then upgrade its coordinate for (X, Y+1), new coordinate direction is north; If the direction when coordinate (X, Y) is south, then upgrade its coordinate for (X+1, Y), new coordinate direction is east; If the direction when coordinate (X, Y) is west, then upgrade its coordinate for (X, Y-1), new coordinate direction is south;
Photoelectric sensor S7 starts working, light source under new coordinate is judged, if photoelectric sensor S7 captures signal and notification controller is found target, controller can allow photoelectric sensor S7 forbid work work in 2 seconds, photoelectric sensor S7 is reopened after 2 seconds, if again capture photosignal, controller can open the solenoid valve of the dry ice container that firefighting robot carries, and starts to carry out sprinkling dry ice to candle until light source disappears; If photoelectric sensor S7 does not capture the light source under new coordinate, firefighting robot will leave current room lattice, continuation be searched and upgrade its coordinate;
12) when firefighting robot searches out light source, and spray after dry ice completes fire extinguishing, six take turns firefighting robot can be parked in impact point, then STM32F407 recalls the path that firefighting robot has been searched for, and give up the target of not searching, the optimal path searching room is found out by Flood Fill algorithm, then firefighting robot gets back to search starting point fast according to this path, STM32F407 can take turns speed that firefighting robot returns by the servocontrol of MC58113 self-regulation motor M according to six, automatically changes vacuum cup to the adsorptive power on ground;
13) in firefighting robot room search process, STM32F407 can to high-speed DC brushless electric machine X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, on-line identification is carried out in the torque of direct current generator M, due to system employing is three Close loop servo control, if there is pulsation in torque, controller can utilize the relation of direct current generator moment and electric current to carry out linear compensation to interference by MC58113, effectively reduce motor torque shake to the impact of navigating during firefighting robot rapid discovery, add its antijamming capability,
14) starting point (0 is got back to when firefighting robot completes whole return trip, 0), control two panels MC58113 makes firefighting robot central point stop by STM32F407, then the PWM wave control signal work of front and back 4 wheel driven is forbidden, and the two-way PWM ripple drive singal driven in adjustment, DC brushless motor X and DC brushless motor Y is moved with contrary direction, under the control of firefighting robot acceierometer sensor A1 and gyroscope G1, turnback is revolved in original place, original place self-locking, waits for next seek command.
The beneficial effect that the present invention has is:
1: in motion process, take into full account battery effect in this system, all the running status of firefighting robot is being monitored and computing based on the STM32F407+MC58113 controller moment, and in the process of cell powers, the electric current of current sensor C1-C7 moment to battery is observed and gives controller protection, 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: instead of stepper motor with DC brushless motor, motor mechanical is rubbed, and without wearing and tearing, without electric spark, and non-maintaining, and the efficiency of DC brushless motor is high, power and torque density high, make the efficiency of system higher;
3: by the servocontrol of six DC brushless motor X, the DC brushless motor Y of MC58113 process firefighting robot, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, single shaft absorption and control direct current generator M, 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;
4: the present invention realizes full SMD components material substantially, achieve veneer and control, not only save control panel and take up room, and be conducive to alleviating of firefighting robot volume and weight;
5: owing to adopting DC brushless motor, make system band loading capability stronger, speed adjustable range is wider, and contrast of regulating speed is steady;
6: the data adopting MC58113 process seven axle servo a large amount of due to this controller and algorithm, effectively prevent " race flies " of program, antijamming capability strengthens greatly;
7: in firefighting robot actual motion process, STM32F407 can adjust the inner servo-controlled pid parameter of MC58113 according to the peripheral ruuning situation of robot in good time, realize segmentation P, PD, PID to control and nonlinear PID controller, make system meet the switching of high-speed cruising hourly velocity;
8, acceierometer sensor A1 and the gyroscope G1 of three axles is introduced at this firefighting robot system, the angle information obtaining firefighting robot can be similar to by integration, achieve instantaneous acceleration when firefighting robot is explored in room, the direct-detection of speed and the indirect detection of angle, and utilize feedback to realize omnidistance navigation and second compensation, be conducive to the stability and the dynamic property that improve firefighting robot;
9: in firefighting robot operational process, STM32F407 can carry out on-line identification to the torque of the direct current generator M of DC brushless motor X, DC brushless motor Y, DC brushless motor Z, DC brushless motor R, DC brushless motor U, DC brushless motor W, single shaft absorption and control, and utilize the relation of motor torque and electric current to eliminate interference by MC58113, decrease burning things which may cause a fire disaster is sought in motor torque shake fast impact on firefighting robot;
10:STM32F407 regulates the servocontrol of direct current generator M can effectively regulate vacuum cup to the adsorptive power on ground by MC58113, eliminates the generation of firefighting robot skidding when seeking at a high speed burning things which may cause a fire disaster;
11: the position after being transformed according to STM32F407 by MC58113, speed and the given command value of acceleration, PWM modulation signal and direction signal is exported again in conjunction with feedbacks such as photoelectric encoder information, current of electric, can direct-driving motor 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;
12: need to realize timesharing according to firefighting robot search power and drive more.When normal search, because required power is less, firefighting robot generally by four-wheel before and after MC58113 release, can adopt the mode of the two-wheel drive of mid-power; And raise speed a little once run into, STM32407 automatically can detect to exist side by side and distribute to rearmounted two power-assisted DC brushless motors by firefighting robot demand torque by MC58113, changed the servocontrol of motor M by MC58113 with Time Controller, drive+four-wheel drive the state of rear-guard during firefighting robot system is switched to naturally, enhance the reliability during search of firefighting robot; And once run into, road dust is more or firefighting robot speed is higher, STM32407 automatically can detect to exist side by side and distribute to front and rear four power-assisted DC brushless motors by firefighting robot demand torque by MC58113, changed the servocontrol of motor M by MC58113 with Time Controller, firefighting robot system be naturally switched to forerunner+in drive+six wheel drive states of rear-guard, further enhancing the reliability during search of firefighting robot;
13: due to adopt forerunner+in drive+compound six wheel-drive mode of rear-guard, when needs acceleration seek or return to origin time, controller is assigned to six DC brushless motors power, once power wheel because ground, physical construction etc. cause ground away from keyboard, STM32407 can redistribute moment of torsion, more moment of torsion is distributed on the driving wheel of non-stall by MC58113, system is made to hightail non-steady state, come back to six axle dynamic equilibrium states, firefighting robot is had and better seeks walking function;
14: when firefighting robot turns to, in order to ensure the stability rotated, by the enable mid-two-way PWM wave control signal of MC58113, and forbid the PWM wave control signal work of front and back four road, adopt two mid-DC brushless motors to realize searching turning, and discharge four power-assisted DC brushless motors of front and rear.
The foregoing is only embodiments of the invention; not thereby the scope of the claims of the present invention is limited; every utilize description of the present invention to do equivalent structure or equivalent flow process conversion; or be directly or indirectly used in other relevant technical field, be all in like manner included in scope of patent protection of the present invention.
Claims (7)
1. six take turns a double-core middling speed firefighting robot servo controller, it is characterized in that, comprise battery, processor, first motor, second motor, 3rd motor, 4th motor, 5th motor, 6th motor, 7th motor and firefighting robot, described battery provides separately the processor described in electric current driving, and described processor sends the first control signal respectively, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal, by the first described control signal, second control signal, 3rd control signal, 4th control signal, 5th control signal, 6th control signal and the 7th control signal control the first described motor respectively, second motor, 3rd motor, 4th motor, 5th motor, the motion of firefighting robot is controlled again, the first described motor after the signal syntheses of the 6th motor and the 7th motor, second motor, 3rd motor, 5th motor, 6th motor and the 7th motor adopt DC brushless motor, and the 4th described motor adopts direct current generator, and wherein, described processor adopts dual core processor, comprises STM32F407 and MC58113.
2. according to claim 1 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, described battery adopts lithium ion battery.
3. according to claim 1 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, the first described control signal, the second control signal, the 3rd control signal, the 4th control signal, the 5th control signal, the 6th control signal and the 7th control signal are PWM wave control signal.
4. according to claim 1 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, the inside of described processor is also provided with master system and kinetic control system, described master system comprises room and explores module, room storage module, path read module, human-computer interface module and online output module, described kinetic control system comprises based on MC58113 seven axle synchronized mixes servo control module, coordinate setting module and I/O control module, wherein, described comprises six axle DC brushless motor firefighting robots search servo control module and single axle vacuum sucker suction servo control module based on MC58113 seven axle synchronized mixes servo control module.
5. according to claim 1 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, described six take turns firefighting robot servo controller also comprises ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope, and described ultrasonic sensor, current sensor, photoelectric sensor, voltage sensor, acceierometer sensor and gyroscope are connected with processor communication.
6. according to claim 5 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, the quantity of described ultrasonic sensor is 6, the quantity of current sensor is 7, photoelectric sensor, voltage sensor, acceierometer sensor and gyrostatic quantity is 1.
7. according to claim 1 six take turns double-core middling speed firefighting robot servo controller, it is characterized in that, described six take turns firefighting robot servo controller also comprises photoelectric encoder, and described photoelectric encoder is arranged on the first motor, the second motor, the 3rd motor, the 4th motor, the 5th motor, the 6th motor and the 7th motor respectively.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510519930.2A CN105159324A (en) | 2015-08-24 | 2015-08-24 | Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510519930.2A CN105159324A (en) | 2015-08-24 | 2015-08-24 | Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN105159324A true CN105159324A (en) | 2015-12-16 |
Family
ID=54800209
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510519930.2A Pending CN105159324A (en) | 2015-08-24 | 2015-08-24 | Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105159324A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105856236A (en) * | 2016-06-08 | 2016-08-17 | 江苏若博机器人科技有限公司 | Wireless transmission four-kernel four-shaft crawler type rapid natural gas pipeline robot control system |
| CN105856238A (en) * | 2016-06-12 | 2016-08-17 | 江苏若博机器人科技有限公司 | Four-core six-axis wireless-transmission track type rapid natural gas pipeline robot control system |
| CN106100463A (en) * | 2016-08-16 | 2016-11-09 | 中山微宇电器科技有限公司 | A kind of six axle Serve Motor Control Circuits System |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001199356A (en) * | 2000-01-17 | 2001-07-24 | Fuji Electric Co Ltd | Omni-directional moving vehicle and method for controlling it |
| CN202083961U (en) * | 2011-04-15 | 2011-12-21 | 泰怡凯电器(苏州)有限公司 | Robot for cleaning glass |
| CN102841557A (en) * | 2012-09-26 | 2012-12-26 | 苏州工业园区职业技术学院 | Four-axis full-automatic high-speed dispensing robot servo-control system |
| CN103472839A (en) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | Fast exploring controller of four-wheel micro-mouse based on double processors |
| CN104216407A (en) * | 2014-08-27 | 2014-12-17 | 深圳市沐顺通电子科技有限公司 | Wall climbing cleaning robot |
-
2015
- 2015-08-24 CN CN201510519930.2A patent/CN105159324A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001199356A (en) * | 2000-01-17 | 2001-07-24 | Fuji Electric Co Ltd | Omni-directional moving vehicle and method for controlling it |
| CN202083961U (en) * | 2011-04-15 | 2011-12-21 | 泰怡凯电器(苏州)有限公司 | Robot for cleaning glass |
| CN102841557A (en) * | 2012-09-26 | 2012-12-26 | 苏州工业园区职业技术学院 | Four-axis full-automatic high-speed dispensing robot servo-control system |
| CN103472839A (en) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | Fast exploring controller of four-wheel micro-mouse based on double processors |
| CN104216407A (en) * | 2014-08-27 | 2014-12-17 | 深圳市沐顺通电子科技有限公司 | Wall climbing cleaning robot |
Non-Patent Citations (1)
| Title |
|---|
| 张飞飞等: "基于STM32的智能灭火机器人设计", 《甘肃科技》 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105856236A (en) * | 2016-06-08 | 2016-08-17 | 江苏若博机器人科技有限公司 | Wireless transmission four-kernel four-shaft crawler type rapid natural gas pipeline robot control system |
| CN105856236B (en) * | 2016-06-08 | 2018-11-13 | 江苏若博机器人科技有限公司 | It is wirelessly transferred the quick natural gas line robot control system of four core, four axis crawler type |
| CN105856238A (en) * | 2016-06-12 | 2016-08-17 | 江苏若博机器人科技有限公司 | Four-core six-axis wireless-transmission track type rapid natural gas pipeline robot control system |
| CN105856238B (en) * | 2016-06-12 | 2018-11-16 | 江苏若博机器人科技有限公司 | Four core, six axis is wirelessly transferred the quick natural gas line robot control system of crawler type |
| CN106100463A (en) * | 2016-08-16 | 2016-11-09 | 中山微宇电器科技有限公司 | A kind of six axle Serve Motor Control Circuits System |
| CN106100463B (en) * | 2016-08-16 | 2019-03-01 | 中山微宇电器科技有限公司 | A kind of six axis Serve Motor Control circuit systems |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105137975A (en) | Six-wheel double-core automatic high speed fire extinguishing robot servo controller | |
| CN105116918A (en) | Double-core two-wheel middling speed fire extinguishing robot servo controller | |
| CN105159325A (en) | STM32F407 and FPGA-based two-wheel high-speed fire-extinguishing robot servo controller | |
| CN105005249A (en) | Fully automatic four-wheel two-core high speed fire extinguishing robot servo controller | |
| CN105137974A (en) | Six-wheel double-core automatic intermediate speed fire extinguishing robot servo controller | |
| CN105045288A (en) | Single-wheel full-automatic middle-speed fire-fighting robot servo control system | |
| CN105320146A (en) | Single-wheel dual-core middle-speed fire extinguishing robot servo control system | |
| CN105045289A (en) | Single-wheel full-automatic high-speed fire-fighting robot servo control system | |
| CN105005308A (en) | Six-wheel fire-fighting robot servo controller based on STM32F407 | |
| CN105169610A (en) | Dual-core servo control system based on two-wheeled high-speed fully automatic fire extinguishing robot | |
| CN105116897A (en) | Double-core high-speed four-wheeled picomouse full digital navigation servo controller | |
| CN103472840A (en) | Probe controller based on ARM9 four-wheeled microcomputer mouse | |
| CN105159323A (en) | Six-wheel single-core fully automated type low-speed fire-extinguishing robot servo controller | |
| CN105137983A (en) | Double-core high speed two-wheel mini mouse spurting controller and control method | |
| CN105159324A (en) | Six-wheel and dual-core type medium-speed fire-extinguishing robot servo controller | |
| CN105320147A (en) | Single-wheel dual-core low-speed fire extinguishing robot servo control system | |
| CN105138017A (en) | Double-core servo control system based on two wheel intermediate speed automatic fire extinguishing robot | |
| CN105068558A (en) | Single-wheeled dual-core high speed fire-fighting robot servo control system | |
| CN105116891A (en) | Double-core middle-speed two-wheeled picomouse sprint controller and control method thereof | |
| CN105116898A (en) | Single-core low-speed two-wheeled picomouse sprint controller | |
| CN105137979A (en) | Double-core intermediate speed six-wheel mini mouse spurting controller and control method | |
| CN205055284U (en) | Double -core servo control system based on full automatic fire extinguishing robot of two -wheeled intermediate speed | |
| CN105137976A (en) | Four-wheel double-core middling speed fire extinguishing robot servo controller | |
| CN205055285U (en) | Double -core servo control system based on high -speed automatic fire extinguishing robot entirely of two -wheeled | |
| CN105005318A (en) | Fully automatic four-wheel low speed fire extinguishing robot servo 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: 20151216 |
|
| RJ01 | Rejection of invention patent application after publication |