CN111879314B - Multi-sensor fusion roadway driving equipment real-time positioning system and method - Google Patents

Multi-sensor fusion roadway driving equipment real-time positioning system and method Download PDF

Info

Publication number
CN111879314B
CN111879314B CN202010796825.4A CN202010796825A CN111879314B CN 111879314 B CN111879314 B CN 111879314B CN 202010796825 A CN202010796825 A CN 202010796825A CN 111879314 B CN111879314 B CN 111879314B
Authority
CN
China
Prior art keywords
coordinate system
laser
machine body
light source
geodetic
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
Application number
CN202010796825.4A
Other languages
Chinese (zh)
Other versions
CN111879314A (en
Inventor
秦念稳
肖正航
李建华
陈晓伟
曾苗筠
李兆阳
张宏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Railway Construction Heavy Industry Group Co Ltd
Original Assignee
China Railway Construction Heavy Industry Group Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by China Railway Construction Heavy Industry Group Co Ltd filed Critical China Railway Construction Heavy Industry Group Co Ltd
Priority to CN202010796825.4A priority Critical patent/CN111879314B/en
Publication of CN111879314A publication Critical patent/CN111879314A/en
Application granted granted Critical
Publication of CN111879314B publication Critical patent/CN111879314B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/20—Instruments for performing navigational calculations
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

The invention provides a multi-sensor fused real-time positioning system for roadway excavation equipment, which comprises a laser transmitter, excavation equipment, an inertia unit, a front camera, a rear camera, a radar transmitter, a radar receiver and an industrial personal computer. The invention also provides a real-time positioning method, which adopts a real-time positioning system to establish a space coordinate system; acquiring the coordinates of laser spots in a geodetic coordinate system through pointing laser; acquiring the coordinates of a laser spot and a light source under a machine body coordinate system; obtaining the coordinates of the laser spot and the light source under a geodetic coordinate system through the coordinate conversion relation between the machine body and the geodetic; obtaining the distance between the ranging radars; and finally, resolving the pose of the tunneling equipment body in the geodetic coordinate system. The invention can realize real-time automatic positioning of the tunneling equipment, does not need manual intervention in the positioning process, realizes safe and efficient tunneling and provides technical support for unmanned and intelligent tunneling equipment of coal mines.

Description

Multi-sensor fusion roadway driving equipment real-time positioning system and method
Technical Field
The invention relates to the technical field of engineering machinery positioning, in particular to a multi-sensor fusion roadway tunneling equipment real-time positioning system and method.
Background
With the development of coal mine equipment technology, the mechanized excavation of domestic coal mine tunnels is gradually realized in a manual-based excavation mode, but the positioning technology of excavation equipment in the tunnels at the present stage is not broken through, the coal mine tunnel excavation still depends on visual operation of an operator, and the positioning technology becomes the bottleneck technology to be broken through most urgently in the intelligent excavation of the coal mine excavation equipment.
The coal mine tunnel excavation mainly adopts a cantilever type excavator or a comprehensive excavator, and is matched with a continuous belt conveying device to realize rapid excavation and transportation. In the excavation link, a measuring team continuously measures from a geodetic standard until the measuring team reaches the roadway, a laser pointing light source is arranged to point to the excavation surface from the rear, and the top surface, the bottom surface and the two sides of the excavation surface are determined by taking pointing laser spots as the standard. Due to the fact that coal dust concentration of a digging site is extremely high and visibility is extremely poor, the excessive and insufficient digging amount of the top bottom plate and the two sides needs to be measured repeatedly by manually holding the tape measure in a hand mode. On one hand, by the construction in the mode, personnel must stand near the excavation face to operate, coal dust is large and is close to an unsupported area, and great health threat and potential safety hazard are caused to operators; on the other hand, the shape control of the excavation section depends on manual visual and empirical judgment, the over-excavation and the fluctuation of the cutting section are serious, the inconvenience is brought to supporting, and the collapse risk is increased.
At present, the coal mine tunnel boring machine positioning mainly comprises three pose measurement methods based on a total station instrument, an inertial sensor and machine vision. The underground measurement environment of the coal mine is severe, and the sight line is easily shielded, so that the measurement result based on the total station is inaccurate; the measurement system based on the inertial sensor is easily influenced by factors such as equipment vibration or error accumulation of the sensor; machine vision based is currently a preferred measurement method. The prior art basically applies a total station to carry out positioning measurement, so that the prior art has certain limitations.
In summary, a system and a method for real-time positioning of multi-sensor integrated tunneling equipment are urgently needed to solve the problems in the prior art.
Disclosure of Invention
The invention aims to provide a multi-sensor fusion roadway driving equipment real-time positioning system and method, and aims to solve the problems that the existing coal mine roadway driving equipment positioning method cannot acquire all pose parameters of a machine body in a geodetic coordinate system, is limited in real-time measurement, inaccurate in measurement result, and incapable of realizing safe and efficient intelligent driving.
In order to achieve the aim, the invention provides a multi-sensor fused real-time positioning system for roadway tunneling equipment, which comprises a laser transmitter, tunneling equipment, an inertia unit, a front camera, a rear camera, a radar transmitter, a radar receiver and an industrial personal computer;
the laser emitter is arranged on a top plate of the roadway; an inertia unit is arranged on the tunneling equipment; the front camera is arranged at the front end of the machine body of the tunneling equipment, and the rear camera is arranged at the rear end of the machine body of the tunneling equipment; the radar transmitter is arranged on the laser transmitter, forms a distance measuring radar module with the radar receiver arranged at the rear end of the body of the tunneling equipment, and measures the distance between the laser transmitter and the tunneling equipment; the industrial personal computer is used for processing data information of the front camera, the rear camera, the ranging radar module and the inertial unit, calculating the position of the tunneling equipment body and completing real-time positioning of the tunneling equipment under a geodetic coordinate system.
Further, the inertial unit includes a triaxial accelerometer and a triaxial gyroscope for measuring a pitch angle and a roll angle of the body of the excavating equipment relative to the ground level.
Furthermore, the front camera is a binocular vision module formed by two cameras, and the rear camera is a monocular camera; and optical filters are additionally arranged in front of the lenses of the front camera and the rear camera.
Further, the ranging radar module is an ultra-wideband radar.
The invention also provides a multi-sensor fused real-time positioning method for the tunneling equipment, which adopts the real-time positioning system for the tunneling equipment and comprises the following steps:
the method comprises the following steps: establishing a coordinate system including a geodetic coordinate system O 0 X 0 Y 0 Z 0 Coordinate system of fuselage O 1 X 1 Y 1 Z 1 Front camera coordinate system O C1 X C1 Y C1 Z C1 And the rear camera coordinate system O C2 X C2 Y C2 Z C2 ；
Step two: acquiring the coordinates of laser spots in a geodetic coordinate system through pointing laser;
step three: acquiring the coordinates of a laser spot and a light source under a machine body coordinate system;
step four: obtaining the coordinates of the laser spot and the light source under a geodetic coordinate system through the coordinate conversion relation between the machine body and the geodetic;
step five: obtaining the distance between the ranging radars;
step six: and resolving the pose of the tunneling equipment body in the geodetic coordinate system.
Further, the method for acquiring the coordinates of the laser spot in the geodetic coordinate system specifically comprises the following steps: knowing the direction of the roadway pointing to the laser, and obtaining coordinate information of the light spot relative to the light source through the distance from the laser light source to the light spot; calculating the coordinates of laser spots in a geodetic coordinate system according to a formula (1) in combination with the installation position of a laser transmitter on a roadway roof;
Figure BDA0002625936910000031
wherein, [ X ] D Y D Z D ] T Is the coordinate of the laser spot in the geodetic coordinate system;
[cosx S_D cosy S_D cosz S_D ] T is a direction vector pointing to the laser;
L S_D is the distance between the laser source and the spot;
[X S Y S Z S ] T is the coordinate of the laser source in the geodetic coordinate system.
Further, the method for acquiring the coordinates of the laser spot and the light source in the coordinate system of the body specifically comprises the following steps: acquiring the direction of a laser spot through a front camera, and acquiring the distance from the front camera to the laser spot based on binocular vision to obtain coordinate information of the spot relative to the front camera; calculating the coordinates of the laser spot in a machine body coordinate system according to a formula (2) by combining the installation position of the front camera on the machine body of the tunneling equipment;
Figure BDA0002625936910000032
wherein, [ x ] C1 y C1 z C1 ] T Is the coordinate of the laser spot under the coordinate system of the machine body;
[cosx C1 cosy C1 cosz C1 ] T the direction vector of the laser spot relative to the front camera is shown;
L C1_D is the distance between the front camera and the laser spot;
[Δx C1 Δy C1 Δz C1 ] T is a vector between the origin of the coordinate system of the front camera and the coordinate system of the body;
acquiring the direction of a laser light source through a rear camera to obtain coordinate information of the light source relative to the rear camera; calculating the coordinates of the laser light source under a machine body coordinate system according to a formula (3) in combination with the installation position of the rear camera on the machine body of the tunneling equipment;
Figure BDA0002625936910000033
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser light source under the body coordinate system;
[cosx C2 cosy C2 cosz C2 ] T the direction vector of the laser light source relative to the rear camera is shown;
L C2_S is a rear camera and laserThe distance between the light sources;
[Δx C2 Δy C2 Δz C2 ] T is the vector between the origin of the rear camera coordinate system and the fuselage coordinate system.
Further, according to the mutual conversion relation between the machine body coordinate system and the geodetic coordinate system, combining the obtained coordinates of the laser spot in the machine body coordinate system, and calculating the coordinates of the laser spot in the geodetic coordinate system according to a formula (4);
Figure BDA0002625936910000041
wherein [ Δ X Δ Y Δ Z [ ]] T Representing a vector between the machine body coordinate system and the origin of the geodetic coordinate system, namely the coordinates of the machine body of the tunneling equipment under the geodetic coordinate system;
[x C1 y C1 z C1 ] T is the coordinate of the laser spot under the coordinate system of the machine body;
RX0, RY0, RZ0 are rotation matrices of the fuselage relative to the earth coordinate system along the X0, Y0, Z0 axes, respectively;
Figure BDA0002625936910000042
alpha, beta and gamma respectively represent the rolling angle, the pitch angle and the yaw angle of the machine body relative to a geodetic coordinate system;
calculating the coordinates of the laser light source under a geodetic coordinate system according to a formula (6) by combining the acquired coordinates of the laser light source under the body coordinate system;
Figure BDA0002625936910000043
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser source in the coordinate system of the machine body.
Further, the distance between the radars is calculated by formula (7) through the coordinate information of the radar transmitter and the radar receiver;
Figure BDA0002625936910000044
wherein L is LD1_LD2 Is the distance between ranging radars;
[x LD1 y LD1 z LD1 ] T is the coordinates of the radar transmitter in the geodetic coordinate system;
[x LD2 y LD2 z LD2 ] T the coordinate of the radar receiver in the geodetic coordinate system is obtained by converting the radar receiver from the body coordinate system to the geodetic coordinate system.
Figure BDA0002625936910000051
Wherein, [ x ] C y C z C ] T Is the installation position coordinate of the radar receiver under the coordinate system of the body.
Further, the sixth step is specifically: the pose of the airplane body comprises six parameters including transverse offset, longitudinal offset, elevation offset, rolling angle, pitch angle and yaw angle, and is respectively represented by delta X, delta Y, delta Z, alpha, beta and gamma, wherein the alpha and the beta are obtained by an inertial unit arranged on the airplane body, and the delta X, the delta Y, the delta Z and the gamma are unknown quantities to be solved; the method comprises the following steps that in the attitude calculation process, the distance LC2_ S from a rear camera to a laser light source, the distance LS _ D from a roadway pointing to the laser light source to a laser spot, the pitch angle theta of the laser light source relative to the rear camera and seven unknowns to be solved are included, the formulas (4), (6) and (7) are combined and substituted into other formulas correspondingly, and the unknowns are solved:
Figure BDA0002625936910000052
the equation set (9) actually comprises seven nonlinear equations, seven unknowns when errors at two ends of the equation set are minimized are solved, six pose parameters of the tunneling equipment under a geodetic coordinate system are obtained, and complete pose information of the machine body is obtained.
The technical scheme of the invention has the following beneficial effects:
based on machine vision and ultra wide band technology, the invention obtains the yaw angle and pitch angle of a facula and a light source relative to the front and rear cameras through the front and rear cameras, obtains the distance information between the laser light source and the machine body through the ultra wide band radar, obtains the pitch angle and roll angle of the machine body relative to the ground horizontal plane through the inertia unit, and finally calculates the real-time pose information of the tunneling equipment. The invention has simple structure, complete acquired positioning information of the machine body and high reliability. By additionally arranging the optical filter in front of the camera lens, the roadway pointing laser light source and the light spots can be quickly identified in the coal mine construction environment with extremely poor visibility, and the efficiency and the accuracy of the positioning of the tunneling equipment are improved. Automatic positioning is carried out in real time, and manual intervention is not needed in the positioning process. The safe and efficient roadway excavation is realized, and the technical support is provided for the unmanned and intelligent coal mine roadway excavation equipment.
In addition to the objects, features and advantages described above, other objects, features and advantages of the present invention are also provided. The present invention will be described in further detail below with reference to the drawings.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic diagram of a multi-sensor fused roadway real-time positioning system;
FIG. 2 is a schematic diagram of spatial coordinate system definition;
FIG. 3 is a flow chart of roadway real-time location;
the device comprises a laser transmitter 1, a tunneling device 2, an inertia unit 3, a front camera 4, a rear camera 5, a radar transmitter 6, a radar receiver 7, a radar receiver 8 and an industrial personal computer.
Detailed Description
Embodiments of the invention will be described in detail below with reference to the drawings, but the invention can be implemented in many different ways, which are defined and covered by the claims.
Example 1:
referring to fig. 1 to 3, the embodiment of the invention provides a multi-sensor fusion roadway tunneling device real-time positioning system and method, which are applied to the real-time positioning of tunneling devices in coal mine roadways.
Referring to fig. 1, a multi-sensor fused real-time positioning system for roadway excavation equipment comprises a laser transmitter 1, excavation equipment 2, an inertial unit 3, a front camera 4, a rear camera 5, a radar transmitter 6, a radar receiver 7 and an industrial personal computer 8;
the laser emitter 1 is arranged on a roof of a coal mine tunnel, emits directional laser along the tunneling direction of the tunnel, and forms laser spots on a mining working face; the tunneling equipment 2 is provided with an inertia unit 3, and a pitch angle and a roll angle of the machine body of the tunneling equipment 2 relative to the ground horizontal plane are measured; the front camera 4 is arranged at the front end of the machine body of the tunneling device 2, can identify laser spots on a mining working surface and acquire coordinate information of the spots; the rear camera 5 is arranged at the rear end of the machine body of the tunneling device 2, can identify a laser light source and acquire coordinate information of the light source; the radar transmitter 6 is arranged on the laser transmitter 1, forms a ranging radar module with a radar receiver 7 arranged at the rear end of the body of the tunneling equipment 2, and measures the distance between the laser transmitter 1 and the tunneling equipment 2; the industrial personal computer 8 is used for processing data information of the sensors such as the camera, the ranging radar and the inertial unit, calculating the position of the tunneling equipment body and completing real-time positioning of the coal mine tunneling equipment under a geodetic coordinate system.
The inertial unit 3 includes a three-axis accelerometer and a three-axis gyroscope. The structure realizes the advantage complementation of the accelerometer and the gyroscope, can measure the pitch angle and the roll angle of the tunneling equipment body relative to the ground horizontal plane, and avoids the measurement error caused by the shaking of the body.
The front camera 4 is a binocular vision module formed by two cameras, and the rear camera 5 is a monocular camera. The binocular vision module can acquire the distance between the front camera and the laser spot. Specifically, optical filters are additionally arranged in front of lenses of the front camera 4 and the rear camera 5, so that the tunnel pointing laser light source and the light spots can be rapidly identified in a coal mine construction environment with extremely poor visibility.
The ranging radar module is specifically an ultra wide band radar, has strong anti-interference performance and has good target identification and distance resolution capabilities.
Fig. 3 is a real-time positioning process of the multi-sensor integrated coal mine tunneling equipment, and by applying the real-time positioning system, the real-time positioning method of the tunneling equipment mainly comprises the following steps:
the method comprises the following steps: establishing coordinate system, establishing geodetic coordinate system O 0 X 0 Y 0 Z 0 Coordinate system of fuselage O 1 X 1 Y 1 Z 1 Front camera coordinate system O C1 X C1 Y C1 Z C1 Rear camera coordinate system O C2 X C2 Y C2 Z C2 (ii) a Wherein the geodetic coordinate system Z 0 With the axis vertically upwards, Y 0 Axial direction is consistent with that of tunnelling, X 0 Axis and Y 0 Axis, Z 0 The axes constitute a right-hand coordinate system. See fig. 2.
Step two: acquiring the coordinates of laser spots in a geodetic coordinate system through pointing laser;
knowing the direction of the roadway pointing to the laser, and obtaining coordinate information of the light spot relative to the light source through the distance from the laser light source to the light spot; calculating the coordinates of laser spots in a geodetic coordinate system according to a formula (1) in combination with the installation position of a laser transmitter on a roadway roof;
Figure BDA0002625936910000071
wherein, [ X ] D Y D Z D ] T Is the coordinate of the laser spot in the geodetic coordinate system;
[cosx S_D cosy S_D cosz S_D ] T is a direction vector pointing to the laser;
L S_D is the distance between the laser source and the spot;
[X S Y S Z S ] T is the coordinates of the laser source in the geodetic coordinate system.
Step three: acquiring the coordinates of a laser spot and a light source under a machine body coordinate system;
acquiring the direction of a laser spot through a front camera, and acquiring the distance from the front camera to the laser spot based on binocular vision to obtain coordinate information of the spot relative to the front camera; calculating the coordinates of the laser spot in a machine body coordinate system according to a formula (2) by combining the installation position of the front camera on the machine body of the tunneling equipment;
Figure BDA0002625936910000081
wherein, [ x ] C1 y C1 z C1 ] T Is the coordinate of the laser spot under the coordinate system of the machine body;
[cosx C1 cosy C1 cosz C1 ] T the direction vector of the laser spot relative to the front camera is shown;
L C1_D is the distance between the front camera and the laser spot;
[Δx C1 Δy C1 Δz C1 ] T is the vector between the origin of the front camera coordinate system and the fuselage coordinate system.
Acquiring the direction of a laser light source through a rear camera to obtain coordinate information of the light source relative to the rear camera; calculating the coordinates of the laser light source under a machine body coordinate system according to a formula (3) in combination with the installation position of the rear camera on the machine body of the tunneling equipment;
Figure BDA0002625936910000082
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser light source under the body coordinate system;
[cosx C2 cosy C2 cosz C2 ] T is arranged behind the laser light sourceA direction vector of the camera;
L C2_S is the distance between the rear camera and the laser source;
[Δx C2 Δy C2 Δz C2 ] T is the vector between the origin of the rear camera coordinate system and the fuselage coordinate system.
Step four: obtaining the coordinates of the laser spot and the light source under a geodetic coordinate system through the coordinate conversion relation between the machine body and the geodetic;
according to the mutual conversion relation between the machine body coordinate system and the geodetic coordinate system, combining the obtained coordinates of the laser spot in the machine body coordinate system, and calculating the coordinates of the laser spot in the geodetic coordinate system according to a formula (4);
Figure BDA0002625936910000083
wherein [ Δ X Δ Y Δ Z [ ]] T Representing a vector between the machine body coordinate system and the origin of the geodetic coordinate system, namely the coordinates of the machine body of the tunneling equipment under the geodetic coordinate system;
[x C1 y C1 z C1 ] T is the coordinate of the laser spot under the coordinate system of the machine body;
R X0 、R Y0 、R Z0 is the respective edge X of the fuselage relative to the geodetic coordinate system 0 、Y 0 、Z 0 A rotation matrix of the shaft;
Figure BDA0002625936910000091
alpha, beta and gamma respectively represent the rolling angle, the pitch angle and the yaw angle of the machine body relative to a ground coordinate system.
Calculating the coordinates of the laser light source in a geodetic coordinate system according to a formula (6) by combining the acquired coordinates of the laser light source in a body coordinate system;
Figure BDA0002625936910000092
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser light source under the coordinate system of the machine body.
Step five: obtaining the distance between the ranging radars;
the distance between the radars can be directly obtained through the ranging module on one hand, and can be calculated by a formula (7) through the coordinate information of the radar transmitter and the radar receiver on the other hand;
Figure BDA0002625936910000093
wherein L is LD1_LD2 Is the distance between ranging radars;
[x LD1 y LD1 z LD1 ] T is the coordinates of the radar transmitter in the geodetic coordinate system;
[x LD2 y LD2 z LD2 ] T the coordinate of the radar receiver in the geodetic coordinate system is obtained by converting the radar receiver from the body coordinate system to the geodetic coordinate system.
Figure BDA0002625936910000094
Wherein, [ x ] C y C z C ] T Is the installation position coordinate of the radar receiver under the coordinate system of the machine body;
step six: and resolving the pose of the tunneling equipment body in the geodetic coordinate system.
The pose of the airplane body comprises six parameters including transverse offset, longitudinal offset, elevation offset, rolling angle, pitch angle and yaw angle, and is respectively represented by delta X, delta Y, delta Z, alpha, beta and gamma, wherein the alpha and the beta are obtained by an inertial unit arranged on the airplane body, and the delta X, the delta Y, the delta Z and the gamma are unknown quantities to be solved; the in-place posture calculation process also comprises a distance L from the rear camera to the laser light source C2_S The distance L from the roadway pointing to the laser light source to the laser spot S_D Laser light source phaseTo the angle of pitch theta of back camera, totally seven unknowns that wait to solve, substitute formula (4) with formula (1), (2), formula (6) is substituted in formula (3), formula (7) is substituted in formula (8), and simultaneous formula (4), (6), (7) are solved to the unknowns:
Figure BDA0002625936910000101
the equation set (9) actually comprises seven nonlinear equations, seven unknowns when errors at two ends of the equation set are minimized are solved, six pose parameters of the tunneling equipment under a geodetic coordinate system are obtained, and complete pose information of the machine body is obtained. Furthermore, the multi-sensor such as the camera, the ranging radar and the inertial unit and an industrial personal computer interface transmit sensing information in real time, and the industrial personal computer performs fusion processing on the signals, so that the real-time positioning of the coal mine roadway tunneling equipment is finally realized.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (4)

