CN116626686A - Heading machine positioning and orientation method, system, equipment and storage medium - Google Patents
Heading machine positioning and orientation method, system, equipment and storage medium Download PDFInfo
- Publication number
- CN116626686A CN116626686A CN202211127259.3A CN202211127259A CN116626686A CN 116626686 A CN116626686 A CN 116626686A CN 202211127259 A CN202211127259 A CN 202211127259A CN 116626686 A CN116626686 A CN 116626686A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- roadway
- machine
- point cloud
- cloud data
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000006243 chemical reaction Methods 0.000 claims abstract description 17
- 238000012545 processing Methods 0.000 claims abstract description 17
- 238000011161 development Methods 0.000 claims abstract description 13
- 238000004590 computer program Methods 0.000 claims description 12
- 238000009434 installation Methods 0.000 claims description 9
- 230000005641 tunneling Effects 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 10
- 239000011159 matrix material Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000005065 mining Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 239000003245 coal Substances 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
Classifications
-
- 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
-
- 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/87—Combinations of systems using electromagnetic waves other than radio waves
-
- 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/88—Lidar systems specially adapted for specific applications
Abstract
The application discloses a heading machine positioning and orientation method, a system, equipment and a storage medium, wherein the method comprises the following steps: during the roadway traveling period of the development machine, each laser radar arranged on the development machine scans the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system, and the roadway three-dimensional point cloud data under each laser radar coordinate system is sent to a development machine terminal; the heading machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system; and the heading machine terminal determines pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
Description
Technical Field
The application relates to the technical field of engineering machinery, in particular to a positioning and orienting method, a system, equipment and a storage medium of a heading machine.
Background
The 3D laser radar continuously scans the external environment by transmitting high-frequency laser beams while rapidly rotating through the laser transmitting assembly, detects information such as the distance and reflectivity of a target object, provides guarantee for positioning, navigation, obstacle avoidance and the like, and mainly faces the fields such as unmanned automobile environment sensing, robot environment sensing, unmanned plane mapping and the like.
In the operation process of the coal mine heading machine, in order to heading in a preset roadway direction and realize automatic positioning and cutting, the position and the posture of the machine in the roadway need to be known. In the prior art, the positioning and orientation of the heading machine are mostly realized by a laser guiding technology, but equipment such as a base station and the like is required to be moved by an operator, and the full-automatic operation cannot be realized.
Disclosure of Invention
The technical problem solved by the scheme provided by the embodiment of the application is how to accurately and reliably realize the positioning and orientation of the heading machine, and the positioning and orientation can be fully automatically operated.
The positioning and orienting method of the heading machine provided by the embodiment of the application comprises the following steps:
during the roadway traveling period of the development machine, each laser radar arranged on the development machine scans the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system, and the roadway three-dimensional point cloud data under each laser radar coordinate system is sent to a development machine terminal;
the heading machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system;
and the heading machine terminal determines pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
According to the embodiment of the application, the positioning and orientation system of the heading machine comprises:
the system comprises a plurality of laser radars arranged on a heading machine, a plurality of driving units and a driving unit, wherein the laser radars are used for scanning the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system and transmitting the roadway three-dimensional point cloud data under each laser radar coordinate system to a heading machine terminal;
the heading machine terminal is used for carrying out coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system; and determining pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
According to an embodiment of the present application, an electronic device includes: a memory; a processor; a computer program; wherein the computer program is stored in the memory and is configured to be executed by the processor to implement a heading machine locating and orienting method.
A computer-readable storage medium according to an embodiment of the present application has a computer program stored thereon; the computer program is executed by the processor to implement a heading machine positioning and orientation method.
According to the scheme provided by the embodiment of the application, the tunnel is scanned by the multiple multi-wire-harness laser radars arranged at different positions on the tunnel, so that the dynamic real-time measurement of six-degree-of-freedom parameters of the tunnel under the tunnel coordinate system of the tunnel is realized, the realization mode is simple, and the result is accurate and reliable.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 is a schematic diagram of a positioning and orientation method of a heading machine provided by an embodiment of the application;
FIG. 2 is a schematic diagram of a coordinate system used in a positioning and orientation method of a heading machine according to an embodiment of the present application;
FIG. 3 is a schematic diagram of a laser radar installation position in a heading machine positioning and orientation method provided by an embodiment of the application;
fig. 4 is a schematic diagram of a lateral radar 1 scanning roadway side wall point cloud in a heading machine positioning and orientation method provided by an embodiment of the application;
fig. 5 is a schematic diagram of a plane model obtained after lateral radar 1 point cloud data is converted into a machine coordinate system in a machine xy coordinate system projection in the heading machine positioning and orientation method provided by the embodiment of the application;
fig. 6 is a schematic diagram of a plane model obtained after lateral radar 1 point cloud data is converted into a machine coordinate system in a machine xz coordinate system projection in a heading machine positioning and orientation method provided by an embodiment of the application;
fig. 7 is a schematic diagram of a plane model obtained after converting data of a point cloud of a top-facing radar into a machine coordinate system in a positioning and orientation method of a heading machine according to an embodiment of the present application in a machine yz coordinate system projection;
FIG. 8 is a flowchart of a heading machine positioning and orientation method provided by an embodiment of the application;
fig. 9 is a schematic diagram of a positioning and orientation system of a heading machine according to an embodiment of the application.
Detailed Description
The following detailed description of the preferred embodiments of the present application is provided in conjunction with the accompanying drawings, and it is to be understood that the preferred embodiments described below are merely illustrative and explanatory of the application, and are not restrictive of the application.
As shown in figure 1, the application establishes a tunnel coordinate system at the head-on position of the tunnel and establishes a machine coordinate system at the cutting rotation center of the heading machine. When the heading machine walks and heading in the tunnel, two multi-line laser radars arranged on the sides of the tunnel are used for scanning the sides of the two sides of the tunnel respectively, so that three-dimensional point cloud of the tunnel is obtained. And converting the three-dimensional point cloud data into a machine coordinate system through the pose relation between the laser radar and the machine coordinate system. And (3) processing by using a PCL plane model algorithm, and obtaining the horizontal direction offset distance, the azimuth angle and the roll angle of the heading machine under a roadway coordinate system according to related parameters. And scanning the tunnel roof by a multi-line laser radar arranged at the upper part of the heading machine to obtain the three-dimensional point cloud of the tunnel roof. And converting the three-dimensional point cloud data into a machine coordinate system through the pose relation between the top radar and the machine coordinate system. And calculating the height direction offset distance and the pitch angle of the heading machine under the tunnel coordinate system through a model algorithm and related parameters. And scanning the roadway head-on through a multi-beam laser radar arranged in front of the heading machine to obtain the heading direction distance of the heading machine in the roadway coordinate system. And extracting point cloud data to obtain the distance from the forward radar to the head on, and obtaining the distance from the machine to the head on according to the pose relation between the forward radar and the machine coordinate system. According to the method, the six-degree-of-freedom parameter dynamic real-time measurement of the heading machine under the tunnel coordinate system is realized through the multiple multi-beam laser radars, the realization mode is simple, and the result is accurate and reliable.
Fig. 8 is a flowchart of a positioning and orientation method of a heading machine according to an embodiment of the present application, as shown in fig. 8, including:
step S101: during the roadway traveling period of the development machine, each laser radar arranged on the development machine scans the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system, and the roadway three-dimensional point cloud data under each laser radar coordinate system is sent to a development machine terminal;
step S102: the heading machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system;
step S103: the heading machine terminal utilizes the roadway three-dimensional point cloud data under the machine coordinate system to determine pose parameters of the heading machine under the roadway coordinate system, and specifically, the pose parameters comprise azimuth angle, roll angle, pitch angle, X-direction offset distance, Y-direction offset distance and Z-direction offset distance.
Each laser radar arranged on the heading machine scans the roadway in real time, and the acquisition of roadway three-dimensional point cloud data under each laser radar coordinate system comprises the following steps: the left laser radar arranged at the left position of the heading machine scans the roadway in real time to obtain roadway left side slope point cloud data under a left laser radar coordinate system; the right laser radar arranged at the right position of the heading machine scans the roadway in real time to obtain roadway right side slope point cloud data under a right laser radar coordinate system; the top-oriented laser radar arranged at the top position of the heading machine scans the roadway in real time to obtain roadway top plate point cloud data under a top-oriented laser radar coordinate system; and the forward laser radar arranged at the forward position of the heading machine scans the roadway in real time to obtain roadway head-on point cloud data under a forward laser radar coordinate system.
Further, the tunneling machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system, and the obtaining of the roadway three-dimensional point cloud data under the machine coordinate system includes: the heading machine terminal performs coordinate coefficient data conversion processing on the roadway left side group point cloud data under the left laser radar coordinate system according to the pose relation between the left laser radar coordinate system and the machine coordinate system to obtain roadway left side group point cloud data under the machine coordinate system; the heading machine terminal performs coordinate coefficient data conversion processing on the roadway right side group point cloud data under the right laser radar coordinate system according to the pose relation between the right laser radar coordinate system and the machine coordinate system to obtain roadway right side group point cloud data under the machine coordinate system; the heading machine terminal performs coordinate coefficient data conversion processing on the roadway roof point cloud data under the top-oriented laser radar coordinate system according to the pose relation between the top-oriented laser radar coordinate system and the machine coordinate system to obtain the roadway roof point cloud data under the machine coordinate system; the machine coordinate system takes a cutting rotation center of the heading machine as an origin, takes the right direction in the plane of the heading machine as an x axis, takes the advancing direction of the heading machine as a y axis, takes the upward direction of the plane of the heading machine as a z axis, and moves along with the movement of the heading machine.
Specifically, the determining, by the heading machine terminal, pose parameters of the heading machine in the roadway coordinate system by using roadway three-dimensional point cloud data in the machine coordinate system includes: the heading machine terminal calculates a left azimuth angle, a left roll angle and a left offset distance of the heading machine in the X direction of the tunnel coordinate system according to the tunnel left side group point cloud data in the machine coordinate system; the heading machine terminal calculates a right azimuth angle, a right roll angle and a right offset distance of the heading machine in the X direction of the tunnel coordinate system according to the tunnel right side group point cloud data in the machine coordinate system; the heading machine terminal determines the azimuth angle of the heading machine under a tunnel coordinate system according to the left azimuth angle and the right azimuth angle, determines the roll angle of the heading machine under the tunnel coordinate system according to the left roll angle and the right roll angle, and determines the X-direction offset distance of the heading machine under the tunnel coordinate system according to the left offset distance of the X-direction and the right offset distance of the X-direction.
Specifically, the determining, by the heading machine terminal, pose parameters of the heading machine in the roadway coordinate system by using roadway three-dimensional point cloud data in the machine coordinate system includes: and the heading machine terminal respectively determines a pitch angle and a Z-direction offset distance of the heading machine under the tunnel coordinate system according to the tunnel roof point cloud data under the machine coordinate system.
Specifically, the determining, by the heading machine terminal, pose parameters of the heading machine in the roadway coordinate system by using roadway three-dimensional point cloud data in the machine coordinate system includes: the heading machine terminal obtains the distance from the forward laser radar to the roadway head-on according to the roadway head-on point cloud data under the forward laser radar coordinate system; the heading machine terminal obtains the installation distance of the forward laser radar in the Y direction of the machine coordinate system, and determines the Y-direction offset distance of the heading machine in the roadway coordinate system by using the installation distance and the distance from the forward laser radar to the roadway head-on.
Fig. 9 is a schematic diagram of a positioning and orientation system of a heading machine according to an embodiment of the present application, as shown in fig. 9, including: the plurality of laser radars 201 are arranged on the heading machine and are used for scanning the roadway in real time to obtain roadway three-dimensional point cloud data under each laser radar coordinate system and sending the roadway three-dimensional point cloud data under each laser radar coordinate system to a heading machine terminal; the heading machine terminal 202 is configured to perform coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relationship between each laser radar coordinate system and the machine coordinate system, so as to obtain roadway three-dimensional point cloud data under the machine coordinate system; and determining pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
Wherein the plurality of lidars comprises: the left laser radar is arranged at the left position of the heading machine and is used for scanning the roadway in real time to obtain roadway left side slope point cloud data under a left laser radar coordinate system; the right laser radar is arranged at the right position of the heading machine and is used for scanning the roadway in real time to obtain roadway right side slope point cloud data under a right laser radar coordinate system; the top-oriented laser radar is arranged at the top position of the heading machine and is used for scanning the roadway in real time to obtain roadway top plate point cloud data under a top-oriented laser radar coordinate system; the forward laser radar is arranged at the forward position of the heading machine and is used for scanning the roadway in real time to obtain roadway head-on point cloud data under a forward laser radar coordinate system.
An electronic device provided by an embodiment of the present application includes: a memory; a processor; a computer program; wherein the computer program is stored in the memory and is configured to be executed by the processor to implement a heading machine locating and orienting method.
A computer-readable storage medium provided by an embodiment of the present application has a computer program stored thereon; the computer program is executed by the processor to implement a heading machine positioning and orientation method.
Specifically, the heading machine positioning and orientation method based on the 3D laser radar provided by the application comprises the following steps:
firstly, establishing a tunnel coordinate system at the head of a tunnel, and establishing a machine coordinate system at a cutting rotation center of a heading machine;
the tunnel coordinate system is established at the head-on position, and the origin of coordinates is positioned on the center line of the tunnel and is equal to the rotation center of the machine in height. A machine coordinate system is established at the cutting center of revolution, the machine coordinate system moving as the machine moves.
Scanning the roadway by using a plurality of multi-beam laser radars arranged on the machine when the heading machine walks in the roadway, and obtaining a three-dimensional point cloud of the roadway;
the development machine at least comprises two lateral radars which are used for acquiring cloud data of lateral sides of two sides of a roadway. And the top radar is used for acquiring the point cloud data of the roadway roof. And the forward radar is used for acquiring roadway head-on point cloud data.
The roadway three-dimensional point cloud acquired by each laser radar is coordinate data of the roadway in a radar coordinate system.
Thirdly, converting the three-dimensional point cloud under the radar coordinate system into a machine coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system;
the pose relation between each laser radar coordinate system and the machine coordinate system is determined by a radar installation mode.
Step four, according to the point cloud data converted into the machine coordinate system, obtaining the six-degree-of-freedom pose of the heading machine relative to the roadway coordinate system through a PCL library correlation algorithm, wherein the six-degree-of-freedom pose specifically comprises the following steps: converting lateral Lei Dadian cloud data into a machine coordinate system through the pose relation between the lateral radar coordinate system and the machine coordinate system, and obtaining azimuth angle, roll angle and lateral offset distance (X-direction offset distance) of the machine under a roadway coordinate system through a PCL plane model algorithm; converting the top radar point cloud data into a machine coordinate system through the pose relation between the top radar coordinate system and the machine coordinate system, and obtaining a pitch angle and a top offset distance (Z-direction offset distance) of the machine under a roadway coordinate system through a PCL plane model or a cylindrical model algorithm; and (3) extracting point cloud data to obtain the distance from the forward radar to the head-on, and obtaining the distance from the machine to the head-on (Y-direction offset distance) according to the pose relation between the forward radar coordinate system and the machine coordinate system.
Fig. 2 is a schematic diagram of a coordinate system adopted in the positioning and orientation method of the heading machine according to an embodiment of the present application, where 1 is a roadway coordinate system and 2 is a machine coordinate system. As can be seen from fig. 2, the tunnel coordinate system XYZ is established at the head-on position, the origin of coordinates is located at the center line of the tunnel and is at the same height as the center of rotation of the machine, the horizontal right direction is the X axis, the tunneling direction is the Y axis, and the Z axis is the Z axis. The tunnel coordinate system belongs to a local coordinate system and changes along with mining and fluctuation of a tunnel; the machine coordinate system xyz takes the cutting rotation center as an origin, takes the right direction in the machine plane as an x axis, takes the machine advancing direction as a y axis and takes the vertical machine plane as a z axis, and moves along with the movement of the machine.
As shown in fig. 3, the tunnel is scanned by a plurality of multi-beam laser radars installed on the machine when the heading machine walks in the tunnel, and a three-dimensional point cloud of the tunnel is obtained. Specifically, the embodiment includes two lateral radars 1 (right laser radar) and 2 (left laser radar) for acquiring cloud data of lateral sides of two sides of a roadway. And the roof radar 3 is used for acquiring the point cloud data of the roadway roof. And the forward radar 4 is used for acquiring roadway head-on point cloud data. The roadway three-dimensional point cloud acquired by each laser radar is coordinate data of the roadway in a radar coordinate system.
As shown in fig. 4, in the embodiment of the present application, a roadway side wall Ping Miandian cloud acquired by a lateral radar 1 installed on the right side of a machine is obtained, and point cloud data acquired by the lateral radar 1 is converted into a machine coordinate system, where the conversion relationship is as follows:
P＝R·P 0 +t (1)
wherein: p is roadway side wall three-dimensional point cloud data converted to a machine coordinate system;
P 0 the roadway side wall three-dimensional point cloud data in a laser radar coordinate system;
r is an attitude rotation matrix of a machine coordinate system relative to a radar coordinate system;
t is a translation vector of the machine coordinate system relative to the radar coordinate system.
The machine installation position is known from the lateral radar 1:
R＝R y R x R z (2)
wherein: ry= [ cos γ0sin γ; 0.1; -sin gamma 0cos gamma, gamma being the rotation angle of the radar coordinate system around the y-axis of the machine coordinate system;
rx= [ 10 0;0cos beta-sin beta; 0sin beta cos beta, beta is the rotation angle of the radar coordinate system around the x axis of the machine coordinate system;
rz= [ cos α -sin α0; sin αcos α0;0 0.1 ], α is the z-axis rotation angle of the radar coordinate system about the machine coordinate system.
t＝[t x ,t y ,t z ] (3)
Wherein: t is t x /t y /t z For the mounting distance of the lateral radar 1 in the x/y/z direction of the machine coordinate system, respectively.
And (3) dividing the point cloud data P converted into a machine coordinate system through a PCL plane model to obtain a roadway side wall plane equation:
a1·x+b1·y+c1·z+d1＝0 (4)
wherein: a1/b1/c1/d1 is the plane equation coefficient.
As shown in fig. 5, the projection of the plane model of formula (4) on the xy plane of the machine coordinate system is shown as the plane model top projection equation:
a1·x+b1·y+d1＝0 (5)
slope k of projection straight line xy = -a1/b1, when k xy >At 0, the straight line inclination angle θ xy =arctan (-a 1/b 1); conversely, θ xy ＝π+arctan(-a1/b1)。
As can be seen from FIG. 5, θ xy =pi/2+yaw1, resulting in azimuth:
yaw1＝θ xy -π/2 (6)
as shown in fig. 6, for projection of the plane model of formula (4) on the machine coordinate system xz plane, the front projection equation of the plane model is:
a1·x+c1·z+d1＝0 (7)
slope k of projection straight line xz = -a1/c1, when k xz >At 0, the straight line inclination angle θ xz =arctan (-a 1/b 1); conversely, θ xz ＝π+arctan(-a1/b1)。
As can be seen from FIG. 6, θ xz Pi/2-roll 1, resulting in roll angle:
roll1＝π/2-θ xz (8)
as can be seen from the formula (4), the machine coordinate origin-to-lateral-plane distance macdris1= |d1|.
The lateral radar 2 is used for acquiring left roadway lateral slope point cloud data, and the processing mode is the same as that of the radar 1, so that yaw2, roll2 and macdris 2 can be obtained.
Calculating the azimuth angle of the heading machine relative to a roadway coordinate system through the radar 1 and the radar 2:
yaw＝(yaw1+yaw2)/2 (9)
likewise, the heading machine rolls angle relative to the tunnel coordinate system:
roll＝(roll1+roll2)/2 (10)
offset distance of the heading machine relative to the tunnel coordinate system in X direction:
macdisX＝(macdis1+macdis2)/2-macdis1 (11)
and (3) performing pose conversion on the point cloud data obtained by the top radar 3 through the formula (1-3) to obtain roadway roof point cloud data under a machine coordinate system. Wherein the attitude rotation matrix and the translation vector are determined by the top-facing radar in a machine-mounted manner.
And (3) dividing the point cloud data converted into a machine coordinate system through a PCL plane model to obtain a roadway roof plane equation:
a3·x+b3·y+c3·z+d3＝0 (12)
wherein: a3/b3/c3/d3 is the plane equation coefficient.
The z-direction offset distance of the machine coordinate system relative to the roadway coordinate system can be obtained according to the formula (12):
macdisZ＝H-H0-|d3| (13)
wherein: h is the design height of the roadway, and H0 is the rotation center height of the machine.
As shown in fig. 7, the tunnel roof plane model is projected on the yz plane of the machine coordinate system, and the side view projection equation of the plane model is:
b3·y+c3·z+d3＝0 (14)
slope k of projection straight line yz = -c3/b3, when k yz >At 0, the straight line inclination angle θ yz ＝arctan(-c3/b3)；Conversely, θ yz ＝π+arctan(-c3/b3)。
From the projection relationship, θ yz =pi/2+pitch, resulting in pitch angle:
pitch＝θ yz -π/2 (15)
the forward radar 4 is used for acquiring roadway head-on point cloud data, and the distance between the radar and the roadway head-on can be obtained by screening the point cloud data in a specific angle range, and the distance between a machine coordinate dot and the roadway coordinate system Y direction can be obtained by combining the forward radar at the machine installation position:
macdisY＝(distY+lidarY)·cos(yaw) (16)
wherein: distY is the distance right ahead of the radar obtained by extracting the head-on point cloud acquired by the forward radar 4;
lidarY is the installation distance of the forward radar 4 in the Y direction of the machine coordinate system;
yaw is the azimuth of the machine in the roadway coordinate system obtained by the formula (9).
According to the scheme provided by the embodiment of the application, the 3D laser radar is used as a basis, so that the positioning and orientation method of the mining heading machine based on the 3D laser radar is realized, the six-degree-of-freedom pose of the heading machine in a roadway coordinate system can be effectively determined in real time, and the automation of the tunneling operation is facilitated.
Although the present application has been described in detail hereinabove, the present application is not limited thereto and various modifications may be made by those skilled in the art in accordance with the principles of the present application. Therefore, all modifications made in accordance with the principles of the present application should be understood as falling within the scope of the present application.
Claims (10)
1. The heading machine positioning and orientation method is characterized by comprising the following steps of:
during the roadway traveling period of the development machine, each laser radar arranged on the development machine scans the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system, and the roadway three-dimensional point cloud data under each laser radar coordinate system is sent to a development machine terminal;
the heading machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system;
and the heading machine terminal determines pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
2. The method of claim 1, wherein the pose parameters include azimuth, roll angle, pitch angle, X-direction offset distance, Y-direction offset distance, and Z-direction offset distance.
3. The method according to claim 2, wherein the step of obtaining roadway three-dimensional point cloud data under each laser radar coordinate system by scanning the roadway in real time by each laser radar arranged on the heading machine comprises:
the left laser radar arranged at the left position of the heading machine scans the roadway in real time to obtain roadway left side slope point cloud data under a left laser radar coordinate system;
the right laser radar arranged at the right position of the heading machine scans the roadway in real time to obtain roadway right side slope point cloud data under a right laser radar coordinate system;
the top-oriented laser radar arranged at the top position of the heading machine scans the roadway in real time to obtain roadway top plate point cloud data under a top-oriented laser radar coordinate system;
and the forward laser radar arranged at the forward position of the heading machine scans the roadway in real time to obtain roadway head-on point cloud data under a forward laser radar coordinate system.
4. The method of claim 3, wherein the tunneling machine terminal performs coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relationship between each laser radar coordinate system and the machine coordinate system, and the obtaining roadway three-dimensional point cloud data under the machine coordinate system includes:
the heading machine terminal performs coordinate coefficient data conversion processing on the roadway left side group point cloud data under the left laser radar coordinate system according to the pose relation between the left laser radar coordinate system and the machine coordinate system to obtain roadway left side group point cloud data under the machine coordinate system;
the heading machine terminal performs coordinate coefficient data conversion processing on the roadway right side group point cloud data under the right laser radar coordinate system according to the pose relation between the right laser radar coordinate system and the machine coordinate system to obtain roadway right side group point cloud data under the machine coordinate system;
the heading machine terminal performs coordinate coefficient data conversion processing on the roadway roof point cloud data under the top-oriented laser radar coordinate system according to the pose relation between the top-oriented laser radar coordinate system and the machine coordinate system to obtain the roadway roof point cloud data under the machine coordinate system;
the machine coordinate system takes a cutting rotation center of the heading machine as an origin, takes the right direction in the plane of the heading machine as an x axis, takes the advancing direction of the heading machine as a y axis, takes the upward direction of the plane of the heading machine as a z axis, and moves along with the movement of the heading machine.
5. The method of claim 4, wherein the heading machine terminal determining pose parameters of the heading machine in the roadway coordinate system using roadway three-dimensional point cloud data in the machine coordinate system comprises:
the heading machine terminal calculates a left azimuth angle, a left roll angle and a left offset distance of the heading machine in the X direction of the tunnel coordinate system according to the tunnel left side group point cloud data in the machine coordinate system;
the heading machine terminal calculates a right azimuth angle, a right roll angle and a right offset distance of the heading machine in the X direction of the tunnel coordinate system according to the tunnel right side group point cloud data in the machine coordinate system;
the heading machine terminal determines the azimuth angle of the heading machine under a tunnel coordinate system according to the left azimuth angle and the right azimuth angle, determines the roll angle of the heading machine under the tunnel coordinate system according to the left roll angle and the right roll angle, and determines the X-direction offset distance of the heading machine under the tunnel coordinate system according to the left offset distance of the X-direction and the right offset distance of the X-direction.
6. The method of claim 4, wherein the heading machine terminal determining pose parameters of the heading machine in the roadway coordinate system using roadway three-dimensional point cloud data in the machine coordinate system comprises:
and the heading machine terminal respectively determines a pitch angle and a Z-direction offset distance of the heading machine under the tunnel coordinate system according to the tunnel roof point cloud data under the machine coordinate system.
7. The method of claim 4, wherein the heading machine terminal determining pose parameters of the heading machine in the roadway coordinate system using roadway three-dimensional point cloud data in the machine coordinate system comprises:
the heading machine terminal obtains the distance from the forward laser radar to the roadway head-on according to the roadway head-on point cloud data under the forward laser radar coordinate system;
the heading machine terminal obtains the installation distance of the forward laser radar in the Y direction of the machine coordinate system, and determines the Y-direction offset distance of the heading machine in the roadway coordinate system by using the installation distance and the distance from the forward laser radar to the roadway head-on.
8. A heading machine positioning and orientation system, comprising:
the system comprises a plurality of laser radars arranged on a heading machine, a plurality of driving units and a driving unit, wherein the laser radars are used for scanning the roadway in real time to acquire roadway three-dimensional point cloud data under each laser radar coordinate system and transmitting the roadway three-dimensional point cloud data under each laser radar coordinate system to a heading machine terminal;
the heading machine terminal is used for carrying out coordinate coefficient data conversion processing on the roadway three-dimensional point cloud data under each laser radar coordinate system according to the pose relation between each laser radar coordinate system and the machine coordinate system to obtain roadway three-dimensional point cloud data under the machine coordinate system; and determining pose parameters of the heading machine under the tunnel coordinate system by utilizing the tunnel three-dimensional point cloud data under the machine coordinate system.
9. An electronic device, comprising: a memory; a processor; a computer program; wherein the computer program is stored in the memory and configured to be executed by the processor to implement the method of any of claims 1-7.
10. A computer-readable storage medium, characterized in that a computer program is stored thereon; the computer program being executed by a processor to implement the method of any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211127259.3A CN116626686A (en) | 2022-09-16 | 2022-09-16 | Heading machine positioning and orientation method, system, equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211127259.3A CN116626686A (en) | 2022-09-16 | 2022-09-16 | Heading machine positioning and orientation method, system, equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116626686A true CN116626686A (en) | 2023-08-22 |
Family
ID=87637095
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211127259.3A Pending CN116626686A (en) | 2022-09-16 | 2022-09-16 | Heading machine positioning and orientation method, system, equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116626686A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117270533A (en) * | 2023-09-20 | 2023-12-22 | 河北伊联智能科技有限公司 | Automatic correction system of heading machine based on laser scanning |
-
2022
- 2022-09-16 CN CN202211127259.3A patent/CN116626686A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117270533A (en) * | 2023-09-20 | 2023-12-22 | 河北伊联智能科技有限公司 | Automatic correction system of heading machine based on laser scanning |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022165973A1 (en) | Three-dimensional scanning method and system, electronic device, and computer equipment | |
| CN108286949B (en) | Movable three-dimensional detection robot system | |
| CN113311873B (en) | Unmanned aerial vehicle servo tracking method based on vision | |
| JP2004508954A (en) | Positioning device and system | |
| CN109407111B (en) | Method for identifying characteristics of tunnel three-dimensional scanner | |
| CN110700839B (en) | Heading machine pose measuring device based on laser scanner and measuring method thereof | |
| CN111156974B (en) | Heading machine pose measurement system based on laser target | |
| CN104793637A (en) | Real-time tracking system and method of mobile equipment | |
| CN111189436A (en) | Heading machine position and attitude measuring system based on laser target tracking | |
| WO2020125732A1 (en) | Zero calibration method and device for robot arm joint sensor | |
| CN112114327B (en) | Coal mine tunnel drilling and anchoring robot precise positioning method and system based on multi-sensor fusion | |
| CN116626686A (en) | Heading machine positioning and orientation method, system, equipment and storage medium | |
| EP4300237A1 (en) | Robot traveling method and apparatus, and electronic device, storage medium and program product | |
| CN111156976B (en) | Position and attitude data measuring system and method for development machine | |
| CN112197756A (en) | Heading machine pose detection system and method based on mechanical arm visual perception | |
| CN115980680A (en) | Calibration method and device | |
| CN111638487B (en) | Automatic parking test equipment and method | |
| CN106646498A (en) | Method for measuring lateral shift of tunneling machine | |
| CN111396047B (en) | Measuring and positioning system and method for coal face equipment group | |
| CN116182743A (en) | Laser radar-based coal mine underground tunneling roadway profile marking system | |
| CN114147723B (en) | Automatic lofting robot system and operation method thereof | |
| CN112857367B (en) | Heading machine pose detection method based on machine vision and inertial navigation | |
| CN115328122B (en) | Unmanned ship obstacle avoidance method and system based on multi-radar sensing | |
| CN116704019A (en) | Drilling and anchoring robot monocular vision positioning method based on anchor rod network | |
| CN114859366A (en) | Positioning and orienting method and device for heading 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 |