CN106094617A

CN106094617A - It is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems

Info

Publication number: CN106094617A
Application number: CN201610436873.6A
Authority: CN
Inventors: 张好明; 鲍庭瑞
Original assignee: Jiangsu Ruobo Robot Technology Co Ltd
Current assignee: Jiangsu Ruobo Robot Technology Co Ltd
Priority date: 2016-06-20
Filing date: 2016-06-20
Publication date: 2016-11-09

Abstract

The invention discloses one and be wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems, described controller uses three nuclear control devices, and including ARM, FPGA and DSP, described ARM, FPGA and DSP carries out communication connection by wireless device.By the way, what the present invention provided is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems, the brand-new three nuclear control patterns of based on ARM+ FPGA+DSP of independent research, controller is with ARM as processor core, six axle permagnetic synchronous motors and the SERVO CONTROL of two axle direct current generators are realized by FPGA, DSP realizes processing in real time and and ARM of image acquisition digital signal) communication, ARM is freed in the middle of complicated work, realize the real time position collection of six axle three-phase permanent magnet synchronous motors, and respond DSP interrupt, realize data communication and storage live signal.

Description

It is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems

Technical field

The present invention relates to the field of large-scale pipeline robot, particularly relate to one and be wirelessly transferred three core eight axle caterpillar type high-speeds Natural gas line robot control system.

Background technology

The pumped (conveying) medium of natural gas line belongs to material inflammable, explosive, the hydrogen sulfide contained in medium, carbon dioxide, trip From the impurity such as water, dust, make the pipeline laid be in inside and outside etching condition, the most internal can produce congestion situations and send out Raw.Add the factors such as environment, geology, meteorology and hydrological disaster, tubing and design defect, operational error or even artificial destruction, The safety of pipeline is threatened by many factors.

On June 4th, 1989, a gas pipeline of the former Soviet Union leaks, and two row are to running a train at distance leakage point 1 When passing through in the railway line outside Gong Li, train friction produces spark and causes the natural-gas blast of leakage, causes people more than 600 dead Dying, hundreds of hectares of forests are burnt；In August, 2000, the gas pipeline of one the 720 mm bore in the southeast, New Mexico is sent out Raw gas explosion, causing fights the most greatly at least causes 10 people dead, and the place beyond more than 30 kilometers can be seen huge Type fireball rushes to sky, and after blast, ground leaves long 25m, the hollow place of deep 6m together；The oil and gas pipeline of China is the most Secondary having an accident, the accident such as pipeline generation explosion, leakage, stopping transportation not only causes huge property loss, and jeopardizes ecology Environment.

Pipe robot is that one can carry one or more sensings along pipe interior or outside walking automatically Device and operation machinery, or complete the electromechanics of a series of pipeline operations under the control of operator under computer controlled automatic Integral system.The research of pipe robot starts from eighties of last century the forties, to the seventies due to microelectric technique, meter Calculating machine technology, the development and progress of automatic technology, external pipeline robot technique has obtained fast development the beginning of the nineties, has ground Make many experimental prototypes, and achieve substantial amounts of achievement in research.

Patrol and examine natural gas line with pipe robot, be possible not only to improve the efficiency of pipe detection, and for improving labor Dynamic condition, reduces labor intensity, and improves working performance, reduces operating cost, guarantees personal safety and suffer from highly important meaning Justice.But domestic the most not using pipe robot to patrol and examine natural gas line, natural gas line blast happens occasionally, and causes huge Big economic loss and environmental pollution.

One practical natural gas tube pipeline robot must possess following components:

1) image capturing system: image capturing system is it appeared that produced problem in pipeline, it is possible to for staff There is provided pipeline impaired and congestion situations, provide reliable basis for changing pipeline or cleaning pipeline；

2) damage acquisition system: damage acquisition system can find the abnormal conditions that tubing outer wall occurs in time, it is to avoid Pipeline and long-term breakage causes anti-pressure ability to weaken, ultimately result in natural gas and reveal in a large number and produce explosion accident and occur；

3) humidity detection and obturator detect: if humidity is excessive, the pumped (conveying) medium of natural gas line is easily formed corrosive pipeline, Moieties can pile up generation congestion situations simultaneously；

4) motor: actuating motor is that the power of pipe robot implements parts, and it converts the energy of power supply in real time, according to pipe machine The instruction of device people's microprocessor performs the robot relevant walking motion in natural gas line；

5) algorithm: algorithm is the soul of natural gas tube pipeline robot, owing to natural gas line is a pipeline closed, internal feelings Condition is extremely complex, and it is another that natural gas tube pipeline robot must use certain intelligent algorithm just a little can accurately arrive in pipeline Outer the most a little form point-to-point patrolling and examining, and real-time storage to gather image, pipeline steam information, pipeline obstruction information, pipeline impaired Situation and damaged location information；

6) microprocessor: microprocessor is the core of natural gas tube pipeline robot, is the brain of natural gas tube pipeline robot； All of information in pipeline, including the humidity in pipeline, congestion situations, pipe damage information and damage position information, motor Status information, battery status information etc. are required for through microprocessor processes and make corresponding judgement；

7) wireless device: in order to find in time and the problem of process, automatic Pipeline robot must use wireless device, real Time the image acquisition patrolled and examined of transmission and damage collection result, and problem place can be determined by master station's secondary.

The domestic research to pipe robot is the most at the early-stage, is all to use monokaryon controller, is in laboratory sample The machine design phase, from large-scale use, there is a certain distance, mainly face problems with:

(1) controlled technique influence, all of pipe robot all uses monokaryon controller, and the computing capability of controller is more weak, Pipe robot cannot quickly process real time environment, and robot ambulation speed is relatively low, and inspection pipeline speed is relatively slow, and stability is relatively Difference；

(2) for using the energy entrained by motor-driven pipe robot all to use chargeable storage, these accumulator It is all that the most unprotected circuit, the life-span is shorter, normally by forming high-voltage great-current energy resource system after simple series connection and parallel connection The abnormal work even interfering with pipe robot often occurs during work；

(3) for using the pipe robot of motor or DC motor Driver, by the shadow of motor own efficiency Ringing, energy utilization rate is relatively low, causes robot displacement in pipeline shorter；

(4) for using the pipe robot of motor or DC motor Driver, by the shadow of power of motor density Ringing, owing to the motor volume used is the biggest, the volume ultimately resulting in robot is relatively big, and heavier-weight has had a strong impact on pipe The range of pipeline robot；

(5) either based on vector controlled or servo control based on orientation on rotor flux algorithm permagnetic synchronous motor System, in addition to carrying out coordinate transform repeatedly and inverse transformation, is also performed to the closed loop control of electric current and speed, thus realizes ratio More complicated and requirement of real-time is higher；Use DSP technology or ARM technology and realize with software mode, system development week Phase is long, and the processor time that this algorithm takies is the most, have impact on the process function of DSP or ARM；Use specially Although processor can be reduced with motion control chip to process the time, but its internal PID regulation can only meet single wanting Ask, it is impossible to meet pipe robot application in complex environment；

(6) pipe robot kinestate oneself adjustment capability is poor, and controlled mode affects, robot attitude in pipeline Parameter identification is poor, and robot None-identified oneself's plane and the angle of pipeline principal plane, robot cannot be in real time according to periphery Environment adjusts the pid parameter of oneself, causes inclination occur during robot ambulation, overturns the most sometimes, cause mission failure；

(7) for there being the natural gas line of obstruction, common wheeled robot is less with contact area of ground, and obstacle climbing ability is relatively Weak, the most even cannot clear the jumps, finally cannot patrol and examine task；

(8) the most most of pipe robot, poor to the Context awareness in pipeline, is all the collection image stored by analysis Carrying out analysis conduit situation, real-time condition distinguishes poor；

(9) for using the power-actuated pipe robot of four-wheel, the power adjustment capability relatively two-wheeled power of robot drives Move and increase, the acceleration under the simple operating mode of pipe robot can be met, but run into the pipeline with certain slope, demand Power is relatively big, this under the conditions of cause demand power to be not being met so that dynamic performance reduces；

(10) value measured during the work of single three-axis gyroscope can be drifted about in time, through work three-axis gyroscope meeting for a long time Accumulate extra error, eventually result in pipe robot and lose position in the duct.