1. A multi-sensor fusion roadway driving equipment real-time positioning method is characterized by comprising the following steps:
the method comprises the following steps: establishing a coordinate system including a geodetic coordinate system O 0 X 0 Y 0 Z 0 Coordinate system of fuselage O 1 X 1 Y 1 Z 1 Front camera coordinate system O C1 X C1 Y C1 Z C1 And the rear camera coordinate system O C2 X C2 Y C2 Z C2 ；
Step two: acquiring the coordinates of laser spots in a geodetic coordinate system through pointing laser;
the method for acquiring the coordinates of the laser spot in the geodetic coordinate system specifically comprises the following steps: knowing the direction of the roadway pointing to the laser, and obtaining coordinate information of the light spot relative to the light source through the distance from the laser light source to the light spot; calculating the coordinates of laser spots in a geodetic coordinate system according to a formula (1) in combination with the installation position of a laser transmitter on a roadway roof;
Figure FDA0003706260700000011
wherein, [ X ] D Y D Z D ] T Is the coordinate of the laser spot in the geodetic coordinate system;
[cosx S_D cosy S_D cosz S_D ] T is a direction vector pointing to the laser;
L S_D is the distance between the laser source and the spot;
[X S Y S Z S ] T is the coordinate of the laser light source under the geodetic coordinate system;
step three: acquiring the coordinates of a laser spot and a light source under a machine body coordinate system;
the method for acquiring the coordinates of the laser spot and the light source under the machine body coordinate system specifically comprises the following steps: acquiring the direction of a laser spot through a front camera, and acquiring the distance from the front camera to the laser spot based on binocular vision to obtain coordinate information of the spot relative to the front camera; calculating the coordinates of the laser spot in a machine body coordinate system according to a formula (2) by combining the installation position of the front camera on the machine body of the tunneling equipment;
Figure FDA0003706260700000012
wherein, [ x ] C1 y C1 z C1 ] T Is the coordinate of the laser spot under the coordinate system of the machine body;
[cosx C1 cosy C1 cosz C1 ] T the direction vector of the laser spot relative to the front camera is shown;
L C1_D between the front camera and the laser spotThe distance of (d);
[Δx C1 Δy C1 Δz C1 ] T is a vector between the origin of the coordinate system of the front camera and the coordinate system of the body;
acquiring the direction of a laser light source through a rear camera to obtain coordinate information of the light source relative to the rear camera; calculating the coordinates of the laser light source under a machine body coordinate system according to a formula (3) in combination with the installation position of the rear camera on the machine body of the tunneling equipment;
Figure FDA0003706260700000021
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser light source under the body coordinate system;
[cosx C2 cosy C2 cosz C2 ] T the direction vector of the laser light source relative to the rear camera is shown;
L C2_S is the distance between the rear camera and the laser source;
[Δx C2 Δy C2 Δz C2 ] T is a vector between the origin of the coordinate system of the rear camera and the coordinate system of the body;
step four: obtaining the coordinates of the laser spot and the light source under a geodetic coordinate system through the coordinate conversion relation between the machine body and the geodetic;
according to the mutual conversion relation between the machine body coordinate system and the geodetic coordinate system, combining the obtained coordinates of the laser spot in the machine body coordinate system, and calculating the coordinates of the laser spot in the geodetic coordinate system according to a formula (4);
Figure FDA0003706260700000022
wherein [ Δ X Δ Y Δ Z [ ]] T Representing a vector between the machine body coordinate system and the origin of the geodetic coordinate system, namely the coordinates of the machine body of the tunneling equipment under the geodetic coordinate system;
[x C1 y C1 z C1 ] T is the coordinate of the laser spot under the coordinate system of the machine body;
R X0 、R Y0 、R Z0 is the respective edge X of the fuselage relative to the geodetic coordinate system 0 、Y 0 、Z 0 A rotation matrix of the shaft;
Figure FDA0003706260700000023
alpha, beta and gamma respectively represent the rolling angle, the pitch angle and the yaw angle of the machine body relative to a geodetic coordinate system;
calculating the coordinates of the laser light source under a geodetic coordinate system according to a formula (6) by combining the acquired coordinates of the laser light source under the body coordinate system;
Figure FDA0003706260700000031
wherein, [ x ] C2 y C2 z C2 ] T Is the coordinate of the laser light source under the body coordinate system;
step five: obtaining the distance between the ranging radars;
the distance between the radars is calculated by a formula (7) through the coordinate information of the radar transmitter and the radar receiver;
Figure FDA0003706260700000032
wherein L is LD1_LD2 Is the distance between ranging radars;
[x LD1 y LD1 z LD1 ] T is the coordinates of the radar transmitter in the geodetic coordinate system;
[x LD2 y LD2 z LD2 ] T the coordinate of the radar receiver under the geodetic coordinate system is obtained by converting the radar receiver from a machine body coordinate system to the geodetic coordinate system;
Figure FDA0003706260700000033
wherein, [ x ] C y C z C ] T Is the installation position coordinate of the radar receiver under the coordinate system of the machine body;
step six: resolving the pose of the tunneling equipment body in a geodetic coordinate system;
the method comprises the following steps: the pose of the airplane body comprises six parameters including transverse offset, longitudinal offset, elevation offset, rolling angle, pitch angle and yaw angle, and is respectively represented by delta X, delta Y, delta Z, alpha, beta and gamma, wherein the alpha and the beta are obtained by an inertial unit arranged on the airplane body, and the delta X, the delta Y, the delta Z and the gamma are unknown quantities to be solved; the in-place posture calculation process also comprises a distance L from the rear camera to the laser light source C2_S The distance L from the roadway pointing to the laser light source to the laser spot S_D The pitch angle theta of the laser light source relative to the rear camera is seven unknown quantities to be solved, the formulas (4), (6) and (7) are combined and substituted into other formulas correspondingly, and the unknown quantities are solved:
Figure FDA0003706260700000041
the equation set (9) actually comprises seven nonlinear equations, seven unknowns when errors at two ends of the equation set are minimized are solved, six pose parameters of the tunneling equipment under a geodetic coordinate system are obtained, and complete pose information of the machine body is obtained.
2. A multi-sensor fusion roadway driving equipment real-time positioning system is used for realizing the multi-sensor fusion roadway driving equipment real-time positioning method of claim 1, and is characterized by comprising a laser transmitter, driving equipment, an inertia unit, a front camera, a rear camera, a radar transmitter, a radar receiver and an industrial personal computer;
the front camera is a binocular vision module formed by two cameras, and the rear camera is a monocular camera; optical filters are additionally arranged in front of the lenses of the front camera and the rear camera;
the laser emitter is arranged on a top plate of the roadway; an inertia unit is arranged on the tunneling equipment; the front camera is arranged at the front end of the machine body of the tunneling equipment, and the rear camera is arranged at the rear end of the machine body of the tunneling equipment; the radar transmitter is arranged on the laser transmitter, forms a distance measuring radar module with the radar receiver arranged at the rear end of the body of the tunneling equipment, and measures the distance between the laser transmitter and the tunneling equipment; the industrial personal computer is used for processing data information of the front camera, the rear camera, the ranging radar module and the inertial unit, calculating the position of the tunneling equipment body and completing real-time positioning of the tunneling equipment under a geodetic coordinate system.
3. The system of claim 2, wherein the inertial unit comprises a three-axis accelerometer and a three-axis gyroscope for measuring pitch and roll angles of the body of the tunnelling equipment relative to ground level.
4. The system of claim 2, wherein the range radar module is an ultra-wideband radar.
CN202010796825.4A 2020-08-10 2020-08-10 Multi-sensor fusion roadway driving equipment real-time positioning system and method Active CN111879314B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010796825.4A CN111879314B (en) 2020-08-10 2020-08-10 Multi-sensor fusion roadway driving equipment real-time positioning system and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010796825.4A CN111879314B (en) 2020-08-10 2020-08-10 Multi-sensor fusion roadway driving equipment real-time positioning system and method

