Depth camera and inertial navigation fused coal mine heading machine positioning method and device

Abstract

The embodiment of the invention provides a depth camera and inertial navigation fused coal mine heading machine positioning method and device. The method comprises the following steps: acquiring an anchor rod point cloud image, acceleration information and gyroscope information; calculating the pose of the inertial navigation information; the anchor rod point cloud image preprocessing comprises point cloud straight-through filtering, point cloud statistical filtering and point cloud segmentation; carrying out anchor rod point cloud registration; resolving a visual information pose; and fusing vision and inertial navigation information to obtain pose change information of the heading machine. On the basis of strapdown inertial navigation, vision is introduced as an auxiliary positioning system, a supported anchor rod is taken as a research target, errors accumulated along with time drift of inertial navigation are corrected by using position data of point cloud registration measurement of the anchor rod by a depth camera, the advantages of the two sensors are combined, and the system can adapt to a complex coal mine environment and is high in accuracy. And long-time accurate positioning of the coal mine tunnel heading machine is ensured.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of coal mine tunnel boring machine positioning, and relate to but are not limited to a coal mine tunnel boring machine positioning method and device integrating a depth camera and an inertial navigation system. Background Art

[0002] Coal is the cornerstone of my country's energy, and the intelligentization of coal mines is the only way for the high-quality development of the coal industry. As a key equipment in coal mine tunnel excavation, the positioning technology of the roadheader is one of the key technologies for the intelligentization of coal mine roadheaders. Coal mine tunnel excavation requires high positioning accuracy of the roadheader. It is difficult to achieve precise positioning with positioning technology based on a single sensor, and the positioning method based on the target has complex problems of occlusion and target migration process. Therefore, the positioning method of coal mine roadheaders is studied to achieve precise positioning of roadheaders without targets, and solve the problem of precise positioning of roadheaders in the complex environment of coal mine tunnels.

[0003] Due to the limitations of the underground environment, a single sensor may be disturbed or fail, resulting in reduced measurement accuracy and stability of the tunnel boring machine. The existing positioning methods have certain limitations. For example, the total station will reduce the accuracy of medium and long-distance positioning due to its own technical limitations; vision / lidar has high requirements for computing resources and is easily affected by factors such as occlusion and lighting in complex environments. Therefore, in complex underground environments, fusion positioning technology plays an important role in improving the accuracy and robustness of positioning.

[0004] Depth cameras are widely used because they are not affected by light and can obtain the distance and depth information of objects in the scene in real time. Depth information can be used to more accurately understand and analyze the scene, provide more information for subsequent processing and decision-making, and better achieve positioning and navigation tasks in complex environments. Point cloud registration technology can be used to align point cloud data from multiple perspectives or time periods to achieve accurate tracking of the target. Summary of the invention

[0005] Based on the problems in the related art, an embodiment of the present invention provides a coal mine roadheader positioning method and device integrating a depth camera and an inertial navigation system.

[0006] The technical solution of the embodiment of the present invention is achieved as follows:

[0007] An embodiment of the present invention provides a coal mine roadheader positioning method integrating a depth camera and an inertial navigation system, the method comprising:

[0008] Acquire an anchor point cloud image when the tunnel boring machine is running, as well as acceleration information and gyroscope information when the tunnel boring machine is running;

[0009] Preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data;

[0010] Performing feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix;

[0011] Determining first position information of the roadheader based on a position increment obtained by solving the target rigid body transformation matrix;

[0012] Performing posture calculation on the acceleration information and the gyroscope information to obtain inertial navigation error information of the tunnel boring machine;

[0013] Based on the inertial navigation error information and the first position information, establishing a target state equation and a target observation equation of a depth camera and inertial navigation fusion system;

[0014] The target state equation and the target observation equation are applied to a pre-constructed error state Kalman filter for filtering estimation and discretization processing, so as to realize the information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

[0015] An embodiment of the present invention provides a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system, the device comprising:

[0016] An acquisition module, used to acquire an anchor point cloud image when the tunnel boring machine is running, as well as acceleration information and gyroscope information when the tunnel boring machine is running;

[0017] A preprocessing module, used for preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data;

[0018] A registration module, used for performing feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix;

[0019] A determination module, configured to determine first position information of the roadheader based on a position increment obtained by solving the target rigid body transformation matrix;

[0020] A solution module, used for performing posture solution on the acceleration information and the gyroscope information to obtain inertial navigation error information of the tunnel boring machine;

[0021] An establishing module, used for establishing a target state equation and a target observation equation of a depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information;

[0022] A fusion module is used to apply the target state equation and the target observation equation to a pre-built error state Kalman filter for filtering estimation and discretization processing, so as to realize the information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

[0023] In some embodiments, the preprocessing module is also used to perform point cloud through filtering on the anchor point cloud image to obtain anchor area point cloud data; perform point cloud statistical filtering on the anchor area point cloud data to obtain initial anchor point cloud data; perform point cloud segmentation on the initial anchor point cloud data to obtain the target anchor point cloud data.

[0024] In some embodiments, the registration module is also used to calculate the FPFH features of each point in the source anchor point cloud data and the target anchor point cloud data respectively through a preset fast point feature histogram to obtain a source FPFH feature descriptor and a target FPFH feature descriptor; randomly sample point pairs of the source FPFH feature descriptor and the target FPFH feature descriptor through a preset sampling consistency algorithm, and estimate the initial rigid body transformation matrix between the source FPFH feature descriptor and the target FPFH feature descriptor to obtain an initial paired point cloud; accurately align the initial paired point cloud by searching the KD tree, determine the nearest neighbor points from the source anchor point cloud data, and establish a point pair correspondence relationship between point pairs; and determine the target rigid body transformation matrix based on the point pair correspondence relationship.

[0025] In some embodiments, the inertial navigation error information includes at least second position information; the establishment module is also used to use the inertial navigation error information as a state quantity of the depth camera and inertial navigation fusion system to determine a state vector; based on the state vector, the target state equation is established; the position difference between the first position information and the second position information is used as an observation quantity of the depth camera and inertial navigation fusion system to determine an observation vector; based on the observation vector, the target observation equation is established.

[0026] In some embodiments, the preprocessing module is also used to determine, based on the characteristics of the anchor point cloud image, to perform filtering on the X-axis and Z-axis in the anchor point cloud image, and to set the threshold ranges of the X-axis and the Z-axis; based on the threshold ranges of the X-axis and the Z-axis, respectively, to remove the point cloud data outside the corresponding coordinate axis of each point cloud data in the anchor point cloud image to obtain the anchor area point cloud data.

[0027] In some embodiments, the preprocessing module is further used to calculate the average distance s between each sampling point in the anchor area point cloud data and each neighboring point in the neighborhood k of the corresponding sampling point. j , In the formula, (x, y, z) is the coordinate value of a sampling point, (x i ,y i ,z i ) is the coordinate value of the i-th neighborhood point, i = 1, 2, 3 ... k, j is the serial number of the sampling point; calculate the average value μ of the multiple average distances, Where n represents the number of sampling points involved in the average distance calculation; based on the average distance and the average value, the standard deviation σ is calculated, Based on the preset standard deviation multiple std, if the average distance of each point is within the interval (μ-std·σ,μ+std·σ), the sampling point is retained; if it exceeds the interval, the sampling point is removed from the anchor area point cloud data.

