Unmanned aerial vehicle positioning method and device for underground mine, electronic equipment and storage medium

Abstract

The invention provides an unmanned aerial vehicle positioning method and device for an underground mine, electronic equipment and a storage medium, and relates to the technical field of unmanned aerial vehicle navigation positioning, and the method comprises the steps: obtaining sensor data of an unmanned aerial vehicle, the sensor data comprising laser radar data and inertial navigation data; laser instant positioning and map construction positioning results under the unmanned aerial vehicle coordinate system are obtained according to sensor data; obtaining a laser inter-frame matching positioning result under the unmanned aerial vehicle coordinate system according to the sensor data; and carrying out information fusion on the laser instant positioning and map construction positioning result and the laser inter-frame matching positioning result to obtain a fusion positioning result. By fusing the laser radar data and the inertial navigation data, complementation of multi-source information is achieved, the overall performance of unmanned aerial vehicle positioning and navigation and the positioning precision and environment adaptability of the unmanned aerial vehicle in a complex environment are improved, and the robustness and reliability of a positioning system are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle navigation and positioning, and in particular to a method, device, electronic equipment and storage medium for positioning an unmanned aerial vehicle in an underground mine. Background Art

[0002] Underground mines, as a typical closed, complex and severely light-poor environment, pose a great challenge to any technology that relies on vision or satellite signals. Machine vision technology, as one of the key technologies in the field of automation and robotics, is highly dependent on clear and sufficient visual input for analysis and decision-making. Among them, drones are used in underground mines for environmental monitoring, equipment inspection, topographic mapping and emergency rescue to improve operational efficiency and safety.

[0003] However, in underground mines, light is difficult to penetrate due to the obstruction of rocks, ores and other geological structures, resulting in extremely limited or even complete lack of visual information. Such extreme lighting conditions greatly restrict the application of machine vision technology, making it impossible to effectively perform tasks such as target identification, path planning or obstacle detection. Secondly, GPS and other satellite-based positioning systems rely on signals from multiple satellites for accurate three-dimensional positioning. However, in underground environments, satellite signals are shielded by thick layers of rock, causing the signal strength to drop sharply or even disappear completely. This means that traditional satellite positioning systems can hardly provide reliable positioning services in underground mines, greatly increasing the difficulty and risk of drones flying in such environments. Therefore, there is an urgent need to solve the problem of failure of traditional machine vision and satellite positioning systems for drones caused by rock obstruction and lack of light. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention provides a method, device, electronic device and storage medium for UAV positioning in underground mines, which effectively solve the problem of failure of traditional machine vision and satellite positioning systems of UAVs caused by rock obstruction and lack of light.

[0005] In a first aspect, the present invention provides a method for positioning an unmanned aerial vehicle in an underground mine, wherein the unmanned aerial vehicle is provided with a laser radar module and an inertial navigation module, and the method comprises:

[0006] Acquiring sensor data of the drone, wherein the sensor data includes lidar data and inertial navigation data;

[0007] Acquire the laser real-time positioning and map construction positioning results in the UAV coordinate system according to the sensor data;

[0008] Acquire the laser frame matching positioning result in the UAV coordinate system according to the sensor data;

[0009] The laser instant positioning and map construction positioning result and the laser frame matching positioning result are fused to obtain a fused positioning result.

[0010] Furthermore, the instant laser positioning and map construction positioning results in the coordinate system of the drone are obtained according to the sensor data, including:

[0011] Obtaining all point cloud information in the laser radar data;

[0012] The point cloud information is matched by using an iterative closest point algorithm to obtain the laser instant positioning and map construction positioning results in the coordinate system of the UAV at each moment;

[0013] The inertial navigation data is used to convert each frame of the point cloud information into the UAV coordinate system to obtain laser real-time positioning and map construction of a global map.

[0014] Furthermore, obtaining the laser frame matching positioning result in the drone coordinate system according to the sensor data includes:

[0015] Obtaining the current frame point cloud data and the previous frame point cloud data pose in the laser radar data;

[0016] Divide the space occupied by the current frame point cloud data into a plurality of grids, each of which includes a laser point cloud set;

[0017] Calculating the mean value and covariance matrix of the laser point cloud set;

[0018] Projecting the current frame of point cloud data according to the pose of the previous frame of point cloud data to obtain the previous frame of point cloud data;

[0019] An evaluation function is calculated based on the previous frame of point cloud data, the average value and the covariance matrix;

[0020] The evaluation function is optimized by using a gradient descent method to obtain the current frame pose;

[0021] The laser frame matching positioning results in the UAV coordinate system at each moment are obtained according to the current frame pose.

