CN118067136A - Positioning method, positioning system, electronic device, storage medium and chip - Google Patents
Positioning method, positioning system, electronic device, storage medium and chip Download PDFInfo
- Publication number
- CN118067136A CN118067136A CN202410179920.8A CN202410179920A CN118067136A CN 118067136 A CN118067136 A CN 118067136A CN 202410179920 A CN202410179920 A CN 202410179920A CN 118067136 A CN118067136 A CN 118067136A
- Authority
- CN
- China
- Prior art keywords
- data
- roadway
- laser radar
- positioning
- operation device
- 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 56
- 238000001914 filtration Methods 0.000 claims abstract description 45
- 230000004927 fusion Effects 0.000 claims abstract description 43
- 238000006243 chemical reaction Methods 0.000 claims description 9
- 238000004891 communication Methods 0.000 claims description 5
- 238000004590 computer program Methods 0.000 claims description 3
- 230000005641 tunneling Effects 0.000 claims description 2
- 239000000428 dust Substances 0.000 description 21
- 238000012545 processing Methods 0.000 description 19
- 230000006870 function Effects 0.000 description 11
- 230000001133 acceleration Effects 0.000 description 4
- 238000001514 detection method Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000035945 sensitivity Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
Landscapes
- Traffic Control Systems (AREA)
- Navigation (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
The invention provides a positioning method, a positioning system, electronic equipment, a storage medium and a chip, and relates to the technical field of underground operation equipment, wherein the positioning method comprises the following steps: acquiring first data through a laser radar of the underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system; determining whether the lidar is disabled; under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway; and under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
Description
Technical Field
The invention relates to the technical field of underground operation equipment, in particular to a positioning method, a positioning system, electronic equipment, a storage medium and a chip.
Background
In the related art, a positioning system of the heading machine tracks photoelectric signals sent by a reflecting prism in real time through a total station, so that the position of the heading machine in a roadway is determined. This approach is greatly affected by the visibility downhole and dust concentration, and is not highly automated.
Disclosure of Invention
In order to solve or improve the technical problems that a heading machine positioning system is greatly influenced by underground visibility and dust concentration and has low automation degree, the invention aims to provide a positioning method for underground operation equipment.
It is another object of the present invention to provide a downhole operation device positioning system.
Another object of the present invention is to provide an electronic device.
It is another object of the present invention to provide a computer readable storage medium.
It is another object of the present invention to provide a chip.
To achieve the above object, a first aspect of the present invention provides a downhole operation device positioning method, the downhole operation device positioning method being implemented by a downhole operation device positioning system, the downhole operation device positioning method comprising: acquiring first data through a laser radar of the underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system; determining whether the lidar is disabled; under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway; and under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
According to the technical scheme of the underground operation equipment positioning method, the underground operation equipment positioning method can determine the data fusion mode according to whether the laser radar fails or not, the positioning result is more accurate, the automation degree is high, and the underground operation equipment positioning method is not easily influenced by underground visibility and dust concentration.
The underground operation equipment positioning method is realized through an underground operation equipment positioning system. Optionally, the downhole operation device positioning system comprises a device body, a laser radar, a millimeter wave radar, an inertial navigation system, and a controller. The device body mainly plays a role of a mounting carrier for a laser radar, a millimeter wave radar, an inertial navigation system and a controller. It should be noted that the device body is a heading machine, and of course, other types of devices are also possible. Further, the laser radar is provided in the apparatus body. The function of the lidar is to determine the position of the device body in the roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Further, the millimeter wave radar is provided to the apparatus body. The millimeter wave radar is used for roughly determining the position of the equipment body in the roadway in a dust environment. Further, the inertial navigation system is arranged on the equipment body. An inertial navigation system (INS, inertial navigation for short) is an autonomous navigation system that does not depend on external information nor radiate energy to the outside. The inertial navigation system is used for roughly determining the position of the equipment body in the roadway and providing angle information of the first coordinate system and the second coordinate system. Optionally, the first coordinate system is a heading machine coordinate system; the second coordinate system is a northeast day coordinate system. In the northeast coordinate system, the X axis is directed to the east, the Y axis is directed to the north, and the Z axis is directed to the zenith.
Further, the controller is arranged on the equipment body. The controller is electrically connected with the laser radar. The controller is electrically connected with the millimeter wave radar. The controller is electrically connected with the inertial navigation system. The controller is configured to perform the steps of the downhole operation device positioning method.
Specifically, the method for positioning the downhole operation equipment comprises the following steps:
The method comprises the steps of acquiring first data through a laser radar of an underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system.
Optionally, the first data is point cloud data. The function of the lidar is to determine the position of the device body in the roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Optionally, the second data is point cloud data. The millimeter wave radar is used for roughly determining the position of the equipment body in the roadway in a dust environment. Optionally, the inertial navigation system comprises an accelerometer. The third data includes acceleration of the device body. Optionally, the inertial navigation system comprises a gyroscope. The third data includes angular velocity.
And a second step of determining whether the laser radar fails.
Determining whether the visibility in the well is greater than a first threshold, and whether the number of point clouds in the first data with the reflection intensity greater than a second threshold is less than a third threshold to determine whether the lidar fails.
And thirdly, under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
And under the condition that the visibility in the pit is larger than a first threshold value and/or the number of point clouds with the reflection intensity larger than a second threshold value in the first data is smaller than a third threshold value, judging that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. Kalman filtering is an algorithm for optimally estimating the state of a system by using a linear system state equation and through system input and output observation data. The optimal estimate can also be considered as a filtering process, since the observed data includes the effects of noise and interference in the system.
And fourthly, under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
Under the condition that the visibility in the pit is not greater than a first threshold value and the number of point clouds with the reflection intensity greater than a second threshold value in the first data is not less than a third threshold value, judging that the laser radar is not invalid, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway.
According to the technical scheme, the positioning method of the underground operation equipment can determine the data fusion mode according to whether the laser radar fails or not, the positioning result is more accurate, the automation degree is high, and the underground operation equipment is not easily influenced by underground visibility and dust concentration.
In addition, the technical scheme provided by the invention can also have the following additional technical characteristics:
In some embodiments, optionally, determining whether the lidar is failed comprises: determining whether the visibility in the well is greater than a first threshold, and whether the number of point clouds in the first data with the reflection intensity greater than a second threshold is less than a third threshold to determine whether the lidar fails.
In this technical solution, the step of determining whether the lidar has failed includes:
Determining whether the visibility in the well is greater than a first threshold, and whether the number of point clouds in the first data with the reflection intensity greater than a second threshold is less than a third threshold to determine whether the lidar fails. Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. Under the condition that two conditions are met simultaneously, the environment where the equipment body is located is determined to be an ideal environment, and the laser radar is not disabled. And under the condition that at least one condition is not met, determining that the environment where the equipment body is positioned is not dust-free, and at the moment, the laser radar is invalid.
In some technical solutions, optionally, in the case of failure of the laser radar, performing data fusion on the second data and the third data through kalman filtering, to determine a position of the downhole operation device in the roadway, including: and under the condition that the visibility in the pit is larger than a first threshold value and/or the number of point clouds is smaller than a third threshold value, the laser radar fails, the second data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in the roadway is determined.
In this technical solution, the step of performing data processing in the event of a failure of the lidar includes:
And under the condition that the visibility in the pit is larger than a first threshold value and/or the number of point clouds is smaller than a third threshold value, the laser radar fails, the second data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in the roadway is determined. Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. And under the condition that at least one condition is not met, determining that the environment where the equipment body is positioned is not dust environment, wherein the laser radar is invalid, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. Because millimeter wave radar is less influenced by dust environment, the accuracy of the positioning result can be improved by the design mode under the dust environment.
In some technical solutions, optionally, under the condition that the laser radar does not fail, performing data fusion on the first data and the third data through kalman filtering, and determining a position of the downhole operation device in the roadway includes: under the condition that the visibility in the pit is not greater than a first threshold value and the number of point clouds is not less than a third threshold value, the laser radar is not invalid, the first data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in a roadway is determined.
In this technical solution, the step of performing data processing in the case that the lidar is not disabled includes:
Under the condition that the visibility in the pit is not greater than a first threshold value and the number of point clouds is not less than a third threshold value, the laser radar is not invalid, the first data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in a roadway is determined. Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. Under the condition that two conditions are met simultaneously, determining that the environment where the equipment body is located is an ideal environment, at the moment, the laser radar is not invalid, performing data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. In an ideal environment (environment with low dust concentration), the laser radar determines the position of the equipment body in the roadway by scanning the target in the roadway, and compared with a mode of adopting a millimeter wave radar, the positioning result is more accurate.
In some embodiments, optionally, the first data comprises point cloud data; and/or the second data comprises first distance data, wherein the first distance data comprises the distance between the millimeter wave radar and the tunneling surface of the roadway and the distance between the millimeter wave radar and the two side walls of the roadway; and/or the third data comprises first angle data of the downhole operation device.
In the technical scheme, first data, namely point cloud data of a scanning roadway, are acquired through a laser radar. Optionally, the point cloud data includes coordinates of a target point cloud. Optionally, the point cloud data includes a reflected intensity. Optionally, the point cloud data comprises second distance data. The second distance data includes a distance between the lidar and a driving surface of the roadway and a distance between the lidar and two sidewalls of the roadway. Optionally, the point cloud data comprises second angle data. The second angle data is the installation angle data of the laser radar.
Alternatively, the second data, i.e., the first distance data, is acquired by the millimeter wave radar. The first distance data includes a distance between the millimeter wave radar and a driving surface of the roadway and a distance between the millimeter wave radar and two side walls of the roadway.
Optionally, the third data, i.e. the first angle data of the downhole operation device, is acquired by means of an inertial navigation system. Optionally, the first angle data is attitude angle data, including yaw angle, pitch angle, and roll angle.
In some embodiments, optionally, the method for positioning a downhole operation device further comprises: before determining whether the laser radar fails, establishing a laser radar coordinate system, a millimeter wave radar coordinate system and a roadway coordinate system according to the first data, the second data and the third data; under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway, wherein the method comprises the following steps: under the condition that the laser radar fails, carrying out coordinate conversion through Kalman filtering according to a millimeter wave radar coordinate system, a roadway coordinate system and third data to determine the position of underground operation equipment in a roadway; under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway, wherein the method comprises the following steps: and under the condition that the laser radar does not fail, carrying out coordinate conversion through Kalman filtering according to a laser radar coordinate system, a roadway coordinate system and third data, and determining the position of underground operation equipment in the roadway.
In the technical scheme, the method for positioning the underground operation equipment further comprises the following specific steps:
Before determining whether the laser radar fails, establishing a laser radar coordinate system, a millimeter wave radar coordinate system and a roadway coordinate system according to the first data, the second data and the third data. Optionally, a target is arranged in the tunnel, and a tunnel coordinate system taking the center of the target as the origin of coordinates is established. Optionally, a lidar coordinate system is established with the center of the lidar as the origin of coordinates. Optionally, a millimeter wave radar coordinate system with the center of the millimeter wave radar as the origin of coordinates is established.
The specific steps of determining the position of the downhole operation equipment in the roadway according to the second data and the third data comprise:
Under the condition that the laser radar fails, coordinate conversion is carried out through Kalman filtering according to a millimeter wave radar coordinate system, a roadway coordinate system and third data, and the position of underground operation equipment in the roadway is determined. Optionally, the third data comprises first angle data of the downhole operation device. The first angle data is attitude angle data including yaw angle, pitch angle, and roll angle.
The specific steps of determining the position of the downhole operation equipment in the roadway according to the first data and the third data comprise:
And under the condition that the laser radar does not fail, carrying out coordinate conversion through Kalman filtering according to a laser radar coordinate system, a roadway coordinate system and third data, and determining the position of underground operation equipment in the roadway. Optionally, the third data comprises first angle data of the downhole operation device. The first angle data is attitude angle data including yaw angle, pitch angle, and roll angle.
A second aspect of the present invention provides a downhole operation device positioning system comprising: an equipment body; the laser radar is arranged on the equipment body; the millimeter wave radar is arranged on the equipment body; the inertial navigation system is arranged on the equipment body; the controller is arranged on the equipment body, is electrically connected with the laser radar, is electrically connected with the millimeter wave radar, is electrically connected with the inertial navigation system and is used for executing the steps of the underground operation equipment positioning method in any technical scheme.
According to the technical scheme of the underground operation equipment positioning system, the underground operation equipment positioning system comprises an equipment body, a laser radar, a millimeter wave radar, an inertial navigation system and a controller. The device body mainly plays a role of a mounting carrier for a laser radar, a millimeter wave radar, an inertial navigation system and a controller. It should be noted that the device body is a heading machine, and of course, other types of devices are also possible. Further, the laser radar is provided in the apparatus body. The function of the lidar is to determine the position of the device body in the roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Further, the millimeter wave radar is provided to the apparatus body. The millimeter wave radar is used for roughly determining the position of the equipment body in the roadway in a dust environment. Further, the inertial navigation system is arranged on the equipment body. An inertial navigation system (INS, inertial navigation for short) is an autonomous navigation system that does not depend on external information nor radiate energy to the outside. The inertial navigation system is used for roughly determining the position of the equipment body in the roadway and providing angle information of the first coordinate system and the second coordinate system. Optionally, the first coordinate system is a heading machine coordinate system; the second coordinate system is a northeast day coordinate system. In the northeast coordinate system, the X axis is directed to the east, the Y axis is directed to the north, and the Z axis is directed to the zenith.
Further, the controller is arranged on the equipment body. The controller is electrically connected with the laser radar, and the controller obtains first data through the laser radar. The controller is electrically connected with the millimeter wave radar, and the controller obtains second data through the millimeter wave radar. The controller is electrically connected with the inertial navigation system, and the controller obtains third data through the inertial navigation system. The controller is configured to perform the steps of the method for positioning a downhole operation device according to any of the above aspects.
A third aspect of the present invention provides an electronic device comprising a processor, a memory and a program or instruction stored on the memory and executable on the processor, the program or instruction when executed by the processor implementing the steps of the method for locating a downhole operation device of any of the above aspects.
A fourth aspect of the invention provides a computer readable storage medium storing a computer program which when executed by a processor performs the steps of the method of locating a downhole operation device of any of the above aspects.
A fifth aspect of the present invention provides a chip comprising a processor and a communication interface, the communication interface being coupled to the processor, the processor being configured to execute programs or instructions to implement the steps of the method for locating a downhole operation device of any of the above-described aspects.
Additional aspects and advantages of the present invention will be made apparent from the description which follows, or may be learned by practice of the invention.
Drawings
FIG. 1 illustrates one of the flowcharts of a method of locating a downhole operation device according to one embodiment of the invention;
FIG. 2 illustrates a second flow chart of a method of locating a downhole operation device according to one embodiment of the invention;
FIG. 3 illustrates one of the block diagrams of the downhole operation device positioning system according to one embodiment of the invention;
FIG. 4 illustrates a second block diagram of a downhole operation device positioning system according to one embodiment of the invention.
The correspondence between the reference numerals and the component names in fig. 3 and 4 is:
300: a downhole operation equipment positioning system; 310: an equipment body; 320: a laser radar; 330: millimeter wave radar; 340: an inertial navigation system; 341: an accelerometer; 342: a gyroscope; 350: a controller; 351: a first data processing module; 352: a second data processing module; 353: an analysis module; 354: an anomaly detection module; 355: a space-time registration module; 356: a kalman fusion module; 357: and a data output module.
Detailed Description
In order that the above-recited objects, features and advantages of embodiments of the present invention can be more clearly understood, a further detailed description of embodiments of the present invention will be rendered by reference to the appended drawings and detailed description thereof. It should be noted that, without conflict, the embodiments of the present invention and features in the embodiments may be combined with each other.
In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but embodiments of the invention may be practiced otherwise than as described herein, and therefore the scope of the invention is not limited to the specific embodiments disclosed below.
Positioning methods, positioning systems, electronic devices, storage media, and chips provided according to some embodiments of the present invention are described below with reference to fig. 1 through 4.
In one embodiment according to the invention, the downhole operation device positioning method is implemented by the downhole operation device positioning system 300. Alternatively, as shown in FIG. 3, the downhole operation device positioning system 300 includes a device body 310, a lidar 320, a millimeter wave radar 330, an inertial navigation system 340, and a controller 350. The device body 310 mainly plays a role of a mounting carrier for the laser radar 320, the millimeter wave radar 330, the inertial navigation system 340, and the controller 350. It should be noted that, the device body 310 is a heading machine, and other types of devices are also possible. Further, the laser radar 320 is provided on the device body 310. The function of lidar 320 is to determine the position of device body 310 in a roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Further, the millimeter wave radar 330 is provided to the apparatus body 310. The millimeter wave radar 330 functions to roughly determine the position of the apparatus body 310 in the roadway in a dust environment. Further, the inertial navigation system 340 is disposed on the device body 310. Inertial navigation system 340 (INS, inertial navigation for short) is an autonomous navigation system that does not depend on external information nor radiates energy to the outside. The inertial navigation system 340 is used to roughly determine the position of the device body 310 in the roadway and provide angle information of the first coordinate system and the second coordinate system. Optionally, the first coordinate system is a heading machine coordinate system; the second coordinate system is a northeast day coordinate system. In the northeast coordinate system, the X axis is directed to the east, the Y axis is directed to the north, and the Z axis is directed to the zenith.
Further, the controller 350 is provided to the apparatus body 310. The controller 350 is electrically connected to the lidar 320. Controller 350 is electrically connected to millimeter-wave radar 330. The controller 350 is electrically connected to the inertial navigation system 340. The controller 350 is used to perform the steps of the downhole operation device positioning method.
Specifically, as shown in fig. 1, the steps of the downhole operation device positioning method include:
S102, acquiring first data through a laser radar of the underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system.
Optionally, the first data is point cloud data. The function of the lidar is to determine the position of the device body in the roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Optionally, the second data is point cloud data. The millimeter wave radar is used for roughly determining the position of the equipment body in the roadway in a dust environment. Optionally, the inertial navigation system comprises an accelerometer. The third data includes acceleration of the device body. Optionally, the inertial navigation system comprises a gyroscope. The third data includes angular velocity.
S104, determining whether the laser radar fails.
Determining whether the visibility in the well is greater than a first threshold, and whether the number of point clouds in the first data with the reflection intensity greater than a second threshold is less than a third threshold to determine whether the lidar fails.
And S106, under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
And under the condition that the visibility in the pit is larger than a first threshold value and/or the number of point clouds with the reflection intensity larger than a second threshold value in the first data is smaller than a third threshold value, judging that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. Kalman filtering is an algorithm for optimally estimating the state of a system by using a linear system state equation and through system input and output observation data. The optimal estimate can also be considered as a filtering process, since the observed data includes the effects of noise and interference in the system.
And S108, under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway.
Under the condition that the visibility in the pit is not greater than a first threshold value and the number of point clouds with the reflection intensity greater than a second threshold value in the first data is not less than a third threshold value, judging that the laser radar is not invalid, carrying out data fusion on the first data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway.
According to the technical scheme, the positioning method of the underground operation equipment can determine the data fusion mode according to whether the laser radar fails or not, the positioning result is more accurate, the automation degree is high, and the underground operation equipment is not easily influenced by underground visibility and dust concentration.
In one embodiment according to the invention, as shown in FIG. 2, the steps of the method of locating a downhole operation device include:
S202, acquiring first data through a laser radar of the underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system.
Optionally, the first data is point cloud data. The function of the lidar is to determine the position of the device body in the roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Optionally, the second data is point cloud data. The millimeter wave radar is used for roughly determining the position of the equipment body in the roadway in a dust environment. Optionally, the inertial navigation system comprises an accelerometer. The third data includes acceleration of the device body. Optionally, the inertial navigation system comprises a gyroscope. The third data includes angular velocity.
S204, determining whether the visibility in the pit is larger than a first threshold value, and determining whether the number of point clouds with the reflection intensity larger than a second threshold value in the first data is smaller than a third threshold value so as to determine whether the laser radar fails.
Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. Under the condition that two conditions are met simultaneously, the environment where the equipment body is located is determined to be an ideal environment, and the laser radar is not disabled. And under the condition that at least one condition is not met, determining that the environment where the equipment body is positioned is not dust-free, and at the moment, the laser radar is invalid.
S206, under the condition that the visibility in the pit is larger than a first threshold value and/or the number of point clouds is smaller than a third threshold value, the laser radar fails, the second data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in the roadway is determined.
Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. And under the condition that at least one condition is not met, determining that the environment where the equipment body is positioned is not dust environment, wherein the laser radar is invalid, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. Because millimeter wave radar is less influenced by dust environment, the accuracy of the positioning result can be improved by the design mode under the dust environment.
And S208, under the condition that the visibility in the pit is not greater than a first threshold value and the number of point clouds is not less than a third threshold value, the laser radar is not invalid, the first data and the third data are subjected to data fusion through Kalman filtering, and the position of the underground operation equipment in the roadway is determined.
Taking the downhole visibility larger than a first threshold value as a first condition; and taking the point cloud quantity, of which the reflection intensity is larger than the second threshold value, in the first data as a second condition, wherein the point cloud quantity is smaller than a third threshold value. Under the condition that two conditions are met simultaneously, determining that the environment where the equipment body is located is an ideal environment, at the moment, the laser radar is not invalid, performing data fusion on the first data and the third data through Kalman filtering, and determining the position of the underground operation equipment in the roadway. In an ideal environment (environment with low dust concentration), the laser radar determines the position of the equipment body in the roadway by scanning the target in the roadway, and compared with a mode of adopting a millimeter wave radar, the positioning result is more accurate.
According to the technical scheme, the positioning method of the underground operation equipment can determine the data fusion mode according to whether the laser radar fails or not, the positioning result is more accurate, the automation degree is high, and the underground operation equipment is not easily influenced by underground visibility and dust concentration.
In some embodiments, optionally, the first data comprises point cloud data. And acquiring first data, namely acquiring point cloud data of the scanning roadway through a laser radar. Optionally, the point cloud data includes coordinates of a target point cloud. Optionally, the point cloud data includes a reflected intensity. Optionally, the point cloud data comprises second distance data. The second distance data includes a distance between the lidar and a driving surface of the roadway and a distance between the lidar and two sidewalls of the roadway. Optionally, the point cloud data comprises second angle data. The second angle data is the installation angle data of the laser radar.
In some embodiments, optionally, the second data comprises first distance data. And acquiring second data, namely acquiring the first distance data, through the millimeter wave radar. The first distance data includes a distance between the millimeter wave radar and a driving surface of the roadway and a distance between the millimeter wave radar and two side walls of the roadway.
In some embodiments, optionally, the third data comprises first angle data of the downhole operation device. And acquiring third data, namely acquiring first angle data of the downhole operation equipment through the inertial navigation system. Optionally, the first angle data is attitude angle data, including yaw angle, pitch angle, and roll angle.
In some embodiments, optionally, the specific steps of the downhole operation device positioning method further comprise:
Before determining whether the laser radar fails, establishing a laser radar coordinate system, a millimeter wave radar coordinate system and a roadway coordinate system according to the first data, the second data and the third data. Optionally, a target is arranged in the tunnel, and a tunnel coordinate system taking the center of the target as the origin of coordinates is established. Optionally, a lidar coordinate system is established with the center of the lidar as the origin of coordinates. Optionally, a millimeter wave radar coordinate system with the center of the millimeter wave radar as the origin of coordinates is established.
The specific steps of determining the position of the downhole operation equipment in the roadway according to the second data and the third data comprise:
Under the condition that the laser radar fails, coordinate conversion is carried out through Kalman filtering according to a millimeter wave radar coordinate system, a roadway coordinate system and third data, and the position of underground operation equipment in the roadway is determined. Optionally, the third data comprises first angle data of the downhole operation device. The first angle data is attitude angle data including yaw angle, pitch angle, and roll angle.
The specific steps of determining the position of the downhole operation equipment in the roadway according to the first data and the third data comprise:
And under the condition that the laser radar does not fail, carrying out coordinate conversion through Kalman filtering according to a laser radar coordinate system, a roadway coordinate system and third data, and determining the position of underground operation equipment in the roadway. Optionally, the third data comprises first angle data of the downhole operation device. The first angle data is attitude angle data including yaw angle, pitch angle, and roll angle.
In some embodiments, optionally, the first threshold is 10m to 20m. The value range of the first threshold is limited, so that the sensitivity of switching between the two data processing modes is improved, and the positioning result is more accurate and reliable.
In some embodiments, optionally, the third threshold is 4 to 8. The value range of the second threshold is limited, so that the sensitivity of switching between the two data processing modes is improved, and the positioning result is more accurate and reliable.
In some embodiments, optionally, the second threshold is 200 to 250. The value range of the third threshold is limited, so that the sensitivity of switching between the two data processing modes is improved, and the positioning result is more accurate and reliable.
In one embodiment according to the present invention, as shown in FIG. 3, a downhole operation device positioning system 300 includes a device body 310, a lidar 320, a millimeter-wave radar 330, an inertial navigation system 340, and a controller 350. The device body 310 mainly plays a role of a mounting carrier for the laser radar 320, the millimeter wave radar 330, the inertial navigation system 340, and the controller 350. It should be noted that, the device body 310 is a heading machine, and other types of devices are also possible. Further, the laser radar 320 is provided on the device body 310. The function of lidar 320 is to determine the position of device body 310 in a roadway by scanning targets in the roadway under ideal circumstances. The ideal environment here refers to the case where the visibility in the well is not greater than the first threshold value, and the number of point clouds in the first data whose reflection intensity is greater than the second threshold value is not less than the third threshold value. Further, the millimeter wave radar 330 is provided to the apparatus body 310. The millimeter wave radar 330 functions to roughly determine the position of the apparatus body 310 in the roadway in a dust environment. Further, the inertial navigation system 340 is disposed on the device body 310. Inertial navigation system 340 (INS, inertial navigation for short) is an autonomous navigation system that does not depend on external information nor radiates energy to the outside. The inertial navigation system 340 is used to roughly determine the position of the device body 310 in the roadway and provide angle information of the first coordinate system and the second coordinate system. Optionally, the first coordinate system is a heading machine coordinate system; the second coordinate system is a northeast day coordinate system. In the northeast coordinate system, the X axis is directed to the east, the Y axis is directed to the north, and the Z axis is directed to the zenith.
Further, the controller 350 is provided to the apparatus body 310. The controller 350 is electrically connected to the lidar 320, and the controller 350 acquires the first data through the lidar 320. The controller 350 is electrically connected to the millimeter wave radar 330, and the controller 350 acquires the second data through the millimeter wave radar 330. The controller 350 is electrically connected to the inertial navigation system 340, and the controller 350 obtains third data through the inertial navigation system 340. The controller 350 is configured to perform the steps of the downhole operation device positioning method of any of the embodiments described above.
It should be noted that the number of lidars 320 is plural, and the lidars 320 are flexibly set according to actual requirements. The number of the millimeter wave radars 330 is plural, and the millimeter wave radars 330 are flexibly set according to actual demands. Alternatively, the number of lidars 320 is three; the number of millimeter wave radars 330 is six.
For the lidar 320 and the millimeter wave radar 330, the mounting accuracy is: the position error is within 1cm, and the angle error is within 0.1 degree.
Optionally, as shown in fig. 4, inertial navigation system 340 includes accelerometer 341 and gyroscope 342. Wherein accelerometer 341 is used to obtain acceleration data. The gyroscope 342 is used to acquire angular velocity data.
Optionally, as shown in fig. 4, the controller 350 includes a first data processing module 351, a second data processing module 352, an parsing module 353, an anomaly detection module 354, a spatiotemporal registration module 355, a kalman fusion module 356, and a data output module 357. Wherein the first data processing module 351 is configured to be connected to the lidar 320 and is capable of receiving and processing first data. The primary functions of the first data processing module 351 are point cloud processing and target identification. Further, the second data processing module 352 is configured to connect with the millimeter wave radar 330 and is capable of receiving and processing second data. The second data processing module 352 primarily functions as point cloud processing.
Further, the anomaly detection module 354 is connected to the first data processing module 351, for determining whether the laser radar 320 is disabled. Further, a spatiotemporal registration module 355 is coupled to the anomaly detection module 354. The spatio-temporal registration module 355 is primarily used for time alignment. Optionally, in the process of coordinate conversion according to the sensor data (first data, second data and third data), time alignment is performed by the spatio-temporal registration module 355. Further, a parsing module 353 (INS) is used in connection with the accelerometer 341 and the gyroscope 342 and is capable of processing the third data. The parsing module 353 is coupled to the spatiotemporal registration module 355.
Further, a kalman fusion module 356 is coupled to the spatiotemporal registration module 355, and the kalman fusion module 356 is coupled to the parsing module 353. The kalman fusion module 356 is used to process the data, and the parsing module 353 is used to calibrate the data. Further, the data output module 357 is connected to the parsing module 353 for outputting the position of the downhole operation device in the roadway.
In one embodiment according to the present invention, an electronic device includes a processor, a memory, and a program or instructions stored on the memory and executable on the processor, which when executed by the processor, implement the steps of the downhole operation device positioning method of any of the embodiments described above.
In an embodiment according to the invention, a computer readable storage medium stores a computer program which, when executed by a processor, implements the steps of the downhole operation device positioning method of any of the embodiments described above.
In one embodiment according to the present invention, the chip includes a processor and a communication interface coupled to the processor for running programs or instructions to implement the steps of the downhole operation device positioning method of any of the embodiments described above.
In the present invention, the terms "first," "second," "third," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance; the term "plurality" means two or more, unless expressly defined otherwise. The terms "mounted," "connected," "secured," and the like are to be construed broadly, and may be, for example, fixedly connected, detachably connected, or integrally connected; "coupled" may be directly coupled or indirectly coupled through intermediaries. The specific meaning of the above terms in the present invention can be understood by those of ordinary skill in the art according to the specific circumstances.
In the description of the present invention, it should be understood that the directions or positional relationships indicated by the terms "upper", "lower", "left", "right", "front", "rear", etc. are based on the directions or positional relationships shown in the drawings, are merely for convenience of describing the present invention and simplifying the description, and do not indicate or imply that the devices or units referred to must have a specific direction, be constructed and operated in a specific direction, and thus should not be construed as limiting the present invention.
In the description of the present specification, the terms "one embodiment," "some embodiments," "particular embodiments," and the like, mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. A method of positioning a downhole operation device, the method of positioning a downhole operation device being implemented by a downhole operation device positioning system, the method of positioning a downhole operation device comprising:
Acquiring first data through a laser radar of the underground operation equipment positioning system, acquiring second data through a millimeter wave radar of the underground operation equipment positioning system, and acquiring third data through an inertial navigation system of the underground operation equipment positioning system;
Determining whether the lidar is disabled;
Under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway;
and under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through the Kalman filtering, and determining the position of the underground operation equipment in the roadway.
2. The method of downhole operation device positioning of claim 1, wherein the determining whether the lidar has failed comprises:
Determining whether the visibility in the well is larger than a first threshold value, and determining whether the number of point clouds with the reflection intensity larger than a second threshold value in the first data is smaller than a third threshold value so as to determine whether the laser radar fails.
3. The method for locating a downhole operation device according to claim 2, wherein in the case of failure of the lidar, the data fusion between the second data and the third data is performed through kalman filtering, and the determining the position of the downhole operation device in the roadway comprises:
and under the condition that the visibility in the pit is larger than the first threshold value and/or the number of the point clouds is smaller than the third threshold value, the laser radar fails, the second data and the third data are subjected to data fusion through the Kalman filtering, and the position of the underground operation equipment in the roadway is determined.
4. The method for locating a downhole operation device according to claim 2, wherein the data fusion of the first data and the third data through the kalman filter is performed to determine the position of the downhole operation device in the roadway if the laser radar is not disabled, comprising:
and under the condition that the visibility in the pit is not greater than the first threshold value and the number of the point clouds is not less than the third threshold value, the laser radar is not invalid, the first data and the third data are subjected to data fusion through the Kalman filtering, and the position of the underground operation equipment in the roadway is determined.
5. The method of locating a downhole operation device of any of claims 1-4, wherein the first data comprises point cloud data; and/or the second data comprises first distance data, wherein the first distance data comprises the distance between the millimeter wave radar and the tunneling surface of the roadway and the distance between the millimeter wave radar and the two side walls of the roadway; and/or the third data comprises first angle data of the downhole operation device.
6. The method of positioning a downhole operation device of any of claims 1-4, further comprising:
before determining whether the laser radar fails, establishing a laser radar coordinate system, a millimeter wave radar coordinate system and a roadway coordinate system according to the first data, the second data and the third data;
under the condition that the laser radar fails, carrying out data fusion on the second data and the third data through Kalman filtering, and determining the position of underground operation equipment in a roadway comprises the following steps:
under the condition that the laser radar fails, according to the millimeter wave radar coordinate system, the roadway coordinate system and the third data, coordinate conversion is carried out through Kalman filtering, and the position of the underground operation equipment in the roadway is determined;
under the condition that the laser radar does not fail, carrying out data fusion on the first data and the third data through the Kalman filtering, and determining the position of the underground operation equipment in the roadway comprises the following steps:
And under the condition that the laser radar does not fail, carrying out coordinate conversion through the Kalman filtering according to the laser radar coordinate system, the roadway coordinate system and the third data, and determining the position of the underground operation equipment in the roadway.
7. A downhole operation device positioning system, comprising:
An apparatus body (310);
A laser radar (320) provided on the device body (310);
a millimeter wave radar (330) provided on the device body (310);
An inertial navigation system (340) provided on the device body (310);
A controller (350) disposed on the device body (310), the controller (350) being electrically connected to the lidar (320), the controller (350) being electrically connected to the millimeter wave radar (330), the controller (350) being electrically connected to the inertial navigation system (340), the controller (350) being configured to perform the steps of the method for positioning a downhole operation device according to any one of claims 1 to 6.
8. An electronic device comprising a processor, a memory and a program or instruction stored on the memory and executable on the processor, the program or instruction when executed by the processor implementing the steps of the method of locating a borehole operation device as claimed in any one of claims 1 to 6.
9. A computer readable storage medium, characterized in that the computer readable storage medium stores a computer program which, when executed by a processor, implements the steps of the method of positioning a downhole operation device according to any of claims 1 to 6.
10. A chip comprising a processor and a communication interface, the communication interface and the processor being coupled, the processor being configured to execute programs or instructions for implementing the steps of the method of positioning a downhole operation device according to any of claims 1 to 6.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410179920.8A CN118067136A (en) | 2024-02-18 | 2024-02-18 | Positioning method, positioning system, electronic device, storage medium and chip |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410179920.8A CN118067136A (en) | 2024-02-18 | 2024-02-18 | Positioning method, positioning system, electronic device, storage medium and chip |
Publications (1)
Publication Number | Publication Date |
---|---|
CN118067136A true CN118067136A (en) | 2024-05-24 |
Family
ID=91096691
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202410179920.8A Pending CN118067136A (en) | 2024-02-18 | 2024-02-18 | Positioning method, positioning system, electronic device, storage medium and chip |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN118067136A (en) |
-
2024
- 2024-02-18 CN CN202410179920.8A patent/CN118067136A/en active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7291158B2 (en) | Information processing method, system, device, program and computer storage medium | |
US11279045B2 (en) | Robot pose estimation method and apparatus and robot using the same | |
EP3907108A1 (en) | External sensing information processing device | |
CN109975773B (en) | Millimeter wave radar calibration method, device, equipment and storage medium | |
US9183178B2 (en) | Onboard device and control method | |
CN111435163B (en) | Ground point cloud data filtering method, device, detection system and storage medium | |
CN107505940A (en) | Double laser radar control method, system and localization method on automated guided vehicle | |
CN113147738A (en) | Automatic parking positioning method and device | |
EP4148385A1 (en) | Vehicle navigation positioning method and apparatus, and base station, system and readable storage medium | |
CN113432533B (en) | Robot positioning method and device, robot and storage medium | |
CN109407086B (en) | Aircraft trajectory generation method and system and trapping system target guiding method | |
CN113758482B (en) | Vehicle navigation positioning method, device, base station, system and readable storage medium | |
CN113296120B (en) | Obstacle detection method and terminal | |
CN113589324A (en) | Unmanned vehicle gradient identification method and system based on laser radar and storage medium | |
CN117590373A (en) | Port unmanned integrated card alignment method, storage medium and electronic equipment | |
CN113376638A (en) | Unmanned logistics trolley environment sensing method and system | |
CN113763262A (en) | Application method of vehicle body filtering technology in point cloud data of automatic driving mine truck | |
CN118067136A (en) | Positioning method, positioning system, electronic device, storage medium and chip | |
CN115902845A (en) | External parameter calibration method of laser radar and related device | |
CN116224396A (en) | Point cloud-based shore bridge area navigation method, system, equipment and storage medium | |
CN111061273B (en) | Autonomous obstacle avoidance fusion method and system for unmanned ship | |
CN114166218A (en) | Indoor positioning navigation system and method based on multi-positioning fusion | |
CN116592871B (en) | Unmanned ship multi-source target information fusion method | |
CN114690797A (en) | Unmanned aerial vehicle and flight control method thereof | |
CN112285756A (en) | High-precision positioning system and method based on IMU and Beidou system |
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 |