A coal mine roadheader positioning method and device integrating depth camera and inertial navigation

Abstract

The embodiment of the present invention provides a method and device for positioning a coal mine roadheader by integrating a depth camera with an inertial navigation system, comprising: collecting anchor point cloud images, acceleration information, and gyroscope information; solving the pose of the inertial navigation information; preprocessing the anchor point cloud images, including point cloud direct filtering, point cloud statistical filtering, and point cloud segmentation; alignment of the anchor point cloud; solving the pose of the visual information; and fusing the visual and inertial navigation information to obtain pose change information of the roadheader. Based on the strapdown inertial navigation system, the present invention introduces vision as an auxiliary positioning system, takes supported anchors as the research target, and uses the position data measured by the depth camera for anchor point cloud alignment to correct the accumulated error of the inertial navigation system due to drift over time. Combining the advantages of the two sensors, the present invention can adapt to complex coal mine environments and ensure the long-term precise positioning of coal mine roadheaders.

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 that integrates a depth camera and an inertial navigation. Background Art

[0002] Coal is the cornerstone of my country's energy resources, and intelligent coal mining is the only way for the coal industry to achieve high-quality development. As a key piece of equipment in coal mine tunneling, the positioning technology of the roadheader is one of the key technologies for intelligent coal mine roadheaders. Coal mine tunneling requires high positioning accuracy for the roadheader. Positioning technology based on a single sensor is difficult to achieve precise positioning, and positioning methods based on targets face complex challenges such as occlusion and target migration. Therefore, research on coal mine roadheader positioning methods aims to achieve precise positioning of the roadheader without a target, thus solving the problem of accurate positioning of the roadheader in the complex environment of coal mine tunnels.

[0003] Due to the constraints of the underground environment, single sensors can be subject to interference or failure, reducing the roadheader's measurement accuracy and stability. Existing positioning methods all have limitations. For example, total stations, due to their inherent technical limitations, can reduce the accuracy of medium- and long-range positioning. Vision and lidar require high computing resources and are susceptible to factors such as occlusion and lighting in complex environments. Therefore, in complex underground environments, fusion positioning technology plays a vital role in improving positioning accuracy and robustness.

[0004] Depth cameras are widely used because they are unaffected by lighting and can obtain real-time distance and depth information about objects in a scene. Depth information allows for more accurate scene understanding and analysis, providing more information for subsequent processing and decision-making, and enabling better positioning and navigation tasks in complex environments. Furthermore, point cloud registration technology can be used to align point cloud data from multiple perspectives or time periods, enabling accurate target tracking. 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 that integrates 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 of the tunnel boring machine during operation, as well as acceleration information and gyroscope information of the tunnel boring machine during operation;

