Underground vehicle positioning method and device

Abstract

The embodiment of the invention provides an underground vehicle positioning method and device, and the method comprises the steps: determining a current state vector at a current moment according to a historical state vector corresponding to a to-be-positioned vehicle at a previous moment and IMU data at the current moment; determining a first Kalman gain corresponding to a laser radar according to the current state vector and original positioning data collected by the laser radar deployed on the to-be-positioned vehicle, so as to adjust the current state vector based on the first Kalman gain; and determining target positioning information of the to-be-positioned vehicle according to the adjusted current state vector. According to the technical scheme, after the current state vector of the target vehicle at the current moment is determined, the current state vector is adjusted according to the original positioning data collected by the deployed laser radar, the target positioning information of the to-be-positioned vehicle is determined, higher positioning precision and real-time performance are achieved in an underground scene, and the positioning accuracy of the to-be-positioned vehicle is improved. The effects of effectively reducing accumulated errors and improving the reliability and usability of overall positioning are achieved.

Description

Technical Field

[0001] This disclosure relates to the field of underground vehicle positioning technology, and in particular to an underground vehicle positioning method and apparatus. Background Technology

[0002] With the deepening of smart mine construction, autonomous driving of underground vehicles has become a core technology, and accurate and reliable real-time positioning is the key to realizing autonomous driving of underground vehicles. However, the unique working conditions of underground mines—enclosed, complex, and harsh—pose a far greater challenge to the positioning of underground vehicles than in surface environments.

[0003] Currently, the main method for real-time positioning of underground vehicles is to fuse multiple sensors based on the recognition status of visual targets to obtain the vehicle positioning result. However, when using visual targets for real-time positioning of underground vehicles, the targets need to be densely deployed in the underground roadways, resulting in high system deployment and maintenance costs. Furthermore, the deployed targets are easily damaged by collisions with mining cars, also leading to high maintenance costs. Summary of the Invention

[0004] This disclosure provides a method and apparatus for locating vehicles in underground mines, enabling the determination of target location information for vehicles to be located. This achieves higher positioning accuracy and real-time performance in underground scenarios, effectively reducing accumulated errors and improving the overall reliability and availability of positioning.

[0005] In a first aspect, embodiments of this disclosure provide a method for locating underground vehicles, the method comprising:

[0006] The current state vector at the current moment is determined based on the historical state vector of the vehicle to be located at the previous moment and the IMU data at the current moment; wherein, the historical state vector and the current state vector both include the pose information, velocity information, IMU bias information and gravity vector of the vehicle to be located.

[0007] Based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, the first Kalman gain corresponding to the lidar is determined, so as to adjust the current state vector based on the first Kalman gain;

[0008] Based on the adjusted current state vector, the target location information of the vehicle to be located is determined.

[0009] Secondly, embodiments of the present invention also provide an underground vehicle positioning device, the device comprising:

[0010] The current state vector determination module is used to determine the current state vector at the current moment based on the historical state vector of the vehicle to be located at the previous moment and the IMU data at the current moment; wherein, the historical state vector and the current state vector both include the pose information, velocity information, IMU bias information and gravity vector of the vehicle to be located.

[0011] The current state vector adjustment module is used to determine the first Kalman gain corresponding to the lidar based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, so as to adjust the current state vector based on the first Kalman gain.

[0012] The target positioning information determination module is used to determine the target positioning information of the vehicle to be located based on the adjusted current state vector.

[0013] Thirdly, embodiments of the present invention also provide an electronic device, the electronic device comprising:

[0014] One or more processors;

[0015] Storage device for storing one or more programs.

[0016] When the one or more programs are executed by the one or more processors, the one or more processors implement the downhole vehicle positioning method as described in any embodiment of the present invention.

[0017] Fourthly, embodiments of the present invention also provide a storage medium containing computer-executable instructions, which, when executed by a computer processor, are used to perform the downhole vehicle positioning method as described in any of the embodiments of the present invention.

[0018] Fifthly, embodiments of the present invention also provide a computer program product, including a computer program, characterized in that, when executed by a processor, the computer program implements the downhole vehicle positioning method as described in any embodiment of the present invention.

[0019] The technical solution of this disclosure first determines the current state vector of the vehicle to be located based on the historical state vector of the vehicle at the previous moment and the IMU data at the current moment. Then, based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, the first Kalman gain corresponding to the lidar is determined, and the current state vector is adjusted based on the first Kalman gain. Finally, based on the adjusted current state vector, the target positioning information of the vehicle to be located is determined. This solves the problem in the prior art where multi-sensor fusion based on the recognition state of visual targets to obtain vehicle positioning results requires dense deployment of visual targets in underground roadways, resulting in high system deployment and maintenance costs. Furthermore, the deployed targets are easily damaged by collisions with mining trucks, leading to high maintenance costs. This disclosure, after determining the current state vector of the target vehicle at the current moment, adjusts the current state vector based on the original positioning data collected by the deployed lidar to determine the target positioning information of the vehicle to be located, achieving higher positioning accuracy and real-time performance in underground scenarios, effectively reducing accumulated errors and improving the overall reliability and availability of positioning. Attached Figure Description

[0020] To more clearly illustrate the technical solutions of exemplary embodiments of the present invention, the accompanying drawings used in describing the embodiments are briefly introduced below. Obviously, the accompanying drawings described are only a portion of the drawings of the embodiments to be described in this invention, and not all of the drawings. For those skilled in the art, other drawings can be obtained from these drawings without any creative effort.

[0021] Figure 1 This is a schematic flowchart of an underground vehicle positioning method provided in an embodiment of this disclosure;

[0022] Figure 2 This is a schematic flowchart of an underground vehicle positioning method provided in an embodiment of this disclosure;

[0023] Figure 3 This is a schematic diagram of the structure of an underground vehicle positioning device provided in an embodiment of the present disclosure;

[0024] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Detailed Implementation

[0025] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0026] Before introducing the technical solutions provided in the embodiments of this disclosure, the application scenarios can be illustrated by example. The technical solutions provided in the embodiments of this disclosure can be applied to scenarios involving real-time positioning of vehicles underground. Based on the technical solutions in the embodiments of this disclosure, after determining the current state vector of the target vehicle at the current moment, the current state vector is adjusted according to the original positioning data collected by the deployed lidar to determine the target positioning information of the vehicle to be located. This achieves higher positioning accuracy and real-time performance in underground scenarios, effectively reducing accumulated errors and improving the overall reliability and availability of positioning.