[0022] Furthermore, the information fusion of the laser instant positioning and map construction positioning result and the laser frame matching positioning result is performed to obtain a fused positioning result, including:

[0023] Predicting the state of the UAV according to the inertial navigation data to obtain a system state equation and a system uncertainty equation of the UAV;

[0024] Observe the system status of the UAV according to the laser real-time positioning and map construction positioning result and the laser frame matching positioning result, and obtain the laser real-time positioning and map construction observation equation and the laser frame matching observation equation of the UAV;

[0025] The system state equation and the system uncertainty equation are updated respectively according to the laser instant positioning and map construction observation equation and the laser frame matching observation equation to obtain the fused positioning result.

[0026] Furthermore, the state prediction of the UAV is performed according to the inertial navigation data to obtain the system state equation and system uncertainty equation of the UAV, including:

[0027] Acquiring acceleration information from the inertial navigation data;

[0028] calculating external variables and process noise based on the acceleration information;

[0029] Determine the system state equation according to the external variables combined with the state transfer matrix;

[0030] The system uncertainty equation is determined based on the process noise in combination with the state transfer matrix and the state covariance matrix.

[0031] Furthermore, the system state of the UAV is observed respectively according to the positioning result of the laser instant positioning and map construction and the positioning result of the laser frame matching, and the laser instant positioning and map construction observation equation and the laser frame matching observation equation of the UAV are obtained, including:

[0032] According to the laser instant positioning and mapping positioning result and the laser instant positioning and mapping positioning observation matrix, the laser instant positioning and mapping observation equation is obtained;

[0033] The laser frame matching observation equation is obtained according to the laser frame matching positioning result combined with the laser frame matching positioning observation matrix.

[0034] Furthermore, the system state equation and the system uncertainty equation are updated respectively according to the laser real-time positioning and map construction observation equation and the laser frame matching observation equation to obtain the fused positioning result, including:

[0035] Obtaining a laser instant positioning and map construction positioning observation matrix covariance according to the laser instant positioning and map construction positioning result;

[0036] Obtaining the laser frame matching positioning observation matrix covariance according to the laser frame matching positioning result;

[0037] Obtaining a first Kalman gain according to the covariance of the laser instant positioning and map construction positioning observation matrix combined with the laser instant positioning and map construction observation matrix and the state covariance matrix;

[0038] Obtaining a second Kalman gain according to the laser frame matching positioning observation matrix covariance combined with the laser frame matching observation matrix and the state covariance matrix;

[0039] According to the first Kalman gain combined with the laser real-time positioning and map construction observation equation, the system state equation and the system uncertainty equation are updated;

[0040] The system state equation and the system uncertainty equation are updated according to the second Kalman gain in combination with the laser frame matching observation equation.

[0041] In a second aspect, the present invention provides a UAV positioning device for underground mines, wherein the UAV is provided with a laser radar module and an inertial navigation module, and the device comprises:

[0042] A data acquisition module, used to acquire sensor data of the drone, wherein the sensor data includes laser radar data and inertial navigation data;

[0043] A first processing module is used to obtain the laser real-time positioning and map construction positioning results in the UAV coordinate system according to the sensor data;

[0044] A second processing module is used to obtain the laser frame matching positioning result in the UAV coordinate system according to the sensor data;

[0045] The positioning fusion module is used to fuse the laser instant positioning and map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

[0046] In a third aspect, the present invention provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the drone positioning method for underground mines as described in the first aspect of the present invention.

[0047] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the method for positioning an underground mine using a drone as described in the first aspect of the present invention is implemented.

[0048] The method, device, electronic device and storage medium for positioning the unmanned aerial vehicle in underground mines provided by the present invention realize the complementarity of multi-source information by fusing laser radar data and inertial navigation data, and improve the overall performance of positioning and navigation of the unmanned aerial vehicle. By processing the multi-source information of the unmanned aerial vehicle movement, it is possible to continuously provide effective position and speed prediction in a complex environment where the GPS signal is unstable or completely missing, thereby improving the positioning accuracy and environmental adaptability of the unmanned aerial vehicle in a complex environment. At the same time, the state of the unmanned aerial vehicle is observed in combination with the laser SLAM positioning results and the laser inter-frame matching positioning results. The laser SLAM technology can realize accurate positioning and map construction in a GPS-free environment. The positioning reliability can be further improved by combining the laser inter-frame matching positioning. The environmental map constructed by the laser SLAM technology can be used to correct the track of the unmanned aerial vehicle, reduce the cumulative error caused by multi-source information processing, significantly improve the robustness and reliability of the positioning system, and ensure the positioning accuracy of the unmanned aerial vehicle during long-term flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0050] Figure 1 This is a first schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention;

[0051] Figure 2 This is a second schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention;

[0052] Figure 3 3 is a schematic diagram of a method for positioning an underground mine by a drone provided by an embodiment of the present invention;

[0053] Figure 4 4 is a schematic diagram of a method for positioning an underground mine using a drone provided by an embodiment of the present invention;

[0054] Figure 5 This is a fifth schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention;

[0055] Figure 6 This is a sixth schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention;

[0056] Figure 7 This is the seventh schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention;

[0057] Figure 8 It is a schematic diagram of the structure of a drone positioning device for underground mines provided by an embodiment of the present invention;

[0058] Fig. 9 It is a structural schematic diagram of an electronic device provided by an embodiment of the present invention.

[0059] Description of main component symbols:

[0060] 800. UAV positioning device for underground mines; 810. Data acquisition module; 820. First processing module; 830. Second processing module; 840. Positioning fusion module; 900. Electronic device; 910. Processor; 920. Communication interface; 930. Memory; 940. Communication bus. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. It should be noted that the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of 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.

[0062] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0063] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used in this specification are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0064] In underground mines, light is difficult to penetrate due to the obstruction of rocks, ores and other geological structures, resulting in extremely limited or even complete lack of visual information. Such extreme lighting conditions greatly restrict the application of machine vision technology, making it impossible to effectively perform tasks such as target identification, path planning or obstacle detection. Secondly, GPS and other satellite-based positioning systems rely on signals from multiple satellites for accurate three-dimensional positioning. However, in underground environments, satellite signals are shielded by thick layers of rock, causing the signal strength to drop sharply or even disappear completely. This means that traditional satellite positioning systems can hardly provide reliable positioning services in underground mines, greatly increasing the difficulty and risk of drones flying in such environments. Therefore, there is an urgent need to solve the problem of failure of traditional machine vision and satellite positioning systems for drones caused by rock obstruction and lack of light.

[0065] Example 1

[0066] The embodiment of the present invention provides a method for positioning a drone in an underground mine, which effectively solves the problem that the traditional machine vision and satellite positioning systems of drones fail due to rock obstruction and lack of light. In the embodiment of the present invention, the drone is equipped with a laser radar module and an inertial navigation module. Figure 1 FIG. 1 is a first schematic diagram of a method for positioning an underground mine using a drone provided by an embodiment of the present invention. Figure 1 As shown, the method comprises the following steps:

[0067] S100: Obtain sensor data of the drone, where the sensor data includes lidar data and inertial navigation data.

[0068] In an embodiment of the present invention, the laser radar module detects the position of the target object by emitting a laser beam and receiving the signal reflected back, thereby obtaining the laser radar data P of the surrounding environment. The inertial navigation module senses the motion state of the drone through an internal inertial measurement unit, including key information such as position, attitude and acceleration. Optionally, the inertial navigation module uses two inertial elements, an accelerometer and a gyroscope, to work, thereby obtaining the inertial navigation data of the drone in three-dimensional space, and the inertial navigation data includes but is not limited to data such as acceleration a, angular velocity w and attitude q.

[0069] S200, obtaining the laser real-time positioning and map construction positioning results in the UAV coordinate system according to the sensor data.

[0070] Figure 2 FIG. 2 is a second schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention. Figure 2As shown in the figure, the positioning results of laser real-time positioning and map construction are obtained by calculating the SLAM (Simultaneous Localization and Mapping) algorithm. The SLAM algorithm specifically includes the following steps:

[0071] S210, obtaining all point cloud information in the laser radar data.

[0072] In the embodiment of the present invention, the laser radar data is recorded and converted into three-dimensional point cloud data, in which each point contains a spatial position, that is, P = {(x1, y1, z1), (x2, y2, z2), ..., (x n ,y n , z n )}, where x n ,y n and z n Represent the coordinates of the nth point cloud in the x, y, and z directions respectively.

[0073] S220, using an iterative closest point algorithm to perform point cloud matching on the point cloud information, and obtaining the laser instant positioning and map construction positioning results in the UAV coordinate system at each moment.

[0074] The Iterative Closest Point (ICP) algorithm iteratively finds the closest point pair between two sets of point clouds and calculates the optimal rotation and translation transformation matrix to align the two sets of point clouds as much as possible. It specifically includes the following steps:

[0075] (1) Obtain the point cloud data collected by the drone at different times and preset an initial transformation matrix.

[0076] (2) Sample from the source point cloud (the point cloud collected by the drone at the current moment) and find the point set corresponding to the target point cloud (the point cloud in the map or the point cloud at the previous moment).

[0077] (3) Calculate the centroid of the corresponding point set and use the centroid to calculate the rotation matrix R and translation vector T.

[0078] (4) Apply the rotation matrix R and translation vector T to perform a rigid transformation on the source point cloud to obtain a new point set.

[0079] (5) Calculate the average distance between the new point set and the target point cloud as a measure of the error function E(R,T).

[0080] (6) If the average distance is less than the preset threshold or the maximum number of iterations is reached, the iteration is terminated; otherwise, return to step (2) to continue the iteration.

[0081] After multiple iterations, when the error function E(R,T) reaches the minimum value, the optimal rotation and translation transformation matrix can be obtained, which aligns the point cloud of the drone at the current moment with the point cloud in the map or the point cloud at the previous moment. The optimal rotation and translation transformation matrix can be used to calculate the laser instant positioning and map construction positioning results Z in the drone coordinate system at each moment. k,slam , Z k,slam The expression is as follows:

[0082]

[0083] In the above formula, x k,s ,y k,s and z k,s They represent the coordinates of the drone in the x, y, and z directions calculated by the SLAM algorithm at time k.

[0084] S230, using inertial navigation data to convert each frame of point cloud information into the drone coordinate system, obtain laser real-time positioning and map construction of the global map.

[0085] Each frame of point cloud information is transformed by applying the corresponding rotation matrix R and translation vector T, and the position w data in the inertial navigation data is used as a supplement and verification. Each frame of point cloud information is transformed from its own local coordinate system to a unified drone coordinate system, and the point cloud data of each frame transformed to the drone coordinate system is spliced ​​together to form a global point cloud map, which is a global map for laser real-time positioning and mapping.

[0086] S300, obtaining the laser frame matching positioning result in the UAV coordinate system according to the sensor data.

[0087] Figure 3 FIG. 3 is a third schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention. Figure 3 As shown, the laser frame matching positioning result is obtained by the laser frame matching algorithm, and the laser frame matching algorithm specifically includes the following steps:

[0088] S310, obtaining the current frame point cloud data and the previous frame point cloud data pose in the laser radar data.

[0089] Get the current frame point cloud data and the previous frame point cloud data pose T in the laser radar data k-1 .

[0090] S320, dividing the space occupied by the point cloud data of the current frame into a plurality of grids, each grid including a laser point cloud set.

[0091] The space occupied by the current frame point cloud data is divided into several grids, each of which includes the laser point cloud set x i＝1，…，n .

[0092] S330, calculating the average value and covariance matrix of the laser point cloud set.

[0093] Calculate the average value q of the laser point cloud collection i and the covariance matrix ∑ i , where the average value q i The expression is as follows:

[0094]

[0095] Covariance matrix∑ i The expression is as follows:

[0096]

[0097] S340 , projecting the current frame of point cloud data according to the pose of the previous frame of point cloud data to obtain the previous frame of point cloud data.

[0098] In the embodiment of the present invention, the previous frame of point cloud data x′ i The expression is as follows:

[0099] x′ i =T k-1 *x i

[0100] S350, obtaining an evaluation function based on the last frame of point cloud data, the average value and the covariance matrix.

[0101] In the embodiment of the present invention, the expression of the evaluation function p is as follows:

[0102]

[0103] S360, optimizing the evaluation function using a gradient descent method to obtain the current frame pose.

[0104] The gradient descent method calculates the gradient of the evaluation function at the current point and updates the parameters in the opposite direction of the gradient (i.e., the steepest descent direction) to gradually approach the optimal solution and finally obtain the current frame pose T k .

[0105] S370, obtaining the laser frame matching positioning result in the UAV coordinate system at each moment according to the current frame pose.

[0106] In the current frame pose T k Get the laser frame matching positioning result Z in the UAV coordinate system at each moment k,m , Z k,m The expression is as follows:

[0107]

[0108] In the above formula, x k,n ,y k,n and z k,n They represent the coordinates of the UAV in the x, y and z directions at time k calculated by the laser frame matching algorithm.

[0109] S400, fusing the laser real-time positioning with the map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

[0110] Figure 4 FIG. 4 is a fourth schematic diagram of a method for positioning an underground mine using a drone provided by an embodiment of the present invention. Figure 4 As shown, the acquisition of fusion positioning results includes the following steps:

[0111] S410, predicting the state of the UAV according to the inertial navigation data, and obtaining the system state equation and system uncertainty equation of the UAV.

[0112] Figure 5 FIG. 5 is a fifth schematic diagram of a method for positioning an underground mine using a drone provided by an embodiment of the present invention. Figure 5 As shown in FIG. 1 , state prediction specifically includes the following steps:

[0113] S411. Obtain acceleration information from inertial navigation data.

[0114] In the embodiment of the present invention, the accelerometer of the inertial navigation module can provide the acceleration a of the drone during motion. x , a y , a z ), where a x 、a y and a z They represent the acceleration components in the x, y and z directions when the drone is moving.

[0115] S412. Calculate external variables and process noise according to the acceleration information.

[0116] By integrating these accelerations, external variables such as the speed and displacement of the drone can be obtained, thereby improving the positioning accuracy of the drone when the GPS signal is poor or unavailable. k The expression is as follows:

[0117]

[0118] In the above formula, dt represents the sampling time of the UAV, v x 、v y and v z They represent the velocity components of the drone in the x, y and z directions respectively.

[0119] Process noise Q k The expression is as follows:

[0120]

[0121] In the above formula, σ a，x , σ a，y , σ a，z They represent the variance of acceleration in the x, y, and z directions respectively.

[0122] S413. Determine the system state equation based on the external variables and the state transfer matrix.

[0123] In the embodiment of the present invention, the state transfer matrix F k It can be set according to the actual situation. Optionally, the state transfer matrix F k The expression is as follows:

[0124]

[0125] The expression of the system state equation of the UAV at a certain moment is as follows:

[0126] x k+1 =F k *x k +u k

[0127] In the above formula, x k represents the state vector of the drone at time k, which represents the position and speed of the drone, x k The expression is as follows:

[0128]

[0129] In the above formula, p x 、p y and p z They represent the position components of the drone in the x, y and z directions respectively, and v x 、v y and v z They represent the velocity components of the drone in the x, y and z directions respectively.

[0130] S414. Determine the system uncertainty equation based on the process noise combined with the state transfer matrix and the state covariance matrix.

[0131] In this embodiment of the present invention, the state covariance matrix P k It can be set according to the actual situation. Optionally, the state covariance matrix P k Initialization looks like this:

[0132]

[0133] The expression of the system uncertainty equation is as follows:

[0134]

[0135] S420, observing the system status of the UAV according to the laser real-time positioning and map construction positioning results and the laser frame matching positioning results, and obtaining the laser real-time positioning and map construction observation equation and the laser frame matching observation equation of the UAV.

[0136] Figure 6 6 is a sixth schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention. Figure 6 As shown in the figure, observing the system status of the drone includes the following steps:

[0137] S421. According to the positioning result of the laser real-time positioning and map construction, combined with the positioning observation matrix of the laser real-time positioning and map construction, the laser real-time positioning and map construction observation equation is obtained.

[0138] In the embodiment of the present invention, the laser real-time positioning and map construction positioning observation matrix H slam It can be set according to the actual situation. Optionally, the laser real-time positioning and map construction positioning observation matrix H slam As shown below:

[0139]

[0140] The expression of the observation equation for laser real-time positioning and map construction is as follows:

[0141] y k，slam =Z k，slam -H slam *x k

[0142] In the above formula, y k，slam It represents the difference between the system observation value and the estimated value of the SLAM state observation of the UAV at time k.

[0143] S422, obtaining a laser frame matching observation equation according to the laser frame matching positioning result and the laser frame matching positioning observation matrix.

[0144] In the embodiment of the present invention, the laser frame matching positioning observation matrix H m It can be set according to the actual situation. Optionally, the laser frame matching positioning observation matrix H m As shown below:

[0145]

[0146] The expression of the laser frame matching observation equation is as follows:

[0147] y k，m =z k，m -H m *x k

[0148] In the above formula, y k，m It represents the difference between the system observation value and the estimated value of the UAV's inter-frame matching state observation at time k.

[0149] S430, according to the laser real-time positioning and map construction observation equation and the laser frame matching observation equation, the system state equation and the system uncertainty equation are updated respectively to obtain a fused positioning result.

[0150] Figure 7 7 is a seventh schematic diagram of the process of the UAV positioning method for underground mines provided by an embodiment of the present invention. Figure 7 As shown, updating the system state equation and the system uncertainty equation respectively includes the following steps:

[0151] S431. Obtain the covariance of the laser real-time positioning and map construction positioning observation matrix according to the laser real-time positioning and map construction positioning results.

[0152] In the embodiment of the present invention, the covariance R of the laser real-time positioning and map construction positioning observation matrix slam It can be set according to the actual situation. Optionally, the laser real-time positioning and map construction positioning observation matrix covariance R slam As shown below:

[0153]

[0154] In the above formula, σ slam,x , σ slam,y and σ slam,z Represents the variance of the coordinates calculated by the SLAM algorithm in the x, y, and z directions respectively.