The stator of permasyn morot and common electrically excited synchronous motor have identical stator structure, simply rotor On instead of excitation pole and the Exciting Windings for Transverse Differential Protection of synchrodrive using Nd-Fe-B rare earth permanent magnetic material as magnetic pole, make the structure of motor Relatively simple, and eliminate easy out of order collector ring and brush, it is achieved that non-brushing, improve the reliable of motor running Property.Because being not required to exciting current, therefore can save the copper loss of Exciting Windings for Transverse Differential Protection, greatly improving the efficiency of motor；Rare earth permanent magnet The use of material makes power of motor density higher, so the volume of motor be can be made smaller, applicable volume requirement is higher Occasion.Permasyn morot, in addition to having obvious energy-saving effect, also has the characteristic that rotating speed is accurate, noise is low, rare earth Permagnetic synchronous motor based on rotor field-oriented or based on vector control system be capable of high accuracy, high dynamic performance, On a large scale speed governing or location control, these characteristics make rare earth permanent-magnet synchronization motor be particularly suitable for being used in pipe robot this In the robot control system that a little requirements are more special.

Crawler-type mobile mechanism is the expansion of wheeled locomotion mechanism, and crawler belt itself plays a part to pave the way continuously to wheel. Relative to ratcheting mechanism, there is plurality of advantages in crawler-type mobile mechanism, such as: bearing area is big, and grounding pressure is little；Resistance to rolling Little, pass through better performances；Off-road mobility is good；Having grouser on crawler belt bearing-surface, be difficult to skid, traction adhesion property is good, favorably In playing bigger pull strength；Displacement crawler-type mobile mechanism is by changing the mechanism form of the position of crawler belt or crawler belt to reach Adapting to the requirement of varying environment, the angle of two crawler belts can regulate, to adapt to different operation calibers.

Summary of the invention

The technical problem that present invention mainly solves is to provide one and is wirelessly transferred three core eight axle caterpillar type high-speed natural gas tubes Pipeline robot control system, the brand-new three nuclear control patterns of based on ARM+ FPGA+DSP of independent research, controller with ARM is Processor core, is realized six axle permagnetic synchronous motors and the SERVO CONTROL of two axle direct current generators by FPGA, and DSP realizes image acquisition Processing in real time and and ARM of digital signal) communication, ARM is freed in the middle of complicated work, it is achieved six axle three-phase permanents The real time position collection of synchronous motor, and respond DSP interrupt, it is achieved data communication and storage live signal.

For solving above-mentioned technical problem, the technical scheme that the present invention uses is: provides one and is wirelessly transferred three cores Eight axle caterpillar type high-speed natural gas line robot control systems, including battery, controller, permagnetic synchronous motor X, permanent-magnet synchronous Motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U, permagnetic synchronous motor W, direct current generator A, direct current Motor B, based on ccd image collecting unit, image storage unit, humidity collection unit, based on Hall effect pipeline inspection gather Unit, wireless device and pipe robot, described battery is provided separately the controller described in electric current driving, described control Device uses three nuclear control devices, and including ARM, FPGA and DSP, described ARM, FPGA and DSP carries out communication even by wireless device Connecing, described is all connected with DSP communication based on ccd image collecting unit and image storage unit, described humidity collection unit All being connected with ARM and FPGA communication with based on Hall effect pipeline inspection collecting unit, described ARM and FPGA sends respectively One control signal, the second control signal, the 3rd control signal, the 4th control signal, the 5th control signal and the 6th control signal, By the first described control signal, the second control signal, the 3rd control signal, the 4th control signal, the 5th control signal and Six control signals control described permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permanent magnet synchronous electric respectively The motion of pipe robot is controlled again after the signal syntheses of machine R, permagnetic synchronous motor U and permagnetic synchronous motor W, described DSP sends the 7th control signal and the 8th control signal respectively, by the 7th described control signal and the 8th control signal difference It is connected with based on ccd image collecting unit communication after direct current generator A described in control, the signal syntheses of direct current generator B.

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

In a preferred embodiment of the present invention, described the first control signal, the second control signal, the 3rd control letter Number, the 4th control signal, the 5th control signal and the 6th control signal be PWM wave control signal.

In a preferred embodiment of the present invention, described ARM uses STM32F746；Described FPGA uses QUICKLOGIC；Described DSP uses TMS320F2812.

In a preferred embodiment of the present invention, described pipe robot includes that robot housing, laser displacement sense Device, magnetic navigation sensor, left fork sensor, right fork sensor, three-axis gyroscope, three axis accelerometer and Timing Belt, Described laser displacement sensor is separately mounted to the front end of robot housing, described left fork sensor and right fork sensing Device lays respectively at the two ends, left and right below laser displacement sensor, and described Timing Belt is separately positioned on the left and right of robot housing Dual-side and respectively with permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permanent magnetism with Step motor U and permagnetic synchronous motor W connects, and described magnetic navigation sensor, three-axis gyroscope and three axis accelerometer set successively Put on robot housing and between permagnetic synchronous motor X and permagnetic synchronous motor Y, described direct current generator A and direct current Motor B is arranged on robot housing and lays respectively at left fork sensor and the top of right fork sensor, described nothing Line apparatus is positioned at the rear end of robot housing.

In a preferred embodiment of the present invention, described laser displacement sensor include front laser displacement sensor, Left laser displacement sensor and right laser displacement sensor, described front laser displacement sensor is just being arranged on robot housing The centre position in front, described left laser displacement sensor and right laser displacement sensor are the most angularly disposed at robot shell The two ends, left and right of body dead ahead.

In a preferred embodiment of the present invention, described Timing Belt uses six axle eight wheel drive mode, is by one Perimeter surface is provided with the closed ring crawler belt of equidistant tooth and corresponding belt wheel is formed.

In a preferred embodiment of the present invention, described high speed natural gas line robot control system is additionally provided with Position machine program, based on ARM main motion control program, based on DSP from motion control program, based on Hall effect pipe damage detect And be wirelessly transferred, described host computer procedure also include pipeline read, location positioning and power information, described based on ARM Main motion controls program and also includes controlling based on FPGA six axle permagnetic synchronous motor SERVO CONTROL, data storage and I/O, described Also include based on FPGA two axle direct current generator SERVO CONTROL with based on DSP image acquisition based on DSP from motion control program, described Based on the transmission of Hall effect pipe damage detection wireless and respectively with based on Hall effect pipeline inspection collecting unit and wireless Device communication connects.

In a preferred embodiment of the present invention, described natural gas line robot control system also includes photoelectric coding Device, described photoelectric encoder is separately mounted to permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permanent magnetism On synchronous motor R, permagnetic synchronous motor U, permagnetic synchronous motor W, direct current generator A and direct current generator B.

The invention has the beneficial effects as follows: the present invention is wirelessly transferred three core eight axle caterpillar type high-speed natural gas tube pipeline robots Control system, in order to improve the utilization rate of the energy and reduce robot volume, native system efficiency and power density are the highest Permagnetic synchronous motor instead of the motor such as motor, direct current generator；In order to improve system acceleration request, system is that these are extraordinary Operating mode adds two lower-powered permagnetic synchronous motors and plays acceleration power-assisted effect, increases system dynamic characteristic；In order to carry The general hill climbing demands of high system, system is that these extraordinary operating modes add two lower-powered permagnetic synchronous motors and play climbing Power-assisted effect, increases system dynamic characteristic；In order to improve arithmetic speed, it is ensured that the stability of automatic pipeline robot system and can By property, the present invention introduces FPGA and digital signal processor DSP in controller based on ARM, is formed based on ARM+ FPGA+ The brand-new three nuclear control devices of DSP, this controller takes into full account the battery effect in this system, workload in control system Six big axle permagnetic synchronous motors and two axle direct current generator servosystem give that FPGA completes, battery cell monitoring, path are read, deviation Process etc. and to give ARM process, give full play to the comparatively faster feature of ARM data processing speed, and image data acquiring and storage Give DSP etc. function to complete, thus achieve the division of labor of ARM, FPGA and DSP, communication between three, can also be carried out simultaneously, Carry out data exchange in real time and call.

Accompanying drawing explanation

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

Fig. 1 is that the present invention is preferable with being wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems one The schematic diagram of embodiment；

Fig. 2 is pipe robot two-dimensional structure schematic diagram；

Fig. 3 is the programme diagram of Fig. 1；

Fig. 4 is that pipe robot patrols and examines schematic diagram.

Detailed description of the invention

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

As it is shown in figure 1, the embodiment of the present invention includes:

One is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems, including battery, controller, forever Magnetic-synchro motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U, permanent-magnet synchronous Motor W, direct current generator A, direct current generator B, based on ccd image collecting unit, image storage unit, humidity collection unit, based on Hall effect pipeline inspection collecting unit, wireless device and pipe robot, described battery is provided separately electric current and drives institute The controller stated, described controller uses three nuclear control devices, and including ARM, FPGA and DSP, described ARM, FPGA and DSP leads to Crossing wireless device and carry out communication connection, described all connects with DSP communication based on ccd image collecting unit and image storage unit Connect, described humidity collection unit and being all connected with ARM and FPGA communication based on Hall effect pipeline inspection collecting unit, described ARM and FPGA send the first control signal, the second control signal, the 3rd control signal, the 4th control signal, the 5th control respectively Signal processed and the 6th control signal, by the first described control signal, the second control signal, the 3rd control signal, the 4th control Signal, the 5th control signal and the 6th control signal control described permagnetic synchronous motor X, permagnetic synchronous motor Y, permanent magnetism respectively Pipeline is controlled again after the signal syntheses of synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and permagnetic synchronous motor W The motion of robot, described DSP sends the 7th control signal and the 8th control signal respectively, by the 7th described control signal Adopt with based on ccd image with after the signal syntheses that the 8th control signal controls described direct current generator A, direct current generator B respectively Collection unit communications connects.Wherein, described battery uses lithium ion battery；Described the first control signal, the second control signal, 3rd control signal, the 4th control signal, the 5th control signal and the 6th control signal are PWM wave control signal.

In above-mentioned, described ARM uses STM32F746；Described FPGA uses QUICKLOGIC；Described DSP uses TMS320F2812。

The brand-new STM32F7 MCU series of products that STMicroelectronics is produced, be the whole world first volume production and Have the microcontroller of 32 bit ARM Cortex-M7 processors.Cortex-M7 is up-to-date release in Cortex-M series of products And the processor core that usefulness is the highest, brand-new STM32F7 MCU be ST STM32 MCU series of products in the highest one of usefulness Product, combines Cortex-M7 core and high-order peripheral unit, can promote application program usefulness, newly-increased New function, extend battery Life-span, guarantee safety and reducing as far as possible use outer member with cost-effective with space etc. unrivaled advantage.

STM32F7 series of products include STM32F745 and STM32F746, and these two products are all equipped with and have floating-point operation The Cortex-M7 core of unit and DSP extended function, the highest 216MHz of arithmetic speed.STM32F7 MCU series of products are by ARM Cortex-M7 usefulness surmounts the advantage of core (such as Cortex-M4) in early days and applies to ultimate attainment, and usefulness reaches nearly DSP two Times.

FPGA have employed logical cell array LCA(Logic Cell Array) such a new ideas, inside includes can Configuration logic module CLB(Configurable Logic Block), output input module IOB(Input Output Block) With three parts of interconnector (Interconnect).The basic characteristics of FPGA mainly have: use FPGA design ASIC circuit, use Family need not throw sheet and produces, and just can obtain the chip share；FPGA can do other full custom or the pilot scale of semi-custom ASIC circuit Print；Abundant trigger and I/O pin is had inside FPGA；FPGA is that in ASIC circuit the design cycle is the shortest, development cost One of device of low, least risk；FPGA uses high speed CHMOS technique, low in energy consumption, can be with CMOS, Transistor-Transistor Logic level compatibility.On The feature of stating makes user can be carried out FPGA inside again by specific placement-and-routing instrument according to the design needs of oneself Combination connects, and designs the special IC of oneself within the shortest time, thus reduces cost, shortens the construction cycle. Owing to FPGA uses the design philosophy of software implementation to realize the design of hardware circuit, system based on FPGA design is thus made to have There are good reusable and amendment property.This brand-new design philosophy is the most gradually applied to drive in high performance exchange and is controlled On, and fast-developing.These characteristics makes FPGA be particularly suitable in servo control, six used especially for the present invention Axle eight is taken turns natural gas line and is patrolled and examined SERVO CONTROL structure, and the servo programe that can greatly reduce STM32F7 controller is write.

TMS320F2812 is 32 fixed-point digital signal processing of novel high-performance of the C28x kernel compatible based on code Device, the instruction execution cycle of C28x kernel has reached 6.67ns, and maximum running frequency can reach 150MHz, F2812 and be integrated with Many peripheral hardwares, it is provided that a whole set of SOC(system on a chip), its On-Chip peripheral mainly includes that 12,2 × 8 tunnel ADC is (during the fastest 80ns conversion Between), 2 road SCI, 1 road SPI, 1 road McBSP, 1 road eCAN interface etc., and with two event manager modules (EVA, EVB).Separately Outward, this device also has 3 32 independent bit CPU intervalometers, and up to 56 GPIO pin being independently programmable.F2812 uses Unified addressing mode, chip internal has the SARAM of 18K, and including MO, M1, L0, L1, H0 totally 5 memory blocks, each memory block keeps Independent, in the uniform machinery cycle, different RAM block can be conducted interviews, thus reduce streamline time delay.And inside F2812 There are the FLASH of 128K word, address space 3D8000h～3F7FFFh, it is adaptable to low-power consumption, high performance control system.In addition F2812 provides external memory storage expansion interface (XINTF), conveniently carries out system extension, and its addressing space can reach 1MB； These characteristics makes F2812 while possessing the data-handling capacity that digital signal processor is remarkable, has again and is suitable to control Sheet in peripheral hardware and interface, can be widely applied to during various high performance system controls, These characteristics makes TMS320F2812 special It is not suitable for the figure collection of crusing robot, image storage and positional information storage.

In order to accurate acquisition pipe interior damage information, this controller be ccd image acquisition system add based on DSP two-dimensional localization servosystem, this two-dimentional system includes direct current generator A and direct current generator B, when DSP is compared by storage image After finding suspicious region, main website adjusted the height of ccd image acquisition system lifting by controlled in wireless direct current generator A, then The angle that ccd image acquisition system rotates is adjusted so that the center of CCD is directed at the center of doubtful object, then by direct current generator B Analyzed suspicious region by main website in real time, find and process this region in time, the image that this region of DSP real-time storage collects and Positional information, patrols and examines pipeline interpretation of result for pipe robot and provides basis for estimation.

In order to accurately guide duct robot detects automatically, the present invention uses two set sensor navigation patterns (a set of magnetic navigation sensor navigates, and a set of laser displacement sensor navigates), pipe robot two-dimensional structure such as Fig. 2 of the present invention Shown in: described pipe robot includes that robot housing K, laser displacement sensor, magnetic navigation sensor ME1, left fork pass Sensor ME2, right fork sensor ME3, three-axis gyroscope G1, three axis accelerometer A1 and Timing Belt T, described laser displacement Sensor is separately mounted to the front end of robot housing K, described left fork sensor ME2 and right fork sensor ME3 respectively Being positioned at the two ends, left and right below laser displacement sensor, described Timing Belt T is separately positioned on the left and right sides of robot housing K Limit and respectively with permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permanent magnet synchronous electric Machine U and permagnetic synchronous motor W connects, and described magnetic navigation sensor ME1, three-axis gyroscope G1 and three axis accelerometer A1 are successively Being arranged on robot housing K and between permagnetic synchronous motor X and permagnetic synchronous motor Y, described direct current generator A is with straight Stream motor B is arranged on robot housing K and lays respectively at left fork sensor ME2 and the top of right fork sensor ME3, Described wireless device I is positioned at the rear end of robot housing K.Wherein, described laser displacement sensor includes laser position, front Displacement sensor LSF, left laser displacement sensor LSL and right laser displacement sensor LSR, described front laser displacement sensor LSF is arranged on the centre position of robot housing K dead ahead, described left laser displacement sensor LSL and right laser displacement and passes The most angularly disposed two ends, left and right at robot housing K dead ahead of sensor LSR.

The magnetic navigation sensor ME1 moment detects the magnetic stripe in pipeline, and sensor is the first navigation criterion according to this, works as magnetic stripe Not existing or time the deviation distance that navigates is bigger, left laser displacement sensor LSL and right laser displacement sensor LSR jointly acts on and sentencing Disconnected direction of advance and the distance of left and right tube wall, and provide navigation foundation, front laser displacement as pipe robot linear motion The differentiation that sensor LSF is pipe robot advance barrier provides according to and stops does criterion.Left fork magnetic navigation sensing Device ME2 and right fork sensor ME3 detects fork, ground mark respectively, then turns left respectively as cross pipeline or turns right Criterion, and natural gas tube pipeline robot accurately can be compensated in this position, this patrols and examines pipeline for pipe robot Calculating position is most important.