[0028] In some embodiments, the preprocessing module is also used to determine the target plane model based on a seed point in the initial anchor point cloud data and two random points in the plane model fitted by the initial seed point; calculate the target distance from the remaining seed points in the initial anchor point cloud data to the target plane model; count the number of points whose target distance is less than a preset distance threshold, and if the number of points exceeds a preset minimum number of support points, determine the target plane model as a qualified model; repeat the above steps until a preset number of iterations is reached, and determine the data in the target plane model with the maximum number of support points as the target anchor point cloud data.

[0029] An embodiment of the present invention provides a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system, comprising: a memory for storing executable instructions; and a processor for implementing the coal mine roadheader positioning method integrating a depth camera and an inertial navigation system when executing the executable instructions stored in the memory.

[0030] An embodiment of the present invention provides a computer-readable storage medium storing executable instructions for causing a processor to execute the executable instructions to implement the above-mentioned coal mine roadheader positioning method integrating the depth camera and inertial navigation.

[0031] The coal mine roadheader positioning method and device for integrating depth camera and inertial navigation provided in an embodiment of the present invention first obtain the anchor point cloud image, acceleration information and gyroscope information when the roadheader is running; pre-process the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; perform feature extraction, coarse alignment and fine alignment on the source anchor point cloud data and target anchor point cloud data of two adjacent frames to obtain the target rigid body transformation matrix; determine the first position information of the roadheader based on the position increment obtained by solving the target rigid body transformation matrix; perform posture solving on the acceleration information and the gyroscope information to obtain the inertial navigation error information of the roadheader; establish the target state equation and target observation equation of the depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information; apply the target state equation and the target observation equation to a pre-constructed error state Kalman filter for filtering estimation and discretization processing to realize the information fusion of the depth camera and the inertial navigation and obtain the posture change information of the roadheader. In this way, the supported anchor bolts are taken as the research target for visual position detection of the tunnel boring machine. Through the preprocessing method of straight-through filtering + statistical filtering + point cloud segmentation, the interference of useless point clouds and noise points is effectively eliminated; the coarse registration + fine registration method is used to obtain the conversion relationship of the anchor bolt point cloud, and the tunnel boring machine position increment is solved according to the point cloud registration result, so as to realize the precise positioning of the tunnel boring machine without a target and improve the stability of the tunnel boring machine position measurement; combined positioning and navigation are carried out based on inertial sensors and visual cameras, and the two types of information are fused with the help of the error state Kalman filter algorithm, which effectively solves the problem that the inertial navigation measurement of the tunnel boring machine posture error accumulates over time and the visual positioning technology is difficult to accurately detect the tunnel boring machine posture due to factors such as vibration noise and magnetic interference, ensuring that the coal mine tunnel boring machine can achieve long-term precise positioning and can adapt to the tunnel boring machine positioning needs in low-speed motion scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 It is a flow chart of a method for positioning a coal mine boring machine by integrating a depth camera and an inertial navigation system provided by an embodiment of the present invention;

[0033] Figure 2 It is a coal mine roadheader positioning solution and coordinate system definition diagram provided by an embodiment of the present invention;

[0034] Figure 3 It is a flow chart of a method for positioning a coal mine boring machine by integrating a depth camera and an inertial navigation system provided by an embodiment of the present invention;

[0035] Figure 4 This is a diagram of a coal mine main laboratory positioning platform provided by an embodiment of the present invention;

[0036] Figure 5 is a point cloud anchor map provided by an embodiment of the present invention;

[0037] Figure 6 The preprocessed point cloud image and the registered point cloud image provided by the embodiment of the present invention;

[0038] Figure 7 is a diagram of X-direction position measurement results provided by an embodiment of the present invention;

[0039] Figure 8 is a diagram of the Y-direction position measurement result provided by an embodiment of the present invention;

[0040] Fig. 9 A schematic diagram of the composition structure of a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system provided in an embodiment of the present invention;

[0041] Fig.10 A schematic diagram of the composition structure of a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with the accompanying drawings. The described embodiments should not be regarded as limiting the present invention. All other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present invention.

[0043] In the following description, reference is made to "some embodiments", which describe a subset of all possible embodiments, but it is understood that "some embodiments" may be the same subset or different subsets of all possible embodiments, and may be combined with each other without conflict. Unless otherwise defined, all technical and scientific terms used in the embodiments of the present invention have the same meaning as those commonly understood by those skilled in the art to which the embodiments of the present invention pertain. The terms used in the embodiments of the present invention are only for the purpose of describing the embodiments of the present invention and are not intended to limit the present invention.

[0044] The following describes an exemplary application of a coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system according to an embodiment of the present invention. The coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system provided by an embodiment of the present invention can be implemented as a terminal or a server. In one implementation, the coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system provided by an embodiment of the present invention can be implemented as various types of terminals such as a laptop computer, a tablet computer, a desktop computer, and a mobile device; in another implementation, the coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system provided by an embodiment of the present invention can also be implemented as a server, wherein the server can be an independent physical server, or a server cluster or a distributed system composed of multiple physical servers, or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, content distribution networks (CDN, Content Delivery Network), and big data and artificial intelligence platforms. The terminal and the server can be directly or indirectly connected by wired or wireless communication, which is not limited in the embodiment of the present invention. Below, an exemplary application of a coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system when implemented as a server will be described.

[0045] The embodiment of the present invention provides a coal mine roadheader positioning method integrating a depth camera and an inertial navigation system, see Figure 1 , Figure 1 is a flow chart of a method for positioning a coal mine boring machine by integrating a depth camera and an inertial navigation system according to an embodiment of the present invention. Figure 1 The steps shown are explained.

[0046] Step S110, obtaining an anchor point cloud image when the tunnel boring machine is running, as well as acceleration information and gyroscope information when the tunnel boring machine is running.

[0047] In some embodiments, the anchor point cloud image is collected by a depth camera disposed on the body of the tunnel boring machine; the acceleration information and the gyroscope information are collected by an inertial sensor disposed on the body of the tunnel boring machine.

[0048] In some embodiments, the anchor point cloud image refers to a three-dimensional data image of the anchor and its surrounding environment when the tunnel boring machine is in operation, obtained by a depth camera.

[0049] In some implementations, the acceleration information refers to the acceleration of the tunnel boring machine during operation obtained by an inertial sensor, and the gyroscope information refers to the rotational angular velocity of the tunnel boring machine during operation obtained by an inertial sensor.

[0050] In the present invention, when the tunnel boring machine is operating normally, the depth camera and inertial sensor arranged on the tunnel boring machine body are started, the depth camera is responsible for collecting anchor depth point cloud image information; the inertial sensor is responsible for obtaining acceleration and gyroscope information.

[0051] Step S120, preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data.

[0052] In some embodiments, preprocessing refers to a preprocessing method of performing through filtering, statistical filtering, and random sampling consistency plane segmentation on the anchor point cloud image, which can effectively eliminate useless point clouds and noise interference.