[0155] S432. Obtain the laser frame matching positioning observation matrix covariance according to the laser frame matching positioning result.

[0156] In the embodiment of the present invention, the covariance of the laser frame matching positioning observation matrix R m It can be set according to the actual situation. Optionally, the covariance R of the laser frame matching positioning observation matrix m As shown below:

[0157]

[0158] In the above formula, σ m,x , σm,y and σ m,z They represent the variance of the coordinates calculated by the laser frame matching algorithm in the x, y and z directions respectively.

[0159] S433. According to the laser real-time positioning and map construction positioning observation matrix covariance, the laser real-time positioning and map construction observation matrix and the state covariance matrix are combined to obtain the first Kalman gain.

[0160] In the embodiment of the present invention, the expression of the first Kalman gain K1 is as follows:

[0161]

[0162] In the above formula, S k1 The covariance R of the positioning observation matrix constructed by laser real-time positioning and map slam , laser real-time positioning and map construction observation matrix H slam and the state covariance matrix P k Jointly determine, S k1 The expression is as follows:

[0163] S k1 =H slam *P k *H slam T +R slam

[0164] S434, obtaining a second Kalman gain according to the laser frame matching positioning observation matrix covariance combined with the laser frame matching observation matrix and the state covariance matrix.

[0165] In the embodiment of the present invention, the expression of the second Kalman gain K2 is as follows:

[0166]

[0167] In the above formula, S k2 The covariance R of the observation matrix is ​​determined by matching the laser frames. m , laser frame matching positioning observation matrix H m and the state covariance matrix P k Jointly determine, S k2 The expression is as follows:

[0168] S k2 =H m *P k *H m T +R m

[0169] S435. Construct observation equations based on the first Kalman gain in combination with the laser real-time positioning and the map, and update the system state equation and the system uncertainty equation.

[0170] In the embodiment of the present invention, the system state equation x is calculated based on the real-time laser positioning and map construction positioning results. k and the system uncertainty P K Update, where the expression for updating the system state equation is as follows:

[0171] x slam，k+1 =x k +K1*y k

[0172] In the above formula, x slam,k+1 Represents the system state equation after the laser real-time positioning and map construction positioning results are updated.

[0173] The expression for row update based on system uncertainty is as follows:

[0174] P slam，k+1 =(I-K1*H slam )*P k

[0175] In the above formula, P slam,k+1 Represents the system uncertainty equation after the updated positioning results of laser real-time positioning and map construction.

[0176] S436, updating the system state equation and the system uncertainty equation according to the second Kalman gain combined with the laser frame matching observation equation.

[0177] In the embodiment of the present invention, the system state equation x is calculated based on the laser frame matching positioning result. k and the system uncertainty P K Update, where the expression for updating the system state equation is as follows:

[0178] x m，k+1 =x k +K2*y k

[0179] In the above formula, x m,k+1 Represents the system state equation after the laser frame matching positioning results are updated.

[0180] The expression for row update based on system uncertainty is as follows:

[0181] P m，k+1 =(I-K2*H m )*P k

[0182] In the above formula, P m,k+1Represents the system uncertainty equation after the laser frame matching positioning result is updated.

[0183] The underground mine drone positioning method provided by the embodiment of the present invention realizes the complementarity of multi-source information by fusing laser radar data and inertial navigation data, thereby improving the overall performance of drone positioning and navigation. By processing multi-source information of drone motion, it is possible to continuously provide effective position and speed prediction in complex environments where GPS signals are unstable or completely missing, thereby improving the positioning accuracy and environmental adaptability of drones in complex environments.

[0184] Example 2

[0185] Based on the same technical concept as the above method embodiment 1, an embodiment of the present invention provides a UAV positioning device for underground mines, and the UAV is provided with a laser radar module and an inertial navigation module. Figure 8 FIG. 1 is a schematic diagram of the structure of a drone positioning device for underground mines provided by an embodiment of the present invention. Figure 8 As shown, the underground mine drone positioning device 800 includes:

[0186] The data acquisition module 810 is used to acquire sensor data of the drone, where the sensor data includes lidar data and inertial navigation data.

[0187] The first processing module 820 is used to obtain the laser real-time positioning and map construction positioning results in the drone coordinate system according to the sensor data.

[0188] The second processing module 830 is used to obtain the laser frame matching positioning result in the drone coordinate system according to the sensor data.

[0189] The positioning fusion module 840 is used to fuse the laser real-time positioning with the map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