The three axis accelerometer measured value (determining pipe robot course) in the long period is correct, and when shorter Interior due to the existence of signal noise, and have certain error.Three-axis gyroscope G1 is then the most accurate within a short period of time, and longer Time then can produce certain error along with the existence of drift, accordingly, it would be desirable to three axis accelerometer A1 and three-axis gyroscope G1 phase Adjust mutually and guarantee the correct of course.For stablizing of improving that natural gas tube pipeline robot navigates in closed conduit walking process Property, it is achieved the most accurately adjusting and lifting independent navigation ability of attitude, the present invention is hard natural gas line robot servo Part system adds three-axis gyroscope G1 and three axis accelerometer A1.Omnidistance unlatching three during pipe robot walking pipeline Axle gyroscope G1 and three axis accelerometer A1, three-axis gyroscope G1 and three axis accelerometer A1 are respectively used for measuring pipe robot The angular velocity of three directions of advance and angular acceleration, controller obtains it according to the angular acceleration recorded and angular velocity by integration Accurately acceleration, speed and angle of inclination.When the attitude of pipe robot changes and exceedes setting threshold values, new at one Sampling period controller is the most immediately to its position compensation, it is to avoid pipe robot is translated into because of tilting excessive in the process of walking The generation of phenomenon, improves stability during its quick walking navigation；If to three-axis gyroscope G1 and three axis accelerometer A1 Being integrated, and it is transformed in navigational coordinate system, pipe robot can not rely on any outside in closed conduit Information just can obtain the information such as its acceleration, speed, yaw angle and position in navigational coordinate system, and produced navigation is believed Breath seriality is good and noise is the lowest, greatly enhances the autonomous inertial navigation ability of pipe robot.Work as pipe robot When reading cross pipeline entrance, robot to realize 90 or turn right 90 action, in this case, three axle gyros Instrument G1 and three axis accelerometer A1 can accurately calculate the angle that robot rotates, it is ensured that its accuracy turned；Work as pipe Pipeline robot is in climbing when patrolling and examining, and three-axis gyroscope G1 and three axis accelerometer A1 can accurately measure ramp angle, ARM root Climbing power demand can be accurately calculated, it is ensured that robot can complete to patrol and examine pipeline according to command speed and appoint according to this angle Business.

Timing Belt T transmission is closed ring crawler belt and the corresponding belt wheel institute group being provided with equidistant tooth by an inner peripheral surface Become.During motion, band tooth is meshed with the teeth groove of belt wheel transmitting movement and power, is a kind of engaged transmission, thus has gear and pass Dynamic, Chain conveyer and the various advantages of Belt Drive.Timing Belt T transmission has gear ratio accurately, without slippage, can obtain constant Speed ratio, can precision drive, stable drive, can shock-absorbing, noise is little, and transmission efficiency is high, is not required to lubrication, pollution-free, is particularly suitable for It is not allow for polluting under the occasion the most severe with working environment and normally works, the particularly suitable spinning transmission of compact conformation, therefore The present invention uses Timing Belt technology to form six axle eight wheel drive mode.

The present invention is to solve the problem that domestic pipeline robot exists, have developed a kind of by six rare-earth permanent-magnetic synchronous electricity The eight of machine differential driving take turns and are wirelessly transferred crawler type three core high speed natural gas tube pipeline robot, six rare earth permanent-magnet synchronization motors Servo control algorithm is completed by FPGA, increases the rapidity of system-computed, the rare-earth permanent-magnetic synchronous electricity that two of which power is bigger Thering is provided energy requirement for robot normal speed when machine is patrolled and examined, four additional lower-powered permanent magnet synchronous electric acc power is equal, Two rare earth permanent-magnet synchronization motors therein provide power when robot accelerates to patrol and examine, and other two small-power rare earth permanent magnets are same Step motor provides power when robot climbs；The multiple of left and right sides take turns respectively by crawler belt mechanical linkages, based on two axle direct currents The servosystem of motor provides positioning function for ccd image acquisition system, and natural gas tube pipeline robot relies on its carry sensors Carry out patrolling and examining major gas pipeline.

The present invention absorbing on the premise of external Dynamic matrix control thought, and independent research is based on ARM+ FPGA+DSP complete New three nuclear control patterns.Controller principle figure such as Fig. 1 of this design: controller, with ARM as processor core, is realized by FPGA Six axle permagnetic synchronous motors and the SERVO CONTROL of two axle direct current generators, DSP realizes processing also in real time of image acquisition digital signal With ARM communication, ARM is freed in the middle of complicated work, it is achieved the real time position of six axle three-phase permanent magnet synchronous motors is adopted Collection, and respond DSP interrupt, it is achieved data communication and storage live signal.

As it is shown on figure 3, described high speed natural gas line robot control system be additionally provided with host computer procedure, based on ARM main motion control program, based on DSP from motion control program, based on Hall effect pipe damage detection and be wirelessly transferred, Described host computer procedure also includes pipeline reading, location positioning and power information, and described controls program based on ARM main motion Also include controlling based on FPGA six axle permagnetic synchronous motor SERVO CONTROL, data storage and I/O, described based on DSP from motion Control program also includes that described imitates based on Hall based on FPGA two axle direct current generator SERVO CONTROL with based on DSP image acquisition Answer the transmission of pipe damage detection wireless and be connected with based on Hall effect pipeline inspection collecting unit and wireless device communication respectively.

For reaching above-mentioned purpose, the present invention takes techniques below scheme, in order to improve the utilization rate of the energy and reduce robot The permagnetic synchronous motor that volume, native system efficiency and power density are the highest instead of the motor such as motor, direct current generator； In order to improve system acceleration request, system is that these extraordinary operating modes add two lower-powered permagnetic synchronous motors and play and add Speed power-assisted effect, increases system dynamic characteristic；In order to improve the general hill climbing demands of system, system is that these extraordinary operating modes add Two lower-powered permagnetic synchronous motors play climbing power-assisted effect, increase system dynamic characteristic；In order to improve arithmetic speed, Ensureing stability and the reliability of automatic pipeline robot system, the present invention introduces FPGA sum in controller based on ARM Word signal processor DSP, forms brand-new three nuclear control devices based on ARM+ FPGA+DSP, and this controller takes into full account that battery exists The effect of this system, the six axle permagnetic synchronous motors that workload in control system is maximum and two axle direct current generator servosystem Give that FPGA completes, battery cell monitoring, path reading, deviation processing etc. give ARM process, give full play to ARM data processing speed Comparatively faster feature, and the function such as image data acquiring and storage is given DSP and completed, thus achieve ARM, FPGA with The division of labor of DSP, can also carry out communication simultaneously, carry out data exchange in real time and call between three.

As shown in Figure 4, for the ARM+ FPGA+DSP tri-nuclear control device designed herein, under power-on state, ARM First judging pipe robot battery SOC (state-of-charge), if battery power is relatively low, controller can send alarm signal Number；If battery power is higher, first inputted patrolling and examining the information such as natural gas line length and radius by wireless device by main website To ARM, then guiding tube pipeline robot is to pipe detection mouth automatically, and robot is introduced into self-locking state, waits inlet valve F1 Opening, after laser displacement sensor LSF determines that valve is opened, pipe robot enters buffer area to be checked, then inlet valve Door F1 closes, and inlet valve F2 opens, and pipe robot enters pipe detection region；The image acquisition system that pipe robot carries System, humidity collection system and pipe damage detection device, wireless device are all opened, pipe robot according to setting speed along patrolling Inspection route fast inspection, ARM is input to FPGA, FPGA magnetic navigation sensor ME1 parameter joins these magnetic navigation sensors ME1 Number be converted into pipe robot under track is patrolled and examined in appointment permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and The PWM ripple of permagnetic synchronous motor R, it is achieved the real-time servo of four permanent magnet synchronous motors controls；It is straight that DSP realizes two axles by FPGA Stream servo positioning system makes CCD the most effectively gather duct size information and store, if DSP has a question to patrolling and examining some position, Will by wireless to main website send interrupt requests and with ARM communication, and by wireless device transmissions suspicious region image, by wireless Control ARM to send cutoff command and makes pipe robot stop, then main website by wireless device realize the collection of CCD secondary image with Fault localization, and by DSP real time record and storage.If robot completes or right-hand rotation is patrolled and examined subsidiary conduit and again returned When returning main pipeline, ARM will open power-assisted permagnetic synchronous motor Z and permagnetic synchronous motor R by FPGA, and FPGA passes according to magnetic navigation Sensor ME1 parameter and rate request secondary adjust pipe robot permagnetic synchronous motor X, permagnetic synchronous motor Y, permanent magnet synchronous electric The PWM output of machine Z and permagnetic synchronous motor R, it is achieved the real-time servo of four permanent magnet synchronous motors controls, and disappears to reduce the energy Consumption, before returning to main channel, DSP will close the information gathering of CCD；If pipe robot is at the process of patrolling and examining, three-axis gyroscope G1 Detecting that robot is in climbing and patrols and examines pipeline, ARM will open power-assisted permagnetic synchronous motor Z, permagnetic synchronous motor by FPGA R, permagnetic synchronous motor U, permagnetic synchronous motor W, adjust pipe machine again according to magnetic navigation sensor ME1 parameter and rate request Device people permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and forever The PWM output of magnetic-synchro motor W, it is achieved the real-time servo of six permanent magnet synchronous motors controls.

With reference to Fig. 1, Fig. 2, Fig. 3 and Fig. 4, its concrete functional realiey is as follows:

1) after pipe robot power supply opening, battery SOC can be judged by ARM, if battery SOC is relatively low, ARM will forbid FPGA works, and six permanent magnet synchronous motors PWM ripples are blocked, and alarm sensor by work and sends alarm signal simultaneously, and ARM leads to Cross wireless device and send replacing power interruptions request to master station；If battery SOC is normal, pipe robot enters and treats work shape State, waits work order；

2) master station is passed, by wireless device, information of calling the roll of the contestants in athletic events the information such as right for weather duct length, radius and pipeline topography, last time Being defeated by ARM, then wireless device guiding tube pipeline robot is to the starting end of pipe detection, in order to precision navigation pipe robot exists Walking in closed conduct, ARM first turns on pipe robot based on three-axis gyroscope and the inertial navigation of three axis accelerometer Pattern；

3) pipe robot ARM begins through magnetic navigation sensor ME1 and reads area navigation magnetic stripe, magnetic navigation sensor ME1's Value of feedback compares with actual set central value, and ARM inputs to FPGA, FPGA this deviation and this straggling parameter is converted into pipeline Robot is permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor under track is patrolled and examined in appointment R, distance to be run for permagnetic synchronous motor U and permagnetic synchronous motor W, speed and acceleration command value, FPGA is in conjunction with motor Current feedback, photoelectric encoder feedback, three axis accelerometer A1 and the feedback of three-axis gyroscope G1, calculate through internal SERVO CONTROL Method obtains the PWM wave control signal that six permanent magnet synchronous motors control, and makes pipe robot fast forward through along navigation magnetic stripe, root Adjust the pid parameter of internal SERVO CONTROL program according to peripheral environment FPGA in real time, make system easily realize segmentation P, PD, PID control System and nonlinear PID controller；During pipe robot advances, work, ARM are examined by front laser displacement sensor LSF in real time Test tube pipeline robot and distance D of front inlet valve F1, in the range of stably stop, ARM allows pipe robot certainly by FPGA Dynamic parking, then original place self-locking；

4) when front laser displacement sensor LSF detects that inlet valve F1 opens, pipe robot will open automatic cruising Pattern, the distance that real time record pipe robot is moved by controller ARM along magnetic stripe, when determining that robot is completely into be checked Behind region, inlet valve F1 will be again switched off, after natural gas leakage device detects that inlet valve F1 completely closes, and inlet valve F2 will open, and now secondary is judged the state of front inlet valve F2 by front laser displacement sensor LSF, determines front valve Open errorless after, pipe robot initially enters patrols and examines region and starts to detect the internal practical situation of natural gas line；

5), after pipe robot enters pipe detection region, main website opens, by control device of wireless, the CCD controlled based on DSP and schemes As acquisition system, DSP starts two axle DC servo alignment systems simultaneously；ARM, first according to the feedback of magnetic navigation sensor ME1, reads Take positional information, three axis accelerometer A1 and the numerical value of three-axis gyroscope G1 that pipe robot is actual, then with setting position Compare, determine that pipe robot off-center distance and the angle of inclination, this deviation signal of ARM input to FPGA, FPGA handle This straggling parameter is converted into pipe robot permagnetic synchronous motor X and permagnetic synchronous motor Y under track is patrolled and examined in appointment and to transport Row distance, speed and acceleration command value, FPGA then in conjunction with motor current feedback, photoelectric encoder feedback, three axles Accelerometer A1 and the feedback of three-axis gyroscope G1, obtain what two permanent magnet synchronous motors controlled through internal servo control algorithm PWM wave control signal, and adjust pipe robot attitude in real time by drive circuit, make pipe robot stable operation at magnetic stripe Immediate vicinity, if by the collection image of feedback, master station finds that pipe robot deviate from navigation track by a relatively large margin, will be logical Crossing the attitude of control device of wireless preferential Calibration pipe robot, simultaneously according to peripheral environment, FPGA adjusts internal servo in real time The pid parameter of control program, makes system easily realize segmentation P, PD, PID and controls and nonlinear PID controller；ARM real time record The air line distance that pipe robot has run, the distance correction sensor S moment detect ground revise mark, once read and repair Equipment, the positional distance information of ARM record to be as the criterion to revise the positional information of mark, to eliminate pipe robot when walking The site error caused；

6) after pipe robot enters detection region, if ARM is by three-axis gyroscope G1 and the feedback of three axis accelerometer A1 Find that robot is in climbing and patrols and examines state, in order to ensure that pipe robot can complete to patrol and examine pipeline according to command speed and appoint Business, according to system speed and acceleration requirement, first ARM calculates climbing power demand, and FPGA combines current of electric, photoelectricity is compiled Code device, three axis accelerometer A1 and the feedback of three-axis gyroscope G1, adjust pipe robot permagnetic synchronous motor X, permanent magnetism in real time The PWM of synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and permagnetic synchronous motor W controls letter Number, make pipe robot meet constant speed and patrol and examine requirements for pipes；In climbing detection process, ARM is according to magnetic navigation sensor ME1's Feedback, reads positional information, three axis accelerometer A1 and the numerical value of three-axis gyroscope G1 that pipe robot is actual, and with setting Position compares, and determines pipe robot off-center distance and the angle of inclination, and FPGA feeds back in conjunction with current of electric, photoelectricity Encoder feedback, three axis accelerometer A1 and the feedback of three-axis gyroscope G1, obtain according to its internal three Close loop servo control programs To the PWM wave control signal of six permanent magnet synchronous motors, and adjust pipe robot attitude in real time by drive circuit, make pipeline Robot stabilized operate in magnetic stripe immediate vicinity, if master station finds pipe robot by a relatively large margin by the collection image of feedback Deviate from navigation track, by the attitude by control device of wireless preferential Calibration pipe robot；ARM real time record robot The air line distance run, the distance correction sensor S moment detect ground revise mark, once read correcting device, ARM The positional distance information of record to be as the criterion to revise the positional information of mark, eliminates the robot position caused when walking Error, when by three-axis gyroscope G1 and three axis accelerometer A1, ARM finds that robot is complete climbing action and enters normal Patrol and examine environment, FPGA block permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and permagnetic synchronous motor W's Pwm control signal, makes system enter permagnetic synchronous motor X and permagnetic synchronous motor Y driving condition；

7) after pipe robot enters normal detection region, if pipe robot left fork sensing during main pipeline is patrolled and examined Device ME2 reads ground turning mark, and first ARM according to ground installation straightener pipeline device people positional information in the duct, disappears Except pipe robot walking error；FPGA according to system speed and acceleration requirement, in conjunction with current of electric, photoelectric encoder, three Axis accelerometer A1 and the feedback of three-axis gyroscope G1, adjust pipe robot permagnetic synchronous motor X and permanent magnet synchronous electric in real time The pwm control signal of machine Y, makes robot stop in distance R, and then FPGA controller combines three-axis gyroscope and three axles accelerate The feedback of degree meter makes pipe robot rotate in place left 90 degree, and pipe robot enters left turnout and cruises；Visit on left turnout During survey, the front laser displacement sensor LSF moment opens and detects doubtful tamper and detects the distance with front terminal； ARM is according to the feedback real time record forward travel distance of front laser displacement sensor LSF, and effective range before the terminal of distance turnout Nei Shi robot effectively stops, and robot rotates in place 180 degree of standards under three-axis gyroscope G1 and three axis accelerometer A1 controls Standby return main channel；Owing to left turnout has been patrolled and examined complete, in order to make pipe robot return quickly in main pipeline, FPGA opens Open power-assisted permagnetic synchronous motor Z and permagnetic synchronous motor R, make system enter and accelerate return state；In whole return course, FPGA is according to system speed and acceleration requirement, in conjunction with current of electric, photoelectric encoder, three axis accelerometer A1 and three axle gyros The feedback of instrument G1, adjusts robot permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permanent-magnet synchronous in real time The pwm control signal of motor R, makes pipe robot can speed up and returns to main pipeline along landing ground magnetic stripe；Enter and normally patrol and examine ring Behind border, FPGA blocks permagnetic synchronous motor Z, the pwm control signal of permagnetic synchronous motor R, makes system enter permagnetic synchronous motor X With permagnetic synchronous motor Y driving condition；