[0053] In some embodiments, the source anchor point cloud data and the target anchor point cloud data refer to the anchor point cloud data ready for registration obtained after through filtering, statistical filtering and random sampling consistency processing, wherein the source anchor point cloud data refers to the original reference point cloud data, that is, the previous frame of anchor point cloud data; the target anchor point cloud data refers to the next frame of anchor point cloud data that needs to be collected during the cyclic operation of the tunnel boring machine.

[0054] Step S130, performing feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix.

[0055] In some embodiments, feature extraction refers to extracting features from source anchor point cloud data and target anchor point cloud data using a fast point feature histogram to obtain a source FPFH feature descriptor and a target FPFH feature descriptor. Here, the fast point feature histogram is a robust multidimensional feature descriptor that constructs a histogram by calculating the spatial relationship between each point in the point cloud and other points in its neighborhood, thereby describing the local geometric information of the point cloud.

[0056] In some embodiments, coarse registration refers to coarse registration of the extracted source FPFH feature descriptor and the target FPFH feature descriptor using a random sampling consistency algorithm to obtain a coarse registration result. Here, coarse registration refers to matching feature points of the source FPFH feature descriptor and the target FPFH feature descriptor to obtain a point pair correspondence.

[0057] In some embodiments, precise registration refers to introducing a KD tree into the ICP algorithm to perform point pair search to complete precise registration of the roadway anchor point cloud.

[0058] In the present invention, the fast point feature histogram (FPFH) is used to perform point pair matching based on the extracted anchor point clouds of two adjacent frames, the sampling consistency (SAC-IA) algorithm is used to complete the coarse alignment, and the KD tree is introduced into the ICP algorithm to perform point pair search to complete the precise alignment of the tunnel anchor point cloud.

[0059] Step S140, determining the first position information of the roadheader based on the position increment obtained by solving the target rigid body transformation matrix.

[0060] In some embodiments, the first position information refers to the actual position change information of the tunnel boring machine in space. The first position information here is obtained by solving the anchor point cloud image data collected by the depth camera, that is, it can be called visual position information.

[0061] In the present invention, the position increment of the measurement camera can be calculated based on the point cloud registration result, that is, the target rigidity change matrix, so as to determine the actual position change of the tunnel boring machine in space and realize the precise positioning of the tunnel boring machine without a target.

[0062] Step S150, performing posture calculation on the acceleration information and the gyroscope information to obtain inertial navigation error information of the tunnel boring machine.

[0063] In some embodiments, the inertial navigation error information of the tunnel boring machine includes at least the heading information, attitude information, speed information and position information of the tunnel boring machine.

[0064] In the present invention, the inertial navigation information posture solution: the gyroscope and accelerometer in the inertial navigation sensor can be used to measure the angular motion information and linear motion information of the tunnel boring machine respectively, and the onboard computer solves the heading, attitude, speed and position of the tunnel boring machine based on the measured angular motion information and linear motion information.

[0065] Step S160: establishing a target state equation and a target observation equation of a depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information.

[0066] In systems based on inertial navigation systems (INS) and deep camera fusion, the target state equation and target observation equation are mathematical models that describe the dynamic behavior of the system and the relationship between the observed data and the system state. These equations are the basis for designing Kalman filters to estimate and predict the state of the system.

[0067] In some embodiments, the target state equation, also known as the system dynamic equation, refers to the law of change of the system state over time without external observation. In the inertial navigation and depth camera fusion system, the system state may include position, velocity, attitude, heading, and possible inertial navigation internal parameters, such as the deviation of the gyroscope and accelerometer.

[0068] In some embodiments, the target observation equation, also called the measurement equation, refers to the relationship between the observation data of the system and the system state. In the inertial navigation and depth camera fusion system, the observation data comes from the visual odometer of the depth camera, which is used to correct the drift and error of the inertial navigation.

[0069] In the present invention, the inertial navigation error is used as the state quantity of the inertial navigation and depth camera fusion system to obtain the state vector, thereby obtaining the state equation, and the difference between the positions measured by the inertial navigation and the visual position is used as the observation quantity, thereby obtaining the observation equation.

[0070] Step S170, applying the target state equation and the target observation equation to a pre-constructed error state Kalman filter for filtering estimation and discretization processing, so as to realize information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

[0071] In the present invention, an ESKF filter is constructed, and the state equation and observation equation of the depth camera and inertial navigation fusion system are applied to the ESKF filtering formula for filtering estimation, which is then discretized and the discretized equation is brought into the time update and measurement update. The first stage is the time update and the second stage is the measurement update. Through the above-mentioned combined filter, the information fusion of the inertial navigation and the depth camera can be realized.

[0072] The method for positioning a coal mine roadheader by integrating a depth camera and an inertial navigation system provided in an embodiment of the present invention, in this way, the visual position detection of the roadheader is performed with the supported anchor bolts as the research target, and the interference of useless point clouds and noise points is effectively eliminated through the preprocessing method of straight-through filtering + statistical filtering + point cloud segmentation; the conversion relationship of the anchor bolt point cloud is obtained by the method of coarse alignment + fine alignment, and the position increment of the roadheader is solved according to the point cloud alignment result, so as to realize the precise positioning of the roadheader in a target-free manner, thereby improving the stability of the position measurement of the roadheader; combined positioning and navigation are performed based on inertial sensors and visual cameras, and the two types of information are fused with the help of an error state Kalman filtering algorithm, which effectively solves the problem that the error of the roadheader posture measured by inertial navigation accumulates over time and the problem that the visual positioning technology is difficult to accurately detect the posture of the roadheader due to factors such as vibration noise and magnetic interference, thereby ensuring that the coal mine tunnel roadheader can achieve long-term precise positioning and can adapt to the positioning needs of the roadheader in low-speed motion scenarios.

[0073] In some embodiments, the above step S120 can be implemented by following the steps S121 to S123:

[0074] Step S121, performing point cloud through filtering on the anchor point cloud image to obtain anchor area point cloud data.

[0075] In the present invention, the three-dimensional point cloud information of the tunnel roof anchor collected by the depth camera is filtered through, irrelevant tunnel environment information is filtered out from the original point cloud, and the point cloud data of the anchor area is extracted.

[0076] In some embodiments, the above step S121 may be implemented through steps S1211 to S1212:

[0077] Step S1211, based on the characteristics of the anchor point cloud image, determine to perform filtering on the X-axis and the Z-axis in the anchor point cloud image, and set the threshold range of the X-axis and the Z-axis.

[0078] In the present invention, the filtering range can be set: according to the characteristics of the anchor point cloud data, it is determined to filter on the X and Z coordinate axes, and the threshold range on the axis is set, and the threshold range includes a minimum value and a maximum value.

[0079] Step S1212: based on the threshold ranges of the X-axis and the Z-axis, point cloud data outside the corresponding coordinate axes of each point cloud data in the anchor point cloud image are removed to obtain the anchor area point cloud data.

[0080] In the present invention, the point cloud is traversed and points are retained or eliminated: the coordinate value of each point cloud data in the selected axis is judged. Based on the X coordinate axis, filtering is performed to set the points whose x coordinates are within the X axis threshold range to be retained, and based on the Z coordinate axis, further filtering is performed to set the points whose z coordinates are within the Z axis threshold range to be retained, thereby removing the point cloud data that is irrelevant to the anchor measurement or affects the measurement accuracy.