[0027] Example 1

[0028] Figure 1 This is a flowchart illustrating an underground vehicle positioning method provided in this embodiment. This embodiment is applicable to real-time positioning of underground vehicles. The method can be executed by an underground vehicle positioning device, which can be implemented in the form of software and / or hardware. The hardware can be a mobile electronic device that can execute the underground vehicle positioning method provided in this technical solution.

[0029] like Figure 1 As shown, the method includes:

[0030] S110. Determine the current state vector of the vehicle to be located at the previous moment based on the historical state vector of the vehicle at the current moment and the IMU data at the current moment.

[0031] The historical state vector and the current state vector both include the pose information, velocity information, IMU bias information, and gravity vector of the vehicle to be located.

[0032] The vehicles to be located are those that require positioning during underground operations, typically used in environments such as mines or tunnels. These vehicles include mining trucks, loaders, and inspection vehicles. The previous moment refers to a point in time immediately preceding the current moment in a time series. Specifically, in IMU data, the previous moment usually refers to the time of the last sample taken during IMU data acquisition. The IMU's sampling frequency determines the data update frequency; therefore, the time interval between the previous moment and the current moment is usually determined by the IMU's sampling frequency. For example, if the IMU samples at a frequency of 100Hz, the data will be updated every 10 milliseconds.

[0033] It should be noted that IMU data is acquired through an inertial measurement unit, including measurements from accelerometers and gyroscopes. Accelerometers measure the acceleration of the vehicle being located in three-dimensional space, while gyroscopes measure its angular velocity. IMU data can be used to calculate the vehicle's velocity, position, and attitude changes. The historical state vector contains the vehicle's state information at the previous moment, including position, velocity, attitude, IMU bias, and gravity vector, which is the foundational information for inferring the current state. The current state vector represents the vehicle's state information at the current moment, typically updated based on the historical state vector and the current IMU data, containing the latest pose, velocity, IMU bias, and gravity vector. The vehicle's pose information describes its position and orientation in space. The vehicle's velocity information describes its velocity in a specific direction. The vehicle's IMU bias information describes potential errors or drifts in the IMU sensors during measurement. The vehicle's gravity vector represents the direction and magnitude of the Earth's gravitational pull on the vehicle.

[0034] It should be noted that, based on the historical state vector of the vehicle to be located at the previous moment, the current state vector can be recursively derived using the IMU data at the current moment, according to the inertial solution method. The inertial solution method can be the fourth-order Runge-Kutta method. The fourth-order Runge-Kutta method, as a numerical integration method, is used to solve ordinary differential equations. In inertial navigation, the fourth-order Runge-Kutta method can be used to recursively derive the state vector from the previous moment to the current moment using time steps.

[0035] It should also be noted that the initial state vector is fundamental to determining the entire state estimation process and can be obtained through the following methods: If the vehicle to be located can receive a GPS signal at the initial moment, its initial position and velocity information can be directly obtained from the GPS to determine the initial state vector. If the vehicle to be located cannot receive a GPS signal at the initial moment, the point cloud data collected by the LiDAR can be registered with a pre-established point cloud map to calculate the initial state vector of the vehicle to be located.

[0036] Specifically, the pose, velocity, IMU bias, and gravity vector corresponding to the previous moment are used as initial values. The fourth-order Runge-Kutta method in inertial calculation is used to integrate the measurements taken by the gyroscope and accelerometer at the current moment within the corresponding time interval to obtain the pose and velocity at the current moment. The bias is then dynamically updated accordingly. The gravity vector is usually kept unchanged or slowly adjusted, that is, the current state vector corresponding to the current moment is calculated step by step.

[0037] S120. Based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, determine the first Kalman gain corresponding to the lidar, and adjust the current state vector based on the first Kalman gain.

[0038] LiDAR (Light Detection and Ranging) determines distance by emitting laser pulses and measuring return time or phase difference, while simultaneously generating two-dimensional or three-dimensional point cloud data in conjunction with a scanning mechanism. A 3D rotating multi-line LiDAR mounted on the vehicle to be located can output ring-shaped, multi-channel three-dimensional point cloud data. The LiDAR can be deployed at a high position slightly forward of the vehicle's centerline, or on a raised mast at the front or top of the vehicle. After acquiring the LiDAR data, it can be converted to the target positioning point coordinate system, which is the vehicle reference coordinate system used for fusion and output. For example, the target positioning point coordinate system could be the coordinate system of the vehicle's water level geometric center or the IMU (Installation Unit) installation center.

[0039] It should be noted that the raw positioning data collected by the LiDAR deployed on the vehicle to be located typically refers to the unfused and unprocessed measurement and metadata directly output by the sensor. Raw positioning data includes point cloud data, i.e., the coordinates of each point after distance and angle transformation. The first Kalman gain corresponding to the LiDAR refers to the Kalman gain used for updating LiDAR measurements. The Kalman gain determines the magnitude of the trade-off between the current state vector and the sensor measurement, based on state uncertainty and measurement noise.

[0040] Specifically, after determining the current state vector at the current moment, the first Kalman gain corresponding to the LiDAR can be determined based on the current state vector and the raw positioning data collected by the LiDAR deployed on the vehicle to be located. Based on the first Kalman gain, the state estimation of the vehicle to be located can be effectively adjusted and optimized, thereby adjusting the current state vector and effectively improving positioning accuracy. By combining the inertial measurement of the IMU with the high-precision measurement of the LiDAR, system errors are effectively reduced.

[0041] S130. Determine the target positioning information of the vehicle to be located based on the adjusted current state vector.

[0042] The target positioning information may include the exact location and attitude of the vehicle to be located at the current moment.

[0043] Specifically, after adjusting the current state vector based at least on the first Kalman gain, the accurate position and attitude of the vehicle to be located at the current moment can be determined based on the adjusted current state vector. The target positioning information of the vehicle to be located is not only used for real-time positioning of the vehicle, but also provides support for subsequent autonomous driving.