[0190] The UAV positioning device for underground mines provided by the embodiment of the present invention combines the laser SLAM positioning results and the laser frame matching positioning results to observe the state of the UAV. The laser SLAM technology can achieve accurate positioning and map construction in a GPS-free environment. The reliability of positioning can be further improved by combining the laser frame matching positioning. The environmental map constructed by the laser SLAM technology can be used to correct the track of the UAV, reduce the cumulative error caused by multi-source information processing, significantly improve the robustness and reliability of the positioning system, and ensure the positioning accuracy of the UAV during long-term flight.

[0191] It can be understood that the implementation method of the drone positioning method for underground mines described in the above-mentioned Example 1 is also applicable to this embodiment and can achieve the same technical effect, so it will not be repeated here.

[0192] Example 3

[0193] Based on the same concept, an embodiment of the present invention further provides an electronic device, Fig. 9 is a schematic diagram of the structure of an electronic device provided by an embodiment of the present invention, such as Fig. 9 As shown, the electronic device 900 may include: a processor 910, a communication interface 920, a memory 930 and a communication bus 940, wherein the processor 910, the communication interface 920 and the memory 930 communicate with each other through the communication bus 940. The processor 910 may call the logic instructions in the memory 930 to execute the steps of the underground mine drone positioning method described in the above embodiments. For example, it includes:

[0194] S100, obtaining sensor data of the drone, where the sensor data includes laser radar data and inertial navigation data;

[0195] S200, obtaining the laser real-time positioning and map construction positioning results in the coordinate system of the UAV according to the sensor data;

[0196] S300, obtaining a laser frame matching positioning result in a UAV coordinate system according to sensor data;

[0197] S400, fusing the laser real-time positioning with the map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

[0198] The processor 910 may be a central processing unit (CPU). The processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or a combination of the above chips.

[0199] In addition, the logic instructions in the above-mentioned memory 930 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0200] The memory 930 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created by the processor, etc. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely disposed relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0201] Example 4

[0202] Based on the same concept, an embodiment of the present invention further provides a computer-readable storage medium, which stores a computer program, and the computer program includes at least one code segment, which can be executed by a main control device to control the main control device to implement the steps of the underground mine drone positioning method described in the above embodiments. For example, it includes:

[0203] S100, obtaining sensor data of the drone, where the sensor data includes laser radar data and inertial navigation data;

[0204] S200, obtaining the laser real-time positioning and map construction positioning results in the coordinate system of the UAV according to the sensor data;

[0205] S300, obtaining a laser frame matching positioning result in a UAV coordinate system according to sensor data;

[0206] S400, fusing the laser real-time positioning with the map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

[0207] Based on the same technical concept, an embodiment of the present invention further provides a computer program, which is used to implement the above method embodiment when executed by a main control device.

[0208] The computer program may be stored in whole or in part on a computer-readable storage medium packaged with the processor, or may be stored in whole or in part on a memory not packaged with the processor.

[0209] Based on the same technical concept, an embodiment of the present invention further provides a processor, which is used to implement the above method embodiment. The above processor can be a chip.

[0210] In summary, the method, device, electronic device and storage medium for positioning the unmanned aerial vehicle in underground mines provided by the present invention realize the complementarity of multi-source information by fusing laser radar data and inertial navigation data, and improve the overall performance of positioning and navigation of the unmanned aerial vehicle. By processing the multi-source information of the unmanned aerial vehicle movement, it is possible to continuously provide effective position and speed prediction in complex environments where the GPS signal is unstable or completely missing, thereby improving the positioning accuracy and environmental adaptability of the unmanned aerial vehicle in complex environments. At the same time, the state of the unmanned aerial vehicle is observed in combination with the laser SLAM positioning results and the laser inter-frame matching positioning results. The laser SLAM technology can realize accurate positioning and map construction in a GPS-free environment. The positioning reliability can be further improved by combining the laser inter-frame matching positioning. The environmental map constructed by the laser SLAM technology can be used to correct the track of the unmanned aerial vehicle, reduce the cumulative error caused by multi-source information processing, significantly improve the robustness and reliability of the positioning system, and ensure the positioning accuracy of the unmanned aerial vehicle during long-term flight.

[0211] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present application. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0212] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

[0213] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for positioning an unmanned aerial vehicle in an underground mine, characterized in that: The drone is provided with a laser radar module and an inertial navigation module, and the method comprises: Acquiring sensor data of the drone, wherein the sensor data includes lidar data and inertial navigation data; Acquire the laser real-time positioning and map construction positioning results in the UAV coordinate system according to the sensor data; Acquire the laser frame matching positioning result in the UAV coordinate system according to the sensor data; The laser instant positioning and map construction positioning result and the laser frame matching positioning result are fused to obtain a fused positioning result.

2. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 1, characterized in that: The instant laser positioning and map construction positioning results in the coordinate system of the unmanned aerial vehicle are obtained according to the sensor data, including: Obtaining all point cloud information in the laser radar data; The point cloud information is matched by using an iterative closest point algorithm to obtain the laser instant positioning and map construction positioning results in the coordinate system of the UAV at each moment; The inertial navigation data is used to convert each frame of the point cloud information into the UAV coordinate system to obtain laser real-time positioning and map construction of a global map.

3. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 1, characterized in that: The step of obtaining the laser frame matching positioning result in the drone coordinate system according to the sensor data includes: Obtaining the current frame point cloud data and the previous frame point cloud data pose in the laser radar data; Divide the space occupied by the current frame point cloud data into a plurality of grids, each of which includes a laser point cloud set; Calculating the mean value and covariance matrix of the laser point cloud set; Projecting the current frame of point cloud data according to the pose of the previous frame of point cloud data to obtain the previous frame of point cloud data; An evaluation function is calculated based on the previous frame of point cloud data, the average value and the covariance matrix; The evaluation function is optimized by using a gradient descent method to obtain the current frame pose; The laser frame matching positioning results in the UAV coordinate system at each moment are obtained according to the current frame pose.

4. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 1, characterized in that: The step of fusing the laser instant positioning and map construction positioning result with the laser frame matching positioning result to obtain a fused positioning result includes: Predicting the state of the UAV according to the inertial navigation data to obtain a system state equation and a system uncertainty equation of the UAV; Observe the system status of the UAV according to the laser real-time positioning and map construction positioning result and the laser frame matching positioning result, and obtain the laser real-time positioning and map construction observation equation and the laser frame matching observation equation of the UAV; The system state equation and the system uncertainty equation are updated respectively according to the laser instant positioning and map construction observation equation and the laser frame matching observation equation to obtain the fused positioning result.

5. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 4, characterized in that: The method of performing state prediction on the UAV according to the inertial navigation data to obtain a system state equation and a system uncertainty equation of the UAV includes: Acquiring acceleration information from the inertial navigation data; calculating external variables and process noise based on the acceleration information; Determine the system state equation according to the external variables combined with the state transfer matrix; The system uncertainty equation is determined based on the process noise in combination with the state transfer matrix and the state covariance matrix.

6. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 4, characterized in that: The system state of the UAV is observed respectively according to the laser instant positioning and map building positioning result and the laser frame matching positioning result to obtain the laser instant positioning and map building observation equation and the laser frame matching observation equation of the UAV, including: According to the laser instant positioning and mapping positioning result and the laser instant positioning and mapping positioning observation matrix, the laser instant positioning and mapping observation equation is obtained; The laser frame matching observation equation is obtained according to the laser frame matching positioning result combined with the laser frame matching positioning observation matrix.

7. The method for positioning an unmanned aerial vehicle in an underground mine according to claim 4, characterized in that: The system state equation and the system uncertainty equation are updated respectively according to the laser real-time positioning and map construction observation equation and the laser frame matching observation equation to obtain the fusion positioning result, including: Obtaining a laser instant positioning and map construction positioning observation matrix covariance according to the laser instant positioning and map construction positioning result; Obtaining the laser frame matching positioning observation matrix covariance according to the laser frame matching positioning result; Obtaining a first Kalman gain according to the covariance of the laser instant positioning and map construction positioning observation matrix combined with the laser instant positioning and map construction observation matrix and the state covariance matrix; Obtaining a second Kalman gain according to the laser frame matching positioning observation matrix covariance combined with the laser frame matching observation matrix and the state covariance matrix; According to the first Kalman gain combined with the laser real-time positioning and map construction observation equation, the system state equation and the system uncertainty equation are updated; The system state equation and the system uncertainty equation are updated according to the second Kalman gain in combination with the laser frame matching observation equation.

8. A drone positioning device for underground mines, characterized in that: The drone is provided with a laser radar module and an inertial navigation module, and the device comprises: A data acquisition module, used to acquire sensor data of the drone, wherein the sensor data includes laser radar data and inertial navigation data; A first processing module is used to obtain the laser real-time positioning and map construction positioning results in the UAV coordinate system according to the sensor data; A second processing module is used to obtain the laser frame matching positioning result in the UAV coordinate system according to the sensor data; The positioning fusion module is used to fuse the laser instant positioning and map construction positioning result and the laser frame matching positioning result to obtain a fused positioning result.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: The processor executes the computer program to implement the drone positioning method for an underground mine as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the drone positioning method for an underground mine is implemented as described in any one of claims 1 to 7.

Images