[0081] In the present invention, after the point cloud direct filtering is completed, the filtering effect is verified. It can be observed in the visualization window whether the point cloud data before and after filtering effectively filters out irrelevant tunnel environment information and extracts the point cloud data of the anchor area.

[0082] Step S122, performing point cloud statistical filtering on the anchor area point cloud data to obtain initial anchor point cloud data.

[0083] In the present invention, outliers and noise in the point cloud data of the anchor area are removed by statistical filtering to ensure the accuracy and real-time performance of subsequent measurement and registration results.

[0084] In some embodiments, the above step S122 may be implemented through steps S1221 to S1224:

[0085] Step S1221, calculating the average distance s between each sampling point in the anchor area point cloud data and each neighboring point in the neighborhood k of the corresponding sampling point j ,

[0086] In the present invention, point cloud statistical filtering is used to eliminate outliers caused by measurement noise. The average distance s between the sampling point p and all points in its k-neighborhood is first calculated. j :

[0087]

[0088] Where (x, y, z) is the coordinate value of the sampling point p, (xi ,y i ,z i ) is the coordinate value of the i-th neighborhood point, i = 1, 2, 3, ..., k, j is the sequence number of the sampling points.

[0089] Step S1222, calculating the average value μ of the plurality of average distances, Where n represents the number of sampling points involved in the average distance calculation.

[0090] In the present invention, the average value of the average distances of all sampling points in a point cloud cluster having n number is calculated.

[0091] Step S1223, based on the average distance and the average value, calculate the standard deviation σ,

[0092]

[0093] In the present invention, the standard deviation of the average distances within the k-neighborhood of all sampling points in the point cloud cluster is obtained.

[0094] Step S1224, based on the preset standard deviation multiple std, if the average distance of each sampling point is within the interval (μ-std·σ,μ+std·σ), the sampling point is retained; if it exceeds the interval, the sampling point is removed from the anchor area point cloud data.

[0095] Step S123, performing point cloud segmentation on the initial anchor point cloud data to obtain the target anchor point cloud data.

[0096] In the present invention, the random sampling consensus (RANSAC) segmentation algorithm is used to strip off the anchor, background and other object point clouds, and extract the target anchor point cloud.

[0097] In some embodiments, the above step S123 may be implemented through steps S1231 to S1234:

[0098] Step S1231, determining a target plane model based on a seed point in the initial anchor point cloud data and two random points in a plane model fitted by the initial seed point.

[0099] Step S1232, calculating the target distances from the remaining seed points in the initial anchor point cloud data to the target plane model.

[0100] Step S1233, counting the number of points whose target distance is less than a preset distance threshold, and if the number of points exceeds a preset minimum number of supporting points, determining the target plane model as a model that meets the conditions.

[0101] Step S1234, repeat the above steps until a preset number of iterations is reached, and the data in the target plane model with the maximum number of support points is determined as the target anchor point cloud data.

[0102] In the present invention, a plane segmentation method with random sampling consistency is used to extract the target point cloud. First, a point is randomly selected from the point cloud as the initial seed point and added to the current plane model. According to the fitted plane model, another two points are randomly selected to form a plane model with the seed point, and the distances from other points to the plane are calculated.

[0103] In the present invention, the number of points whose distance is less than the threshold is counted. If the number of points exceeds the preset minimum number of supporting points, the plane model is regarded as a qualified model. The above steps are repeated until the preset number of iterations is reached. Finally, the plane model with the maximum number of supporting points is selected as the final segmentation result.

[0104] In some embodiments, the above step S130 can be implemented by the following steps S131 to S134:

[0105] Step S131, by presetting a fast point feature histogram, respectively calculating the FPFH features of each point in the source anchor point cloud data and the target anchor point cloud data to obtain a source FPFH feature descriptor and a target FPFH feature descriptor.

[0106] In the present invention, the source anchor point cloud and the target anchor point cloud are read in, and the FPFH features of all points in the two point clouds are calculated. Then, the SAC-IA algorithm is used to randomly sample point pairs from these feature descriptors, and the rigid body transformation matrix between them is estimated to complete the initial alignment.

[0107] Step S132, through a preset sampling consistency algorithm, randomly sample point pairs for the source FPFH feature descriptor and the target FPFH feature descriptor, and estimate the initial rigid body transformation matrix between the source FPFH feature descriptor and the target FPFH feature descriptor to obtain an initial paired point cloud.

[0108] Step S133, by searching the KD tree, the initial paired point cloud is precisely aligned, the nearest neighbor point is determined from the source anchor point cloud data, and a point pair correspondence relationship between the point pairs is established.

[0109] Step S134: determining the target rigid body transformation matrix based on the point pair correspondence.

[0110] In the present invention, the result of the rough registration is finely registered. For each point in the point cloud to be registered, the nearest neighbor point in the reference point cloud is found by searching the KD tree, and the correspondence between the point pairs is established. According to the established point pair correspondence, the optimal rigid body transformation of the point cloud to be registered relative to the reference point cloud is calculated by minimizing the distance error between the points. The rigid body transformation is applied to the point cloud to be registered to obtain an updated point cloud. By calculating the error function and iterating continuously, when the formula When the value is less than the given threshold, the iteration is stopped to obtain the optimal transformation matrix;

[0111] In the formula, R is the rotation matrix, T is the translation transformation matrix, N represents the number of corresponding points in the point set; p i represents the i-th point in the source point cloud to be registered; q i Represents the distance point p in the target point cloud i The point with the shortest Euclidean distance; N c Indicates the number of points involved in the distance error calculation; when the iteration terminates, the final registered point cloud or rigid body transformation parameters are obtained.

[0112] In some embodiments, the inertial navigation error information includes at least the second position information; the above step S160 can be implemented by the following steps S161 to S164:

[0113] Step S161: Using the inertial navigation error information as a state quantity of the depth camera and inertial navigation fusion system to determine a state vector.

[0114] Step S162: establishing the target state equation based on the state vector.

[0115] Step S163: Taking the position difference between the first position information and the second position information as the observation quantity of the depth camera and inertial navigation fusion system to determine the observation vector.

[0116] Step S164: establishing the target observation equation based on the observation vector.

[0117] The following describes an exemplary application of an embodiment of the present invention in a practical application scenario.

[0118] The present invention provides another method for positioning a coal mine roadheader by integrating a depth camera and an inertial navigation system. The method has a reasonable design and a good positioning effect. The method uses supported anchor bolts as the research target to perform visual position detection of the roadheader. The method uses a preprocessing method of straight-through filtering + statistical filtering + point cloud segmentation to effectively eliminate interference from useless point clouds and noise points. The method uses a coarse registration + fine registration method to obtain the conversion relationship of the anchor bolt point cloud, and calculates the roadheader position increment based on the point cloud registration result, thereby realizing precise positioning of the roadheader without a target and improving the stability of the visual measurement system. The error-state Kalman filtering algorithm is used to realize the fusion of the two types of information, thereby ensuring that the coal mine tunnel roadheader can achieve long-term precise positioning.

[0119] Combination Figure 2-3 Another method for positioning a coal mine roadheader by integrating a depth camera and an inertial navigation system provided by the present invention includes:

[0120] Step 1: Information collection from the depth camera and inertial sensor on the TBM body: When the TBM is operating normally, start the depth camera and inertial sensor on the TBM body. The depth camera is responsible for collecting the anchor depth point cloud image information; the inertial sensor is responsible for obtaining acceleration and gyroscope information.

[0121] In this embodiment, the data acquisition frequency of the depth camera is 5 Hz, and the data acquisition frequency of the inertial sensor is 100 Hz.

[0122] It should be noted that the inertial navigation system and the depth camera are both installed above the tunnel boring machine, close to each other to reduce arm errors, and it is necessary to ensure that the depth camera, inertial navigation system and onboard computer are connected to collect tunnel information in real time.

[0123] Step 2: Inertial navigation information posture solution: The gyroscope and accelerometer are used to measure the angular motion information and linear motion information of the tunnel boring machine respectively. The onboard computer solves the heading, attitude, speed and position of the tunnel boring machine based on these measurement information.

[0124] Step 3: Anchor point cloud data preprocessing, using the preprocessing method of straight-through filtering + statistical filtering + RANSAC plane segmentation to effectively eliminate useless point clouds and noise interference.

[0125] It should be noted that since the same reference target anchor cannot always be in the camera's field of view during the movement of the tunnel boring machine, the solution of the tunnel boring machine's dynamic posture requires continuous reference target anchors for solving the tunnel boring machine's coordinate system posture. The two rows of reference target anchors selected cannot disappear from the camera's field of view at the same time, and the position measurement is achieved through point cloud matching of the same reference target anchor between different point cloud frames.

[0126] Step 301: Pass-Through Filtering is performed on the three-dimensional point cloud information of the tunnel roof anchor collected by the depth camera to filter out irrelevant tunnel environment information from the original point cloud and extract point cloud data of the anchor area.

[0127] Step 302: Remove outliers and noise in the point cloud data of the anchor area through statistical filtering to ensure the accuracy and real-time performance of subsequent measurement and registration results.

[0128] Step 303: Use the random sampling consensus (RANSAC) segmentation algorithm to separate the anchor, background and other object point clouds, and extract the target anchor point cloud.

[0129] In this embodiment, in step 301, a method for extracting point cloud data of the anchor area by using straight-through filtering to filter out irrelevant information is described as follows:

[0130] Step 3011: Set the filtering range: According to the characteristics of the anchor point cloud data, determine to filter on the X and Z coordinate axes, and set the threshold range on the axis, which includes a minimum value and a maximum value.

[0131] Step 3012: Traverse the point cloud and retain or remove points: Determine the coordinate value of each point cloud data in the selected axis. Filter based on the X coordinate axis, set to retain points with x coordinates within the X axis threshold range, and further filter based on the Z coordinate axis, set to retain points with z coordinates within the Z axis threshold range. Remove point cloud data that is irrelevant to anchor measurement or affects measurement accuracy.

[0132] Step 3013: After the point cloud direct filtering is completed, the filtering effect is verified. Observe the point cloud data before and after filtering in the visualization window to see whether irrelevant tunnel environment information is effectively filtered out and the point cloud data of the anchor area is extracted.

[0133] In this embodiment, in step 302, a method of removing outliers and noise in the point cloud data of the anchor area by using statistical filtering is described as follows:

[0134] Step 3021: Use point cloud statistical filtering to eliminate outliers caused by measurement noise. First, calculate the average distance s between the sampling point p and all points in its k neighborhood. j :

[0135]

[0136] Where (x, y, z) is the coordinate value of the sampling point p, (x i ,y i ,z i ) is the coordinate value of the i-th neighborhood point, i = 1, 2, 3, ..., k; j is the sequence number of the sampling point.

[0137] Step 3022: Calculate all sampling points s in the point cloud cluster with the number n j The average value μ is:

[0138]

[0139] Where n represents the number of sampling points involved in the average distance calculation.

[0140] Step 3023: Finally, the standard deviation σ of the average distance within the k neighborhood of all sampling points in the point cloud cluster is obtained:

[0141]

[0142] Step 3024: Set the standard deviation multiple std and make a judgment. If the average distance s between the sampling point and all its k neighboring points is j , if it is within the interval (μ-std·σ,μ+std·σ), the point is retained; if it is beyond the interval, the point is removed from the point cloud cluster.

[0143] In this embodiment, the method of extracting the target anchor point cloud using the random sampling consensus (RANSAC) segmentation algorithm in step 303 is as follows:

[0144] Step 3031: Use RANSAC plane segmentation to extract the target point cloud. First, randomly select a point from the point cloud as the initial seed point and add it to the current plane model. According to the fitted plane model, randomly select another two points to form a plane model with the seed point, and calculate the distance from other points to the plane.

[0145] Step 3023: Count the number of points whose distance is less than the threshold. If the number of points exceeds the preset minimum number of support points, the plane model is considered to be a qualified model. Repeat the above steps until the preset number of iterations is reached. Finally, the plane model with the maximum number of support points is selected as the final segmentation result.

[0146] Step 4: Anchor point cloud registration: The fast point feature histogram (FPFH) is used to perform point pair matching based on the extracted anchor point clouds of two adjacent frames, and the sampling consistency (SAC-IA) algorithm is used to complete the coarse registration. The KD tree is introduced into the ICP algorithm for point pair search to complete the precise registration of the roadway anchor point cloud.

[0147] Step 401: read in the source anchor point cloud and the target anchor point cloud, calculate the FPF H features of all points in the two point clouds, randomly sample point pairs from these feature descriptors through the SAC-IA algorithm, and estimate the rigid body transformation matrix between them to complete the initial alignment.

[0148] Step 402: Perform fine registration on the result of the rough registration. For each point in the point cloud to be registered, find the nearest neighbor point in the reference point cloud by searching the KD tree and establish the correspondence between the point pairs. Based on the established point pair correspondence, calculate the optimal rigid body transformation of the point cloud to be registered relative to the reference point cloud by minimizing the distance error between the points. Apply the rigid body transformation to the point cloud to be registered to obtain an updated point cloud. By calculating the error function and iterating continuously, when the formula When the value is less than the given threshold, the iteration is stopped to obtain the optimal transformation matrix.

[0149] In the formula, R is the rotation matrix, T is the translation transformation matrix, N represents the number of corresponding points in the point set; p i represents the i-th point in the source point cloud to be registered; q i Represents the distance point p in the target point cloud i The point with the shortest Euclidean distance; N c Indicates the number of points involved in the distance error calculation; when the iteration terminates, the final registered point cloud or rigid body transformation parameters are obtained.

[0150] Step 5: Visual information pose solution: The position increment of the measurement camera is solved based on the point cloud registration results to determine the actual position change of the tunnel boring machine in space and achieve precise positioning of the tunnel boring machine without a target.

[0151] Step 6: Fusion of visual and inertial navigation information: including the establishment of state equations and observation equations, construction of error state Kalman filter for filtering estimation, realization of the fusion of visual and inertial navigation information, and obtaining more accurate carrier posture change information.