[0044] The technical solution of this disclosure first determines the current state vector of the vehicle to be located based on the historical state vector of the vehicle at the previous moment and the IMU data at the current moment. Then, based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, the first Kalman gain corresponding to the lidar is determined, and the current state vector is adjusted based on the first Kalman gain. Finally, based on the adjusted current state vector, the target positioning information of the vehicle to be located is determined. This solves the problem in the prior art where multi-sensor fusion based on the recognition state of visual targets to obtain vehicle positioning results requires dense deployment of visual targets in underground roadways, resulting in high system deployment and maintenance costs. Furthermore, the deployed targets are easily damaged by collisions with mining trucks, leading to high maintenance costs. This disclosure, after determining the current state vector of the target vehicle at the current moment, adjusts the current state vector based on the original positioning data collected by the deployed lidar to determine the target positioning information of the vehicle to be located, achieving higher positioning accuracy and real-time performance in underground scenarios, effectively reducing accumulated errors and improving the overall reliability and availability of positioning.

[0045] Example 2

[0046] Figure 2 This is a flowchart illustrating the downhole vehicle positioning method provided in this embodiment of the invention. Based on the aforementioned embodiments, it provides a more detailed explanation of determining the first Kalman gain corresponding to the lidar based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be positioned, and adjusting the current state vector based on the first Kalman gain. For specific implementation details, please refer to the technical solution of this embodiment. Technical terms that are the same as or corresponding to those in the above embodiments will not be repeated here.

[0047] like Figure 2 As shown, the method specifically includes the following steps:

[0048] S210. Determine the current state vector of the vehicle to be located at the previous moment based on the historical state vector of the vehicle at the current moment and the IMU data at the current moment.

[0049] S220. Perform point cloud matching between the raw positioning data collected by the lidar on the vehicle to be located and the point cloud map corresponding to the mine roadway to determine the lidar point cloud matching pose of the vehicle to be located at the current moment.

[0050] Point cloud maps, acquired through LiDAR or other sensors, are discrete datasets representing the shape and position of objects in three-dimensional space. In mine tunnels, point cloud maps typically contain a large number of three-dimensional coordinate points, which form the geometry of the tunnel and reflect its contours, ground, and walls. In addition to geometric information, point cloud maps of mine tunnels can also include supplementary information such as color and intensity, which can be used for feature extraction and matching. Radar point cloud pose matching refers to the process of registering the raw positioning data measured by the LiDAR of the vehicle to be located with the point cloud map during point cloud matching to determine the spatial position and orientation of the vehicle at the current moment.

[0051] Specifically, LiDAR is used to acquire raw positioning data of the vehicle to be located at the current moment, including the surrounding environment. The raw positioning data acquired by the LiDAR is preprocessed, including noise reduction, filtering, and downsampling, to reduce computational load and improve matching accuracy. Point cloud matching algorithms, such as ICP or NDT, are applied for point cloud matching. These algorithms optimize the pose of the vehicle to be located by minimizing the distance or error between the raw positioning data and the reference point cloud. During this process, the algorithm iteratively updates the pose of the vehicle to be located until it converges to an optimal solution. Finally, the radar point cloud matching pose at the current moment is determined based on the matching results, including the specific position and attitude information of the vehicle to be located.

[0052] S230. Use the extended Kalman filter algorithm to determine the first covariance matrix at the current time.

[0053] It should be noted that the first covariance matrix is ​​an important indicator for quantifying the uncertainty of the current state vector estimation.

[0054] Specifically, the first covariance matrix at the current moment can be determined based on the covariance matrix of the previous moment, the input excitation matrix, the state transition matrix, and the pre-set process noise covariance matrix.

[0055] For example, the formula for determining the first covariance matrix at the current time can be:

[0056] ;

[0057] in, This refers to the first covariance matrix at the current time. This refers to the covariance matrix of the previous time step; This refers to the state transition matrix; This refers to the pre-set process noise covariance matrix; This refers to the input excitation matrix. The state transition matrix and the input excitation matrix can be determined when the current state vector at the current moment is determined using the inertial solution method. By substituting the covariance matrix of the previous moment, the pre-set process noise covariance matrix, the state transition matrix, the transpose of the state transition matrix, the input excitation matrix, and the transpose of the input excitation matrix into the formula, the first covariance matrix at the current moment can be determined.

[0058] S240. Based on the first covariance matrix at the current time, the radar point cloud matching pose, the first observation matrix, the first positioning observation noise, and the first adaptive factor, determine the first Kalman gain corresponding to the lidar.

[0059] In the Extended Kalman Filter (EKF), the first observation matrix is ​​pre-set and describes how to infer sensor observations such as distance measured by the lidar from the internal states of the system, such as position and velocity. In EKF, the first observation matrix is ​​typically the Jacobian matrix of the current state vector, representing the sensitivity of the observations to changes in the current state variable. The first positioning observation noise refers to a covariance matrix, representing the noise characteristics during the lidar measurement process. The pre-set first positioning observation noise quantifies the uncertainty of the observations and is used to describe the distribution of the observation error. The first adaptive factor is a parameter used to adjust the first Kalman gain.

[0060] It should be noted that the formula for determining the first Kalman gain can be:

[0061] ;

[0062] in, This refers to the first Kalman gain; This refers to the first covariance matrix at the current time. This refers to the first observation matrix; This refers to the noise from the first positioning observation; This refers to the first adaptive factor.

[0063] It should be noted that after determining the first covariance matrix at the current time using the extended Kalman filter algorithm, the first Kalman gain can be calculated by inputting the first covariance matrix at the current time, the pre-determined first observation matrix, the pre-determined first positioning observation noise, and the first adaptive factor into the formula for determining the first Kalman gain.

[0064] Optionally, the first adaptive factor is determined based on the following method: determining the proportion of valid points according to the number of valid points in the original positioning data; dividing the original positioning data into at least one voxel grid, and determining the first entropy value according to the number of voxel grids, the number of points in each voxel grid, and the total number of points corresponding to the original positioning data; determining the root mean square error according to the coordinate information of the registered point cloud data and the coordinate information of the original positioning data; and determining the first adaptive factor according to the proportion of valid points, the first entropy value, and the root mean square error.