8) after pipe robot enters normal detection region, if pipe robot right fork sensing during main pipeline is patrolled and examined Device ME3 reads ground turning mark, and first ARM according to ground installation correction pipe robot positional information in the duct, disappears Except pipe robot walking error；FPGA according to system speed and acceleration requirement, in conjunction with current of electric, photoelectric encoder, three Axis accelerometer A1 and the feedback of three-axis gyroscope G1, adjust robot permagnetic synchronous motor X's and permagnetic synchronous motor Y in real time Pwm control signal, makes pipe robot stop in distance R, and then FPGA combines the feedback of three-axis gyroscope G1 and makes pipe machine Device people rotates in place right 90 degree, and pipe robot enters right turnout and cruises；In the detection process of right turnout, laser position, front The displacement sensor LSF moment opens and detects doubtful tamper and detects the distance with front terminal；ARM is according to front laser displacement The feedback real time record forward travel distance of sensor LSF, and effective range Nei Shi robot effectively stops before the terminal of distance turnout, Pipe robot rotates in place 180 degree under three-axis gyroscope G1 and three axis accelerometer A1 controls and prepares to return to main channel；By Having patrolled and examined complete in right turnout, in order to make pipe robot return quickly in main pipeline, FPGA opens power-assisted permanent-magnet synchronous Motor Z and permagnetic synchronous motor R, makes system enter and accelerates return state；In whole return course, FPGA is according to system speed With acceleration requirement, then in conjunction with current of electric, photoelectric encoder, three axis accelerometer A1 and the feedback of three-axis gyroscope G1, Adjust pipe robot permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permagnetic synchronous motor R in real time Pwm control signal, makes pipe robot can speed up and returns to main pipeline along landing ground magnetic stripe；Enter after normally patrolling and examining environment, FPGA blocks permagnetic synchronous motor Z, the pwm control signal of permagnetic synchronous motor R, makes system enter permagnetic synchronous motor X and forever Magnetic-synchro motor Y driving condition；

9) after pipe robot enters normal detection region, if pipe robot left fork sensing during main pipeline is patrolled and examined Device ME2 and right fork ME3 reads ground turning mark simultaneously, illustrates that pipe robot enters cross pipeline crossing, and ARM is first First according to ground installation correction pipe robot positional information in the duct, eliminate pipe robot walking error；FPGA presses Lighting system speed and acceleration requirement, then in conjunction with current of electric, photoelectric encoder, three axis accelerometer A1 and three-axis gyroscope The feedback of G1, adjusts robot permagnetic synchronous motor X and the pwm control signal of permagnetic synchronous motor Y in real time, makes pipe robot Enter right turnout to cruise, ARM according to the feedback real time record forward travel distance of front laser displacement sensor LSF, and away from Before the terminal of turnout, effective range Nei Shi robot effectively stops, and pipe robot is at three-axis gyroscope G1 and three axis accelerometer A1 control under rotate in place 180 degree return main channels, FPGA opens power-assisted permagnetic synchronous motor Z and permagnetic synchronous motor R, make be System enters four-wheel and accelerates return state；When the feedback of right laser displacement sensor LSR and left laser displacement sensor LSL occurs relatively During amplitude change, illustrating that robot comes into cross mouth position, now ARM starts to revise the mistake of pipe robot walking Difference, it is ensured that pipe robot positional distance information is correct.As right laser displacement sensor LSR and left laser displacement sensor LSL Feedback again occur when higher magnitude changes, illustrating that pipe robot comes into left fork pipeline, now ARM starts to repair The error of positive pipe robot walking, it is ensured that pipe robot positional distance information is correct, and FPGA is according to system speed and acceleration Degree requirement, in conjunction with current of electric, photoelectric encoder, three axis accelerometer G1 and the feedback of three-axis gyroscope G1, adjusts pipe in real time Pipeline robot permagnetic synchronous motor X and the pwm control signal of permagnetic synchronous motor Y, make pipe robot enter left turnout and carry out Cruise, ARM is according to the feedback real time record forward travel distance of front laser displacement sensor LSF, and has before the terminal of distance turnout Making pipe robot effectively stop in the range of effect, pipe robot is former under three-axis gyroscope G1 and three axis accelerometer A1 controls Ground rotation turnback returns to main channel, and FPGA opens power-assisted permagnetic synchronous motor Z and permagnetic synchronous motor R, makes system enter four-wheel Accelerate return state；When the feedback of right laser displacement sensor LSR and left laser displacement sensor LSL occurs that higher magnitude changes Time, illustrating that pipe robot has returned to cross mouth position, ARM starts to revise the error of pipe robot walking, it is ensured that pipeline Robot location's range information is correct, and controller is walked by pipe robot and is steadily parked in crossroad, then after a segment distance It turn 90 degrees in three-axis gyroscope G1 and three axis accelerometer A1 control bottom left and come back to main pipeline detection region；

10) if pipe robot in normal motion by external interference or run into magnetic stripe and rupture, magnetic navigation passes Sensor ME1 cannot read ground magnetic strip information, and ARM will be sensed by control device of wireless and master station's communication, right laser displacement Device LSR and left laser displacement sensor LSL will open work, and the two is by the distance input that records to ARM controller, and controller is right Obtaining off-centered position afterwards compared with setting value, this deviation signal is inputed to FPGA, FPGA by ARM joins this deviation Number is converted into pipe robot and patrols and examines distance to be run for permagnetic synchronous motor X and permagnetic synchronous motor Y under track, speed specifying Degree and acceleration command value, FPGA then in conjunction with motor current feedback, photoelectric encoder feedback, three axis accelerometer A1 and The feedback of three-axis gyroscope G1, obtains, through internal servo control algorithm, the PWM wave control signal that two permanent magnet synchronous motors controls, And adjust pipe robot attitude in real time by drive circuit, make pipe robot stable operation near pipeline planar central, Adjust the pid parameter of internal SERVO CONTROL program according to peripheral environment FPGA in real time, make system easily realize segmentation P, PD, PID Control and nonlinear PID controller；The air line distance that ARM real time record pipe robot has run, during distance correction sensor S Carving detection ground and revise mark, once read correcting device, ARM record position range information will be to revise the position letter of mark Breath is as the criterion, if pipe robot has misread ground in the process of walking revises mark, main website will by radio transmitting device and ARM communication, and automatically revise the positional information of pipe robot, eliminate site error during pipe robot walking；

11) in pipe robot motor process, the CCD moment in image acquisition opens, and DSP real-time storage CCD collect Image, DSP compares the standard pipe information of the image and the setting that gather, if the two comparison result bigger mistake of appearance Difference, in order to prevent maloperation, DSP sends interrupt requests by wireless device to main website immediately, and with ARM communication, ARM is immediately Response DSP interrupt, and allow pipe robot stop by FPGA, DSP is by the internal three closed-loop direct SERVO CONTROL programs of FPGA Adjust the CCD position, output location of direct current generator A and direct current generator B, make ccd image gather center and be directed at doubtful picture centre, CCD secondary acquisition duct size information also passes through radio transmitting device real-time Transmission, and main website analyzes this suspicious region, DSP bis-times in real time Store this image, doubtful be disposed after controlled in wireless reopen pipe robot, FPGA controller opens permanent magnet synchronous electric Machine X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permagnetic synchronous motor R make pipe robot be in boost phase, when reaching After setting speed, FPGA block permagnetic synchronous motor Z and the pwm control signal of permagnetic synchronous motor R, make pipe robot be in The two axle driving conditions of permagnetic synchronous motor X and permagnetic synchronous motor Y also continue to move ahead along pipeline navigation marker；

12) in pipe robot motor process, the humidity sensor moment in humidity collection system opens, ARM real-time storage The humidity information that humidity sensor collects, and compare with the standard pipe humidity information set, if the two comparison knot There is bigger error in fruit, and in order to prevent maloperation, ARM sends interrupt requests by wireless device to main website immediately, and passes through The PWM wave control signal of the internal SERVO CONTROL program active accommodation two permanent magnet synchronous motors X and permagnetic synchronous motor Y of FPGA, The speed reducing pipe robot makes it at a slow speed by doubt region, adjusts internal servo in real time according to peripheral environment FPGA The pid parameter of control program, makes system easily realize segmentation P, PD, PID and controls and nonlinear PID controller；Simultaneously ARM immediately to DSP sends interrupt requests, and the DSP ARM that makes an immediate response interrupts, and adjusts direct currents by the internal three closed-loop direct SERVO CONTROL programs of FPGA The output of motor A and direct current generator B makes it position CCD position, makes ccd image gather center and is directed at doubtful water images center, and Strengthening the comparison of aqueous water in CCD pipeline collection information, DSP stores the doubtful image of steam and the actual position information in this region, And by radio transmitting device real-time Transmission, main website analyzes this suspicious region in real time, confirm errorless after, DSP stores this figure bis-times Picture and positional information, main website restarts ARM by wireless device, controls pipe robot and continue to move ahead, after by suspicious region, ARM Yu FPGA communication, FPGA opens permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permagnetic synchronous motor R makes pipe robot be in boost phase, FPGA block permagnetic synchronous motor Z and permagnetic synchronous motor after reaching setting speed The pwm control signal of R, makes robot be in the two axle driving conditions of permagnetic synchronous motor X and permagnetic synchronous motor Y and along pipe Road navigation marker continues to move ahead；