Publications (2)

Publication Number Publication Date
CN111879314A CN111879314A (en) 2020-11-03
CN111879314B true CN111879314B (en) 2022-08-02

Family

ID=73211300

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010796825.4A Active CN111879314B (en) 2020-08-10 2020-08-10 Multi-sensor fusion roadway driving equipment real-time positioning system and method

Country Status (1)

Country Link
CN (1) CN111879314B (en)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112762906B (en) * 2020-12-30 2022-08-09 中国铁建重工集团股份有限公司 Multi-sensor fusion-based guiding system and guiding method
CN114859366A (en) * 2021-02-04 2022-08-05 上海创力集团股份有限公司 Positioning and orienting method and device for heading machine
CN113075650A (en) * 2021-04-02 2021-07-06 中国铁建重工集团股份有限公司 Underground roadway tunneling equipment real-time positioning method based on UWB and inertial unit
CN113252044A (en) * 2021-05-25 2021-08-13 中国煤炭科工集团太原研究院有限公司 Method for calculating deviation of tunneling equipment body
CN113756815B (en) * 2021-08-16 2024-05-28 山西科达自控股份有限公司 Equipment position image recognition system

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016047482A1 (en) * 2014-09-24 2016-03-31 株式会社デンソー Object detection device
CN107235044A (en) * 2017-05-31 2017-10-10 北京航空航天大学 It is a kind of to be realized based on many sensing datas to road traffic scene and the restoring method of driver driving behavior
CN109597086A (en) * 2018-11-15 2019-04-09 中国直升机设计研究所 A kind of motion measuring method of the outer hanging object of contactless helicopter
CN109634279A (en) * 2018-12-17 2019-04-16 武汉科技大学 Object positioning method based on laser radar and monocular vision
CN110736458A (en) * 2019-12-06 2020-01-31 中国矿业大学(北京) Heading machine autonomous navigation system and method based on dead reckoning
CN111191625A (en) * 2020-01-03 2020-05-22 浙江大学 Object identification and positioning method based on laser-monocular vision fusion
CN111502671A (en) * 2020-04-20 2020-08-07 中铁工程装备集团有限公司 Comprehensive guiding device and method for guiding and carrying binocular camera by shield laser target

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2143396B (en) * 1983-05-21 1987-06-17 Mac Co Ltd Beam riding location system
CN105718888B (en) * 2016-01-22 2019-09-13 北京中科慧眼科技有限公司 Barrier method for early warning and barrier prior-warning device
CN110017817B (en) * 2019-01-24 2021-09-14 中国煤炭科工集团太原研究院有限公司 Coal mine roadway navigation positioning method and device based on roof characteristics
CN109839109B (en) * 2019-02-25 2020-11-24 中国矿业大学 Development machine absolute pose detection method based on image recognition and multi-sensor fusion
CN209927123U (en) * 2019-04-30 2020-01-10 力信测量(上海)有限公司 Heading machine position and attitude measuring system based on space vector constraint
CN111473803B (en) * 2020-05-27 2023-09-05 天津科技大学 Calibration method for mining laser target
CN111485879B (en) * 2020-06-28 2020-10-09 中国铁建重工集团股份有限公司 Heading machine vehicle body and positioning method and positioning system of cutting drum of heading machine vehicle body

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016047482A1 (en) * 2014-09-24 2016-03-31 株式会社デンソー Object detection device
CN107235044A (en) * 2017-05-31 2017-10-10 北京航空航天大学 It is a kind of to be realized based on many sensing datas to road traffic scene and the restoring method of driver driving behavior
CN109597086A (en) * 2018-11-15 2019-04-09 中国直升机设计研究所 A kind of motion measuring method of the outer hanging object of contactless helicopter
CN109634279A (en) * 2018-12-17 2019-04-16 武汉科技大学 Object positioning method based on laser radar and monocular vision
CN110736458A (en) * 2019-12-06 2020-01-31 中国矿业大学(北京) Heading machine autonomous navigation system and method based on dead reckoning
CN111191625A (en) * 2020-01-03 2020-05-22 浙江大学 Object identification and positioning method based on laser-monocular vision fusion
CN111502671A (en) * 2020-04-20 2020-08-07 中铁工程装备集团有限公司 Comprehensive guiding device and method for guiding and carrying binocular camera by shield laser target

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Prediction of roadheaders’ performance using artificial neural network approaches （MLP and KOSFM）;Arash Ebrahimabadi，等;《Journal of Rock Mechanics and Geotechnical Engineering》;20150531;第7卷(第05期);全文 *