[0065] In mining environments, dust particles and water mist are present. When dust particles cover the surface of a target object, they can reduce its reflectivity, preventing the lidar from effectively receiving the reflected signals. In such cases, the lidar may fail to detect objects that should be present, resulting in invalid points. In water mist environments, the lidar may receive multiple reflected signals, leading to confusion in measurement results and also invalid points. The number of valid points in the raw positioning data refers to the number of points in the raw positioning data acquired by the lidar that can be used for further calculation and analysis. The number of valid points in the raw positioning data typically refers to the number of points that are not affected by dust particles and water mist. The valid point percentage is the proportion of valid points to the total number of points in the raw positioning data. The valid point percentage reflects the quality of the data; a higher valid point percentage indicates higher reliability of the lidar data.

[0066] It should be noted that a voxel mesh is a small cube that divides three-dimensional space. The size of each voxel mesh can be set according to the specific application; for example, the size of each voxel mesh can be 0.2m × 0.2m × 0.2m. By dividing the raw positioning data into at least one voxel mesh, the distribution of point cloud data can be better analyzed. The formula for determining the first entropy value can be:

[0067] ;

[0068] in, This refers to the first entropy value; This refers to the number of voxel grids; This refers to the ratio of the number of points within each voxel grid to the total number of points in the original localization data. A higher first entropy value indicates richer features and a more uniform distribution of the point cloud; a lower first entropy value indicates simpler features and a more concentrated distribution of the point cloud.

[0069] It should be noted that the formula for determining the root mean square error is as follows:

[0070] ;

[0071] in, This refers to the root mean square error; This refers to the number of point-to-point pairs; This refers to the coordinate information of each piece of raw positioning data; This refers to the coordinate information of each registration point cloud data. The smaller the value, the higher the registration accuracy.

[0072] It should be noted that after determining the percentage of valid points, the first entropy value, and the root mean square error, one can rely on... Determine the first comprehensive score. When the first comprehensive score is greater than 3, the corresponding first adaptive factor is 1; when the first comprehensive score is less than 1, the corresponding first adaptive factor is 100; when the first comprehensive score is any other value, the corresponding first adaptive factor is... .

[0073] In this embodiment, the first covariance matrix at the current time is updated. Updating the first covariance matrix at the current time includes obtaining the updated first covariance matrix based on the first observation matrix, the first positioning observation noise, the first adaptive factor, the first covariance matrix, and the first Kalman gain.

[0074] It should be noted that after determining the first Kalman gain corresponding to the lidar, the first covariance matrix at the current time can be updated. The formula for updating the first covariance matrix based on the first observation matrix, the first positioning observation noise, the first adaptive factor, the first covariance matrix, and the first Kalman gain can be:

[0075] ;

[0076] in, This refers to the updated first covariance matrix; This refers to the identity matrix: This refers to the first Kalman gain; This refers to the first observation matrix; This refers to the noise from the first positioning observation; This refers to the first adaptive factor; This refers to the first covariance matrix at the current time. Substituting the identity matrix, the first Kalman gain, the first observation matrix, the first positioning observation noise, the first adaptive factor, and the first covariance matrix at the current time into the formula for updating the first covariance matrix yields the updated first covariance matrix.

[0077] Specifically, after determining the first covariance matrix at the current time using the extended Kalman filter algorithm, the first Kalman gain corresponding to the lidar can be determined based on the first covariance matrix at the current time, the matching pose of the radar point cloud, the pre-set first observation matrix, the pre-set first positioning observation noise, and the calculated first adaptive factor. Furthermore, after determining the first Kalman gain corresponding to the lidar, the first covariance matrix at the current time can be updated, resulting in the updated first covariance matrix.

[0078] S250. Based on the pose information in the current state vector and the radar point cloud pose of the vehicle to be located at the current moment, determine the first pose difference value.

[0079] The first pose difference value refers to the difference between the pose information in the current state vector and the radar point cloud matching pose of the vehicle to be located at the current moment.

[0080] Specifically, by subtracting the pose information in the current state vector from the radar point cloud pose of the vehicle to be located at the current moment, the first pose difference value can be determined.

[0081] For example, the formula for determining the first pose difference value can be:

[0082] ;

[0083] in, This refers to the first pose difference value; This refers to the location matched by radar point cloud; This refers to the attitude matching of radar point clouds; This refers to the position information in the current state vector; This refers to the pose information in the current state vector.

[0084] S260. Adjust the current state vector based on the first pose difference value and the first Kalman gain.

[0085] Specifically, based on the pose information in the current state vector and the radar point cloud pose of the vehicle to be located at the current moment, after determining the first pose difference value, the product of the first pose difference value and the first Kalman gain can be determined. Adding this product value to the current state vector yields the adjusted current state vector.

[0086] For example, the formula for determining the adjusted current state vector can be:

[0087] ;

[0088] in, This is the current state vector at the current moment. This is the adjusted current state vector.

[0089] Optionally, semantic matching is performed between the semantic features of the vehicle to be located at the current moment and the semantic map of the mine roadway where the vehicle is located to determine the semantic matching pose of the vehicle at the current moment; the second Kalman gain corresponding to the semantic features is determined based on the second covariance matrix, the semantic matching pose, the second observation matrix, the second localization observation noise, and the second adaptive factor; the second pose difference is determined based on the pose information in the current state vector and the semantic matching pose; and the adjusted current state vector is updated based on the second pose difference and the second Kalman gain.

[0090] The second covariance matrix is ​​either the first covariance matrix before the update or the first covariance matrix after the update.

[0091] Environmental data can be collected using cameras installed on the vehicle to be located. The semantic features of the vehicle at the current moment refer to the features with specific semantic meanings extracted after processing this environmental data. For example, the semantic features of the vehicle at the current moment may include signs, pillars, walls, pipes, and the ground. The semantic map of the mine roadway where the vehicle is located refers to a pre-constructed map containing various geographical information and environmental features within the mine environment. The semantic map includes not only location coordinates but also semantic information for each location. Semantic matching refers to comparing and matching the semantic features perceived by the vehicle with the semantic map of the mine roadway. The semantic matching pose of the vehicle at the current moment refers to the pose of the vehicle in the mine determined after semantic matching.

[0092] Here, the second observation matrix refers to a pre-determined matrix that projects the uncertainty of the state onto the semantic observation domain; the second localization observation noise refers to a covariance matrix that represents the noise characteristics during the measurement process of the camera device. The pre-set second localization observation noise quantifies the uncertainty of the observation values ​​and is used to describe the distribution of the observation error; the second adaptive factor refers to a parameter used to adjust the second Kalman gain.

[0093] It should be noted that the formula for determining the second Kalman gain can be:

[0094] ;

[0095] in, This refers to the second Kalman gain; This refers to the second covariance matrix; This refers to the second observation matrix; This refers to the noise from the second positioning observation; This refers to the second adaptive factor.

[0096] It should be noted that the second Kalman gain can be calculated by inputting the first covariance matrix before or after the update, the pre-determined second observation matrix, the pre-determined second positioning observation noise, and the second adaptive factor into the formula for determining the second Kalman gain.

[0097] The second pose difference refers to the difference between the pose information in the current state vector and the semantic matching pose of the vehicle to be located at the current moment.

[0098] Specifically, the second pose difference can be determined by subtracting the pose information in the current state vector from the semantic matching pose of the vehicle to be located at the current moment.

[0099] For example, the formula for determining the second pose difference value can be:

[0100] ;

[0101] in, This refers to the second pose difference value; This refers to the semantic matching position; This refers to semantic matching posture; This refers to the position information in the current state vector; This refers to the pose information in the current state vector.

[0102] Specifically, based on the pose information in the current state vector and the semantically matched pose of the vehicle to be located at the current moment, after determining the second pose difference, the product of the second pose difference and the second Kalman gain can be determined. Adding this product value to the current state vector updates the adjusted current state vector.

[0103] For example, the formula for updating the adjusted current state vector can be:

[0104] ;

[0105] in, This is the current state vector at the current moment. This is the result obtained after updating the adjusted current state vector.

[0106] In this embodiment, the second adaptive factor is determined as follows: The vehicle to be located is captured by a camera device deployed on the vehicle at the current moment, and semantic features are extracted from the roadway image based on the target model to obtain the semantic features and confidence level of each semantic point in the roadway image; the semantic distribution density is determined based on the number of semantic points and the total number of pixels in the roadway image; for each semantic point, the semantic registration accuracy is determined based on the semantic features of the semantic point, the coordinate information of each semantic point, and the coordinate information of each registered semantic point; the second adaptive factor is determined based on the semantic distribution density, confidence level, and semantic registration accuracy.

[0107] The camera device deployed on the vehicle to be located refers to a camera or camera system installed on the vehicle, responsible for capturing image data of the surrounding environment in real time. The target model refers to a pre-trained deep learning model used to identify and classify different objects or regions in the tunnel images. The target model analyzes the tunnel images to obtain the semantic features and confidence scores of each semantic point. During the semantic feature extraction process, the target model not only outputs the category label for each semantic point but also calculates the associated confidence score.

[0108] It should be noted that semantic distribution density is used to quantify the proportion of semantic points in a lane image. The formula for determining semantic distribution density is as follows: Semantic registration accuracy describes the accuracy of matching between all semantic points. The formula for determining semantic registration accuracy is:

[0109] ;

[0110] in, This refers to semantic registration accuracy; The number of semantic points; Refers to semantic category The weights are calculated as follows: for example, the weights for semantic categories such as signs and pillars can be 0.8, the weights for semantic categories such as walls and pipes can be 0.5, and the weights for semantic categories such as ground can be 0.4. This refers to the coordinate information of each semantic point; This refers to the coordinate information of each registered semantic point. The formula for determining the second adaptive factor can be:

[0111] ;

[0112] It should be noted that when the second comprehensive score is greater than 3.5, the second adaptive factor is 1; when the second comprehensive score is less than 2.5, the second adaptive factor is 100; and when the second comprehensive score is any other value, the second adaptive factor is 10.

[0113] Optionally, the third Kalman gain is determined based on the second covariance matrix, wheel speed meter speed, third observation matrix, third positioning observation noise, and third adaptive factor; the first speed difference is determined based on the speed information in the current state vector and the wheel speed meter speed; and the adjusted current state vector is updated based on the first speed difference and the third Kalman gain.

[0114] The wheel speed is determined based on wheel speed sensors deployed on the vehicle to be located.

[0115] It should be noted that wheel speed sensors can be used to measure the rotational speed of the wheels on the vehicle to be positioned. The wheel speed measurement is the angular velocity measured by the wheel speed sensor. The third observation matrix is ​​a Jacobian matrix that linearizes the state vector to the wheel speed measurement domain. The third positioning observation noise refers to a covariance matrix representing the noise characteristics during the wheel speed sensor measurement process. The pre-set third positioning observation noise quantifies the uncertainty of the observation values ​​and is used to describe the distribution of the observation error; the third adaptive factor refers to a parameter used to adjust the third Kalman gain.

[0116] It should be noted that the formula for determining the third Kalman gain can be:

[0117] ;

[0118] in, This refers to the third Kalman gain; This refers to the second covariance matrix; This refers to the third observation matrix; This refers to the noise from the third positioning observation; This refers to the third adaptive factor.

[0119] It should be noted that the third Kalman gain can be calculated by inputting the first covariance matrix before or after the update, the predetermined third observation matrix, the predetermined third positioning observation noise, and the third adaptive factor into the formula for determining the third Kalman gain.

[0120] The first speed difference refers to the difference between the speed information in the current state vector and the wheel speed meter speed of the vehicle to be located at the current moment.

[0121] Specifically, the first speed difference can be determined by subtracting the speed information in the current state vector from the wheel speed meter speed of the vehicle to be located at the current moment.

[0122] For example, the formula for determining the first velocity difference can be:

[0123] ;

[0124] in, This refers to the difference in first velocity; This refers to the wheel speed meter reading at the current moment; This refers to the wheel speed meter reading at the current moment.

[0125] Specifically, based on the wheel speed readings of the vehicle to be located at the current moment and the speed information in the current state vector, after determining the first speed difference, the product of the first speed difference and the third Kalman gain can be determined. Adding this product to the current state vector updates the adjusted current state vector.

[0126] For example, the formula for updating the adjusted current state vector can be:

[0127] ;

[0128] in, This is the current state vector at the current moment. This is the result obtained after updating the adjusted current state vector.