[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 roadheader;

[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-built error state Kalman filter for filtering estimation and discretization processing to achieve information fusion of the depth camera and 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 to preprocess the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data;

[0018] A registration module 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;

[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 calculation module, configured to perform posture calculation on the acceleration information and the gyroscope information to obtain inertial navigation error information of the roadheader;

[0021] An establishment module, configured to establish 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; and 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 the 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 the state quantity of the depth camera and inertial navigation fusion system to determine the 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 the observation quantity of the depth camera and inertial navigation fusion system to determine the observation vector; based on the observation vector, establish the target observation equation.

[0026] In some embodiments, the preprocessing module is also used to determine the filtering on the X-axis and Z-axis in the anchor point cloud image based on the characteristics of the anchor point cloud image, and set the threshold range of the X-axis and the Z-axis; based on the threshold range of the X-axis and the Z-axis, 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.

[0027] In some embodiments, the pre-processing module is further configured 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 , Where (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 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.

[0029] An embodiment of the present invention provides a coal mine roadheader positioning device that integrates a depth camera and an inertial navigation system, including: a memory for storing executable instructions; and a processor for implementing the above-mentioned coal mine roadheader positioning method that integrates 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 provided by the embodiment of the present invention with the fusion of depth camera and inertial navigation 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; solve the posture of 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 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 inertial navigation and obtain the posture change information of the roadheader. In this way, the visual position detection of the tunnel boring machine is carried out with the supported anchor bolts as the research target. 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 coarse registration + fine registration method, 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 problems of the accumulation of inertial navigation measurement tunnel boring machine posture error over time and the difficulty of visual positioning technology in accurately detecting 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 This is a flow chart of a method for positioning a coal mine roadheader by integrating a depth camera with an inertial navigation system, provided by an embodiment of the present invention;

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

[0034] Figure 3 This is a flow chart of a method for positioning a coal mine roadheader by integrating a depth camera with 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 This 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 This is a diagram of the X-direction position measurement results provided by an embodiment of the present invention;

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

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

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

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

[0043] In the following description, references to "some embodiments" describe a subset of all possible embodiments. However, 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 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 for the purpose of describing the embodiments of the present invention only and are not intended to limit the present invention.

[0044] The following describes an exemplary application of a coal mine roadheader positioning device that integrates a depth camera and inertial navigation according to an embodiment of the present invention. The coal mine roadheader positioning device that integrates a depth camera and inertial navigation provided by an embodiment of the present invention can be implemented as a terminal or a server. In one implementation, the coal mine roadheader positioning device that integrates a depth camera and inertial navigation provided by an embodiment of the present invention can be implemented as various types of terminals such as laptops, tablets, desktop computers, and mobile devices. In another implementation, the coal mine roadheader positioning device that integrates a depth camera and inertial navigation 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 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 delivery networks (CDNs), and big data and artificial intelligence platforms. The terminal and the server can be directly or indirectly connected via wired or wireless communication, which is not limited in the embodiment of the present invention. The following describes an exemplary application of a coal mine roadheader positioning device that integrates a depth camera and inertial navigation as a server.

[0045] The embodiment of the present invention provides a coal mine roadheader positioning method integrating a depth camera and an inertial navigation system. Figure 1 , Figure 1 This is a flow chart of a method for positioning a coal mine roadheader 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 installed on the tunnel boring machine body; the acceleration information and gyroscope information are collected by an inertial sensor installed on the tunnel boring machine body.

[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 roadheader during operation obtained by an inertial sensor, and the gyroscope information refers to the rotational angular velocity of the roadheader 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 straight-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 involves extracting features from the source anchor point cloud data and the target anchor point cloud data using a fast point feature histogram to obtain a source FPFH feature descriptor and a target FPFH feature descriptor. 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 coarsely registering 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 achieve 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 two adjacent frames of anchor point clouds, the sampling consistency (SAC-IA) algorithm is used to complete the coarse alignment, and the KD tree is introduced into the ICP algorithm for point pair search to complete the precise alignment of the tunnel anchor point cloud.

[0059] Step S140: Determine 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, which 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 in a target-free manner.

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

[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 depth camera fusion, the target state equation and target observation equation are mathematical models that describe the system's dynamic behavior and the relationship between observation data and the system's state. These equations are the basis for designing Kalman filters, which are used to estimate and predict the system's state.

[0067] In some embodiments, the target state equation, also known as the system dynamics equation, refers to the time-dependent evolution of the system state in the absence of external observations. In an inertial navigation and depth camera fusion system, the system state may include position, velocity, attitude, heading, and possibly internal inertial navigation parameters such as gyroscope and accelerometer bias.

[0068] In some embodiments, the target observation equation, also known as the measurement equation, refers to the relationship between a system's observation data and its state. In an inertial navigation and depth camera fusion system, the observation data comes from the depth camera's visual odometry and is used to correct for inertial navigation drift and errors.

[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 derive the state vector, thereby obtaining the state equation, and the difference between the positions measured by inertial navigation and vision is used as the observation quantity, thereby obtaining the observation equation.

[0070] Step S170: Apply the target state equation and the target observation equation to a pre-built error state Kalman filter for filtering estimation and discretization processing to achieve information fusion of the depth camera and inertial navigation, and obtain the posture change information of the roadheader.

[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, information fusion of inertial navigation and depth camera can be realized.

[0072] The embodiment of the present invention provides a coal mine roadheader positioning method that integrates depth camera and inertial navigation. In this way, the visual position of the roadheader is detected with the supported anchor bolt as the research target, and the interference of useless point clouds and noise 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 coarse alignment + fine alignment method, and the roadheader position increment is solved according to the point cloud alignment result, thereby realizing the precise positioning of the roadheader in a target-free manner and improving the stability of the roadheader position measurement; 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 the error state Kalman filter algorithm, which effectively solves the problem that the error of the roadheader posture measured by inertial navigation accumulates over time and the visual positioning technology is difficult to accurately detect the posture of the roadheader due to factors such as vibration noise and magnetic interference, ensuring that the coal mine tunnel roadheader can achieve long-term precise positioning and can adapt to the roadheader positioning needs in low-speed motion scenarios.

[0073] In some embodiments, the above step S120 can be implemented by the following 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 subjected to through-filtering, 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 can 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 ranges 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, filtering is determined on the X and Z coordinate axes, and a threshold range on the axis is set, where 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 this method, the point cloud is traversed and points are retained or eliminated. The coordinate values ​​of each point cloud data in the selected axis are judged. Filtering is performed based on the X coordinate axis, setting the points whose x coordinates are within the X-axis threshold to be retained. Further filtering is performed based on the Z coordinate axis, setting the points whose z coordinates are within the Z-axis threshold to be retained. This removes point cloud data that is irrelevant to the anchor measurement or affects the measurement accuracy.

[0081] In the present invention, after the point cloud is directly filtered, the filtering effect is verified. The point cloud data before and after filtering can be observed in the visualization window to see whether irrelevant roadway environment information is effectively filtered out and the point cloud data of the anchor area is extracted.

[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, thereby ensuring 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, 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 ,

[0086] In the present invention, point cloud statistical filtering is used to eliminate outliers caused by measurement noise. First, the average distance s between the sampling point p and all points in its k-neighborhood is 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 point.

[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: Calculate the standard deviation σ based on the average distance and the average value.

[0092]

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

[0094] In step S1224, a judgment is made based on a 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 separate 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: Calculate 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 , repeating the above steps until a preset number of iterations is reached, and determining the data in the target plane model with the maximum number of support points 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 of other points to the plane are calculated.

[0103] In this method, the number of points whose distance is less than the threshold is counted. If the number of points exceeds the preset minimum number of support points, the plane model is considered to meet the requirements. The above steps are repeated 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.

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

[0105] Step S131 , by using a preset 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 estimate the rigid body transformation matrix between them to complete the initial alignment.

[0107] Step S132: randomly sampling point pairs of the source FPFH feature descriptor and the target FPFH feature descriptor through a preset sampling consistency algorithm, and estimating 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 , performing precise registration on the initial paired point cloud by searching the KD tree, determining the nearest neighbor points from the source anchor point cloud data, and establishing point pair correspondences between the point pairs.

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

[0110] In this invention, the results of the coarse registration are 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. Based on 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 the 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] Where 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: Using 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.

[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 coal mine roadheader positioning method that integrates a depth camera and an inertial navigation system. The method has a reasonable design and good positioning effect. The method uses supported anchor bolts as the research target to perform visual position detection of the roadheader. 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 used to obtain the conversion relationship of the anchor bolt point cloud, and the roadheader position increment is calculated based on the point cloud alignment result, realizing the precise positioning of the roadheader in a target-free manner and improving the stability of the visual measurement system; the error-state-based Kalman filtering algorithm is used to realize the fusion of the two types of information, ensuring that the coal mine tunnel roadheader can achieve long-term precise positioning.

[0119] Combine Figure 2-3 Another coal mine roadheader positioning method provided by the present invention that integrates a depth camera and an inertial navigation system 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 installed on the TBM body. The depth camera is responsible for collecting anchor depth point cloud image information; the inertial sensor is responsible for obtaining acceleration and gyroscope information.

[0121] In this embodiment, the depth camera data acquisition frequency is 5 Hz, and the inertial sensor data acquisition frequency 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 calculates the heading, attitude, speed and position of the tunnel boring machine based on this measurement information.

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

[0125] It should be noted that since the same reference target anchor cannot remain in the camera's field of view throughout the TBM's motion, solving the TBM's dynamic pose requires a continuous series of reference target anchors for solving the TBM's coordinate system pose. The two selected rows of reference target anchors cannot disappear from the camera's field of view simultaneously. Position measurement is achieved by matching 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 the 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. The threshold range includes a minimum value and a maximum value.

[0131] Step 3012: Traverse the point cloud and retain or remove points: Determine the coordinate values ​​of each point cloud data point along the selected axis. Filter based on the X-axis, retaining points whose x-coordinates fall within the X-axis threshold. Further filter based on the Z-axis, retaining points whose z-coordinates fall within the Z-axis threshold. Remove point cloud data that is irrelevant to the anchor measurement or affects measurement accuracy.

[0132] Step 3013: After the point cloud direct filtering is completed, the filtering effect is verified by observing the point cloud data before and after filtering in the visualization window to see whether irrelevant roadway 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 statistical filtering method is used to remove outliers and noise in the point cloud data of the anchor area. The specific process is 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 a number n j The average value μ:

[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 outside 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 two other 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 meet the requirements. 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: Based on the extracted anchor point clouds of two adjacent frames, the fast point feature histogram (FPFH) is used for point pair matching, 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 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 using 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 coarse 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 obtain the 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] Where 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 calculation: Calculating the position increment of the measurement camera based on the point cloud registration results can determine the actual position change of the tunnel boring machine in space, achieving precise positioning of the tunnel boring machine without a target.

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

[0152] It's important to note that strapdown inertial navigation systems offer excellent autonomous positioning capabilities, but their positioning errors drift and accumulate over time. Visual positioning systems struggle to maintain stability and accuracy in coal mine environments due to factors like vibration, noise, and magnetic interference. However, visual measurement methods are gaining popularity due to their non-contact nature and lack of cumulative errors. Therefore, research on combined positioning and navigation technologies based on inertial sensors and visual cameras can address the positioning needs of roadheaders 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 derive 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, 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 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] Where, 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] Where 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 the visual fusion of inertial navigation and depth camera is characterized in that an ESKF filter is constructed in step 602, and the method of realizing the information fusion of inertial navigation and depth camera by combining the filter is as follows:

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

[0168]

[0169] Where, is the state estimate at the previous moment; ν k is the control input; x k is the real state at the current k moment.

[0170]

[0171] Where, 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] Where 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 measurement 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 predicted state X k and the predicted mean square error state

[0184]

[0185]

[0186] The second stage is the measurement update, which includes calculating 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-mentioned 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 posture data, the point cloud image technology is used to collect 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 roadheader and provide the actual measured position information of the roadheader. 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, 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. 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 two adjacent frames of anchor point clouds after preprocessing are registered using a high-precision registration algorithm based on SAC-IA and optimized ICP. The registered point cloud is shown in the figure below. Figure 6 (b) 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 used as the true value, and the position information of the roadheader calculated by information fusion is used as the measurement. The positioning results of the roadheader in the x direction and the y direction are calculated respectively. Figure 7 、 Figure 8 As shown in the figure, the fused positioning curve closely matches the actual position curve. Within a distance of 20.32m, the average position error in the tunnel width direction is 24.60mm, with a maximum error of 61.04mm. The average position error in the tunneling direction is 16.06mm, with a maximum error of 43.77mm. Therefore, leveraging the advantages of inertial navigation and visual measurement improves the anti-interference capability and environmental adaptability of position measurement, ensuring positioning accuracy and stability in complex practical application scenarios.

[0194] In summary, the present invention 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 used as the research target to carry out visual position detection of the tunnel boring machine. 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 rod point cloud is obtained by the coarse alignment + fine alignment method, 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 filter algorithm, which effectively solves the problems of the accumulation of inertial navigation measurement tunnel boring machine posture error over time and the difficulty of visual positioning technology in accurately detecting the tunnel boring machine posture due to vibration noise, magnetic interference and other factors, 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.

[0196] Figure 9 FIG. 1 is a schematic diagram of the structure of a coal mine roadheader positioning device integrating a depth camera and an inertial navigation system according to an embodiment of the present invention. Figure 9 As shown, the coal mine roadheader positioning device 900 fused with a depth camera and an inertial navigation comprises: an acquisition module 901 for acquiring 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 for preprocessing the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; a registration module 903 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 904 for calculating the 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; and 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 the point pairs; and determine the target rigid body transformation matrix based on the point pair correspondence relationship.

[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 the state quantity of the depth camera and inertial navigation fusion system to determine the 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 the observation quantity of the depth camera and inertial navigation fusion system to determine the 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 the filtering on the X-axis and Z-axis in the anchor point cloud image based on the characteristics of the anchor point cloud image, and set the threshold range of the X-axis and the Z-axis; based on the threshold range of the X-axis and the Z-axis, 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.

[0201] In some embodiments, the pre-processing module 902 is further configured 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 , Where (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 embodiment of the present invention is similar to the description of the above-mentioned method embodiment and has similar beneficial effects as the method embodiment, so it will not be repeated here. 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 embodiment of the present invention, if the above-mentioned coal mine roadheader positioning method integrating 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, or the part that contributes to the relevant technology, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling 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 embodiment of the present invention is 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 that integrates a depth camera and an inertial navigation system. Figure 10 FIG. 1 is a schematic diagram of the structure of a coal mine roadheader positioning device that integrates a depth camera and an inertial navigation system according to an embodiment of the present invention. Figure 10As shown, the coal mine roadheader positioning device 1000 integrating a depth camera and inertial navigation includes at least: a processor 1001 and a computer-readable storage medium 1002 configured to store executable instructions. The processor 1001 generally controls the overall operation of the coal mine roadheader positioning device 1000 integrating a depth camera and inertial navigation. 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 various modules in the processor 1001 and the coal mine roadheader positioning device 1000 integrating a depth camera and inertial navigation. This can be achieved using flash memory (FLASH) or 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, such as 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 disc, or a compact disc read-only memory (CD-ROM); 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 examples, 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, such as one or more scripts in a Hypertext Markup Language (HTML) document, in a single file dedicated to the program in question, or in multiple coordinating files (e.g., files storing one or more modules, subroutines, or code portions). As examples, executable instructions may be deployed to be executed on one electronic device, or on multiple electronic devices located in one location, or on multiple electronic devices distributed across multiple locations and interconnected by a communication network.

[0210] The above description is merely an embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, and improvements made within the spirit and scope of the present invention are included in the scope of protection of the present invention.

[0211] It should be understood that "one embodiment" or "an embodiment" mentioned throughout the specification means that the 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 numbers 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 for description only 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 comprising a series of elements includes not only those elements, but also 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 a..." does not exclude the presence of other identical elements in the process, method, article or device comprising the element. In the 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. In actual implementation, there may be other division methods, 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 description is merely an embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A coal mine roadheader positioning method integrating depth camera and inertial navigation, characterized in that: The method comprises: Acquire an anchor point cloud image of the tunnel boring machine during operation, as well as acceleration information and gyroscope information of the tunnel boring machine during operation; 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 roadheader; 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-built error state Kalman filter for filtering estimation and discretization processing to achieve information fusion of the depth camera and 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 anchor area point cloud data; Performing 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.

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 features of each point in the source anchor point cloud data and the target anchor point cloud data are calculated respectively to obtain a source FPFH feature descriptor and a target FPFH feature descriptor; Randomly sampling point pairs of the source FPFH feature descriptor and the target FPFH feature descriptor using a preset sampling consistency algorithm, and estimating an initial rigid body transformation matrix between the source FPFH feature descriptor and the target FPFH feature descriptor to obtain an initial paired point cloud; By searching the KD tree, the initial paired point cloud is precisely registered, the nearest neighbor points are determined from the source anchor point cloud data, and a point pair correspondence relationship is established between the point pairs; Based on the point pair correspondence, the target rigid body transformation matrix is ​​determined.

4. The method according to claim 1, wherein The inertial navigation error information includes at least 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 includes: Using 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, 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 anchor area point cloud data 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, 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.

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 corresponding sampling point neighborhood k 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 sequence number of the sampling point; Calculate the average μ of a plurality of the 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.

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; 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.

8. A coal mine roadheader 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 to preprocess the anchor point cloud image to obtain source anchor point cloud data and target anchor point cloud data; A registration module 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, configured to determine first position information of the roadheader based on a position increment obtained by solving the target rigid body transformation matrix; A calculation module, configured to perform posture calculation on the acceleration information and the gyroscope information to obtain inertial navigation error information of the roadheader; An establishment module, configured to establish 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