Also Published As

Publication number Publication date
CN111879314A (en) 2020-11-03

Similar Documents

Publication Publication Date Title
CN111879314B (en) Multi-sensor fusion roadway driving equipment real-time positioning system and method
CN105241444B (en) A kind of boom-type roadheader spatial pose automatic checkout system and its measurement method
CN109579831B (en) Visual auxiliary guide method and system for mining boom-type roadheader
CN102207382B (en) Pose measure system of cantilever type heading machine
CN109974715B (en) Tunneling machine autonomous navigation system and method combining strapdown inertial navigation and light spot identification
CN110736446B (en) Pose identification system and method for cantilever type heading machine
CN110736458B (en) Heading machine autonomous navigation system and method based on dead reckoning
US20160341041A1 (en) Mine vehicle and method of determining position and direction of monitored object
CN105737825B (en) A kind of cutting head of roadheader position measuring system
CN108398955B (en) Heading machine attitude control system and method
CN112414394A (en) Real-time positioning system and method for underground roadway driving equipment
CN102322857A (en) Position and posture measuring system and method for mechanical equipment
CN113075650A (en) Underground roadway tunneling equipment real-time positioning method based on UWB and inertial unit
CN104729501A (en) Rotating-sector-laser-based position and pose measurement method of cantilever excavator
CN105136134A (en) Heading machine position and posture detection and adjustment method and system
CN202066500U (en) Pose measuring system of cantilever type development machine
CN112720532B (en) Machine crowd is strutted to stable intelligent monitoring of country rock and precision
CN111121735A (en) Tunnel, subway and mine excavation tunneling autonomous positioning and orienting system and method
CN111637888A (en) Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement
CN110530358A (en) Car body navigation positional device and navigation system and method
CN112114327A (en) Coal mine tunnel drilling and anchoring robot precise positioning method and system based on multi-sensor fusion
CN110095135B (en) Method and device for positioning and orienting heading machine
CN113970329A (en) Strapdown inertial navigation and laser sensing combined heading machine pose detection system and method
CN113252063A (en) Excavation equipment depth measuring method based on total station
CN210268669U (en) Inertial navigation positioning system for underground mining mobile equipment

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