[0152] It should be noted that the strapdown inertial navigation system has good autonomous positioning performance, but its positioning error will drift over time and gradually accumulate. The stability and accuracy of the visual positioning system in the coal mine environment are difficult to guarantee due to factors such as vibration noise and magnetic interference, but the visual measurement method is popular for its advantages of non-contact and no cumulative error. Therefore, the research on combined positioning and navigation technology based on inertial sensors and visual cameras can meet the positioning needs of tunnel boring machines in low-speed motion scenarios.

[0153] Step 601: Using the inertial navigation error as the state quantity of the depth camera and inertial navigation fusion system to obtain a state vector, thereby obtaining a state equation, and using the difference between the positions measured by the inertial navigation and the visual position as the observation equation.

[0154] Step 602: Construct an ESKF filter, apply the state equation and observation equation of the depth camera and inertial navigation fusion system to the ESKF filter formula for filter estimation, and then discretize it, and bring the discretized equation into the time update and measurement update. The first stage is the time update, and the second stage is the measurement update. Through the above combined filter, the information fusion of the inertial navigation and the depth camera can be realized.

[0155] Step 6011: Using the inertial navigation error as the state quantity of the depth camera and inertial navigation fusion system, the state vector can be obtained:

[0156]

[0157] In the formula, is the attitude error angle, δv is the velocity error, δL and δλ are the position errors, ε is the gyroscope error, and ▽ is the accelerometer error;

[0158] Step 6012: Obtain a state equation from the state vector;

[0159]

[0160] Where X is the discrete state vector; F i is the system state transfer matrix; B i is the system noise matrix; W is the system noise vector; W k is the system noise vector of the kth state; W j is the system noise vector of the jth state; Q k is the variance of process noise; δ k,j is the Kronecker delta function, which is 1 when k=j and 0 otherwise; this formula represents the statistical characteristics of the system noise.

[0161] Step 6013: The visual measurement system mainly obtains the position of the target carrier, so the difference between the position measured by the inertial navigation and the visual measurement is used as the observation value:

[0162]

[0163] Where Z is the discrete measurement vector; H is the measurement matrix; V is the measurement noise vector, ins represents the position coordinates of the target carrier in the inertial coordinate system; vi represents the position coordinates of the target carrier in the visual measurement system coordinate system; the statistical characteristics meet the following conditions:

[0164]

[0165] In the formula, R k is the variance of the measurement noise; V k is the measurement noise vector of the kth state; Vj is the measurement noise vector of the jth state; d k,j Same as δ k,j .

[0166] The above-mentioned method for positioning a coal mine roadheader based on inertial navigation and depth camera visual fusion is characterized in that an ESKF filter is constructed in step 602, and a method for realizing information fusion of inertial navigation and depth camera by combining the filter is as follows:

[0167] Step 6021: Construct an ESKF filter, apply the state equation and observation equation of the depth camera and inertial navigation fusion system to the ESKF filter formula, and perform filter estimation, as shown below:

[0168]

[0169] In the formula, is the state estimate of the previous moment; k is the control input; x k is the real state at the current k moment.

[0170]

[0171] In the formula, is the covariance of the predictions in the time update step; F k-1 is the state transfer matrix; is the transpose of the state transfer matrix; is the covariance estimate of the previous moment; B k-1 is the control input matrix; Q k is the process noise covariance; is the transpose of the control input matrix.

[0172]

[0173] In the formula, K k is the Kalman gain; G k is the observation matrix; is the transpose of the observation matrix; C k is the measurement noise matrix; R k is the measurement noise covariance; is the transpose of the measurement noise matrix.

[0174]

[0175] Where I is the unit matrix; K k , G k are the products of the Kalman gain and the observation matrix respectively.

[0176]

[0177] In the formula, according to the Kalman gain K k and measurement residuals Update state estimate; y k is the actual measured value; g(·) is the measurement model; The predicted state.

[0178] Step 6022: Discretize the state equation and measurement equation of the depth camera and inertial navigation fusion system according to the sampling time, thereby obtaining the discrete state equation:

[0179] X k =F k-1 X k-1 +B k-1 W k ,

[0180] For the state equation, the first-order Taylor approximation is:

[0181]

[0182]

[0183] Step 6023: Substitute the discretized state equation and measurement equation into the time update and measurement update. The first stage is the time update, which includes the prediction of the state X k and the predicted mean square error state

[0184]

[0185]

[0186] The second stage is the measurement update, which includes the calculation of the filter gain and the optimal estimate of the true value based on the predicted value and the measured value;

[0187]

[0188] Update the status and covariance of the inertial navigation, visual depth camera and inertial navigation fusion system;

[0189]

[0190]

[0191] Through the above combined filters, information fusion of inertial navigation and depth camera can be achieved.

[0192] When the present invention is used, Figure 4As shown in the figure, the strapdown inertial navigation system is used to record the robot's position data, the point cloud image technology is used to collect the point cloud data in the environment, and the position information is solved through the algorithm; the total station measurement equipment is used to measure the position of the tunnel boring machine and provide the actual measured position information of the tunnel boring machine. Figure 5 As shown in the figure, the original point cloud is obtained by using the point cloud acquisition system, including the roadway roof information, the roof anchor information and part of the side wall information. In order to obtain the required target anchor point cloud, the irrelevant roadway environment information is first filtered out from the original point cloud by straight-through filtering, and the point cloud data of the anchor area is extracted. Secondly, the noise generated by the depth camera measurement error, operator experience and complex environment during the experiment is filtered out by statistical filtering, and then the target anchor point cloud is separated by the RANSAC point cloud segmentation algorithm. The pre-processed point cloud is shown in the figure. Figure 6 Finally, the pre-processed anchor point clouds of two adjacent frames are registered using a high-precision registration algorithm based on SAC-IA and optimized ICP. The registered point cloud is shown in Figure 6 (b) as shown.

[0193] The error state Kalman filter algorithm is used to fuse the inertial navigation and visual information. The position measured by the total station is taken as the true value, and the position information of the tunnel boring machine calculated by information fusion is taken as the measurement. The positioning results of the tunnel boring machine in the x direction and the y direction are calculated respectively. Figure 7 , Figure 8 As shown in the figure, it can be seen that the fused positioning curve is very close to the actual position curve; within the distance range of 20.32m, the average position error in the tunnel width direction is 24.60mm, and the maximum error is 61.04mm; the average position error in the excavation direction is 16.06mm, and the maximum error is 43.77mm. Therefore, by utilizing the advantages of inertial navigation and visual measurement, the anti-interference ability and environmental adaptability of position measurement are improved, thereby ensuring the accuracy and stability of positioning in complex practical application scenarios.

[0194] In summary, the present invention comprehensively utilizes the advantages of inertial navigation measurement technology and visual measurement technology to effectively achieve more stable and accurate positioning performance, which plays an important role in improving the accuracy and robustness of positioning in complex underground environments.

[0195] In the present invention, the supported anchor rods are taken as the research target to carry out visual position detection of the tunnel boring machine. Through the preprocessing method of straight-through filtering + statistical filtering + point cloud segmentation, the interference of useless point clouds and noise points is effectively eliminated; the coarse alignment + fine alignment method is adopted to obtain the conversion relationship of the anchor rod point cloud, and the tunnel boring machine position increment is solved according to the point cloud alignment result, so as to realize the precise positioning of the tunnel boring machine without a target, and improve the stability of the tunnel boring machine position measurement; combined positioning and navigation are carried out based on inertial sensors and visual cameras, and the two types of information are fused with the help of the error state Kalman filtering algorithm, which effectively solves the problems of the accumulation of inertial navigation measurement of tunnel boring machine posture error over time and the difficulty of visual positioning technology in accurately detecting the posture of the tunnel boring machine due to factors such as vibration noise and magnetic interference, thereby ensuring that the tunnel boring machine in the coal mine can achieve long-term precise positioning and can adapt to the positioning needs of the tunnel boring machine in low-speed motion scenarios.