[0129] In this embodiment, the third adaptive factor is determined as follows: a first speed difference is determined based on speed information and wheel speed meter speed; and a third adaptive factor is determined based on the first speed difference and wheel speed meter speed.

[0130] It should be noted that when the wheel speedometer speed is less than or equal to 0.5 m / s, no slippage is considered to have occurred. When the wheel speedometer speed is greater than 0.5 m / s, the degree of slippage is determined based on the ratio of the first speed difference to the wheel speedometer speed. When the ratio is less than 10%, it is considered slight slippage; when it is greater than or equal to 10% and less than or equal to 20%, it is considered moderate slippage; and when it is greater than 20%, it is considered severe slippage. When no slippage or slight slippage is considered to have occurred, the third adaptive factor is 1; when there is moderate slippage, the third adaptive factor is 10; and when there is severe slippage, the adjusted current state vector is not updated using the third Kalman gain.

[0131] S270. Determine the target positioning information of the vehicle to be located based on the adjusted current state vector.

[0132] The technical solution of this disclosure involves determining the current state vector based on the historical state vector of the vehicle to be located at the previous moment and the IMU data at the current moment. Then, the raw positioning data collected by the lidar on the vehicle to be located is matched with the point cloud map corresponding to the mine roadway to determine the radar point cloud matching pose of the vehicle to be located at the current moment. Next, the extended Kalman filter algorithm is used to determine the first covariance matrix at the current moment. Further, based on the first covariance matrix, the radar point cloud matching pose, the first observation matrix, the first positioning observation noise, and the first adaptive factor, the first Kalman gain corresponding to the lidar is determined. Further, based on the pose information in the current state vector and the radar point cloud matching pose of the vehicle to be located at the current moment, a first pose difference value is determined. Further, based on the first pose difference value and the first Kalman gain, the current state vector is adjusted. Finally, based on the adjusted current state vector, the target positioning information of the vehicle to be located is determined. In complex environments such as mine tunnels, the fusion of IMU data and radar point clouds can effectively improve positioning accuracy and robustness, reducing the impact of single sensor errors. Furthermore, by dynamically adjusting the gain using adaptive factors, cumulative drift is reduced and convergence speed is accelerated, providing a high-precision and low-latency positioning reference for subsequent applications such as autonomous driving.

[0133] Example 3

[0134] Figure 3 This is a schematic diagram of the structure of the underground vehicle positioning device provided in the embodiments of this disclosure, as shown below. Figure 3 As shown, the device includes: a current state vector determination module 310, a current state vector adjustment module 320, and a target positioning information determination module 330.

[0135] The current state vector determination module is used to determine the current state vector at the current moment based on the historical state vector of the vehicle to be located at the previous moment and the IMU data at the current moment; wherein, both the historical state vector and the current state vector include the pose information, velocity information, IMU bias information, and gravity vector of the vehicle to be located; the current state vector adjustment module is used to determine the first Kalman gain corresponding to the LiDAR based on the current state vector and the original positioning data collected by the LiDAR deployed on the vehicle to be located, so as to adjust the current state vector based on the first Kalman gain; the target positioning information determination module is used to determine the target positioning information of the vehicle to be located based on the adjusted current state vector.

[0136] The technical solution of this disclosure first determines the current state vector of the vehicle to be located based on the historical state vector of the vehicle at the previous moment and the IMU data at the current moment. Then, based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, the first Kalman gain corresponding to the lidar is determined, and the current state vector is adjusted based on the first Kalman gain. Finally, based on the adjusted current state vector, the target positioning information of the vehicle to be located is determined. This solves the problem in the prior art where multi-sensor fusion based on the recognition state of visual targets to obtain vehicle positioning results requires dense deployment of visual targets in underground roadways, resulting in high system deployment and maintenance costs. Furthermore, the deployed targets are easily damaged by collisions with mining trucks, leading to high maintenance costs. This disclosure, after determining the current state vector of the target vehicle at the current moment, adjusts the current state vector based on the original positioning data collected by the deployed lidar to determine the target positioning information of the vehicle to be located, achieving higher positioning accuracy and real-time performance in underground scenarios, effectively reducing accumulated errors and improving the overall reliability and availability of positioning.

[0137] Based on the above technical solutions, the current state vector adjustment module 320 includes: a first Kalman gain determination submodule and a current state vector determination submodule.

[0138] The first Kalman gain determination submodule is used to perform point cloud matching between the raw positioning data collected by the lidar on the vehicle to be located and the point cloud map corresponding to the mine roadway, and determine the lidar point cloud matching pose of the vehicle to be located at the current time; determine the first covariance matrix at the current time using the extended Kalman filter algorithm; and determine the first Kalman gain corresponding to the lidar based on the first covariance matrix at the current time, the lidar point cloud matching pose, the first observation matrix, the first positioning observation noise, and the first adaptive factor.

[0139] The current state vector determination submodule is used to determine the first pose difference value based on the pose information in the current state vector and the radar point cloud matching pose of the vehicle to be located at the current time; and to adjust the current state vector based on the first pose difference value and the first Kalman gain.

[0140] Based on the above technical solutions, the first Kalman gain determination submodule further includes: a first adaptive factor determination unit, used to determine the proportion of valid points based on the number of valid points in the original positioning data; after dividing the original positioning data into at least one voxel grid, determine a first entropy value based on the number of voxel grids, the number of points in each voxel grid, and the total number of points corresponding to the original positioning data; determine the root mean square error based on the coordinate information of the registered point cloud data and the coordinate information of the original positioning data; and determine a first adaptive factor based on the proportion of valid points, the first entropy value, and the root mean square error.

[0141] Based on the above technical solutions, the device further includes: a first covariance matrix update module, used to obtain an updated first covariance matrix based on the first observation matrix, the first positioning observation noise, the first adaptive factor, the first covariance matrix, and the first Kalman gain.

[0142] Based on the above technical solutions, the device further includes: a current state vector update module, used to semantically match the semantic features of the vehicle to be located at the current time with the semantic map of the mine roadway where the vehicle to be located is located, to determine the semantic matching pose of the vehicle to be located at the current time; to determine the second Kalman gain corresponding to the semantic features based on the second covariance matrix, the semantic matching pose, the second observation matrix, the second positioning observation noise, and the second adaptive factor; wherein, the second covariance matrix is ​​the first covariance matrix before the update or the first covariance matrix after the update; to determine the second pose difference value based on the pose information in the current state vector and the semantic matching pose; and to update the adjusted current state vector based on the second pose difference value and the second Kalman gain.

[0143] Based on the above technical solutions, the current state vector update module further includes a second adaptive factor determination submodule, which is used to acquire the lane image of the vehicle to be located at the current time based on the camera device deployed on the vehicle to be located, extract semantic features from the lane image according to the target model, and obtain the semantic features and confidence of each semantic point in the lane image; determine the semantic distribution density based on the number of semantic points and the total number of pixels in the lane image; for each semantic point, determine the semantic registration accuracy based on the semantic features of the semantic point, the coordinate information of each semantic point and the coordinate information of each registered semantic point; and determine the second adaptive factor based on the semantic distribution density, the confidence, and the semantic registration accuracy.

[0144] Based on the above technical solutions, the device further includes: a current state vector update module, used to determine a third Kalman gain based on the second covariance matrix, the wheel speed sensor speed, the third observation matrix, the third positioning observation noise, and the third adaptive factor; wherein, the wheel speed sensor speed is determined based on wheel speed sensors deployed on the vehicle to be positioned; a first speed difference is determined based on the speed information in the current state vector and the wheel speed sensor speed; and the adjusted current state vector is updated based on the first speed difference and the third Kalman gain.

[0145] Based on the above technical solutions, the current state vector re-update module further includes: a third adaptive factor determination submodule, used to determine a first speed difference based on the speed information and the wheel speed meter speed; and to determine a third adaptive factor based on the first speed difference and the wheel speed meter speed.

[0146] The underground vehicle positioning device provided in this disclosure can execute the underground vehicle positioning method provided in any embodiment of this disclosure, and has the corresponding functional modules and beneficial effects of the method.

[0147] It is worth noting that the various units and modules included in the above-mentioned device are only divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be realized; in addition, the specific names of each functional unit are only for easy differentiation and are not used to limit the protection scope of the embodiments of this disclosure.

[0148] Example 4

[0149] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this disclosure. Refer to the following... Figure 4 It illustrates an electronic device suitable for implementing embodiments of the present disclosure (e.g., Figure 4 The diagram below shows the structure of the terminal device or server 500. The terminal device in this embodiment may include, but is not limited to, mobile terminals such as mobile phones, laptops, digital radio receivers, PDAs (personal digital assistants), PADs (tablet computers), PMPs (portable multimedia players), and vehicle terminals (e.g., vehicle navigation terminals). Figure 4 The electronic device shown is merely an example and should not be construed as limiting the functionality and scope of the embodiments disclosed herein.

[0150] like Figure 4As shown, electronic device 500 may include a processing unit (e.g., a central processing unit, a graphics processing unit, etc.) 501, which can perform various appropriate actions and processes according to a program stored in read-only memory (ROM) 502 or a program loaded from storage device 508 into random access memory (RAM) 503. The RAM 503 also stores various programs and data required for the operation of electronic device 500. The processing unit 501, ROM 502, and RAM 503 are interconnected via bus 504. An edit / output (I / O) interface 505 is also connected to bus 504.

[0151] Typically, the following devices can be connected to I / O interface 505: input devices 506 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 507 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; storage devices 508 including, for example, magnetic tapes, hard disks, etc.; and communication devices 509. Communication device 509 allows electronic device 500 to communicate wirelessly or wiredly with other devices to exchange data. Although Figure 4 An electronic device 500 with various devices is shown; however, it should be understood that it is not required to implement or possess all of the devices shown. More or fewer devices may be implemented or possessed alternatively.

[0152] In particular, according to embodiments of this disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this disclosure include a computer program product comprising a computer program carried on a non-transitory computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device 509, or installed from a storage device 508, or installed from a ROM 502. When the computer program is executed by the processing device 501, it performs the functions defined in the methods of embodiments of this disclosure.

[0153] The names of messages or information exchanged between multiple devices in the embodiments of this disclosure are for illustrative purposes only and are not intended to limit the scope of such messages or information.

[0154] The electronic device provided in this embodiment and the underground vehicle positioning method provided in the above embodiments belong to the same inventive concept. Technical details not described in detail in this embodiment can be found in the above embodiments, and this embodiment has the same beneficial effects as the above embodiments.

[0155] Example 5

[0156] This disclosure provides a computer storage medium storing a computer program that, when executed by a processor, implements the downhole vehicle positioning method provided in the above embodiments.

[0157] It should be noted that the computer-readable medium described in this disclosure can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this disclosure, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In this disclosure, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium can be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wires, optical fibers, RF (radio frequency), etc., or any suitable combination thereof.

[0158] In some implementations, the server may communicate using any currently known or future-developed network protocol such as HTTP (Hypertext Transfer Protocol) and may interconnect with digital data communication (e.g., communication networks) of any form or medium. Examples of communication networks include local area networks (“LANs”), wide area networks (“WANs”), the Internet (e.g., the Internet), and peer-to-peer networks (e.g., ad hoc peer-to-peer networks), as well as any currently known or future-developed networks.

[0159] The aforementioned computer-readable medium may be included in the aforementioned electronic device; or it may exist independently and not assembled into the electronic device.

[0160] The aforementioned computer-readable medium carries one or more programs that, when executed by the electronic device, cause the electronic device to:

[0161] The current state vector at the current moment is determined based on the historical state vector of the vehicle to be located at the previous moment and the IMU data at the current moment; wherein, the historical state vector and the current state vector both include the pose information, velocity information, IMU bias information and gravity vector of the vehicle to be located.

[0162] Based on the current state vector and the original positioning data collected by the lidar deployed on the vehicle to be located, the first Kalman gain corresponding to the lidar is determined, so as to adjust the current state vector based on the first Kalman gain;

[0163] Based on the adjusted current state vector, the target location information of the vehicle to be located is determined.

[0164] Computer program code for performing the operations of this disclosure can be written in one or more programming languages ​​or a combination thereof, including but not limited to object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0165] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this disclosure. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0166] The units described in the embodiments of this disclosure can be implemented in software or hardware. The names of the units are not, in some cases, intended to limit the specific unit.