13) in pipe robot motor process, the front laser displacement sensor LSF moment opens, and ARM processes front orientation in real time Confidence ceases, and when having anomalies in conduit running front, front laser displacement sensor LSF probe value will appear from exception, ARM with FPGA communication, and by the internal SERVO CONTROL program active accommodation two permanent magnet synchronous motors X's and permagnetic synchronous motor Y of FPGA PWM wave control signal, the speed reducing pipe robot makes it drive towards at a slow speed barrier, adjusts in real time according to peripheral environment FPGA The pid parameter of internal SERVO CONTROL program, makes system easily realize segmentation P, PD, PID and controls and nonlinear PID controller；Simultaneously ARM sends interrupt requests to master station and DSP immediately, and the DSP ARM that makes an immediate response interrupts, and is watched by three closed-loop direct inside FPGA The output taking control program adjustment direct current generator A and direct current generator B makes it position CCD position, makes ccd image gather center alignment Doubtful obturator picture centre, CCD secondary acquisition duct size information is also by radio transmitting device real-time Transmission, and main website analyzes in real time This suspicious region, DSP stores the doubtful image of blocking and actual position information, the pipe robot designed due to this in this region Being many wheel crawlers structure, FPGA controls pipe robot and opens permagnetic synchronous motor X, permagnetic synchronous motor Y, permanent magnetism together simultaneously Walk motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U, permagnetic synchronous motor W are over barrier and continue to move ahead, when Behind suspicious region, ARM Yu FPGA communication, FPGA controller block permagnetic synchronous motor Z and the PWM of permagnetic synchronous motor R Control signal, makes pipe robot be in the two axle driving conditions of permagnetic synchronous motor X and permagnetic synchronous motor Y and along pipeline Navigation marker continues to move ahead；

14) in pipe robot motor process, pipeline inspection sensor based on Hall effect is by work, when before conduit running Side's probe value occurs abnormal, ARM Yu FPGA communication, and by internal two permanent-magnet synchronous of SERVO CONTROL program active accommodation of FPGA The PWM wave control signal of motor X and permagnetic synchronous motor Y, the speed reducing pipe robot makes it drive towards at a slow speed pipe damage Suspicious region, adjusts the pid parameter of internal SERVO CONTROL program in real time, makes system easily realize segmentation according to peripheral environment FPGA P, PD, PID control and nonlinear PID controller；ARM sends interrupt requests to master station and DSP immediately simultaneously, and DSP makes an immediate response ARM Interrupt, and adjusted the output location of direct current generator A and direct current generator B by the internal three closed-loop direct SERVO CONTROL programs of FPGA CCD position, makes ccd image gather alignment suspicious lesion pipeline position, center, CCD secondary acquisition duct size information by wireless biography Defeated device real-time Transmission, main website analyzes this suspicious region in real time, and DSP storage finds doubtful pipe damage image, as do not found pipe Road damage image, DSP will record suspicious lesion actual position information, and labelling outer damage, after by suspicious region, ARM With FPGA communication, FPGA opens permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permagnetic synchronous motor R and makes Pipe robot is in boost phase, FPGA block permagnetic synchronous motor Z and permagnetic synchronous motor R after reaching setting speed Pwm control signal, makes robot be in the two axle driving conditions of permagnetic synchronous motor X and permagnetic synchronous motor Y and along pipeline Navigation marker continues to move ahead；

15) in pipe robot motor process, ARM can store in the moment the pipeline location of process or process Reference point, FPGA is converted into pipe robot permagnetic synchronous motor X and permanent magnetism under track is patrolled and examined in appointment this location parameter Distance, speed and acceleration command value to be run for synchronous motor Y, FPGA is then in conjunction with current feedback, the photoelectricity volume of motor Code device feedback, three axis accelerometer A1 and the feedback of three-axis gyroscope G1, obtain two permanent magnetism through internal servo control algorithm same The PWM ripple that step motor controls, makes pipe robot quickly move ahead according to setting speed, and simultaneously according to peripheral environment, FPGA is real-time Adjust the pid parameter of internal SERVO CONTROL program, make system easily realize segmentation P, PD, PID and control and nonlinear PID controller；

16) if pipe robot detective distance solves and occurs that endless loop will send interrupt requests to ARM in motor process, Then ARM can send interrupt requests to master station to interrupting doing very first time response original place self-locking；Master station passes through wireless device weight Newly reset ARM, transmits the information such as new position, and ARM is according to pipeline magnetic bar navigation mark feedback and right laser displacement sensor LSR With the feedback of left laser displacement sensor LSL, adjust permanent-magnet synchronous X-motor and the speed of permagnetic synchronous motor Y, pipe machine in real time Device people starts to patrol and examine from new position；

17) permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permanent magnet synchronous electric it are contained in Photoelectric encoder on machine U, permagnetic synchronous motor W, direct current generator A and direct current generator B can export its position signalling A and position letter Number B, position signalling A pulse and the B pulsed logic state of photoelectric encoder often change once, ARM(or DSP) in location register Device can add 1 according to the traffic direction of motor or subtract 1；The position signalling A pulse of photoelectric encoder and B pulse and Z pulse are together Time when being low level, just produce an INDEX signal to ARM(or DSP) internal register, record permagnetic synchronous motor absolute Position, is then convert into pipe robot particular location in pipe detection system；

18) pipe robot calculates battery SOC at running ARM in real time according to its internal algorithm, if controller finds electricity When source energy is relatively low, ARM with DSP communication, and can close ccd image collecting work and image storage work by DSP, and passes through The internal three closed loop servo-control system programs of FPGA adjust the PWM output of permagnetic synchronous motor X and permagnetic synchronous motor Y, pipeline Robot drives towards exit with slower speed under controlled in wireless, it is ensured that robot can arrive exit smoothly；

19) during pipe robot is patrolled and examined, if there is pulsation in the torque that servo controller detects permagnetic synchronous motor, The permagnetic synchronous motor used due to the present invention controls to be based on vector controlled, and therefore FPGA controller can be easy to compensate this and does Disturb, decrease the motor torque impact on pipe robot motor process；

20) during pipe robot drives towards outlet valve, the front laser displacement sensor LSF that it carries can moment inspection Survey the displacement between itself and valve, when determine outlet valve F3 in open mode, pipe robot will open cruise mode, control The distance that device ARM real time record pipe robot has moved along magnetic stripe, when determining that pipe robot is treated completely into outlet Behind inspection region, outlet valve F3 will close, and natural gas aspirator is by aspirating the natural gas situation in region to be checked, when natural diarrhea due to stagnation of QI Dew device is not detected by region to be checked when having natural gas to remain, and outlet valve F4 will open, now front laser displacement sensor Secondary is judged the state of front exit valve F4 by LSF, determine front valve open errorless after, pipe robot roll away from detection pipe Road, returns to detect terminal, waits next sense command.

The invention have the advantages that:

1: in pipe robot motor process, take into full account battery effect in this system, based on ARM+ FPGA+ Its state is all being monitored and computing by the DSP dual-core controller moment, has both avoided the lithium caused due to heavy-current discharge The generation of ion battery overaging phenomenon, can effectively predict again the energy of battery, patrol and examine for pipe robot and provided Effect ensures；

2: processed pipe robot six permagnetic synchronous motors SERVO CONTROL based on rotor vector controlled and two axles by FPGA The SERVO CONTROL of direct current generator so that control fairly simple, substantially increase arithmetic speed, solves control algolithm and takies ARM The problem longer with the dsp operation cycle, shortens the construction cycle short, and program transportability ability is strong；

3: the present invention realizes full SMD components material substantially, it is achieved that veneer controls, and not only saves panel and takes up room, And beneficially the alleviating of pipe robot volume and weight；

4: it is conventional that the pipe robot navigation system of the present invention uses permagnetic synchronous motor to instead of in conventional machines people's system Motor, direct current generator, DC brushless motor, due to its small volume, efficiency is higher so that pipe robot volume is permissible Reducing further, energy utilization rate is greatly improved；

5: owing to permagnetic synchronous motor uses vector controlled so that speed adjustable range is relatively wide, contrast of regulating speed is steady, even if at low speed The pulsating torque of stage motor is the least, the dynamic property of beneficially raising system；

6: owing to this controller uses DSP to process figure collection and the mass data of storage and algorithm, FPGA process six axles forever Magnetic-synchro motor and the SERVO CONTROL of two axle direct current generators, free ARM from hard work amount, effectively prevent " race flies " of program, capacity of resisting disturbance is greatly enhanced；

7: in the controlling, FPGA can adjust six axle permagnetic synchronous motors according to robot periphery ruuning situation in good time and two axles are straight Stream pid parameter within motor servo, it is achieved segmentation P, PD, PID control and nonlinear PID controller, make system meet real-time work Condition requires to change；

8: equipped with humidity collection system on pipe robot, the humidity abnormal area in tunnel can be detected easily, can Effectively find the existence of pipeline water droplet；