[0196] Fig. 9 FIG. 1 is a schematic diagram of the structure of a coal mine boring machine positioning device integrating a depth camera and an inertial navigation system provided by an embodiment of the present invention. Fig. 9 As shown, a coal mine roadheader positioning device 900 integrating a depth camera and an inertial navigation comprises: an acquisition module 901, which is used to acquire an anchor point cloud image when the roadheader is running, as well as acceleration information and gyroscope information when the roadheader is running; a preprocessing module 902, which is used to preprocess the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; a registration module 903, which is used to perform feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix; a determination module 904, which is used to calculate a position based on the target rigid body transformation matrix. Increment, determine the first position information of the tunnel boring machine; a solving module 905, used to solve the acceleration information and the gyroscope information to obtain the inertial navigation error information of the tunnel boring machine; an establishing module 906, used to establish the target state equation and the target observation equation of the depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information; a fusion module 907, used to apply the target state equation and the target observation equation to a pre-constructed error state Kalman filter for filtering estimation and discretization processing, so as to realize the information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

[0197] In some embodiments, the preprocessing module 902 is also used to perform point cloud through filtering on the anchor point cloud image to obtain anchor area point cloud data; perform point cloud statistical filtering on the anchor area point cloud data to obtain initial anchor point cloud data; perform point cloud segmentation on the initial anchor point cloud data to obtain the target anchor point cloud data.

[0198] In some embodiments, the registration module 903 is also used to calculate the FPFH features of each point in the source anchor point cloud data and the target anchor point cloud data respectively through a preset fast point feature histogram to obtain a source FPFH feature descriptor and a target FPFH feature descriptor; randomly sample point pairs of the source FPFH feature descriptor and the target FPFH feature descriptor through a preset sampling consistency algorithm, and estimate the initial rigid body transformation matrix between the source FPFH feature descriptor and the target FPFH feature descriptor to obtain an initial paired point cloud; accurately align the initial paired point cloud by searching the KD tree, determine the nearest neighbor points from the source anchor point cloud data, and establish a point pair correspondence relationship between point pairs; based on the point pair correspondence relationship, determine the target rigid body transformation matrix.

[0199] In some embodiments, the inertial navigation error information includes at least second position information; the establishment module 906 is also used to use the inertial navigation error information as a state quantity of the depth camera and inertial navigation fusion system to determine a state vector; based on the state vector, establish the target state equation; use the position difference between the first position information and the second position information as an observation quantity of the depth camera and inertial navigation fusion system to determine an observation vector; based on the observation vector, establish the target observation equation.

[0200] In some embodiments, the preprocessing module 902 is also used to determine, based on the characteristics of the anchor point cloud image, to perform filtering on the X-axis and Z-axis in the anchor point cloud image, and to set the threshold ranges of the X-axis and the Z-axis; based on the threshold ranges of the X-axis and the Z-axis, respectively, to remove the point cloud data of each point cloud data in the anchor point cloud image outside the corresponding coordinate axis to obtain the anchor area point cloud data.

[0201] In some embodiments, the preprocessing module 902 is further used to calculate the average distance s between each sampling point in the anchor area point cloud data and each neighboring point in the neighborhood k of the corresponding sampling point. j , In the formula, (x, y, z) is the coordinate value of a sampling point, (x i ,y i ,z i ) is the coordinate value of the i-th neighborhood point, i = 1, 2, 3 ... k, j is the serial number of the sampling point; calculate the average value μ of the multiple average distances, Where n represents the number of sampling points involved in the average distance calculation; based on the average distance and the average value, the standard deviation σ is calculated, Based on the preset standard deviation multiple std, if the average distance of each sampling point is within the interval (μ-std·σ,μ+std·σ), the sampling point is retained; if it exceeds the interval, the sampling point is removed from the anchor area point cloud data.

[0202] In some embodiments, the preprocessing module 902 is also used to determine the target plane model based on a seed point in the initial anchor point cloud data and two random points in the plane model fitted by the initial seed point; calculate the target distance from the remaining seed points in the initial anchor point cloud data to the target plane model; count the number of points whose target distance is less than a preset distance threshold, and if the number of points exceeds the preset minimum number of support points, determine the target plane model as a qualified model; repeat the above steps until the preset number of iterations is reached, and determine the data in the target plane model with the maximum number of support points as the target anchor point cloud data.

[0203] It should be noted that the description of the device of the embodiment of the present invention is similar to the description of the above method embodiment, and has similar beneficial effects as the method embodiment, so it is not repeated. For technical details not disclosed in the device embodiment, please refer to the description of the method embodiment of the present invention for understanding.

[0204] It should be noted that in the embodiments of the present invention, if the above-mentioned coal mine boring machine positioning method integrating the depth camera and inertial navigation is implemented in the form of a software function module and sold or used as an independent product, it can also be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present invention can be essentially or partly reflected in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a terminal to execute all or part of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a U disk, a mobile hard disk, a read-only memory (ROM), a magnetic disk or an optical disk. In this way, the embodiments of the present invention are not limited to any specific combination of hardware and software.

[0205] Correspondingly, an embodiment of the present invention provides a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system. Fig.10 FIG. 1 is a schematic diagram of the structure of a coal mine boring machine positioning device that integrates a depth camera and an inertial navigation system according to an embodiment of the present invention. Fig.10As shown, the coal mine boring machine positioning device 1000 fused with a depth camera and an inertial navigation system at least includes: a processor 1001 and a computer-readable storage medium 1002 configured to store executable instructions, wherein the processor 1001 generally controls the overall operation of the coal mine boring machine positioning device 1000 fused with a depth camera and an inertial navigation system. The computer-readable storage medium 1002 is configured to store instructions and applications executable by the processor 1001, and can also cache data to be processed or processed by each module in the processor 1001 and the coal mine boring machine positioning device 1000 fused with a depth camera and an inertial navigation system, which can be implemented through a flash memory (FLASH) or a random access memory (Random Access Memory, RAM).

[0206] An embodiment of the present invention provides a storage medium storing executable instructions, wherein the executable instructions are stored. When the executable instructions are executed by a processor, the processor will be caused to execute the method provided by the embodiment of the present invention, for example, Figure 1 The method shown.

[0207] In some embodiments, the storage medium can be a computer-readable storage medium, for example, a ferroelectric random access memory (FRAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a flash memory, a magnetic surface memory, an optical disk, or a compact disk read-only memory (CD-ROM), etc.; it can also be various devices including one or any combination of the above memories.

[0208] In some embodiments, executable instructions may be in the form of a program, software, software module, script or code, written in any form of programming language (including compiled or interpreted languages, or declarative or procedural languages), and may be deployed in any form, including as a stand-alone program or as a module, component, subroutine or other unit suitable for use in a computing environment.

[0209] As an example, executable instructions may, but need not necessarily, correspond to a file in a file system, may be stored as part of a file storing other programs or data, for example, in one or more scripts in a Hyper Text Markup Language (HTML) document, in a single file dedicated to the program in question, or in multiple coordinated files (e.g., files storing one or more modules, subroutines, or code portions). As an example, executable instructions may be deployed to be executed on one electronic device, or on multiple electronic devices located at one location, or on multiple electronic devices distributed at multiple locations and interconnected by a communication network.

[0210] The above description is only an embodiment of the present invention and is not intended to limit the protection scope of the present invention. Any modification, equivalent replacement and improvement made within the spirit and scope of the present invention are included in the protection scope of the present invention.

[0211] It should be understood that "one embodiment" or "an embodiment" mentioned throughout the specification means that specific features, structures or characteristics related to the embodiment are included in at least one embodiment of the present invention. Therefore, "in one embodiment" or "in an embodiment" appearing throughout the specification does not necessarily refer to the same embodiment. In addition, these specific features, structures or characteristics can be combined in one or more embodiments in any suitable manner. It should be understood that in various embodiments of the present invention, the size of the serial number of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention. The serial numbers of the above-mentioned embodiments of the present invention are only for description and do not represent the advantages and disadvantages of the embodiments.

[0212] It should be noted that, in this article, the terms "comprise", "include" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method or device. In the absence of further restrictions, an element defined by the statement "comprises one..." does not exclude the presence of other identical elements in the process, method, article or device including the element. In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as: multiple units or components can be combined, or can be integrated into another system, or some features can be ignored, or not executed.

[0213] The above is only an embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A coal mine boring machine positioning method integrating depth camera and inertial navigation, characterized in that: The method comprises: Acquire an anchor point cloud image when the tunnel boring machine is running, as well as acceleration information and gyroscope information when the tunnel boring machine is running; Preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; Performing feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix; Determining first position information of the roadheader based on a position increment obtained by solving the target rigid body transformation matrix; Performing posture calculation on the acceleration information and the gyroscope information to obtain inertial navigation error information of the tunnel boring machine; Based on the inertial navigation error information and the first position information, establishing a target state equation and a target observation equation of a depth camera and inertial navigation fusion system; The target state equation and the target observation equation are applied to a pre-constructed error state Kalman filter for filtering estimation and discretization processing, so as to realize the information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

2. The method according to claim 1, characterized in that The preprocessing of the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data includes: Performing point cloud through filtering on the anchor point cloud image to obtain point cloud data of the anchor area; Performing point cloud statistical filtering on the anchor area point cloud data to obtain initial anchor point cloud data; The initial anchor point cloud data is segmented to obtain the target anchor point cloud data.

3. The method according to claim 1, characterized in that The step of performing feature extraction, coarse registration, and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix includes: By presetting a fast point feature histogram, the FPFH feature of each point in the source anchor point cloud data and the target anchor point cloud data is calculated respectively to obtain a source FPFH feature descriptor and a target FPFH feature descriptor; By using a preset sampling consistency algorithm, randomly sampling point pairs are performed on the source FPFH feature descriptor and the target FPFH feature descriptor, and an initial rigid body transformation matrix between the source FPFH feature descriptor and the target FPFH feature descriptor is estimated to obtain an initial paired point cloud; By searching the KD tree, the initial paired point cloud is precisely aligned, the nearest neighbor point is determined from the source anchor point cloud data, and a point pair correspondence relationship between the point pairs is established; Based on the point pair correspondence, the target rigid body transformation matrix is ​​determined.

4. The method according to claim 1, characterized in that: The inertial navigation error information at least includes second position information; The establishing of a target state equation and a target observation equation of a depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information comprises: Using the inertial navigation error information as the state quantity of the depth camera and inertial navigation fusion system to determine a state vector; Based on the state vector, establishing the target state equation; Taking the position difference between the first position information and the second position information as the observation quantity of the depth camera and inertial navigation fusion system to determine the observation vector; Based on the observation vector, the target observation equation is established.

5. The method according to claim 2, characterized in that: The step of performing point cloud through filtering on the anchor point cloud image to obtain point cloud data of the anchor area includes: Based on the characteristics of the anchor point cloud image, determine to perform filtering on the X-axis and the Z-axis in the anchor point cloud image, and set the threshold range of the X-axis and the Z-axis; Based on the threshold ranges of the X-axis and the Z-axis respectively, the point cloud data of each point cloud data in the anchor point cloud image outside the corresponding coordinate axis is removed to obtain the anchor area point cloud data.

6. The method according to claim 2, characterized in that The step of performing point cloud statistical filtering on the anchor area point cloud data to obtain initial anchor point cloud data includes: Calculate the average distance s between each sampling point in the anchor area point cloud data and each neighboring point in the neighborhood k of the corresponding sampling point j , Where (x, y, z) is the coordinate value of a sampling point, (xi, yi, zi) is the coordinate value of the i-th neighborhood point, i = 1, 2, 3…k, j is the serial number of the sampling point; Calculate the average μ of a plurality of said average distances, In the formula, n represents the number of sampling points involved in the average distance calculation; Based on the average distance and the average value, the standard deviation σ is calculated, Based on the preset standard deviation multiple std, if the average distance of each sampling point is within the interval (μ-std·σ,μ+std·σ), the sampling point is retained; if it exceeds the interval, the sampling point is removed from the anchor area point cloud data.

7. The method according to claim 2, characterized in that: The step of performing point cloud segmentation on the initial anchor point cloud data to obtain the target anchor point cloud data includes: Determine a target plane model based on a seed point in the initial anchor point cloud data and two random points in a plane model fitted by the initial seed point; Calculating target distances from remaining seed points in the initial anchor point cloud data to the target plane model; Counting the number of points whose target distance is less than a preset distance threshold, and if the number of points exceeds a preset minimum number of supporting points, determining the target plane model as a model that meets the conditions; The above steps are repeated until a preset number of iterations is reached, and the data in the target plane model with the maximum number of support points is determined as the target anchor point cloud data.

8. A coal mine boring machine positioning device integrating a depth camera and an inertial navigation system, characterized in that: The device comprises: An acquisition module, used to acquire an anchor point cloud image when the tunnel boring machine is running, as well as acceleration information and gyroscope information when the tunnel boring machine is running; A preprocessing module, used for preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; A registration module, used for performing feature extraction, coarse registration and fine registration on the source anchor point cloud data and the target anchor point cloud data of two adjacent frames to obtain a target rigid body transformation matrix; A determination module, configured to determine first position information of the roadheader based on a position increment obtained by solving the target rigid body transformation matrix; A solution module, used for performing posture solution on the acceleration information and the gyroscope information to obtain inertial navigation error information of the tunnel boring machine; An establishing module, used for establishing a target state equation and a target observation equation of a depth camera and inertial navigation fusion system based on the inertial navigation error information and the first position information; A fusion module is used to apply the target state equation and the target observation equation to a pre-built error state Kalman filter for filtering estimation and discretization processing, so as to realize the information fusion of the depth camera and the inertial navigation, and obtain the posture change information of the tunnel boring machine.

Images