[0167] The functions described above in this document can be performed at least in part by one or more hardware logic components. For example, exemplary types of hardware logic components that can be used, without limitation, include: field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), system-on-a-chip (SoCs), complex programmable logic devices (CPLDs), and so on.

[0168] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0169] The above description is merely a preferred embodiment of this disclosure and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of this disclosure is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features disclosed in this disclosure that have similar functions.

[0170] Furthermore, while the operations are described in a specific order, this should not be construed as requiring these operations to be performed in the specific order shown or in a sequential order. In certain environments, multitasking and parallel processing may be advantageous. Similarly, while several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of this disclosure. Certain features described in the context of individual embodiments may also be implemented in combination in a single embodiment. Conversely, various features described in the context of a single embodiment may also be implemented individually or in any suitable sub-combination in multiple embodiments.

[0171] Although the subject matter has been described using language specific to structural features and / or methodological logic, it should be understood that the subject matter defined in the appended claims is not necessarily limited to the specific features or actions described above. Rather, the specific features and actions described above are merely illustrative examples of implementing the claims.

Claims

1. A method of locating a vehicle down a well, characterised by, The method comprises: determining a current state vector of a current moment according to a historical state vector corresponding to a previous moment of a vehicle to be positioned and IMU data of the current moment, wherein the historical state vector and the current state vector both comprise pose information, speed information, IMU bias information and a gravity vector of the vehicle to be positioned; determining a first Kalman gain corresponding to a laser radar deployed on the vehicle to be positioned according to the current state vector and raw positioning data collected by the laser radar, so as to adjust the current state vector based on the first Kalman gain; determining target positioning information of the vehicle to be positioned according to the adjusted current state vector.

2. The method of claim 1, wherein, The determination of the first Kalman gain corresponding to the laser radar according to the current state vector and the raw positioning data collected by the laser radar deployed on the vehicle to be positioned comprises: performing point cloud matching on the raw positioning data collected by the laser radar on the vehicle to be positioned and a point cloud map corresponding to a mine tunnel, so as to determine a radar point cloud matching pose corresponding to the current moment of the vehicle to be positioned; determining a first covariance matrix of the current moment by using an extended Kalman filtering algorithm; determining the first Kalman gain corresponding to the laser radar according to the first covariance matrix of the current moment, the radar point cloud matching pose, a first observation matrix, a first positioning observation noise and a first adaptive factor.

3. The method of claim 1, wherein, The adjustment of the current state vector based on the first Kalman gain comprises: determining a first pose difference value according to the pose information in the current state vector and the radar point cloud matching pose corresponding to the current moment of the vehicle to be positioned; adjusting the current state vector according to the first pose difference value and the first Kalman gain.

4. The method of claim 2, wherein, The first adaptive factor is determined based on the following manner: determining an effective point proportion according to the number of effective points in the raw positioning data; after dividing the raw positioning data into at least one voxel grid, determining a first entropy value according to the number of voxel grids, the number of points in each voxel grid and the total number of points corresponding to the raw positioning data; determining a root mean square error according to coordinate information of registered point cloud data and coordinate information of raw positioning data; determining a first adaptive factor according to the effective point proportion, the first entropy value and the root mean square error.

5. The method of claim 2, wherein, The method further comprises: updating the first covariance matrix of the current moment; The updating of the first covariance matrix of the current moment comprises: obtaining an updated first covariance matrix according to the first observation matrix, the first positioning observation noise, the first adaptive factor, the first covariance matrix and the first Kalman gain.

6. The method of claim 1, wherein, The method further comprises: performing semantic matching on the semantic features corresponding to the current moment of the vehicle to be positioned and a semantic map of a mine tunnel where the vehicle to be positioned is located, so as to determine a semantic matching pose corresponding to the current moment of the vehicle to be positioned; determine a second Kalman gain corresponding to the semantic feature according to the second covariance matrix, the semantic matching pose, a second observation matrix, a second positioning observation noise and a second adaptive factor; wherein the second covariance matrix is the first covariance matrix before updating or the first covariance matrix after updating; determine a second pose difference value according to the pose information in the current state vector and the semantic matching pose; update the adjusted current state vector according to the second pose difference value and the second Kalman gain.

7. The method of claim 6, wherein, The second adaptive factor is determined based on the following manner: collect a lane image of the vehicle to be positioned at the current time according to a camera device arranged on the vehicle to be positioned, and perform semantic feature extraction on the lane image according to a target model to obtain semantic features and confidence of each semantic point in the lane image; determine a semantic distribution density according to the number of semantic points and the total number of pixel points of the lane image; for each semantic point, determine a semantic registration accuracy according to the semantic feature of the semantic point, the coordinate information of each semantic point and the coordinate information of each registration semantic point; determine a second adaptive factor according to the semantic distribution density, the confidence and the semantic registration accuracy.

8. The method of claim 1 or 6, wherein, The method further comprises: determine a third Kalman gain according to the second covariance matrix, the wheel speed, a third observation matrix, a third positioning observation noise and a third adaptive factor; wherein the wheel speed is determined according to a wheel speed sensor arranged on the vehicle to be positioned; determine a first speed difference value according to the speed information in the current state vector and the wheel speed; update the adjusted current state vector according to the first speed difference value and the third Kalman gain.

9. The method of claim 8, wherein, The third adaptive factor is determined based on the following manner: determine a first speed difference value according to the speed information and the wheel speed; determine a third adaptive factor according to the first speed difference value and the wheel speed.

10. A downhole vehicle positioning apparatus, characterized by comprise: a current state vector determination module, configured to determine a current state vector at a current time according to a historical state vector corresponding to the vehicle to be positioned at a previous time and IMU data at the current time; wherein the historical state vector and the current state vector both comprise pose information, speed information, IMU bias information and a gravity vector of the vehicle to be positioned; a current state vector adjustment module, configured to determine a first Kalman gain corresponding to a laser radar arranged on the vehicle to be positioned according to the current state vector and original positioning data collected by the laser radar, so as to adjust the current state vector based on the first Kalman gain; a target positioning information determination module, configured to determine target positioning information of the vehicle to be positioned according to the adjusted current state vector.

Images