9: equipped with image capturing system on pipe robot, the abnormal feelings such as pipe interior corrosive pipeline can be detected easily Condition, and effectively store its image；

10: master station staff can be made accurately to judge suspicious region based on wireless image transmission, wireless remote control technology can simultaneously Well to solve the emergency situations that pipe robot occurs, such as solve deadlock, restart pipe robot etc.；

11: image based on DSP storage function makes pipe robot facilitate staff to read after completing task and patrols and examines result, Pipeline corrupted information and particular location, then on-call maintenance can be read easily from storage result；

12: equipped with fault localization acquisition system based on Hall effect on pipe robot, pipeline can be detected easily The abnormal conditions such as external pipe corrosion and damage, be conducive to pinpointing the problems early pipeline；

13: the addition of three-axis gyroscope G1 and three axis accelerometer A1 can the most accurately detect pipe robot deviation pipeline plane Angle of inclination, ARM can be monitored and pass through FPGA the moment to this angle, and to adjust the PWM ripple of permagnetic synchronous motor accordingly defeated Go out, and moment communication main website, running into emergency can be controlled by master station's adapter, effectively controls the attitude of pipe robot；

14: the addition of magnetic navigation sensor ME1 and laser displacement sensor makes system navigation have certain redundancy, greatly Improve the stability of pipe robot, and make this pipe robot can patrol and examine non-equant complicated pipeline；

15: the addition of Timing Belt technology makes eight wheels all have power, and the addition of crawler belt simultaneously effectively increases pipeline machine The area that people contacts in the duct, makes pipe robot can improve environment fit effectively by having obstruction object area Ying Xing；

16: two axle ccd images based on direct current generator A and direct current generator B gather alignment system can make image acquisition more reliable, Carry out pipe damage for main website and obstruction information is analyzed in real time and provided reliable basis；

17: turning navigation marker in ground coordinates left and right sides laser displacement sensor to make system can easily read duct size information, Be conducive to pipe robot location in complicated pipeline and the elimination of site error；

18: three axles when pipe robot is turned can be the most accurately measured in the addition of three-axis gyroscope G1 and three axis accelerometer A1 Accelerometer A1, speed and angle, improve reliable basis for pipe robot turning navigation in complicated pipeline；

19: the addition of multiple stage power-assisted permagnetic synchronous motor makes the power performance adjustment of system have optional so that robot The power demand under different operating mode can be met so that the adaptation ability of robot is strengthened；

20: the addition of three-axis gyroscope G1 and three axis accelerometer A1 can the most accurately detect three axles during pipe robot climbing Accelerometer A1, speed and ramp angle, this angle can be detected in the moment by ARM, and effectively opens power-assisted permanent magnet synchronous electric Machine is patrolled and examined the acclive pipeline of tool for pipe robot and is provided power demand；

21: the addition of three-axis gyroscope G1 and three axis accelerometer A1 can effectively measure what pipe robot occurred at line navigation Three axis accelerometer A1, speed and direction skew, improve for pipe robot inertial navigation in complicated pipeline and reliably depend on According to.

To sum up telling, the present invention is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems, In order to improve the utilization rate of the energy and reduce the permanent magnet synchronous electric that robot volume, native system efficiency and power density are the highest Machine instead of the motor such as motor, direct current generator；In order to improve system acceleration request, system is that these extraordinary operating modes add Two lower-powered permagnetic synchronous motors play acceleration power-assisted effect, increase system dynamic characteristic；General in order to improve system Hill climbing demands, system is that these extraordinary operating modes add two lower-powered permagnetic synchronous motors and play climbing power-assisted effect, Increase system dynamic characteristic；In order to improve arithmetic speed, it is ensured that the stability of automatic pipeline robot system and reliability, this Bright introducing FPGA and digital signal processor DSP in controller based on ARM, is formed based on ARM+ FPGA+DSP brand-new Three nuclear control devices, this controller takes into full account the battery effect in this system, six axles that workload in control system is maximum Permagnetic synchronous motor and two axle direct current generator servosystem give that FPGA completes, battery cell monitoring, path reading, deviation processing etc. are handed over To ARM process, give full play to the comparatively faster feature of ARM data processing speed, and the function such as image data acquiring and storage is handed over Complete to DSP, thus achieve the division of labor of ARM, FPGA and DSP, communication can also be carried out simultaneously between three, enter in real time Row data exchange and call.

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

Claims

1. one kind is wirelessly transferred three core eight axle caterpillar type high-speed natural gas line robot control systems, it is characterised in that include Battery, controller, permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permanent-magnet synchronous Motor U, permagnetic synchronous motor W, direct current generator A, direct current generator B, based on ccd image collecting unit, image storage unit, humidity Collecting unit, based on Hall effect pipeline inspection collecting unit, wireless device and pipe robot, described battery individually carries For the controller described in electric current driving, described controller uses three nuclear control devices, including ARM, FPGA and DSP, described ARM, FPGA and DSP carry out communication connection by wireless device, described based on ccd image collecting unit and image storage unit All be connected with DSP communication, described humidity collection unit and based on Hall effect pipeline inspection collecting unit all with ARM and FPGA Communication connects, and described ARM and FPGA sends the first control signal, the second control signal, the 3rd control signal, the 4th control respectively Signal processed, the 5th control signal and the 6th control signal, by the first described control signal, the second control signal, the 3rd control Signal, the 4th control signal, the 5th control signal and the 6th control signal control described permagnetic synchronous motor X, permanent magnetism respectively Synchronous motor Y, permagnetic synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and the signal syntheses of permagnetic synchronous motor W Controlling the motion of pipe robot the most again, described DSP sends the 7th control signal and the 8th control signal respectively, by described The 7th control signal and the 8th control signal control described direct current generator A, direct current generator B respectively signal syntheses after with Connect based on ccd image collecting unit communication.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that described battery is adopted Use lithium ion battery.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that the first described control Signal processed, the second control signal, the 3rd control signal, the 4th control signal, the 5th control signal and the 6th control signal are PWM wave control signal.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that described ARM adopts Use STM32F746；Described FPGA uses QUICKLOGIC；Described DSP uses TMS320F2812.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that described pipe machine Device people includes robot housing, laser displacement sensor, magnetic navigation sensor, left fork sensor, right fork sensor, three axles Gyroscope, three axis accelerometer and Timing Belt, described laser displacement sensor is separately mounted to the front end of robot housing, Described left fork sensor and right fork sensor lay respectively at the two ends, left and right below laser displacement sensor, and described is same Step band be separately positioned on robot housing limit, the left and right sides and respectively with permagnetic synchronous motor X, permagnetic synchronous motor Y, permanent magnetism Synchronous motor Z, permagnetic synchronous motor R, permagnetic synchronous motor U and permagnetic synchronous motor W connect, described magnetic navigation sensor, Three-axis gyroscope and three axis accelerometer are successively set on robot housing and are positioned at permagnetic synchronous motor X and permanent magnet synchronous electric Between machine Y, described direct current generator A and direct current generator B is arranged on robot housing and lays respectively at left fork sensor With the top of right fork sensor, described wireless device is positioned at the rear end of robot housing.

High speed natural gas line robot control system the most according to claim 5, it is characterised in that described laser position Displacement sensor includes front laser displacement sensor, left laser displacement sensor and right laser displacement sensor, described front Laser displacement sensor is arranged on the centre position of robot housing dead ahead, described left laser displacement sensor and right laser The most angularly disposed two ends, left and right at robot housing dead ahead of displacement transducer.

High speed natural gas line robot control system the most according to claim 5, it is characterised in that described Timing Belt Use six axle eight wheel drive mode, be closed ring crawler belt and the corresponding belt wheel institute being provided with equidistant tooth by an inner peripheral surface Composition.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that described natural gas Pipe robot control system be additionally provided with host computer procedure, based on ARM main motion control program, based on DSP from motor control Program, based on Hall effect pipe damage detection and be wirelessly transferred, described host computer procedure also include pipeline read, position Location and power information, described also includes based on FPGA six axle permagnetic synchronous motor servo based on ARM main motion control program Control, data store and I/O controls, and described also includes based on FPGA two axle direct current generator from motion control program based on DSP SERVO CONTROL and based on DSP image acquisition, described based on the transmission of Hall effect pipe damage detection wireless and respectively with based on Hall effect pipeline inspection collecting unit and wireless device communication connect.

High speed natural gas line robot control system the most according to claim 1, it is characterised in that described high speed sky So feed channel robot control system also includes that photoelectric encoder, described photoelectric encoder are separately mounted to permagnetic synchronous motor X, permagnetic synchronous motor Y, permagnetic synchronous motor Z and permagnetic synchronous motor R, permagnetic synchronous motor U, permagnetic synchronous motor W, direct current On motor A and direct current generator B.