CN110736458B - Heading machine autonomous navigation system and method based on dead reckoning - Google Patents
Heading machine autonomous navigation system and method based on dead reckoning Download PDFInfo
- Publication number
- CN110736458B CN110736458B CN201911238056.XA CN201911238056A CN110736458B CN 110736458 B CN110736458 B CN 110736458B CN 201911238056 A CN201911238056 A CN 201911238056A CN 110736458 B CN110736458 B CN 110736458B
- Authority
- CN
- China
- Prior art keywords
- heading machine
- machine
- navigation
- uwb radio
- heading
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a dead reckoning-based boom-type roadheader autonomous navigation system and method, and belongs to the field of autonomous navigation of underground coal mine mobile equipment. The system comprises a reference UWB (ultra Wide band) radio distance measurement module group, a body UWB radio distance measurement module, a strapdown inertial navigation system, an omnidirectional odometer, a navigation computer and a far-end display and control device. Before the development machine advances, determining the initial position of the development machine by adopting a UWB radio pulse ranging method; when the heading machine runs forwards, the strapdown inertial navigation system measures the attitude angle and the course angle of the heading machine in real time, simultaneously measures the mileage increment of the heading machine by the omnidirectional odometer, and the navigation computer generates a running track according to a dead reckoning algorithm; when the heading machine runs to the preset working position, the position of the heading machine is calibrated by using the UWB radio pulse ranging method again. And the navigation computer sends the navigation information to the remote display and control system in real time, so that the autonomous navigation of the heading machine in the tunneling process is realized.
Description
Technical Field
The invention relates to a cantilever type heading machine in the field of underground coal mine mobile equipment, in particular to an autonomous navigation system and an autonomous navigation method for the heading machine.
Background
The unmanned and intelligent coal mining becomes a development hotspot in the coal mining field more and more. The cantilever type heading machine is one of core equipment for coal mining, autonomous positioning navigation is a key foundation for coal mine unmanned and intelligent, and accurate pose detection of the heading machine is a guarantee for coal mine roadway directional heading. In the process of tunneling a coal mine tunnel, once the tunneling direction deviates, the normal production of the whole mining area can be influenced, and in severe cases, geological disasters in the tunnel can be caused, so that underground safety accidents are caused, and the personal safety of all constructors on the fully-mechanized excavation surface is endangered. The directional tunneling means widely applied in coal mines at present is a laser pointing method, the method is to install a laser pointing instrument at a known position behind a tunneling roadway, and a miner visually observes the position of a laser point formed by the laser pointing instrument on the section of the roadway in front of the roadway to judge the tunneling direction. The detection mode relying on naked eyes of people has limited precision, cannot obtain the exact position and posture data of the heading machine, and places miners in dangerous heading faces.
In recent years, many scholars and scientific research institutions have studied the navigation problem of the heading machine. The existing heading machine navigation technologies at home and abroad comprise: 1. the pose detection method based on the total station utilizes the light reflection principle to calculate the position and the attitude angle of the fuselage in a roadway by detecting the spatial positions of a plurality of prisms installed on different positions of the fuselage; 2. the method comprises the steps that the position and the attitude of the heading machine are measured through a laser signal transmitted by a point laser or a fan-shaped laser transmitter received by a laser receiver or a laser target; 3. the method comprises the steps of placing a laser spot on a heading machine body or a roadway behind the heading machine, and sensing the relative position of the laser spot by using binocular vision so as to solve the position and the posture of the heading machine; the method comprises the following steps of (IGPS) (indoor Global Positioning System) technology, installing a laser receiver at a known position of a machine body of the heading machine, and realizing absolute Positioning of the heading machine by measuring a three-dimensional coordinate of the laser receiver under a transmitting station coordinate system; the method comprises the steps of obtaining the acceleration and the angular velocity of the heading machine by utilizing a three-axis accelerometer and a three-axis gyroscope through an inertia principle, and obtaining position and attitude information through time integration.
The method completes the detection of the position and the posture of the heading machine to a certain extent, but has certain limitations:
(1) under the severe environment that the dust concentration of the coal mine tunneling working flour is high, the vibration of a machine body is large, and the space is narrow, the measurement precision of a plurality of precision instruments can be greatly reduced.
(2) Except for the method using the inertial measurement technology, other measurement methods have poor dynamic measurement capability, and the advancing track and the posture of the heading machine cannot be obtained in real time. The position information obtained by using the inertial technology is dispersed violently along with time, and pure inertial navigation is not suitable for a slowly-moving carrier of a heading machine.
In view of the limitations of the above methods, in order to realize the autonomous navigation of the boom-type roadheader and dynamically acquire the position and attitude data of the roadheader, a foundation is laid for the intelligent directional tunneling of the roadheader. By analyzing the defects above and combining the process flow of the excavator, the invention provides the excavator autonomous navigation system and method combining a strapdown inertial navigation system, an omnidirectional odometer and a UWB (ultra Wide band) radio distance measurement module.
The invention content is as follows:
the invention aims to provide an autonomous navigation system and method of a cantilever type tunneling machine in a coal mine, which solve the problem that the position and posture information of the cantilever type tunneling machine cannot be acquired in real time under the severe environment of a tunneling working face, realize autonomous accurate directional tunneling of a coal mine tunnel and lay a foundation for unmanned and intelligent coal mining.
The technical scheme is as follows: the purpose of the invention is realized as follows: the autonomous navigation system comprises a reference UWB radio ranging module group, a body UWB radio ranging module, a strapdown inertial navigation system, an omnidirectional odometer, a navigation computer and a far-end display and navigation device. The strapdown inertial navigation system and the navigation computer are arranged in an explosion-proof electric control box of the machine body of the heading machine; the omnidirectional odometer is arranged on a chassis of the development machine and consists of an obstacle clearing shell, a tensioning device, a rotary table, a measuring wheel bracket, an odometer encoder, a rotary table and a measuring wheel; the UWB radio ranging module of the machine body is arranged in the center of the upper surface of the development machine; the reference UWB wireless positioning module group consists of 3 UWB wireless ranging modules, is arranged above a tunnel at the rear of the development machine, and the installation position of the module is determined by a coal mine surveying and mapping department and is used as a reference for navigation.
The heading machine autonomous navigation system based on dead reckoning is characterized in that: the strapdown inertial navigation system consists of a three-axis optical fiber gyroscope and a three-axis accelerometer, and can measure the angular velocity and the acceleration of three axes of the development machine under a machine body coordinate system in real time. Sending the angular speed and acceleration information to a navigation computer in an explosion-proof electric control box, and before the heading machine advances, obtaining an initial attitude angle and a course angle of the heading machine according to a strapdown inertial navigation initial alignment algorithm; and when the heading machine advances, the real-time attitude angle and heading of the heading machine can be obtained according to the attitude updating algorithm.
The heading machine autonomous navigation system based on dead reckoning is characterized in that: the omnidirectional odometer consists of an obstacle clearing shell, a tensioning device, a rotary table, a measuring wheel bracket, a mileage coder and a measuring wheel. The obstacle clearing shell is arranged at the center of a chassis of the tunneling machine, the arc edge of the lower part of the obstacle clearing shell is 3cm away from the ground, the main body of the obstacle clearing shell is cylindrical, and the bottom of the obstacle clearing shell is provided with a radian towards the outside so as to realize an obstacle clearing function; the tensioning device is arranged in the obstacle clearing shell and applies tensioning force to the measuring wheel, so that the measuring wheel is always in contact with the ground and has damping capacity; the turntable is connected below the tensioning device, so that the measuring wheel can realize 360-degree steering; the upper part of the measuring wheel bracket is connected with a rotating disc and a measuring wheel in a lower connection mode; and the mileage encoder is axially arranged on a shaft connected with the measuring wheel and the measuring wheel bracket, and is used for detecting the mileage increment of the heading machine in real time.
The heading machine autonomous navigation system based on dead reckoning is characterized in that: the reference UWB wireless positioning module group consists of 3 UWB wireless distance measuring modules, the module support is hung and installed at the top of a roadway behind the heading machine, and the position of the module support is determined by a coal mine surveying and mapping department and is used as a reference for navigation. The UWB radio module may transmit high frequency radio pulses on the order of pulses 3.4-10.6 Ghz. The UWB radio modules may receive radio signals from each other, thereby enabling two-way communication between the radio modules. A high-precision atomic clock is arranged in the UWB radio module, and can accurately record the time of sending the pulse signal and the time of receiving the pulse signal.
The heading machine autonomous navigation system based on dead reckoning is characterized in that: the navigation computer is arranged in an explosion-proof electric cabinet of the machine body of the heading machine, is in data communication with the strapdown inertial navigation system, the mileage encoder and the UWB radio distance measuring module of the machine body through an interface circuit, and sends the calculated navigation information to the far-end display and control system through a communication optical cable.
The autonomous navigation method of the heading machine autonomous navigation system based on dead reckoning comprises the following steps:
(1) before the heading machine performs heading work, a surveying and mapping department personnel accurately determines the installation position of the reference UWB radio ranging module group in the roadway according to the design requirement of the roadway, and the installation position is used as the reference of the autonomous navigation of the heading machine. And establishing a roadway plane coordinate system by taking the reference position as a coordinate origin, determining the coordinates of each module by measuring the installation position of the reference UWB radio ranging module on the module support, and inputting the coordinate information into a navigation computer in the explosion-proof electric control box to complete navigation initialization.
(2) After the heading machine is started, before the heading machine starts to run. The 3 UWB radio ranging modules sequentially transmit high-frequency radio pulse signals with different phases to the body UWB radio ranging module arranged on the body, and the body UWB radio module immediately reflects a pulse signal with a self-phase to the reference UWB radio module at the moment of receiving the pulse signal. Rubidium atomic clock pulse signals inside the UWB radio modules propagate twice between the two UWB radio ranging modules. And sending the time information to a navigation computer in the explosion-proof electric cabinet, converting the time information into distance information according to a radio ranging algorithm formula to obtain the distance relationship between every two of the 3 reference UWB radio modules and the body UWB radio modules, and calculating the initial coordinate of the heading machine in a roadway-to-plane coordinate system to serve as the starting point of navigation.
(3) Starting to carry out the step (2) at the same time, carrying out initial alignment by the strapdown inertial navigation system, sending the angle increment measured by the triaxial fiber gyroscope and the acceleration increment measured by the triaxial accelerometer to a navigation computer in an explosion-proof electric cabinet, and solving an attitude conversion matrix of a carrier coordinate system and a navigation coordinate system according to an initial alignment algorithm so as to solve the initial roll angle theta of the heading machineoAngle of pitch gammaoAngle of course
(4) And the navigation computer sends the initial position, the course and the attitude information of the development machine to a remote display and control system through a communication optical cable, and the position, the course and the attitude of the development machine are adjusted in time according to a preset track.
(5) The heading machine drives to a preset working position of the section of the roadway, the omnidirectional odometer moves along with the heading machine, a measuring wheel of the omnidirectional odometer is always in contact with the ground by virtue of a tensioning device, a mileage encoder is driven to rotate by the measuring wheel, a square wave pulse signal is sent to a navigation computer, and the distance increment in a micro time period can be obtained. Meanwhile, the strapdown inertial navigation system outputs angle increment information to the navigation computer in real time in the driving process of the heading machine, and the real-time rolling of the heading machine can be obtained through calculation according to an attitude updating algorithmAngle theta, pitch angle gamma and course angle
. The navigation computer can obtain the position coordinates of the heading machine in real time through a dead reckoning algorithm by utilizing the mileage increment information obtained by the omnidirectional odometer and the course angle obtained by the strapdown inertial navigation system, and generates the heading machine advancing track. And the navigation computer sends the navigation information of the heading machine to the remote-end display and control system in real time, and the heading machine is adjusted according to the preset design requirements of the roadway.
(6) And the ground control system adjusts the heading machine to a preset heading direction according to the attitude information of the heading machine when the heading machine runs to a preset working position. The heading machine begins to cut the section of the roadway, continuous impact is generated between a cutting head of the heading machine and coal rocks in the cutting process to enable the machine body to generate violent vibration, reaction force is generated by cutting to enable the machine body to sideslip and incline, and the position and the posture of the heading machine are changed seriously. And (3) repeating the step (2) to determine the position of the heading machine.
(7) And (5) after cutting is finished, retreating the tunneling machine for a certain distance to make room for supporting work of a newly tunneled roadway, and repeating the step (5) in the retreating process.
(8) And (5) after the support work is finished, starting a new tunneling cycle, and repeating the steps (2) - (7).
(9) And (3) when the heading machine exceeds the ranging range of the UWB radio ranging module, moving the module support forwards by a staff of a roadway surveying and mapping department, and repeating the step (1).
Compared with the prior art, the heading machine autonomous navigation system and method based on dead reckoning have obvious advantages and beneficial effects. By means of the technical scheme, the autonomous navigation system and the autonomous navigation method of the development machine based on dead reckoning, provided by the invention, can achieve considerable technical progress and practicability, have industrial wide utilization value and at least have the following advantages:
(1) the method adopted by the invention is combined with the actual process flow of the heading machine, and the navigation work is divided into four parts of positioning before driving, navigation during driving, adjustment of the cutting process and positioning after cutting, so that the cutting link with the largest interference in the heading process is skillfully avoided, and the driving track and the dynamic attitude of the heading machine are accurately measured.
(2) The invention utilizes the UWB radio ranging module group to measure the distance to determine the initial position of the development machine, and provides a reference for the navigation of the development machine. Compared with laser ranging, the UBW wireless ranging mode has stronger penetrating power in high-concentration dust in a coal mine tunnel, the effective ranging range is 100m, and the positioning precision is higher (within 2 cm).
(3) The invention adopts the combined navigation of the strapdown inertial navigation and the milemeter, and the strapdown inertial navigation and the milemeter do not need to interact with the external environment in the navigation process, thereby being particularly suitable for the severe environment under a coal mine. The strapdown inertial navigation provides two attitude angles of the heading machine in real time and also provides a course angle, and the precision of the strapdown inertial navigation can reach 0.01 degree.
(4) The invention adopts a UWB radio ranging module group, an inertial navigation system, an omnidirectional mileage totalization, a navigation host and the like to output the accurate attitude information, azimuth information and position information of the heading machine in real time. The method completely replaces the traditional mode of judging by naked eyes of miners, depends on digital navigation information, solves the problems of deviation, over-underexcavation and the like in the process of tunneling the roadway, improves the quality and the efficiency of the molding of the roadway, frees the mine from dangerous working environment, and lays a foundation for unmanned and intelligent tunneling working faces.
In conclusion, the autonomous navigation system of the heading machine, which is uniquely designed by the invention, aims to acquire the position and posture information of the cantilever type heading machine in real time under the severe environment of a heading working face so as to realize the directional heading of a coal mine tunnel and lay a foundation for the unmanned and intelligent coal mine. The invention combines the characteristics of the tunneling process flow and adopts different modes to detect the position and the posture of the tunneling machine in different process stages, thereby greatly improving the precision of the navigation system. The system is suitable for underground tunneling fields such as mine tunnel tunneling, highway tunnel tunneling, subway tunnel tunneling and the like, and not only has high economic value, but also has good social value. The method has the advantages and practical value, the independent positioning and orientation principle and the structural design are not published or practical and really belong to innovation, a great breakthrough is made in the technical field of roadway independent tunneling, good and practical effects are produced, and the method is a novel, improved and practical new design.
Drawings
In the figure:
FIG. 1 is a system diagram of the present invention
FIG. 2 is a top view of the system architecture of the present invention
FIG. 3 is a left side view of the system architecture of the present invention
FIG. 4 is a schematic diagram of UWB radio ranging module distribution
FIG. 5 is a schematic sectional view of an omnidirectional odometer
FIG. 6 is a schematic diagram of the autonomous navigation principle of the present invention
1: the heading machine 2: UWB radio ranging module group
3: reference UWB radio ranging module 4: reference UWB radio ranging module
5: reference UWB radio ranging module 6: UWB radio ranging module for body
7: the omnidirectional odometer 8: navigation computer
9: the strapdown inertial navigation system 10: three-axis accelerometer
11: three-axis optical fiber gyro 12: remote display and control system
13: UWB radio pulse signal 14: roadway edge
15: the explosion-proof electric cabinet 16: module support
17: the obstacle clearing housing 18: tension device
19: the rotating disc 20: measuring wheel support
21: the mileage encoder 22: measuring wheel
23: the support rod 24: crawler belt of development machine
25: heading machine travel track 26: at time t-1, the heading machine centroid position 27: at time t, the position of the center of mass of the development machine
Detailed Description
To further illustrate the technical means and effects of the present invention for achieving the predetermined objects, the following detailed description will be given to the embodiments, structures, features and effects of the dead reckoning-based heading machine autonomous positioning and orienting system and the method thereof according to the present invention with reference to the accompanying drawings and preferred embodiments.
The heading machine autonomous navigation system and method based on dead reckoning according to the preferred embodiment of the invention, as shown in fig. 1, is composed of a reference UWB radio ranging module group 2, a machine body UWB radio ranging module 6, an omnidirectional odometer 7, a navigation computer 8, a strapdown inertial navigation system 9, and a far-end display and control system 12. The reference UWB radio ranging module group 2 comprises three reference UWB radio ranging modules (3, 4 and 5) which are arranged behind the development machine 1; the Ultra Wide Band (UWB) wireless distance measuring device comprises a machine body UWB wireless distance measuring module 6, an omnidirectional odometer 7, a navigation computer 8 and a strapdown inertial navigation system 9, wherein the strapdown inertial navigation system comprises a triaxial accelerometer 10 and a triaxial fiber gyroscope 11 which are arranged on a heading machine body; the remote display and control system 12 is located in a ground control room. The reference UWB radio ranging modules (3, 4 and 5) can perform wireless communication and ranging with the body UWB radio ranging module 6; the body UWB wireless positioning module 6, the omnidirectional odometer 7, the strapdown inertial navigation system 9 and the far-end display and control system 12 can be connected with the navigation computer 8 through communication cables and carry out two-way communication.
As shown in fig. 2, in the autonomous navigation system of the heading machine based on dead reckoning, behind the heading machine 1, a reference UWB radio ranging module group 2 is provided, wherein three reference UWB radio ranging modules (3, 4, 5) are distributed in any triangle; the machine body UWB radio ranging module 6 is arranged at the core of the upper surface of the machine body of the development machine 1; the strapdown inertial navigation system 9 and the navigation computer 8 are arranged in an explosion-proof electric cabinet 15 of the heading machine 1.
As shown in fig. 3, the heading machine autonomous navigation system omnidirectional odometer 7 based on dead reckoning is installed on the chassis of the heading machine 1, located between two heading machine tracks 24 and right below the machine body UWB wireless ranging module 6, so that the measuring wheels 22 thereof are always kept in contact with the ground.
As shown in fig. 4, the reference UWB radio ranging module group 2 is composed of reference UWB radio ranging modules 3, 4, and 5, which can communicate with the body UWB wireless positioning module 6 and perform two-to-two ranging. The staff of the surveying and mapping department accurately determines the hanging position of the module support 16 as a navigation reference, and the downward projection of the intersection point of the three support rods 23 is used as the origin of a roadway coordinate system. Three reference UWB radio ranging modules (3, 4, 5) are mounted on the cradling bar 23, whose position in the cradling bar 23 is known (i.e. the coordinates in the roadway coordinate system are known). When the position of the heading machine 1 is detected, the three modules of the reference UWB radio ranging module group 2 respectively carry out radio ranging on the UWB radio ranging module 6 of the machine body, and the coordinates of the UWB radio ranging module 6 of the machine body in the roadway coordinate system (namely the coordinates of the heading machine in the roadway coordinate system) are calculated through a positioning algorithm to be used as the starting point of navigation.
As shown in fig. 5, the omnidirectional odometer 7 is mainly composed of a measuring wheel 22, a mileage encoder 21, a measuring wheel bracket 20, a turntable 19, a tensioner 18, and an obstacle clearance housing 17. When the heading machine 1 advances, the tensioning device 18 applies tension to the measuring wheel 22 to enable the measuring wheel to be always in contact with the ground, the obstacle clearing shell 17 moves forwards along with the heading machine 1, and the radian part of the bottom of the obstacle clearing shell clears road obstacles to provide a smooth ground for the measuring wheel 22. The measuring wheel 22 drives the mileage encoder 21 to rotate, the mileage encoder 21 outputs pulse signals, and the real-time mileage increment of the heading machine 1 can be calculated.
As shown in fig. 6, in order to highlight the autonomous navigation principle of the heading machine based on dead reckoning, the heading machine at the initial position is reduced and the center of mass of the heading machine on the travel track is replaced with a mass point. Setting a tunnel coordinate system OXY, wherein the installation reference of the reference UWB radio ranging module group is taken as an original point O, the central line of the tunnel is taken as a Y axis, and the X axis is vertical to the Y axis; the machine body coordinate system AX 'Y' takes the center of mass of the heading machine as an origin, and the X 'axis, the Y' axis and the X, Y axis are parallel.
Before the navigation starts, the initial position A0 of the heading machine 1 is firstly determined by the reference UWB radio ranging module group 2 (X)0，Y0) The coordinates of the three reference UWB radio distance measuring modules (3, 4, 5) in the roadway coordinate system are known as (X)01，Y01)、(X02，Y02)(X03，Y03). The distances between the three reference positioning modules and the body positioning module are respectively R through the distance measurement of the reference UWB radio distance measurement module group 21、R2、R3The position solution equation can be listed:
wherein the system of equations contains three equations, but only X0、Y0Two unknowns, here by redundant equations to improve the positioning solution accuracy.
When the development machine 1 starts to move, the roll angle of the development machine 1 is set as theta, the pitch angle is set as gamma, and the course angle is set as gamma
. The strapdown inertial navigation system 9 measures the angular velocities and the accelerations of the heading machine in three axial directions in real time, calculates an attitude conversion matrix C of the heading machine according to a strapdown inertial navigation attitude update algorithm, and can obtain the attitude conversion matrix C according to a dead axle rotation principle:
the roll angle theta, pitch angle gamma and course angle can be calculated
Comprises the following steps:
the omnidirectional odometer 7 moves along with the heading machine, the grating in the driving mileage encoder 21 of the measuring wheel 22 rotates, and a square wave pulse signal is output. Pulse count with zero output is NZThe forward single-turn through grating count is NFReverse single pass raster count of NRWhen N is presentZWhen the value changes NFAnd NRZero clearing with the number of gratings NL. With the diameter d of the measuring wheel 22, the distance S at any sampling period is:
the starting position (X) can be obtained by the above formula0，Y0) Course angle increment
And distance increment delta S, and the position A of the heading machine at any time can be calculated by combiningt(Xt，Yt). Comprises the following steps:
the autonomous navigation system and method of the development machine comprises the following implementation steps:
(1) before the heading machine performs heading work, a surveying and mapping department personnel accurately determines the installation position of the reference UWB radio ranging module group in the roadway according to the design requirement of the roadway, and the installation position is used as the reference of the autonomous navigation of the heading machine. And establishing a roadway plane coordinate system by taking the reference position as a coordinate origin, determining the coordinate of the module support through the installation position of the measurement reference UWB radio ranging module on the module support, and inputting the information into a navigation computer in the explosion-proof electric control box to complete navigation initialization.
(2) After the heading machine is started, before the heading machine starts to run. The 3 UWB radio ranging modules sequentially transmit high-frequency radio pulse signals with different phases to the body UWB radio ranging module arranged on the body, and the body UWB radio module immediately reflects a pulse signal with a self-phase to the reference UWB radio module at the moment of receiving the pulse signal. Rubidium atomic clock pulse signals inside the UWB radio modules propagate twice between the two UWB radio ranging modules.
Sending the time information to a navigation computer in an explosion-proof electric cabinet according to a formula
Wherein, R is a linear distance between the two modules, t3 is a receiving time, t1 is a transmitting time, t2 is a time packet, and c is an electromagnetic wave propagation speed, so as to convert time information into distance information, thereby obtaining a distance relationship between every two of the 3 reference UWB radio modules and the body UWB radio module, and obtaining an initial coordinate of the heading machine in a roadway-to-plane coordinate system as a starting point of navigation.
(3) Starting to carry out the step (2) at the same time, carrying out initial alignment by the strapdown inertial navigation system, sending the angle increment measured by the three-axis gyroscope and the acceleration increment measured by the three-axis accelerometer to a navigation computer in the explosion-proof electric cabinet, and solving an attitude conversion matrix of a carrier coordinate system and a navigation coordinate system according to an initial alignment algorithm so as to solve the initial roll angle theta of the heading machineoAngle of pitch gammaoAngle of course
(4) And the navigation computer sends the initial position, the course and the attitude information of the development machine to a remote display and control system through a communication optical cable, and timely adjusts the position, the course and the attitude of the development machine according to a preset track.
(5) The heading machine drives to a preset working position of the section of the roadway, the omnidirectional odometer moves along with the heading machine, a measuring wheel of the omnidirectional odometer is always in contact with the ground by virtue of a tensioning device, a mileage encoder is driven to rotate by the measuring wheel, a square wave pulse signal is sent to a navigation computer, and the distance increment in a micro time period can be obtained. Meanwhile, the strapdown inertial navigation system also outputs angle increment information to the navigation computer in real time in the driving process of the heading machine, and can calculate and obtain the real-time roll angle theta, pitch angle gamma and course angle of the heading machine according to an attitude updating algorithm
. The navigation computer can obtain the position coordinates of the heading machine in real time through a dead reckoning algorithm by utilizing the mileage increment information obtained by the omnidirectional odometer and the course angle obtained by the strapdown inertial navigation system, and generates the heading machine advancing track. And the navigation computer sends the navigation information of the heading machine to a remote display and control system in real time, and the heading machine is adjusted according to the preset design requirements of the roadway.
(6) And the ground control system adjusts the heading machine to a preset heading direction according to the attitude information of the heading machine when the heading machine runs to a preset working position. The heading machine begins to cut the section of the roadway, continuous impact is generated between a cutting head of the heading machine and coal rocks in the cutting process to enable the machine body to generate violent vibration, and meanwhile, the position and the posture of the heading machine are changed seriously due to the fact that the machine body is sideslipped and inclined due to reactive force generated by cutting. And (3) repeating the step (2) to determine the position of the heading machine.
(7) And (5) after cutting is finished, retreating the tunneling machine for a certain distance to make room for supporting work of a newly tunneled roadway, and repeating the step (5) in the retreating process.
(8) And (5) after the support work is finished, starting a new tunneling cycle, and repeating the steps (2) - (7).
(9) And (3) when the heading machine exceeds the ranging range of the UWB radio ranging module, moving the module support forwards by a staff of a roadway surveying and mapping department, and repeating the step (1).
Claims (6)
1. A heading machine autonomous navigation system based on dead reckoning is characterized in that: the system comprises: the system comprises a reference UWB radio distance measurement module group, a machine body UWB radio distance measurement module, a strapdown inertial navigation system, an omnidirectional odometer, a navigation computer and a far-end display and navigation device, wherein the strapdown inertial navigation system and the navigation computer are arranged in an explosion-proof electric cabinet of the machine body of the heading machine; the omnidirectional odometer is arranged on a chassis of the development machine and consists of a barrier clearing shell, a tensioning device, a rotary table, a measuring wheel bracket, an odometer encoder and a measuring wheel; the UWB radio ranging module of the machine body is arranged in the center of the upper surface of the development machine; the reference UWB radio ranging module group consists of 3 reference UWB radio ranging modules, is arranged above a tunnel at the rear of the development machine, and the installation position of the reference UWB radio ranging module group is determined by a coal mine surveying and mapping department and is used as a reference for navigation;
determining the suspension position of the module support as a navigation reference, and taking the downward projection of the intersection point of the three support rods as the origin of a roadway coordinate system; three reference UWB radio ranging modules are arranged on the support rods, and the positions of the three reference UWB radio ranging modules are known, namely the coordinates in a roadway coordinate system are known; when the position of the heading machine is detected, the three modules of the reference UWB radio ranging module group respectively carry out radio ranging on the UWB radio ranging module of the machine body, and the coordinates of the UWB radio ranging module of the machine body in a roadway coordinate system, namely the coordinates of the heading machine in the roadway coordinate system, are solved through a positioning algorithm and serve as the starting point of navigation;
when the heading machine advances, the tensioning device applies tensioning force to the measuring wheel to enable the measuring wheel to be always in contact with the ground, the obstacle clearing shell advances along with the heading machine, and the radian part at the bottom of the obstacle clearing shell eradicates road surface obstacles to provide a smooth ground for the measuring wheel; the measuring wheel drives the mileage encoder to rotate, the mileage encoder outputs pulse signals, and the real-time mileage increment of the development machine is calculated;
the method comprises the following steps of (1) reducing a heading machine at an initial position, replacing a mass center for the heading machine on a driving track with mass points, and setting a roadway coordinate system OXY to be an original point O by taking the installation reference of a reference UWB radio ranging module group as an origin, wherein the center line of the roadway is a Y axis, and the X axis is vertical to the Y axis; the machine body coordinate system AX 'Y' takes the center of mass of the heading machine as an origin, and an X 'axis, a Y' axis and an X, Y axis are parallel;
before navigation begins, the initial position A0 (x) of the heading machine is determined by using a reference UWB radio ranging module group0，y0) The coordinates of the three reference UWB radio ranging modules in the roadway coordinate system are known as (x)O1，yO1)、(xO2，yO2)(xO3，yO3) (ii) a The distances between the three reference UWB radio ranging modules and the body UWB radio ranging module are respectively R through the ranging of the reference UWB radio ranging module group1、R2、R3The position solution equation can be listed:
wherein the system of equations comprises three equations, but only x0、y0Two unknowns, here by redundant equations to improve positioning resolution accuracy;
when the development machine starts to move, the roll angle of the development machine is set as theta, the pitch angle is set as gamma, and the course angle is set as
The strapdown inertial navigation system measures the angular velocity and the acceleration of the heading machine in three axial directions in real time, calculates an attitude conversion matrix C of the heading machine according to a strapdown inertial navigation attitude following new algorithm, and can be obtained according to a dead axle rotation principle:
the roll angle theta, pitch angle gamma and course angle can be calculated
Comprises the following steps:
the omnidirectional odometer moves along with the heading machine, the measuring wheel drives the grating in the odometer encoder to rotate, square wave pulse signals are output, and the pulse count output by recording the zero position is NZThe forward single-turn through grating count is NFReverse single pass raster count of NRWhen N is presentZWhen the value changes NFAnd NRZero clearing with the number of gratings NLAnd the diameter of the measuring wheel is d, the distance increment Delta S in any sampling period is as follows:
δ s (t) represents the measurement error of the distance increment, and the starting position (x) can be obtained by the above formula0，y0) The heading angle increment and the distance increment delta S are combined to calculate the position A of the heading machine at any timet(xt，yt)：
2. The dead reckoning-based heading machine autonomous navigation system according to claim 1, characterized in that: the strapdown inertial navigation system consists of a three-axis optical fiber gyroscope and a three-axis accelerometer, and can measure the angular velocity and the acceleration of three axes of the development machine under a machine body coordinate system in real time; sending the angular speed and acceleration information to a navigation computer in an explosion-proof electric control box, and before the heading machine advances, obtaining an initial attitude angle and a course angle of the heading machine according to a strapdown inertial navigation initial alignment algorithm; and when the heading machine advances, the real-time attitude angle and heading of the heading machine can be obtained according to the attitude updating algorithm.
3. The dead reckoning-based heading machine autonomous navigation system according to claim 1, characterized in that: the obstacle clearing shell is arranged at the center of a chassis of the tunneling machine, the arc edge of the lower part of the obstacle clearing shell is 3cm away from the ground, the main body of the obstacle clearing shell is cylindrical, and the bottom of the obstacle clearing shell is provided with a radian towards the outside so as to realize an obstacle clearing function; the tensioning device is arranged in the obstacle clearing shell and applies tensioning force to the measuring wheel, so that the measuring wheel is always in contact with the ground and has damping capacity; the turntable is connected below the tensioning device, so that the measuring wheel can realize 360-degree steering; the upper part of the measuring wheel bracket is connected with a rotating disc and a measuring wheel in a lower connection mode; and the mileage encoder is axially arranged on a shaft connected with the measuring wheel and the measuring wheel bracket, and is used for detecting the mileage increment of the heading machine in real time.
4. The dead reckoning-based heading machine autonomous navigation system according to claim 1, characterized in that: the UWB radio ranging module can emit high-frequency radio pulse with 3.4-10.6Ghz magnitude pulse, and the UWB radio ranging module can receive radio signals from each other, so that two-way communication between the radio ranging modules is realized, and the UWB radio ranging module is internally provided with a high-precision atomic clock which can accurately record the time when the pulse signal is emitted and received.
5. The dead reckoning-based heading machine autonomous navigation system according to claim 1, characterized in that: the navigation computer is arranged in an explosion-proof electric cabinet of the machine body of the heading machine, is in data communication with the strapdown inertial navigation system, the mileage encoder and the UWB radio distance measuring module of the machine body through an interface circuit, and sends the calculated navigation information to the far-end display and control system through a communication optical cable.
6. An autonomous navigation method using the dead reckoning-based heading machine autonomous navigation system of claim 1 is:
(1) before the heading machine performs heading work, a surveying and mapping department personnel accurately determines the installation position of a reference UWB radio ranging module group in a roadway according to the design requirement of the roadway, the installation position is used as the reference of autonomous navigation of the heading machine, a roadway plane coordinate system is established by taking the reference position as a coordinate origin, the coordinates of each module are determined by measuring the installation position of the reference UWB radio ranging module on a module support, and the coordinate information is input into a navigation computer in an explosion-proof electric control box to complete navigation initialization;
(2) before the development machine starts to run, 3 UWB radio ranging modules sequentially transmit high-frequency radio pulse signals with different phases to a body UWB radio ranging module arranged on a body, the body UWB radio ranging module also immediately reflects a pulse signal with a self phase to a reference UWB radio ranging module at the moment of receiving the pulse signal, rubidium atomic clock in the UWB radio ranging module records the time of the pulse signal which is transmitted twice between the two UWB radio ranging modules and transmits the time information to a navigation computer in an explosion-proof electric cabinet, converting time information into distance information according to a radio ranging algorithm formula to obtain the distance relationship between every two of 3 reference UWB radio ranging modules and a machine body UWB radio ranging module, and solving the initial coordinate of the heading machine in a roadway-to-plane coordinate system to serve as a starting point of navigation;
(3) starting to carry out the step (2) at the same time, carrying out initial alignment by the strapdown inertial navigation system, sending the angle increment measured by the triaxial fiber gyroscope and the acceleration increment measured by the triaxial accelerometer to a navigation computer in an explosion-proof electric cabinet, and solving an attitude conversion matrix of a carrier coordinate system and a navigation coordinate system according to an initial alignment algorithm so as to solve the initial roll angle theta of the heading machineoAngle of pitch gammaoAngle of course
(4) The navigation computer sends the initial position, course and attitude information of the development machine to a remote display and control system through a communication optical cable, and timely adjusts the position, course and attitude of the development machine according to a preset track;
(5) the heading machine drives to a preset working position of the section of the roadway, the omnidirectional odometer moves along with the heading machine, a measuring wheel of the omnidirectional odometer is always in contact with the ground by virtue of a tensioning device, a mileage encoder is driven to rotate by the measuring wheel, a square wave pulse signal is sent to a navigation computer, and a path increment in a tiny time period can be obtained; meanwhile, the strapdown inertial navigation system also outputs angle increment information to the navigation computer in real time in the driving process of the heading machine, and can calculate and obtain the real-time roll angle theta, pitch angle gamma and course angle of the heading machine according to an attitude updating algorithm
The navigation computer can obtain the position coordinates of the heading machine in real time through a dead reckoning algorithm by utilizing mileage increment information obtained by the omnidirectional odometer and a course angle obtained by the strapdown inertial navigation system, and generates a heading track of the heading machine; the navigation computer sends the navigation information of the development machine to the development machine in real timeThe remote display and control system adjusts the heading machine according to the preset design requirement of the roadway;
(6) the heading machine runs to a preset working position, the ground control system adjusts the heading machine to a preset heading direction according to attitude information of the heading machine, the heading machine starts to cut the section of the roadway, continuous impact is generated between a cutting head of the heading machine and coal rocks in the cutting process to enable the machine body to generate violent vibration, meanwhile, the cutting generates a reaction force to enable the machine body to sideslip and incline, the position and the attitude of the heading machine are seriously changed, and the step (2) is repeated to determine the position of the heading machine;
(7) after cutting, the heading machine retreats for a certain distance to make space for supporting work of a newly excavated roadway, and the step (5) is repeated in the retreating process;
(8) after the supporting work is finished, starting a new tunneling cycle, and repeating the steps (2) - (7);
(9) and (3) when the heading machine exceeds the ranging range of the UWB radio ranging module, moving the module support forwards by a staff of a roadway surveying and mapping department, and repeating the step (1).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911238056.XA CN110736458B (en) | 2019-12-06 | 2019-12-06 | Heading machine autonomous navigation system and method based on dead reckoning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911238056.XA CN110736458B (en) | 2019-12-06 | 2019-12-06 | Heading machine autonomous navigation system and method based on dead reckoning |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110736458A CN110736458A (en) | 2020-01-31 |
| CN110736458B true CN110736458B (en) | 2021-10-22 |
Family
ID=69274410
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911238056.XA Active CN110736458B (en) | 2019-12-06 | 2019-12-06 | Heading machine autonomous navigation system and method based on dead reckoning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110736458B (en) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111307133B (en) * | 2020-03-04 | 2022-03-22 | 中国铁建重工集团股份有限公司 | Vertical shaft verticality measuring method, vertical shaft posture detecting equipment and storage medium |
| CN111308955A (en) * | 2020-03-18 | 2020-06-19 | 辽宁工程技术大学 | Technology and method of remote collaborative control for fully mechanized excavation equipment |
| CN111417069A (en) * | 2020-03-27 | 2020-07-14 | 安徽理工大学 | Positioning device and method for coal mine underground transportation robot with wheel type odometer and UWB integrated |
| CN111412911A (en) * | 2020-04-07 | 2020-07-14 | 中国煤炭科工集团太原研究院有限公司 | Multi-sensor combined navigation system of coal mine underground continuous coal mining robot |
| CN111323044A (en) * | 2020-04-14 | 2020-06-23 | 中国矿业大学(北京) | Two-dimensional displacement measuring device and method for heading machine |
| CN111561543B (en) * | 2020-04-17 | 2021-12-07 | 中国矿业大学 | Vibration reduction system of tunneling machine inertial navigation combined positioning device and using method thereof |
| RU2752433C1 (en) * | 2020-04-17 | 2021-07-28 | Китайский Университет Горного Дела И Технологии | Vibration suppression system for integrated positioning device with inertial navigation contained in underweeling header and method for its application |
| CN111879314B (en) * | 2020-08-10 | 2022-08-02 | 中国铁建重工集团股份有限公司 | Multi-sensor fusion roadway driving equipment real-time positioning system and method |
| CN112781590A (en) * | 2020-12-17 | 2021-05-11 | 上海隧道工程有限公司 | Heading machine guiding system and method based on optical fiber gyroscope |
| CN113340245A (en) * | 2021-04-29 | 2021-09-03 | 中煤科工开采研究院有限公司 | Mining fully-mechanized excavating face excavation progress measuring system and method |
| CN114674309A (en) * | 2022-02-25 | 2022-06-28 | 中国矿业大学 | A combined positioning system for roadheader based on strapdown inertial navigation and its positioning method |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103410512A (en) * | 2013-08-15 | 2013-11-27 | 中国矿业大学 | Geological environment information-fused absolute positioning device and method for coal cutter |
| CN105178967A (en) * | 2015-05-12 | 2015-12-23 | 中国矿业大学(北京) | Autonomous positioning and directing system and method of excavator |
| CN109059942A (en) * | 2018-08-22 | 2018-12-21 | 中国矿业大学 | A kind of high-precision underground navigation map building system and construction method |
| CN109780370A (en) * | 2019-01-21 | 2019-05-21 | 深圳大学 | A pipeline three-dimensional curve measuring robot and its realization method |
| CN109931958A (en) * | 2019-03-27 | 2019-06-25 | 中国矿业大学 | A device and method for calibrating the working face of a shearer based on UWB |
| CN109974715A (en) * | 2019-04-30 | 2019-07-05 | 中国矿业大学(北京) | The development machine autonomous navigation system and method for inertial navigation and spot identification combination |
| CN111323044A (en) * | 2020-04-14 | 2020-06-23 | 中国矿业大学(北京) | Two-dimensional displacement measuring device and method for heading machine |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2304388A2 (en) * | 2008-07-14 | 2011-04-06 | Mine Safety Appliances Company | System and method of determining the location of mobile personnel |
-
2019
- 2019-12-06 CN CN201911238056.XA patent/CN110736458B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103410512A (en) * | 2013-08-15 | 2013-11-27 | 中国矿业大学 | Geological environment information-fused absolute positioning device and method for coal cutter |
| CN105178967A (en) * | 2015-05-12 | 2015-12-23 | 中国矿业大学(北京) | Autonomous positioning and directing system and method of excavator |
| CN109059942A (en) * | 2018-08-22 | 2018-12-21 | 中国矿业大学 | A kind of high-precision underground navigation map building system and construction method |
| CN109780370A (en) * | 2019-01-21 | 2019-05-21 | 深圳大学 | A pipeline three-dimensional curve measuring robot and its realization method |
| CN109931958A (en) * | 2019-03-27 | 2019-06-25 | 中国矿业大学 | A device and method for calibrating the working face of a shearer based on UWB |
| CN109974715A (en) * | 2019-04-30 | 2019-07-05 | 中国矿业大学(北京) | The development machine autonomous navigation system and method for inertial navigation and spot identification combination |
| CN111323044A (en) * | 2020-04-14 | 2020-06-23 | 中国矿业大学(北京) | Two-dimensional displacement measuring device and method for heading machine |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110736458A (en) | 2020-01-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110736458B (en) | Heading machine autonomous navigation system and method based on dead reckoning | |
| CN107389125B (en) | Underground space autonomous positioning multi-sensor intelligent detection robot | |
| CN105736007B (en) | Merge shield machine positioning and deviation-rectifying system and the method for formation information | |
| CN105178967B (en) | Development machine autonomous positioning orientation system and method | |
| CN102322857B (en) | Position and posture measuring system and method for mechanical equipment | |
| CN103760517B (en) | Underground scanning satellite high-precision method for tracking and positioning and device | |
| CN109974715B (en) | Tunneling machine autonomous navigation system and method combining strapdown inertial navigation and light spot identification | |
| CN104776843A (en) | Boom-type roadheader body and cutting head pose detection method | |
| CN112012759A (en) | A tunnelling is equipped with navigation positioning system for coal mine tunnel | |
| CN111879314B (en) | Multi-sensor fusion roadway driving equipment real-time positioning system and method | |
| CN101676517A (en) | Horizontal guiding drill pipeline locus autonomous surveying and drawing location system and surveying and drawing location method thereof | |
| CN110847905A (en) | Autonomous navigation system and method for coal mining machine | |
| CN109296370A (en) | A kind of driving method and system of auto-mapping positioning | |
| CN202033028U (en) | Three-dimensional in-vehicle mining subsidence dynamic monitoring system | |
| CN111121735A (en) | Tunnel, subway and mine excavation tunneling autonomous positioning and orienting system and method | |
| WO2018233721A1 (en) | Boring and positioning system and method for constructing winding underpass | |
| CN107121082A (en) | Coal mine roadway based on fiber-optic inertial technology continuous linear detection means and method | |
| CN113075650A (en) | Underground roadway tunneling equipment real-time positioning method based on UWB and inertial unit | |
| CN101482621A (en) | Method for monitoring acceleration of sliding posture of sliding body | |
| CN113970329A (en) | Strapdown inertial navigation and laser sensing combined heading machine pose detection system and method | |
| JP6438898B2 (en) | Miner position tracking and mapping | |
| CN204705359U (en) | A kind of boom-type roadheader fuselage and cutting head pose detection system | |
| CN111396047A (en) | Measuring and positioning system and method for coal face equipment group | |
| Luo et al. | Positioning accuracy evaluation for the collaborative automation of mining fleet with the support of memory cutting technology | |
| CN211950491U (en) | Autonomous navigation system of coal mining machine |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |