CN107328308A - A kind of autonomous type detecting a mine robot system and detecting a mine method - Google Patents
A kind of autonomous type detecting a mine robot system and detecting a mine method Download PDFInfo
- Publication number
- CN107328308A CN107328308A CN201710595932.9A CN201710595932A CN107328308A CN 107328308 A CN107328308 A CN 107328308A CN 201710595932 A CN201710595932 A CN 201710595932A CN 107328308 A CN107328308 A CN 107328308A
- Authority
- CN
- China
- Prior art keywords
- mine
- robot
- arm
- detecting
- detection
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 20
- 238000001514 detection method Methods 0.000 claims abstract description 154
- 239000003973 paint Substances 0.000 claims abstract description 79
- 230000033001 locomotion Effects 0.000 claims abstract description 53
- 238000005183 dynamical system Methods 0.000 claims abstract description 39
- 239000007921 spray Substances 0.000 claims description 83
- 230000007246 mechanism Effects 0.000 claims description 67
- 239000002184 metal Substances 0.000 claims description 35
- 229910052751 metal Inorganic materials 0.000 claims description 35
- 238000005507 spraying Methods 0.000 claims description 21
- 238000007592 spray painting technique Methods 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 11
- 239000013598 vector Substances 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000008569 process Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 239000002421 finishing Substances 0.000 description 42
- 238000004891 communication Methods 0.000 description 18
- 238000010586 diagram Methods 0.000 description 12
- 230000003993 interaction Effects 0.000 description 8
- 239000007788 liquid Substances 0.000 description 8
- 230000000694 effects Effects 0.000 description 6
- 230000009194 climbing Effects 0.000 description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 5
- 238000004886 process control Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- PCHJSUWPFVWCPO-UHFFFAOYSA-N gold Chemical compound [Au] PCHJSUWPFVWCPO-UHFFFAOYSA-N 0.000 description 3
- 239000010931 gold Substances 0.000 description 3
- 229910052737 gold Inorganic materials 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 239000011248 coating agent Substances 0.000 description 2
- 238000000576 coating method Methods 0.000 description 2
- 230000008602 contraction Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000000843 powder Substances 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 241001269238 Data Species 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 239000002360 explosive Substances 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000002045 lasting effect Effects 0.000 description 1
- 231100000518 lethal Toxicity 0.000 description 1
- 230000001665 lethal effect Effects 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000000465 moulding Methods 0.000 description 1
- 238000009659 non-destructive testing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
- 230000003319 supportive effect Effects 0.000 description 1
- 230000007306 turnover Effects 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
Classifications
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F41—WEAPONS
- F41H—ARMOUR; ARMOURED TURRETS; ARMOURED OR ARMED VEHICLES; MEANS OF ATTACK OR DEFENCE, e.g. CAMOUFLAGE, IN GENERAL
- F41H11/00—Defence installations; Defence devices
- F41H11/12—Means for clearing land minefields; Systems specially adapted for detection of landmines
- F41H11/16—Self-propelled mine-clearing vehicles; Mine-clearing devices attachable to vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
Abstract
The invention discloses a kind of autonomous type detecting a mine robot system and detecting a mine method, the system includes car body, the car body is provided with mobile chassis system 2, mechanical arm detection system 3 and paint finishing 4, and mobile chassis system 2 is connected with mechanical arm detection system 3 and paint finishing 4 respectively;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3 includes manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, provides power source for whole device and does with reuse;Control system 7 connects dynamical system 6 and detection system 9 respectively;Detection system 9 is located at the front end of manipulator motion system 8.The present invention can carry out intelligence, accurate, efficient, lossless detection and indicate to minefield thunder point, so as to provide safe and reliable ensure for the later stage removal of mines;And the detecting a mine robot of the present invention carries out detecting a mine mark, lasts a long time.
Description
Technical field
The present invention relates to a kind of autonomous type detecting a mine robot system and detecting a mine method, belong to mine-detecting technique field.
Background technology
Land mine is a kind of lethal larger explosive weapon, and the belligerent side of war can generally use substantial amounts of land mine,
The military power of enemy is cut down with this.But it is due to indiscriminately laying and using for land mine, causes belligerent countries' tract to be lain waste, Ren Minliu
From becoming homeless, had a strong impact on economic construction and the life of the people, thus mine-detecting technique just turn into these belligerent countries in the urgent need to
Technology.
The patent of Application No. 201620688512.6 discloses a kind of self-balancing mine detection vehicle for adapting to many places, and it can be with
In a variety of places, the detecting a mine work in the case of completion is various has ensured the safe coefficient of related personnel's detecting a mine work.But should
Scheme still has problems with:It disclose only if it find that danger stops at once, and sends alarm, it is not specified that visiting
Measure and how to be disposed after land mine, thus thunder quickly can not be torn open according to the realization of detecting a mine situation.Application No. 02100602.4
Patent application discloses a kind of quick mine detection vehicle of blanket type, and the program is directly ignited after land mine is detected, mine detection vehicle is broken
Bad power is very big, and the life-span for directly resulting in mine detection vehicle reduces significantly;In addition, the obstacle climbing ability of the mine detection vehicle is general, cause to use
Scope is limited.In addition, such scheme there is problems:1st, cost is higher;2nd, the control mode underaction of detecting devices,
And the detection area size of detecting devices depends entirely on the translational speed size of car body, cause the investigative range of mine detection vehicle by
Limit, and it is less efficient to detect a mine.
In addition, the latitude and longitude coordinates, height above sea level and course angle of robot can be obtained using differential global positioning system, but
It is that GPS has problems with:1st, the turnover rate of gps system data is not high, typically can only achieve 5hz, that is, 200ms outputs one
Secondary data, but robot system requires higher data updating rate;2nd, when robot enters some regions, such as tunnel, high building
Between when, gps signal may be temporarily lost with, but also need to lasting acquisition pose.
The content of the invention
It is an object of the present invention to provide a kind of autonomous type detecting a mine robot system and detecting a mine method, it can effectively be solved
Certainly problems of the prior art, realize the accurate, efficient of land mine, lossless detection and mark, for the later stage removal of mines provides safely,
It is reliable to ensure.
In order to solve the above technical problems, the present invention is adopted the following technical scheme that:A kind of autonomous type detecting a mine robot system,
Including car body, the car body is provided with mobile chassis system, mechanical arm detection system and paint finishing, mobile chassis system difference
It is connected with mechanical arm detection system and paint finishing;Described mobile chassis system include mobile platform system, dynamical system and
Control system, mechanical arm detection system includes manipulator motion system and detection system, wherein, dynamical system is installed on mobile flat
The rear end of platform system, provides power source for whole device and does with reuse;Control system connects dynamical system, the motion of tool arm respectively
System and detection system;Detection system is located at the front end of manipulator motion system.
It is preferred that, described paint finishing includes telescoping mechanism, oscillating arm mechanisms and spray body, telescoping mechanism and swing arm machine
Structure is connected with spray body;Described spray body includes shower nozzle, so as to reach the effect of paint finishing sector region spraying
(shower nozzle coverage is fan-shaped).
It is furthermore preferred that described telescoping mechanism includes:A motors, rack and nozzle arm, the A motors are provided with A gears, A
Wheel and rack is connected, and rack is connected with nozzle arm, and nozzle arm connects shower nozzle, is not only simple in structure, with low cost, and can be with
Flexibly, the accurately stretching and contraction of control spray body.
It is furthermore preferred that described oscillating arm mechanisms include:B motors and arc-shaped rack, the B motors are provided with B gears, B teeth
Wheel is connected with arc-shaped rack, and arc-shaped rack is connected with nozzle arm, is not only simple in structure, with low cost, and can flexibly, accurately
Control spray body swing.
It is furthermore preferred that described spray body also includes:Air compressor machine and paint spray pump, described air compressor machine connect with paint spray pump
Connect, paint spray pump is connected with shower nozzle, so as to use paint to carry out the spraying sign of land mine as coating;Compared to utilizing liquid
Liquid such as fountain pen ink in tank are indicated using the coating powder in paint can, brighter using paint sign in the present invention
It is aobvious, it is smaller by external environment influence, and also paint is not easily accessible soil, and the retention time is more long.
Described paint spray pump can use barrier film paint spray pump, and land mine is carried out more preferably in rugged environment so as to realize
Spraying sign, improve spray efficiency, and make it that the time that the paint of spraying is kept is longer.
It is further preferred that described spray body also includes:Two-way electromagnetic valve and/or three-way magnetic valve, described two
Three-way electromagnetic valve is connected with air compressor machine and shower nozzle respectively, and three-way magnetic valve is connected with air compressor machine and shower nozzle respectively, so that spraying
Control is relatively reliable, stably, and three-way magnetic valve can connect two shower nozzles, and more large area can be sprayed every time, improve spraying
Efficiency, and then improve the detecting a mine efficiency of whole detecting a mine mechanical device.
In foregoing autonomous type detecting a mine robot system, described manipulator motion system is using metal machinery arm and non-gold
Belong to the combination arm of arm, one end of described nonmetallic arm connects detection system, other end connection metal machinery arm;Described metal
Mechanical arm is used in doublejointed mechanical arm, multi-joint mechanical arm or frame-type mechanical arm, the present invention, described manipulator motion system
System is using metal machinery arm and the combination arm of nonmetallic arm, so as to greatly improve the detection of detection system (such as GPR)
Precision, reduction interference;In addition, described metal machinery arm uses doublejointed mechanical arm, multi-joint mechanical arm, so as to realize
More flexible control detecting devices, effectively expands the investigative range and detection area of mine detection vehicle, so as to further increase detecting a mine
Efficiency;Simultaneously described metal machinery arm uses frame-type mechanical arm, and not only structure design exquisiteness is simple and reliable, and cost compared with
It is low;In addition, after detection system detects land mine, control system can be reclaimed with control machinery arm kinematic system and allowed to spray body
Go out space, then spray body stretches out and swing, reach shower nozzle and spraying sign is carried out above land mine, so as to have
Effect reduces the volume of detecting a mine device, and realizes that more flexibly, efficiently, accurately control paint finishing carries out spraying sign;This
Outside, described metal machinery arm uses doublejointed mechanical arm, multi-joint mechanical arm or frame structure, so as to play preferably
Dust-proof effect, substantially increases the life-span of detecting a mine mechanical device.
In the present invention, in addition to intelligent control module and positioning navigation module in mobile chassis system, intelligence control
Molding block is electrically connected with positioning navigation module and control system respectively, so as to be easy to detecting a mine robot according to location navigation mould
The information that block is provided more reasonably carries out path planning.
It is preferred that, in addition to tele-control system and the remote communication module in mobile chassis system, described is remote
Journey communication module is connected with tele-control system and intelligent control module respectively, and detecting a mine robot is by remote communication module and far
Process control system is entered line number and passed and figure biography communication, so as to facilitate the work of the remote Real Time Observation detecting a mine robot of operating personnel
Make state, detection data, and it is remote to the progress such as the path planning of detecting a mine robot, travel speed, working condition, avoidance
Control, detects and sign is sprayed after land mine, so as to realize being removed mines for the later stage for the high precisely high reliability of more efficient, high intelligence
Safe and reliable ensure is provided.
In the present invention, described mobile platform system is multi-track mobile platform, so that the detecting a mine machinery of the present invention
Device has more preferable obstacle climbing ability, greatly expands the application scenario of the detecting a mine mechanical device of the present invention.
It is preferred that, described mobile platform system includes:The first driving wheel of both sides is respectively arranged on, two are provided with per side,
And two the first driving wheels per side are connected by crawler belt;Also include the second active being connected with least one first driving wheel
Wheel, the second described driving wheel is connected by crawler belt with driven pulley, so that mobile platform system has more preferable supportive
Stronger power and can be provided for whole device so that detecting a mine mechanical device of the invention has more preferable obstacle climbing ability.
It is further preferred that described mobile platform system includes:The first driving wheel of both sides is respectively arranged on, is provided with per side
Two, and often two the first driving wheels of side are connected by crawler belt;Also connected one to one including four with the first driving wheel
Second driving wheel (coaxially connected), the second described driving wheel is connected by crawler belt with driven pulley, so as to further raising
The obstacle climbing ability of detecting a mine robot, adapts it to more complicated changeable environment.
In foregoing autonomous type detecting a mine robot system, in addition to the avoidance module located at car body rear and front end, it is described
Avoidance module is connected with intelligent control module, so as to realize that mine detection vehicle carries out automatic obstacle-avoiding during detecting a mine, is further improved
Detecting a mine efficiency.
It is preferred that, described positioning navigation module includes:Odometer, inertial navigation and GPS geo-location system, described mileage
Meter, inertial navigation and GPS geo-location system be connecteds with intelligent control module respectively, odometer for recording relative displacement, and inertial navigation,
GPS geo-location system realizes outdoor high precision position and gesture stability together, it is hereby achieved that accurately geographical position letter
Breath and ambient condition information, realize more accurate, rational path planning and more accurately obtain the position of land mine, be convenient for
Mark, investigation.
It is preferred that, in addition to:Industrial camera, described industrial camera respectively with remote communication module and intelligent control module
Connection, the environmental information around ambient condition information and detection system so as to gather detecting a mine robot is monitored.
It is furthermore preferred that described industrial camera includes at least 3, it is respectively arranged in manipulator motion system and before car body
Two ends, so as to accurately obtain the environmental information around mine detection vehicle and the surface conditions information of detection system front end, are carried afterwards
Supply rear deminer is monitored and recorded, and also allowing for the later stage in addition more accurately removes mines.
In the present invention, described intelligent control module uses one or more industrial computer, in particular by many industrial computers,
Various control algolithms can be separately operable on different industrial computers, improve operational efficiency and communication speed, for example, many
A large amount of image datas of industrial camera can be handled individually on an industrial computer;In addition, different industrial computers can be run not
Same operating system (such as linux and windows), to adapt to different device drives.
It is preferred that, described avoidance module is made up of two single-line type laser radars;Using two laser radars, fill respectively
In the front and rear of robot, so as to cover the environment that robot surrounding is whole so that detecting a mine robot can know
All obstacles near all around, can avoidance when no matter advancing or retreating;Reduce system cost simultaneously.
In foregoing autonomous type detecting a mine robot system, described tele-control system includes display screen, man-machine interaction mould
Block and remote control module, described remote control module connect with display screen, human-computer interaction module and remote communication module respectively
Connect, consequently facilitating operating personnel can from the working condition of tele-control system Real Time Observation detecting a mine robot, detection data and
The video that multiple cameras are shot, can also be easy to operating personnel to control detecting a mine robot to carry out avoidance manually.
It is preferred that, described tele-control system can use backpack suitcase remote control, so as to reduce system into
This, and the backpack suitcase remote control small volume, it is lightweight, it is easy to carry, is adapted to the field work of complexity landform.
The method detected a mine using aforementioned system, is comprised the following steps:
S1, control car body is moved to the position that face is specified;
S2, calculates coordinate of the key point in region to be detected in world's rectangular coordinate system, and (can pass through remote control system
System) setting search coverage;
S3, treats search coverage and carries out " Z " font detective path contexture by self;
S4, control car body is detected a mine along path planning traveling (can return detection system by remote communication module simultaneously
Detection data and positioning navigation module shoot video information);After mechanical arm detection system detects land mine, car body stops
Under, detecting a mine work is interrupted, mine location is recorded, and calculate the movement locus of spray body;Manipulator motion system is withdrawn, and is controlled
Paint finishing processed carries out spraying sign to land mine point;
S5, after being indicated to land mine point, car body continues detection operations according to path planning until complete area to be detected of detection
Domain, or to land mine point indicate after, car body return origin, after removing mines continue plan detection.
It is preferred that, coordinate bag of the key point for calculating region to be detected in world's rectangular coordinate system described in step S2
Include following steps:
S21, chooses three points (point at general chosen area edge) not on the same line in region to be detected, point
(can be by GPS) obtain the latitude and longitude coordinates of these three points and the latitude and longitude coordinates and course angle (longitude and latitude of robot body
Coordinate is to be shown to operator to see, or operator assigns instruction and used);
S22, sets up world cartesian coordinates system (robot uses world cartesian coordinates system when moving):With one of them
Point be origin, two other point in a point and origin between straight line as world cartesian coordinates system X-axis;In ground level
The straight line of middle vertical X axis and process origin is used as the Y-axis of world cartesian coordinates system;Vertical ground level and upward vector is used as generation
The Z axis of boundary's rectangular coordinate system;
S23, by coordinate system change coordinate of the key point for obtaining region to be detected in world's rectangular coordinate system and
Position rectangular co-ordinate and attitude angle of the robot body in world's rectangular coordinate system.
A world cartesian coordinates system is all updated before starting working every time, the positional precision in whole work is can guarantee that;
And just can determine that a world cartesian coordinates system only with three points.After selected world cartesian coordinates system, all fortune of robot
Dynamic operation, which is all mapped in this coordinate system, to be performed, and is simplified calculating, is improved detecting a mine efficiency.
It is preferred that, step S3 comprises the following steps:
S31, by region to be detected in world's rectangular coordinate system the corresponding rectangle that is set to (if be manually set not advise
Then polygon, then robot be filled with including the rectangle in set region automatically);
S32, " Z " font detective path contexture by self is carried out in world's rectangular coordinate system to the rectangle, until walking area
Domain covers whole rectangle plane.
The search coverage of different shapes that user sets uniformly is converted into rectangle in the present invention, so as to simplify problem
Complexity, improves the efficiency of autonomous path planning.
It is preferred that, the movement locus of the calculating spray body described in step S4 comprises the following steps:
S41, sets up robot coordinate system (spraying needs robot coordinate system and tool coordinates system):It is several with robot body
What center is origin, and using robot front as robot coordinate system's Y-axis positive direction, robot is used as using robot front-right
Coordinate system X-axis positive direction;
S42, obtains relative position of the land mine in the overlay area of detection system (such as mine detection radar);
S43, according to the fixed position relation between detection system and car body, land mine is obtained in machine by coordinate translation conversion
Coordinate in people's coordinate system;
S44, sets up Spray painting tool polar coordinate system:Using the pivot angle summit of oscillating arm mechanisms as origin, with robot coordinate system Y
Axle is as Spray painting tool polar coordinate system X-axis, and polar coordinate system plane is parallel with robot coordinate system's plane;
S45, according to the fixed position relation between oscillating arm mechanisms and car body, obtains Spray painting tool polar coordinate system and robot
The transforming relationship matrix of coordinate system, and then obtain coordinate (ρ, θ) of the land mine in Spray painting tool polar coordinate system;
S46, the coordinate of described land mine in Spray painting tool polar coordinate system is the pivot angle θ and swing arm needs of oscillating arm mechanisms
The length ρ of stretching.
By using the above method, then in spraying, it is not necessary to world cartesian coordinates system, it is not required that location navigation system
System, just knows that land mine can just realize that precise positioning is coated in the position of radar-covered area, spray efficiency is higher.
It is furthermore preferred that also including:The position skew exported according to inertial navigation and odometer and attitude angle offset information, to step
Position rectangular co-ordinate and attitude angle of the robot body obtained in S23 in world's rectangular coordinate system carry out data fusion and repaiied
Just, the position rectangular co-ordinate and attitude angle of more accurately robot body are obtained;Specifically include following steps:
S231, exports 3-axis acceleration and three shaft angle acceleration, to described 3-axis acceleration and three shaft angles using inertial navigation
Acceleration is integrated to obtain three axial velocities and three axis angular rates;
Three described axial velocities are integrated by S232, obtain the position skew of three direction of principal axis;Z-axis angular speed is carried out
Integration, obtains posture angular variation;Position on position skew on odometer output posture angular direction, described posture angular direction is inclined
Move the linear position skew for being decomposed into xy axle both directions in world's rectangular coordinate system;
S233, the robot that the rectangular co-ordinate of the robot body that gps data is calculated is calculated with inertial navigation, odometer
The position skew of body is merged (so as to export the position of more accurate robot body using Kalman filter
Rectangular co-ordinate data);The attitude angle for the robot body that the attitude angle of the GPS robot bodies calculated and inertial navigation are calculated
Skew is merged (so as to the more accurate attitude angle data of output device human body) using Kalman filter.
Compared with prior art, the present invention carries mechanical arm detection system and paint finishing by using mobile chassis system
Intelligence, accurate, efficient, lossless detection are carried out to minefield thunder point and is indicated, the alternative artificial detection operations of high intensity high-risk, from
And provide safe and reliable ensure for the later stage removal of mines;In addition, the present invention detects land mine using non-destructive testing technology, security is good,
Robot or operating personnel are not injured;And the present invention detecting a mine robot carry out land mine mark, the life-span compared with
It is long.Further it is proposed that mine-detecting technique, automaticity is high, since detection to finding, mark land mine, need not
Personnel's Attended Operation, personnel only need to the working condition of supervisory-controlled robot a long way off, can improve operating efficiency, can subtract again
The work load of light deminer.In addition, in the present invention, described manipulator motion system using metal machinery arm with it is nonmetallic
The combination arm of arm, one end of described nonmetallic arm connects detection system, other end connection metal machinery arm;Described metal machine
Tool arm uses doublejointed mechanical arm, multi-joint mechanical arm or frame structure, so as to realize more flexible control detecting devices,
Effectively expand the investigative range and detection area of mine detection vehicle, so as to further increase detecting a mine efficiency;Mechanical arm described in simultaneously
Kinematic system can greatly improve the spy of detection system (such as GPR) using the combination arm of metal machinery arm and nonmetallic arm
Survey precision, reduction interference;In addition, after detection system detects land mine, control system can be returned with control machinery arm kinematic system
Receive and concede space to spray body, then spray body stretches out and swing, reach shower nozzle and sprayed above land mine
Indicate, so as to effectively reduce the volume of detecting a mine device, and realization is more flexible, efficient, accurately control paint finishing
Carry out spraying sign;In addition, described metal machinery arm is using doublejointed mechanical arm, multi-joint mechanical arm or frame-type machinery
Arm, so as to play preferable dust-proof effect, substantially increases the life-span of detecting a mine mechanical device.In addition, in the present invention, it is described
Mobile platform system use multi-track mobile platform so that the present invention detecting a mine mechanical device there is more preferable obstacle detouring energy
Power, greatly expands the application scenario of the detecting a mine mechanical device of the present invention, goes for various environment and landform;Particularly institute
The mobile platform system stated includes:The first driving wheel of both sides is respectively arranged on, two, and two first masters per side are provided with per side
Driving wheel is connected by crawler belt;Also include four the second driving wheels (coaxially connected) connected one to one with the first driving wheel, institute
The second driving wheel stated is connected by crawler belt with driven pulley, so that mobile platform system has more preferable support performance and is
Whole device provides stronger power so that detecting a mine mechanical device of the invention has more preferable obstacle climbing ability.By using this
The detecting a mine method of invention, especially treats search coverage and carries out " Z " font detective path contexture by self and control car body along planning
Path is advanced, so as to realize the detecting a mine of blanket type, is prevented from omitting, is improved the accuracy of detection.In addition, in the present invention,
Described telescoping mechanism includes:A motors, rack and nozzle arm, the A motors are provided with A gears, the connection of A wheel and racks, tooth
Bar is connected with nozzle arm, nozzle arm connection shower nozzle, is not only simple in structure, with low cost, and can flexibly, accurately control spray
Apply the stretching and contraction of mechanism;Described oscillating arm mechanisms include:B motors and arc-shaped rack, the B motors are provided with B gears, B
Gear is connected with arc-shaped rack, and arc-shaped rack is connected with nozzle arm, is not only simple in structure, with low cost, and can flexibly, it is accurate
The swing of true control spray body.Finally, the present invention uses and is merged the data of GPS, inertial navigation and odometer, so that
Higher precision, the pose data of higher Refresh Data rate can be obtained.
Brief description of the drawings
Fig. 1 is the overall structure diagram of the detecting a mine robot system of the present invention;
Fig. 2 is the structural representation of mobile chassis system;
Fig. 3 is the structural representation of double tracked mobile platforms;
Fig. 4 is the structural representation of wheel moving platform;
Fig. 5 is a kind of structural representation of multi-track mobile platform;
Fig. 6 is another structural representation of multi-track mobile platform;
Fig. 7 is the structural representation of mechanical arm detection system;
Fig. 8 is the structural representation of paint finishing;
Fig. 9 is the schematic diagram that manipulator motion system is withdrawn;
Figure 10 is the front view that paint finishing is stretched out;
Figure 11 is the top view that paint finishing stretches out swing arm centre position;
Figure 12 is the top view that paint finishing stretches out swing arm front and back position;
Figure 13 is a kind of schematic diagram of implementation of spray body;
Figure 14 is the schematic diagram of another implementation of spray body;
Figure 15 uses the schematic diagram of doublejointed mechanical arm for metal machinery arm in manipulator motion system;
Figure 16 is multi-joint mechanical arm schematic diagram;
Figure 17 is connecting-rod mechanism type mechanical arm schematic diagram;
Figure 18 is metal framework mechanical arm schematic diagram;
Figure 19 connects block diagram for the circuit of the present invention;
Figure 20 is to treat the schematic diagram that search coverage carries out " Z " font detective path contexture by self;
Figure 21 is the relation schematic diagram of robot coordinate system and tool coordinates system;
Figure 22 is the schematic diagram for the attitude angle being converted into course angle in world cartesian coordinates system.
Reference:1- tele-control systems, 2- mobile chassis systems, 3- mechanical arm detection systems, 4- paint finishings, 5-
Mobile platform system, 6- dynamical systems, 7- control systems, 8- manipulator motion systems, 9- detection systems, 10- telescoping mechanisms,
11- oscillating arm mechanisms, 12- spray bodies, 13- air compressor machines, 14- shower nozzles, 15- paint spray pumps, 16- paint cans, 18- two-way electromagnetic valves,
19- three-way magnetic valves, 20- water pumps, 21- liquid tanks, the driving wheels of 22- second, 23- driven pulleys, 24- nozzle arms, 25- avoidance moulds
Block, 26- industrial cameras, 27- odometers, 28- inertial navigations, 29-GPS global positioning systems, 30- display screens, 31- man-machine interaction moulds
Block, 32- remote control modules, 33- positioning navigation modules, 34- intelligent control modules, 35- remote communication modules.
The present invention is further illustrated with reference to the accompanying drawings and detailed description.
Embodiment
Embodiments of the invention 1:A kind of autonomous type detecting a mine robot system, as shown in figure 1, including car body, the car body
Mobile chassis system 2, mechanical arm detection system 3 and paint finishing 4 are provided with, mobile chassis system 2 is detected with mechanical arm respectively
System 3 is connected with paint finishing 4;As shown in Fig. 2 described mobile chassis system 2 includes mobile platform system 5, dynamical system 6
With control system 7, as shown in Fig. 7, Fig. 9, mechanical arm detection system 3 includes manipulator motion system 8 and detection system 9, wherein,
Dynamical system 6 is installed on the rear end of mobile platform system 5, provides power source for whole device and does with reuse;7 points of control system
Lian Jie not dynamical system 6, tool arm kinematic system 8 and detection system 9;Detection system 9 is located at the front end of manipulator motion system 8.
As shown in Fig. 8, Figure 10~Figure 12, described paint finishing 4 includes telescoping mechanism 10, oscillating arm mechanisms 11 and spray body 12, stretches
Contracting mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle 14.Described is flexible
Mechanism 10 includes:A motors, rack and nozzle arm 24, the A motors are provided with A gears, the connection of A wheel and racks, rack and spray
Head arm 24 is connected, the connection shower nozzle 14 of nozzle arm 24.Described oscillating arm mechanisms 11 include:B motors and arc-shaped rack, the B motors
B gears are provided with, B gears are connected with arc-shaped rack, and arc-shaped rack is connected with nozzle arm.As shown in figure 13, described flush coater
Structure 12 also includes:Air compressor machine 13 and paint spray pump 15, described air compressor machine 13 are connected with paint spray pump 15, paint spray pump 15 and shower nozzle 14
Connection.Described spray body 12 also includes:Two-way electromagnetic valve 18 and/or three-way magnetic valve 19, described two-way electromagnetic valve 18
It is connected respectively with air compressor machine 13 and shower nozzle 14, three-way magnetic valve 19 is connected with air compressor machine 13 and shower nozzle 14 respectively.Described machinery
Arm kinematic system 8 is using the combination arm of metal machinery arm and nonmetallic arm, and one end of described nonmetallic arm connects detection system
9, other end connection metal machinery arm;Described metal machinery arm uses multi-joint mechanical arm (as shown in figure 16).Such as Figure 19 institutes
Show, in addition to intelligent control module 34 and positioning navigation module 33 in mobile chassis system 2,34 points of intelligent control module
Do not electrically connected with positioning navigation module 33 and control system 7.Also include tele-control system 1 and in mobile chassis system 2
Remote communication module 35, described remote communication module 35 is connected with tele-control system 1 and intelligent control module 34 respectively.
Described mobile platform system 5 uses multi-track mobile platform.Described mobile platform system 5 includes:It is respectively arranged on both sides
First driving wheel, two are provided with per side, and two the first driving wheels of every side are connected by crawler belt;Also include with least one the
Second driving wheel 22 of one driving wheel connection, the second described driving wheel 22 is connected (such as schemed by crawler belt with driven pulley 23
5th, shown in Fig. 6, it is specifically as follows:Described mobile platform system 5 includes:The first driving wheel of both sides is respectively arranged on, is set per side
There are two, and two the first driving wheels of every side are connected by crawler belt;Also connected one to one including four with the first driving wheel
The second driving wheel 22 (coaxially connected), the second described driving wheel 22 is connected by crawler belt with driven pulley 23.Or it is described
Mobile platform system 5 includes:The first driving wheel of both sides is respectively arranged on, two, and two first actives per side are provided with per side
Wheel is connected by crawler belt;Also include the second driving wheel 22 being connected with one of them first driving wheel of every side, the second described master
Driving wheel 22 is connected by crawler belt with driven pulley 23;Two described the second driving wheels 22 are located at same one end).Also include being located at car body
The avoidance module 25 of rear and front end, described avoidance module 25 is connected with intelligent control module 34.Described positioning navigation module
33 include:Odometer 27, inertial navigation 28 and GPS geo-location system 29, described odometer 27, inertial navigation 28 and GPS global locations
System 29 is connected with intelligent control module 34 respectively.Also include:Industrial camera 26, described industrial camera 26 leads to long-range respectively
News module 35 and intelligent control module 34 are connected.Described industrial camera 26 includes at least 3, is respectively arranged on manipulator motion system
System 8 on and car body rear and front end.Described intelligent control module 34 uses one or more industrial computer.Described avoidance module
25 are made up of two single-line type laser radars.Described tele-control system 1 includes display screen 30, human-computer interaction module 31 and remote
Process control module 32, described remote control module 32 respectively with display screen 30, human-computer interaction module 31 and remote communication module
35 connections.Described tele-control system 1 can use backpack suitcase remote control.
Embodiment 2:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Also include the intelligent control module 34 in mobile chassis system 2
With positioning navigation module 33, intelligent control module 34 is electrically connected with positioning navigation module 33 and control system 7 respectively.Also include remote
Process control system 1 and the remote communication module 35 in mobile chassis system 2, described remote communication module 35 respectively with far
Process control system 1 and intelligent control module 34 are connected.As shown in figure 3, described mobile platform system 5 can be using double crawler belt movements
Platform.Described manipulator motion system 8 using metal machinery arm and nonmetallic arm combination arm, the one of described nonmetallic arm
End connection detection system 9, other end connection metal machinery arm;Described metal machinery arm uses doublejointed mechanical arm (such as Figure 15
It is shown).
Embodiment 3:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described manipulator motion system 8 is using metal machinery arm and non-gold
Belong to the combination arm of arm, one end of described nonmetallic arm connects detection system 9, other end connection metal machinery arm;Described gold
Belong to mechanical arm and use doublejointed mechanical arm or multi-joint mechanical arm.As shown in figure 4, described mobile platform system 5 can be using wheel
Formula mobile platform (including common four-wheel and omni-directional wheel).Described manipulator motion system 8 using metal machinery arm with it is nonmetallic
The combination arm of arm, one end of described nonmetallic arm connects detection system 9, other end connection metal machinery arm;Described metal
Mechanical arm uses linkage (as shown in figure 17).
Embodiment 4:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described paint finishing 4 includes telescoping mechanism 10, the and of oscillating arm mechanisms 11
Spray body 12, telescoping mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle
14.Described spray body 12 also includes:Air compressor machine 13 and paint spray pump 15, described air compressor machine 13 are connected with paint spray pump 15,
Paint spray pump 15 is connected with shower nozzle 14.Described spray body 12 also includes:Two-way electromagnetic valve 18 and/or three-way magnetic valve 19, institute
The two-way electromagnetic valve 18 stated is connected with air compressor machine 13 and shower nozzle 14 respectively, three-way magnetic valve 19 respectively with air compressor machine 13 and shower nozzle 14
Connection.Described manipulator motion system 8 using metal machinery arm and nonmetallic arm combination arm, the one of described nonmetallic arm
End connection detection system 9, other end connection metal machinery arm;Described metal machinery arm uses metal framework mechanical arm (such as
Shown in Figure 18).
Embodiment 5:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described paint finishing 4 includes telescoping mechanism 10, the and of oscillating arm mechanisms 11
Spray body 12, telescoping mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle
14.Described oscillating arm mechanisms 11 include:B motors and arc-shaped rack, the B motors are provided with B gears, B gears and arc-shaped rack
Connection, arc-shaped rack is connected with nozzle arm.
Embodiment 6:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described paint finishing 4 includes telescoping mechanism 10, the and of oscillating arm mechanisms 11
Spray body 12, telescoping mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle
14.Described telescoping mechanism 10 includes:A motors, rack and nozzle arm 24, the A motors are provided with A gears, A wheel and racks
Connection, rack is connected with nozzle arm 24, the connection shower nozzle 14 of nozzle arm 24.
Embodiment 7:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described paint finishing 4 includes telescoping mechanism 10, the and of oscillating arm mechanisms 11
Spray body 12, telescoping mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle
14.Described telescoping mechanism 10 can be realized using existing mode, such as:Electric pushrod (cost is high).Described oscillating arm mechanisms
11 can be realized using existing mode, such as:Electric pushrod (cost is high).As shown in figure 14, described spray body 12 is also wrapped
Include:Water pump 20 and liquid tank 21, described water pump 20 are connected with liquid tank 21 and shower nozzle 14 respectively.Or described spray body
12 can also be realized using in the following manner:Including air compressor machine 13 and paint can 16, paint can 16 respectively with air compressor machine 13 and shower nozzle
14 connections.
Embodiment 8:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.
Embodiment 9:A kind of autonomous type detecting a mine robot system, including car body, the car body are provided with mobile chassis system
2nd, mechanical arm detection system 3 and paint finishing 4, mobile chassis system 2 respectively with mechanical arm detection system 3 and the phase of paint finishing 4
Even;Described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7, mechanical arm detection system 3
Including manipulator motion system 8 and detection system 9, wherein, dynamical system 6 is installed on the rear end of mobile platform system 5, is whole
Device provides power source and done with reuse;Control system 7 connects dynamical system 6, tool arm kinematic system 8 and detection system 9 respectively;
Detection system 9 is located at the front end of manipulator motion system 8.Described paint finishing 4 includes telescoping mechanism 10, the and of oscillating arm mechanisms 11
Spray body 12, telescoping mechanism 10 and oscillating arm mechanisms 11 are connected with spray body 12;Described spray body 12 includes shower nozzle
14.Described telescoping mechanism 10 includes:A motors, rack and nozzle arm 24, the A motors are provided with A gears, A wheel and racks
Connection, rack is connected with nozzle arm 24, the connection shower nozzle 14 of nozzle arm 24.Described oscillating arm mechanisms 11 include:B motors and arc-shaped gear
Bar, the B motors are provided with B gears, and B gears are connected with arc-shaped rack, and arc-shaped rack is connected with nozzle arm.Described flush coater
Structure 12 also includes:Air compressor machine 13 and paint spray pump 15, described air compressor machine 13 are connected with paint spray pump 15, paint spray pump 15 and shower nozzle 14
Connection.Described spray body 12 also includes:Two-way electromagnetic valve 18 and/or three-way magnetic valve 19, described two-way electromagnetic valve 18
It is connected respectively with air compressor machine 13 and shower nozzle 14, three-way magnetic valve 19 is connected with air compressor machine 13 and shower nozzle 14 respectively.Described machinery
Arm kinematic system 8 is using the combination arm of metal machinery arm and nonmetallic arm, and one end of described nonmetallic arm connects detection system
9, other end connection metal machinery arm;Described metal machinery arm uses doublejointed mechanical arm, multi-joint mechanical arm or frame-type
Mechanical arm.Also include intelligent control module 34 and positioning navigation module 33 in mobile chassis system 2, intelligent control module
34 electrically connect with positioning navigation module 33 and control system 7 respectively.Also include tele-control system 1 and located at mobile chassis system
Remote communication module 35 on 2, described remote communication module 35 connects with tele-control system 1 and intelligent control module 34 respectively
Connect.
The method detected a mine using system described in 1~embodiment of embodiment 9, is comprised the following steps:
S1, control car body is moved to the position that face is specified;
S2, calculates coordinate of the key point in region to be detected in world's rectangular coordinate system, and set search coverage;
S3, treats search coverage and carries out " Z " font detective path contexture by self;
S4, control car body is detected a mine along path planning traveling;After mechanical arm detection system detects land mine, car body stops
Under, detecting a mine work is interrupted, mine location is recorded, and calculate the movement locus of spray body;Manipulator motion system is withdrawn, and is controlled
Paint finishing processed carries out spraying sign to land mine point;
S5, after being indicated to land mine point, car body continues detection operations according to path planning until complete area to be detected of detection
Domain, or to land mine point indicate after, car body return origin, after removing mines continue plan detection.
Coordinate of the key point in the calculating region to be detected described in step S2 in world's rectangular coordinate system includes following
Step:
S21, chooses three points (point at general chosen area edge) not on the same line in region to be detected, point
(can be by GPS) obtain the latitude and longitude coordinates of these three points and the latitude and longitude coordinates and course angle (longitude and latitude of robot body
Coordinate is to be shown to operator to see, or operator assigns instruction and used);
S22, sets up world cartesian coordinates system (robot uses world cartesian coordinates system when moving):With one of them
Point be origin, two other point in a point and origin between straight line as world cartesian coordinates system X-axis;In ground level
The straight line of middle vertical X axis and process origin is used as the Y-axis of world cartesian coordinates system;Vertical ground level and upward vector is used as generation
The Z axis of boundary's rectangular coordinate system;
S23, by coordinate system change coordinate of the key point for obtaining region to be detected in world's rectangular coordinate system and
Position rectangular co-ordinate and attitude angle of the robot body in world's rectangular coordinate system.
In step S23, course angle can be converted into the attitude angle in world cartesian coordinates system in the following manner:
As shown in figure 22, robot actual heading angle is the positive north, and 0 ° of true course angle, world cartesian coordinates system X-axis is pointed to
N60E, 60 ° of true course angle, Y-axis points to N30W, and 330 ° of true course angle is not difficult to find out, robot pointing direction and the world from figure
The angle of rectangular coordinate system X-axis is 60 °, so attitude angle of the robot in world's rectangular coordinate system is exactly 60 °.
The true course angle of hypothetical world rectangular coordinate system X-axis is a °, and the true course angle of robot is r °, then robot exists
Attitude angle in world cartesian coordinates system can be calculated by a-r.
Step S3 comprises the following steps:
S31, by region to be detected in world's rectangular coordinate system the corresponding rectangle that is set to (if be manually set not advise
Then polygon, then robot be filled with including the rectangle in set region automatically);
S32, " Z " font detective path contexture by self is carried out in world's rectangular coordinate system to the rectangle, until walking area
Domain covers whole rectangle plane.
The movement locus of calculating spray body described in step S4 comprises the following steps, as shown in figure 21:
S41, sets up robot coordinate system (spraying needs robot coordinate system and tool coordinates system):It is several with robot body
What center is origin, and using robot front as robot coordinate system's Y-axis positive direction, robot is used as using robot front-right
Coordinate system X-axis positive direction;
S42, obtains relative position of the land mine in the overlay area of detection system (9) (such as mine detection radar);
S43, according to fixed position relation of the detection system (9) between car body, obtains land mine by coordinate translation conversion and exists
Coordinate in robot coordinate system;
S44, sets up Spray painting tool polar coordinate system:Using the pivot angle summit of oscillating arm mechanisms as origin, with robot coordinate system Y
Axle is as Spray painting tool polar coordinate system X-axis, and polar coordinate system plane is parallel with robot coordinate system's plane;
S45, according to the fixed position relation between oscillating arm mechanisms and car body, obtains Spray painting tool polar coordinate system and robot
The transforming relationship matrix of coordinate system, and then obtain coordinate (ρ, θ) of the land mine in Spray painting tool polar coordinate system;
S46, the coordinate of described land mine in Spray painting tool polar coordinate system is the pivot angle θ and swing arm needs of oscillating arm mechanisms
The length ρ of stretching.
During spraying, first, the motor of control oscillating arm mechanisms is rotated so that pendulum arm angle is equal to land mine polar angle, so
The motor of control telescoping mechanism is rotated afterwards, stretches out shower nozzle so that pendulum arm length is equal to land mine polar coordinate vectors length, you can so that
Shower nozzle is reached directly over land mine, is then opened electromagnetic valve and is sprayed (as painted).
Optionally, in addition to:The position skew exported according to inertial navigation and odometer and attitude angle offset information, to step
Position rectangular co-ordinate and attitude angle of the robot body obtained in S23 in world's rectangular coordinate system carry out data fusion and repaiied
Just, the position rectangular co-ordinate and attitude angle of more accurately robot body are obtained;Specifically include following steps:
S231, exports 3-axis acceleration and three shaft angle acceleration, to described 3-axis acceleration and three shaft angles using inertial navigation
Acceleration is integrated to obtain three axial velocities and three axis angular rates;
Three described axial velocities are integrated by S232, obtain the position skew of three direction of principal axis;Z-axis angular speed is carried out
Integration, obtains posture angular variation;Position on position skew on odometer output posture angular direction, described posture angular direction is inclined
Move the linear position skew for being decomposed into xy axle both directions in world's rectangular coordinate system;
S233, the robot that the rectangular co-ordinate of the robot body that gps data is calculated is calculated with inertial navigation, odometer
The position skew of body is merged (so as to export the position of more accurate robot body using Kalman filter
Rectangular co-ordinate data);The attitude angle for the robot body that the attitude angle of the GPS robot bodies calculated and inertial navigation are calculated
Skew is merged (so as to the more accurate attitude angle data of output device human body) using Kalman filter.
Pose includes position and posture, and position is the coordinate that robot is moved in the plane, because robot is only on ground
Motion, so posture only needs to course angle.
A kind of operation principle of embodiment of the present invention:
The detecting a mine robot system of the present invention, by intelligent control module 34 to mobile chassis system 2, mechanical arm detection system
3 and paint finishing 4 be controlled, including the mobile route of mobile platform system 5, speed, direction, manipulator motion system 8
Extension is with reclaiming, the flexible of spray body 12, swing arm, spraying sign etc..
Specifically, the present invention passes through remote communication module 35 using the remote control module 32 in tele-control system 1
Intelligent control module 34 on remote control car body closely directly operates intelligent control module 34, controls the movement of car body
Chassis system 2 is moved to the position that face is specified;The key point in region to be detected is calculated in world's rectangular coordinate system using GPS
Coordinate, and (using tele-control system 1) setting search coverage;Intelligent control module 34 is according to positioning navigation module 33
The data of (such as odometer 27, inertial navigation 28, GPS geo-location system 29) treat search coverage and carry out detective path contexture by self;
Control car body is advanced along path planning, while being transferred back to the video information that industrial camera 26 is gathered by remote communication module 35
Tele-control system 1 checks the ambient conditions around mine detection vehicle and the ring around detection system 9 for technical staff by display screen
Border situation.After mechanical arm detection system 3 detects land mine, data are transmitted to intelligent control module 34, intelligent control module 4 is remembered
The position of land mine is recorded, control system 7 is then sent information to, the control machinery arm kinematic system 8 of control system 7 is withdrawn to flush coater
Structure 12 concedes space, and then spray body 12 stretches out and swing, reaches shower nozzle 14 and spraying sign is carried out above land mine;
After being indicated to land mine point, car body continues detection operations according to path planning until a detection completely region to be detected, or over the ground
After thunder point sign, car body returns to origin, continues to plan detection after removing mines;So as to realize that more efficient, high intelligence is high precisely
Being removed mines for the later stage for high reliability provides safe and reliable ensure.During detecting a mine, if avoidance module 25 finds barrier,
Then robot system is stopped and attempts to bypass, as obstacle can not be bypassed, then switches to Remote pattern manual by operating personnel
(described human-computer interaction module 31 may include the industrial handle of multiple three axles, button, knob, instruction to remote control human-computer interaction module 31
Lamp etc., user can may be used also by human-computer interaction module to the intelligent control module transmission control information in mobile chassis system
Principal states, error message to obtain robot system by indicator lamp etc.) cut-through.
In particular, described mobile chassis system 2 includes mobile platform system 5, dynamical system 6 and control system 7.
Dynamical system 6 is installed on the rear end of mobile platform system 5, provides power source for whole system and does with reusing, whole to balance
The weight of individual detecting a mine mechanical device;Control system 7 is installed on the top of mobile platform system 5, the whole intelligent detecting a mine device of control
Path planning, travel speed, working condition etc.:Control system 7 control dynamical system 6 drive mobile platform system 5 (can be
Wheel moving platform, double tracked mobile platforms or multi-track mobile platform etc.) car body is moved to the position that face is specified.
Mechanical arm detection system 3 includes manipulator motion system 8 and detection system 9, and described detection system 9 is located at machinery
The front end of arm kinematic system 8.Control system 7 controls car body to be advanced along path planning and control machinery arm kinematic system 8 drives detection
System 9 carries out mine detection.During detection, the mechanical arm extension (driving nonmetallic arm and detection system) of manipulator motion system 8,
Detection system 9 is placed in search coverage by mechanical arm under the coordination of control system 7, and detection terminates rear mechanical arm and withdrawn backward, band
Dynamic detection system 9 is returned on mobile platform system 5, completes detection.In the present invention, described manipulator motion system 8 can be used
Motor, decelerator, chain, rack pinion combination, so as to be moved back and forth before and after driving mobile support arm, realize that mechanical arm is visited
The recovery of examining system 3 and detection operations.
Paint finishing 4 includes telescoping mechanism 10, oscillating arm mechanisms 11 and spray body 12.When mechanical arm detection system 3 is detected
To after land mine, detection system is withdrawn, and spray body 12 is driven by telescoping mechanism 10 and oscillating arm mechanisms 11, is run to above land mine,
Then spraying sign is carried out to land mine and neighboring area by shower nozzle 14.In the present invention, described telescoping mechanism 10 includes:A electricity
Machine, rack and nozzle arm 24, the A motors are provided with A gears, and A wheel and racks connection, rack is connected with nozzle arm 24, sprayed
Head arm 24 connects shower nozzle 14.A motor-driven gears rackwork drives nozzle arm 24, and is run along guide rail, so as to reach shower nozzle
The effect being moved forward or rearward.In the present invention, described oscillating arm mechanisms 11 include:B motors and arc-shaped rack, the B motors
B gears are provided with, B gears are connected with arc-shaped rack, and arc-shaped rack is connected with nozzle arm.Using B motor-driven gears and arc
Rack realizes the swing of nozzle arm 24, so as to reach the effect of the sector region of paint finishing 4 spraying.It is described in the present invention
Spray body 12 also include:Air compressor machine 13 and paint spray pump 15, described air compressor machine 13 are connected with paint spray pump 15, paint spray pump 15
It is connected with shower nozzle 14.Described spray body 12 also includes:Two-way electromagnetic valve 18 and/or three-way magnetic valve 19, described two-way
Magnetic valve 18 is connected with air compressor machine 13 and shower nozzle 14 respectively, and three-way magnetic valve 19 is connected with air compressor machine 13 and shower nozzle 14 respectively.Or
Spray body 12 described in person also includes:Water pump 20 and liquid tank 21, described water pump 20 respectively with liquid tank 21 and shower nozzle 14
Connection;Or described spray body 12 also includes:Air compressor machine 13 and paint can 16, described air compressor machine 13 and paint can 16
Connection, paint can 16 is connected with shower nozzle 14.So as to suction out paint, colored liquid (such as red fountain pen ink) or powder conduct
Spray finishing material, progress land mine sign is sprayed by shower nozzle 14.
Treat the operation principle that search coverage carries out " Z " font detective path contexture by self:
By region to be detected in robot coordinate system the corresponding rectangle that is set to (if be manually set to be irregular polygon
Shape, then robot be filled with including the rectangle in set region automatically).
As shown in figure 20, first using some summit of rectangle as starting point, in figure by taking A points as an example, along robot coordinate system's x-axis
Direction marches to B points;
A left side is turn 90 degrees, along the parallel y-axis direction personal position of one machine of traveling;
A left side is turn 90 degrees again, and AD sides are marched to along x-axis opposite direction;
The right side is turn 90 degrees, the personal position of a machine of advancing along the y-axis direction;
The right side is turn 90 degrees again, and BC sides are marched to along X-direction;
So circulation, until travel region covers whole rectangle plane.
Due to known to each side position coordinates in each summit of rectangle, it is only necessary to calculate many point coordinates in z zag trajectories, then machine
People sequentially goes to these coordinate positions and can completed.
Claims (11)
1. a kind of autonomous type detecting a mine robot system, it is characterised in that including car body, the car body is provided with mobile chassis system
(2), mechanical arm detection system (3) and paint finishing (4), mobile chassis system (2) respectively with mechanical arm detection system (3) and spray
Painting system (4) is connected;Described mobile chassis system (2) includes mobile platform system (5), dynamical system (6) and control system
(7), mechanical arm detection system (3) includes manipulator motion system (8) and detection system (9), wherein, dynamical system (6) is installed
Rear end in mobile platform system (5), provides power source for whole device and does with reuse;Control system (7) connects dynamic respectively
Force system (6), tool arm kinematic system (8) and detection system (9);Detection system (9) is located at before manipulator motion system (8)
End.
2. autonomous type detecting a mine robot system according to claim 1, it is characterised in that described paint finishing (4) bag
Include telescoping mechanism (10), oscillating arm mechanisms (11) and spray body (12), telescoping mechanism (10) and oscillating arm mechanisms (11) with spraying
Mechanism (12) is connected;Described spray body (12) includes shower nozzle (14).
3. autonomous type detecting a mine robot system according to claim 2, it is characterised in that described telescoping mechanism (10) bag
Include:A motors, rack and nozzle arm (24), the A motors are provided with A gears, the connection of A wheel and racks, rack and nozzle arm
(24) connect, nozzle arm (24) connection shower nozzle (14).
4. autonomous type detecting a mine robot system according to claim 2, it is characterised in that described oscillating arm mechanisms (11) bag
Include:B motors and arc-shaped rack, the B motors are provided with B gears, and B gears are connected with arc-shaped rack, arc-shaped rack and nozzle arm
Connection.
5. autonomous type detecting a mine robot system according to claim 2, it is characterised in that described spray body (12) is also
Including:Air compressor machine (13) and paint spray pump (15), described air compressor machine (13) are connected with paint spray pump (15), paint spray pump (15) and spray
Head (14) connection.
6. autonomous type detecting a mine robot system according to claim 1, it is characterised in that described manipulator motion system
(8) using the combination arm of metal machinery arm and nonmetallic arm, one end of described nonmetallic arm connects detection system (9), another
End connection metal machinery arm;Described metal machinery arm uses doublejointed mechanical arm, multi-joint mechanical arm or frame-type mechanical arm.
7. the method detected a mine using any one of claim 1~6 system, it is characterised in that comprise the following steps:
S1, control car body is moved to the position that face is specified;
S2, calculates coordinate of the key point in region to be detected in world's rectangular coordinate system, and set search coverage;
S3, treats search coverage and carries out " Z " font detective path contexture by self;
S4, control car body is detected a mine along path planning traveling;After mechanical arm detection system (3) detects land mine, car body stops
Under, detecting a mine work is interrupted, mine location is recorded, and calculate the movement locus of spray body;Manipulator motion system (8) is withdrawn,
And control paint finishing (4) to carry out spraying sign to land mine point;
S5, after being indicated to land mine point, car body continues detection operations until detecting complete region to be detected according to path planning, or
After person indicates to land mine point, car body returns to origin, continues to plan detection after removing mines.
8. detecting a mine method according to claim 7, it is characterised in that the pass in the calculating region to be detected described in step S2
Coordinate of the key point in world's rectangular coordinate system comprises the following steps:
S21, chooses three points not on the same line in region to be detected, and the latitude and longitude coordinates of these three points are obtained respectively
And the latitude and longitude coordinates and course angle of robot body;
S22, sets up world cartesian coordinates system:With one of point for origin, between the point and origin in two other point
Straight line as world cartesian coordinates system X-axis;The straight line of vertical X axis and process origin is used as world right angle to sit in ground level
Mark the Y-axis of system;Vertical ground level and upward vector as world cartesian coordinates system Z axis;
S23, coordinate and machine of the key point for obtaining region to be detected in world's rectangular coordinate system are changed by coordinate system
Position rectangular co-ordinate and attitude angle of the human body in world's rectangular coordinate system.
9. detecting a mine method according to claim 7, it is characterised in that step S3 comprises the following steps:
S31, is set to rectangle by region to be detected is corresponding in world's rectangular coordinate system;
S32, carries out " Z " font detective path contexture by self, until travel region is covered in world's rectangular coordinate system to the rectangle
Cover whole rectangle plane.
10. detecting a mine method according to claim 7, it is characterised in that the fortune of the calculating spray body described in step S4
Dynamic rail mark comprises the following steps:
S41, sets up robot coordinate system:Using robot body geometric center as origin, robot is used as using robot front
Coordinate system Y-axis positive direction, robot coordinate system's X-axis positive direction is used as using robot front-right;
S42, obtains relative position of the land mine in the overlay area of detection system (9);
S43, according to fixed position relation of the detection system (9) between car body, land mine is obtained in machine by coordinate translation conversion
Coordinate in people's coordinate system;
S44, sets up Spray painting tool polar coordinate system:Using the pivot angle summit of oscillating arm mechanisms as origin, made with robot coordinate system's Y-axis
For Spray painting tool polar coordinate system X-axis, polar coordinate system plane is parallel with robot coordinate system's plane;
S45, according to the fixed position relation between oscillating arm mechanisms and car body, obtains Spray painting tool polar coordinate system and robot coordinate
The transforming relationship matrix of system, and then obtain coordinate (ρ, θ) of the land mine in Spray painting tool polar coordinate system;
S46, the coordinate of described land mine in Spray painting tool polar coordinate system is that the pivot angle θ of oscillating arm mechanisms and swing arm need to stretch out
Length ρ.
11. detecting a mine method according to claim 8, it is characterised in that also include:The position exported according to inertial navigation and odometer
Skew and attitude angle offset information are put, to the position of the robot body that has been obtained in step S23 in world's rectangular coordinate system
Rectangular co-ordinate and attitude angle carry out data fusion amendment;Specifically include following steps:
S231,3-axis acceleration and three shaft angle acceleration are exported using inertial navigation, and described 3-axis acceleration and three shaft angles are accelerated
Degree is integrated to obtain three axial velocities and three axis angular rates;
Three described axial velocities are integrated by S232, obtain the position skew of three direction of principal axis;Z-axis angular speed is integrated,
Obtain posture angular variation;Position skew on position skew on odometer output posture angular direction, described posture angular direction exists
The linear position skew for being decomposed into xy axle both directions in world cartesian coordinates system;
S233, the robot body that the rectangular co-ordinate of the robot body that gps data is calculated is calculated with inertial navigation, odometer
Position skew merged using Kalman filter;The attitude angle of the GPS robot bodies calculated and inertial navigation are calculated
The posture angular variation of the robot body gone out is merged using Kalman filter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710595932.9A CN107328308A (en) | 2017-07-20 | 2017-07-20 | A kind of autonomous type detecting a mine robot system and detecting a mine method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710595932.9A CN107328308A (en) | 2017-07-20 | 2017-07-20 | A kind of autonomous type detecting a mine robot system and detecting a mine method |
Publications (1)
Publication Number | Publication Date |
---|---|
CN107328308A true CN107328308A (en) | 2017-11-07 |
Family
ID=60227621
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710595932.9A Pending CN107328308A (en) | 2017-07-20 | 2017-07-20 | A kind of autonomous type detecting a mine robot system and detecting a mine method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107328308A (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108345303A (en) * | 2018-01-24 | 2018-07-31 | 五邑大学 | A kind of novel electric vehicle charging service system based on guiding band and robot |
CN108469819A (en) * | 2018-03-19 | 2018-08-31 | 杭州晶智能科技有限公司 | A kind of z font return path planing methods of automatic dust absorption machine people |
CN110362090A (en) * | 2019-08-05 | 2019-10-22 | 北京深醒科技有限公司 | A kind of crusing robot control system |
CN113280687A (en) * | 2021-05-12 | 2021-08-20 | 广州卫富科技开发有限公司 | Method for realizing area imaging and positioning of thunder field target detection signal based on ground mobile robot |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5869967A (en) * | 1995-01-27 | 1999-02-09 | Lobbe Xenex Gmbh & Co. | Device for the detection of objects, especially explosive objects, lying in the earth |
JP2003148899A (en) * | 2001-11-08 | 2003-05-21 | Kenzo Nonami | Mine detecting robot |
JP2007322033A (en) * | 2006-05-30 | 2007-12-13 | Fuji Heavy Ind Ltd | Operating machine and control method of operating machine |
US7683821B1 (en) * | 2006-10-25 | 2010-03-23 | Niitek, Inc. | Sensor sweeper for detecting surface and subsurface objects |
CN106088544A (en) * | 2016-06-08 | 2016-11-09 | 以恒激光科技(北京)有限公司 | Building wall auto spray painting machine people |
CN107289819A (en) * | 2017-06-20 | 2017-10-24 | 上海圭目机器人有限公司 | A kind of autonomous intelligent detecting a mine mechanical device and detecting a mine method |
CN207300057U (en) * | 2017-07-20 | 2018-05-01 | 上海圭目机器人有限公司 | A kind of autonomous type detecting a mine robot system |
-
2017
- 2017-07-20 CN CN201710595932.9A patent/CN107328308A/en active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5869967A (en) * | 1995-01-27 | 1999-02-09 | Lobbe Xenex Gmbh & Co. | Device for the detection of objects, especially explosive objects, lying in the earth |
JP2003148899A (en) * | 2001-11-08 | 2003-05-21 | Kenzo Nonami | Mine detecting robot |
JP2007322033A (en) * | 2006-05-30 | 2007-12-13 | Fuji Heavy Ind Ltd | Operating machine and control method of operating machine |
US7683821B1 (en) * | 2006-10-25 | 2010-03-23 | Niitek, Inc. | Sensor sweeper for detecting surface and subsurface objects |
CN106088544A (en) * | 2016-06-08 | 2016-11-09 | 以恒激光科技(北京)有限公司 | Building wall auto spray painting machine people |
CN107289819A (en) * | 2017-06-20 | 2017-10-24 | 上海圭目机器人有限公司 | A kind of autonomous intelligent detecting a mine mechanical device and detecting a mine method |
CN207300057U (en) * | 2017-07-20 | 2018-05-01 | 上海圭目机器人有限公司 | A kind of autonomous type detecting a mine robot system |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108345303A (en) * | 2018-01-24 | 2018-07-31 | 五邑大学 | A kind of novel electric vehicle charging service system based on guiding band and robot |
CN108469819A (en) * | 2018-03-19 | 2018-08-31 | 杭州晶智能科技有限公司 | A kind of z font return path planing methods of automatic dust absorption machine people |
CN110362090A (en) * | 2019-08-05 | 2019-10-22 | 北京深醒科技有限公司 | A kind of crusing robot control system |
CN113280687A (en) * | 2021-05-12 | 2021-08-20 | 广州卫富科技开发有限公司 | Method for realizing area imaging and positioning of thunder field target detection signal based on ground mobile robot |
CN113280687B (en) * | 2021-05-12 | 2022-06-07 | 广州卫富科技开发有限公司 | Method for realizing area imaging and positioning of thunder field target detection signal based on ground mobile robot |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107328308A (en) | A kind of autonomous type detecting a mine robot system and detecting a mine method | |
Bajracharya et al. | Autonomy for mars rovers: Past, present, and future | |
Biesiadecki et al. | The mars exploration rover surface mobility flight software driving ambition | |
Wilcox | Robotic vehicles for planetary exploration | |
Cantelli et al. | UAV/UGV cooperation for surveying operations in humanitarian demining | |
CN109917786A (en) | A kind of robot tracking control and system operation method towards complex environment operation | |
CN109901580A (en) | A kind of unmanned plane cooperates with unmanned ground robot follows diameter obstacle avoidance system and its method | |
CN107368073A (en) | A kind of full ambient engine Multi-information acquisition intelligent detecting robot system | |
CN106313086A (en) | Remote control system and method for coal mine rescue robot | |
CN112461227B (en) | Wheel type chassis robot inspection intelligent autonomous navigation method | |
CN109579831A (en) | Mining boom-type roadheader visualization auxiliary guidance method and system | |
CN107608348A (en) | A kind of autonomous type graticule robot system and line-marking method | |
RU2658684C1 (en) | Multi-agent robotic technical system | |
CN108082415A (en) | A kind of underwater steel construction robot operated on surface | |
Muscato et al. | ROBOVOLC: a robot for volcano exploration result of first test campaign | |
CN113485346A (en) | Autonomous navigation method of mobile robot in nuclear accident complex environment | |
Shaffer et al. | Position estimator for underground mine equipment | |
Cantelli et al. | Autonomous cooperation between UAV and UGV to improve navigation and environmental monitoring in rough environments | |
Tunstel et al. | Mars exploration rover mobility and robotic arm operational performance | |
US20230384339A1 (en) | Measuring Distance Traversed or Speed | |
CN207300057U (en) | A kind of autonomous type detecting a mine robot system | |
CN107289819A (en) | A kind of autonomous intelligent detecting a mine mechanical device and detecting a mine method | |
CN207268586U (en) | A kind of autonomous type detecting a mine robot system | |
Kanellakis et al. | Towards autonomous aerial scouting using multi-rotors in subterranean tunnel navigation | |
Bajracharya et al. | Target tracking, approach, and camera handoff for automated instrument placement |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20171107 |
|
WD01 | Invention patent application deemed withdrawn after publication |