Pose determination method, device, system and program product
By identifying multiple degradation factors in radar point cloud data and IMU data of mechanical equipment and performing weighted fusion when their confidence levels meet the conditions, the pose determination problem of traditional SLAM technology in degraded environments is solved, achieving pose determination with high accuracy and robustness.
Patent Information
- Application Number
- CN202511240715.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-09-01
- Publication Date
- 2025-12-09
AI Technical Summary
In degraded environments, traditional SLAM technology struggles to accurately and robustly determine the pose information of mechanical devices, especially in environments with sparse features or lack of texture information, where multi-sensor fusion technology has limitations.
By acquiring radar point cloud data and IMU data of mechanical equipment, multiple degradation factors are identified, and weighted fusion is performed when these factors exceed the corresponding thresholds. Pose information is determined only when the confidence level of radar point cloud data is higher than the threshold. Combined with dynamic threshold adjustment and progressive weight adjustment, accuracy and robustness in degradation environments are ensured.
It enables accurate and robust determination of the pose information of mechanical equipment in multiple dimensions, reduces positioning errors and ghosting, and improves mapping and positioning accuracy in degraded environments.
Smart Images
Figure CN121091304A_ABST
Abstract
Description
Technical Field
[0001] This disclosure relates to the field of map construction technology, and in particular to a pose determination method, apparatus, system and program product. Background Technology
[0002] With the rapid development of robotics and autonomous driving technologies, Simultaneous Localization and Mapping (SLAM) technology has become a core technology for navigation in autonomous driving systems. Summary of the Invention
[0003] This disclosure proposes a pose determination method. By acquiring first sensing data of a mechanical device, multiple degradation factors are determined. Based on these factors, it is then determined whether the mechanical device is in a degraded environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical device is considered to be in a degraded environment. Next, in determining the pose of the mechanical device, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical device is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical device is in a degraded environment, it achieves multi-dimensional determination of whether the environment in which the mechanical device is located is a degraded environment, resulting in better accuracy and robustness in determining the environment in which the mechanical device is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0004] According to some embodiments of the first aspect of this disclosure, a pose determination method is provided, comprising: acquiring first sensing data of a mechanical device, wherein the first sensing data includes radar point cloud data and inertial measurement unit (IMU) data; determining a plurality of degradation factors based on the first sensing data; performing a first weighted fusion on the radar point cloud data and the IMU data when each degradation factor is greater than a degradation threshold corresponding to each degradation factor, thereby determining a confidence level of the radar point cloud data and a first fusion result; and determining pose information of the mechanical device based on the first fusion result when the confidence level of the radar point cloud data is greater than the confidence threshold.
[0005] In some embodiments, performing a first weighted fusion of radar point cloud data and IMU data to determine the confidence level of the radar point cloud data and the first fusion result includes: determining a first covariance matrix of the pose information of the mechanical equipment based on the IMU data; determining a first weight and a second weight of the radar point cloud data based on the first covariance matrix and the noise of the radar point cloud data; performing a first weighted fusion of the IMU data and the radar point cloud data based on the first weight and the second weight to obtain a first fusion result and a second covariance matrix of the pose information of the mechanical equipment; and determining the confidence level of the radar point cloud data based on the position variance in the second covariance matrix.
[0006] In some embodiments, determining multiple degradation factors based on first sensing data includes: for radar point cloud data, determining a first degradation factor among multiple degradation factors based on the difference between the radar point cloud data and historical radar point cloud data; and for IMU data, determining a second degradation factor among multiple degradation factors based on the difference between a first relative pose of the IMU data within a first time period and a second relative pose of the radar point cloud data within a first time period.
[0007] In some embodiments, historical radar point cloud data includes historical local point clouds stitched together from multiple frames of historical radar point clouds. Determining a first degradation factor among multiple degradation factors based on the differences between the radar point cloud data and the historical radar point cloud data includes: determining the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud based on the radar point cloud data and the historical local point cloud; determining the matching probability between each point cloud in the radar point cloud data and the historical local point cloud based on the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud; and determining the first degradation factor based on the matching probability between each point cloud in the radar point cloud data and the historical local point cloud.
[0008] In some embodiments, the first relative pose includes a first relative translation and a first relative rotation, and the second relative pose includes a second relative translation and a second relative rotation. Determining the second degradation factor among multiple degradation factors based on the difference between the first relative pose of the IMU data and the second relative pose of the radar point cloud data within a first time period includes: determining the translational residual between the first and second relative translations; determining the rotational residual between the first and second relative rotations; and determining the second degradation factor based on the translational and rotational residuals.
[0009] In some embodiments, the first sensing data further includes camera image data, and determining the plurality of degradation factors based on the first sensing data further includes: removing dynamic feature points from the camera image data; detecting the feature point density of the camera image data after removing dynamic feature points; and determining a third degradation factor among the plurality of degradation factors based on the feature point density.
[0010] In some embodiments, the first sensing data further includes wheel speed odometer data, and the pose determination method further includes: performing a first weighted fusion of the IMU data and the wheel speed odometer data when the confidence level of the radar point cloud data is less than or equal to a confidence level threshold, and determining a second fusion result; and determining the pose information of the mechanical equipment based on the second fusion result.
[0011] In some embodiments, the pose determination method further includes: when at least one degradation factor among a plurality of degradation factors is less than the degradation threshold corresponding to the degradation factor, performing a second weighted fusion on at least two items in the first sensing data to obtain a third fusion result; and determining the pose information of the mechanical device based on the third fusion result.
[0012] In some embodiments, after determining the first weight and the second weight, the pose determination method further includes: within a second time period, the weight of the IMU data is adjusted exponentially from the first initial weight to the first weight, and the weight of the radar point cloud data is adjusted exponentially from the second initial weight to the second weight.
[0013] In some embodiments, the degradation threshold corresponding to each degradation factor is dynamically determined based on the mean and standard deviation of historical degradation thresholds.
[0014] In some embodiments, the pose determination method further includes: synchronizing the first sensing data in time; performing distortion correction processing on the radar point cloud data in the time-synchronized first sensing data based on the IMU data in the time-synchronized first sensing data; and updating the first sensing data based on the time-synchronized first sensing data and the distortion-corrected radar point cloud data.
[0015] In some embodiments, where the first sensing data further includes camera image data, the pose determination method further includes: performing noise processing on the camera image data in the time-synchronized first sensing data; and updating the first sensing data based on the noise-processed camera image data.
[0016] According to some embodiments of the second aspect of this disclosure, a pose determination apparatus is provided, comprising: an acquisition unit configured to acquire first sensing data of a mechanical device, wherein the first sensing data includes radar point cloud data and inertial measurement unit (IMU) data; a first determination unit configured to determine a plurality of degradation factors based on the first sensing data; a second determination unit configured to, when each degradation factor in the plurality of degradation factors is greater than a degradation threshold corresponding to each degradation factor, perform weighted fusion of the radar point cloud data and the IMU data according to a first fusion algorithm to determine the confidence level of the radar point cloud data and the first fusion result; and a third determination unit configured to, when the confidence level of the radar point cloud data is greater than the confidence threshold, determine the pose information of the mechanical device based on the first fusion result.
[0017] According to some embodiments of the third aspect of this disclosure, a pose determination apparatus is provided, comprising: a memory and a processor coupled to the memory, the processor being configured to execute the pose determination method of any of the above embodiments based on instructions stored in the memory.
[0018] According to some embodiments of the fourth aspect of this disclosure, a pose determination system is provided, comprising: a pose determination device as described in any of the above embodiments; and a plurality of sensors configured to send first sensing data to the pose determination device.
[0019] According to some embodiments of the fifth aspect of this disclosure, a computer-readable storage medium is provided that stores computer instructions thereon, which, when executed by a processor, implement the pose determination method in any of the above embodiments.
[0020] According to some embodiments of the sixth aspect of this disclosure, a computer program product is provided, including computer instructions that, when executed by a processor, implement the pose determination method in any of the above embodiments. Attached Figure Description
[0021] The accompanying drawings, which form part of this specification, illustrate embodiments of this disclosure and, together with the specification, serve to explain the principles of this disclosure.
[0022] This disclosure can be more clearly understood with reference to the accompanying drawings and the following detailed description.
[0023] Figure 1 Schematic diagrams illustrating some embodiments of the pose determination method of this disclosure are shown.
[0024] Figure 2 Schematic diagrams illustrating other embodiments of the pose determination method of this disclosure are shown.
[0025] Figure 3 Schematic diagrams illustrating further embodiments of the pose determination method of this disclosure.
[0026] Figure 4 Schematic diagrams showing some embodiments of the pose determination apparatus of this disclosure are provided.
[0027] Figure 5 Schematic diagrams showing other embodiments of the pose determination apparatus of this disclosure are provided.
[0028] Figure 6 Schematic diagrams illustrating some embodiments of the pose determination system of this disclosure are shown. Detailed Implementation
[0029] Various exemplary embodiments of the present disclosure will now be described in detail with reference to the accompanying drawings. It should be noted that, unless otherwise specifically stated, the relative arrangement, numerical expressions, and values of the components and steps set forth in these embodiments do not limit the scope of the present disclosure.
[0030] At the same time, it should be understood that, for ease of description, the dimensions of the various parts shown in the accompanying drawings are not drawn according to actual scale.
[0031] The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit this disclosure or its application or use.
[0032] Techniques, methods, and equipment known to those skilled in the art may not be discussed in detail, but where appropriate, they should be considered part of the specification.
[0033] In all examples shown and discussed herein, any specific values should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values.
[0034] It should be noted that similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be discussed further in subsequent figures.
[0035] SLAM technology utilizes sensor data uploaded from multiple sensors (such as LiDAR, cameras, inertial measurement units (IMUs), wheel speed and odometers) to fuse and localize data while generating a point cloud map that describes the surrounding environment. However, when mechanical equipment enters a degraded environment, SLAM technology often suffers from matching failures and large localization errors due to sparse features. A degraded environment refers to an environment lacking sufficient feature points or texture information, making it difficult for sensors to acquire enough constraints for accurate localization and mapping. Examples include open environments, tunnels, narrow corridors, or environments with repetitive structures.
[0036] Traditional SLAM technology often suffers from ghosting or drifting in point cloud maps due to frame-to-frame matching failures and large localization errors in degraded environments. To address the limitations of single sensors, multi-sensor fusion techniques utilize tight and loose coupling to improve the robustness and accuracy of mapping and localization. However, in degraded environments, multi-sensor fusion techniques also exhibit limitations in determining the pose information of mechanical equipment.
[0037] Therefore, how to determine the pose information of mechanical equipment in any environment with good accuracy and robustness is a problem that needs to be solved. In order to solve the problem of how to determine the pose information of mechanical equipment in any environment with good accuracy and robustness, this disclosure proposes a pose determination method, as follows.
[0038] Figure 1 Schematic diagrams illustrating some embodiments of the pose determination method of this disclosure are shown.
[0039] like Figure 1 As shown, the pose determination method includes steps 110 to 140, and the pose determination method is executed by the pose determination device.
[0040] In step 110, the first sensing data of the mechanical equipment is acquired, wherein the first sensing data includes radar point cloud data and IMU data.
[0041] For example, the first sensing data includes data collected by multiple sensors, and may also include camera image data and wheel speed odometer data, etc.
[0042] In step 120, multiple degradation factors are determined based on the first sensing data.
[0043] A degradation factor is a factor that indicates whether the environment in which mechanical equipment is located has degraded. Subsequently, the relationship between the degradation factor and the corresponding degradation threshold can be used to determine whether environmental degradation has occurred.
[0044] In step 130, if each degradation factor among the multiple degradation factors is greater than the degradation threshold corresponding to each degradation factor, the radar point cloud data and IMU data are subjected to a first weighted fusion to determine the confidence level of the radar point cloud data and the first fusion result.
[0045] By judging the relationship between multiple degradation factors and their corresponding degradation thresholds, degradation detection is achieved through a multi-index collaborative judgment method, which ensures the accuracy and robustness of degradation detection, thereby ensuring the accuracy and robustness of the pose determination process.
[0046] For example, the degradation threshold corresponding to each degradation factor can be determined based on the historical degradation threshold corresponding to that degradation factor, or it can be determined based on the first sensing data.
[0047] In step 140, if the confidence level of the radar point cloud data is greater than the confidence level threshold, the pose information of the mechanical equipment is determined based on the first fusion result.
[0048] If the confidence level of radar point cloud data is greater than the confidence level threshold, it indicates that the radar point cloud data is reliable. Therefore, the pose information of mechanical equipment can be directly determined based on the first fusion result of radar point cloud data and IMU data.
[0049] After determining the position and orientation information of the mechanical equipment, step 110 is executed again to redetermine multiple degradation factors and their corresponding degradation thresholds.
[0050] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0051] In some embodiments, the degradation threshold corresponding to each degradation factor is dynamically determined based on the mean and standard deviation of historical degradation thresholds.
[0052] For example, the degradation threshold corresponding to each degradation factor can be dynamically determined by calculating the mean and standard deviation of historical degradation thresholds within a sliding window with a preset step size. As another example, the sliding window could include 10 historical degradation thresholds.
[0053] By determining the current degradation threshold based on the mean and standard deviation of historical degradation thresholds, the accuracy of the degradation threshold can be guaranteed, enabling accurate determination of whether environmental degradation has occurred, thereby ensuring the accuracy and robustness of the pose determination process.
[0054] The following describes how to determine multiple degradation factors based on the first sensor data, as detailed below.
[0055] In some embodiments, determining multiple degradation factors based on first sensing data includes: for radar point cloud data, determining a first degradation factor among multiple degradation factors based on the difference between the radar point cloud data and historical radar point cloud data; and for IMU data, determining a second degradation factor among multiple degradation factors based on the difference between a first relative pose of the IMU data within a first time period and a second relative pose of the radar point cloud data within a first time period.
[0056] By determining multiple degradation factors across multiple dimensions, and then using these factors to determine whether environmental degradation has occurred, the degradation status of the environment is fully considered. This allows for a more accurate assessment of whether environmental degradation has occurred, reducing the probability of misjudgment and improving the accuracy and robustness of the pose determination process.
[0057] In some embodiments, historical radar point cloud data includes historical local point clouds stitched together from multiple frames of historical radar point clouds. Determining a first degradation factor among multiple degradation factors based on the differences between the radar point cloud data and the historical radar point cloud data includes: determining the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud based on the radar point cloud data and the historical local point cloud; determining the matching probability between each point cloud in the radar point cloud data and the historical local point cloud based on the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud; and determining the first degradation factor based on the matching probability between each point cloud in the radar point cloud data and the historical local point cloud.
[0058] For example, the pose transformation between the current radar frame (radar point cloud data) and the historical local map (historical local point cloud) is determined based on the iterative nearest point matching algorithm. The first degradation factor is the uncertainty of the matching between radar frames. The larger the first degradation factor, the less reliable the matching is, that is, the greater the possibility that the mechanical equipment is in a degraded environment. The calculation process of the first degradation factor is shown in formulas (1) to (2).
[0059] (1)
[0060] (2)
[0061] in, This represents the matching probability of each point cloud in the radar point cloud data with historical local point clouds. and This represents the minimum distance between each point in the radar point cloud data and the historical local point cloud. denoted by Gaussian kernel width, E represents the first degradation factor, and N represents the number of points in the radar point cloud data.
[0062] For example, multiple frames of historical radar point cloud can be converted into 5 frames of historical radar point cloud.
[0063] The first degradation factor is determined by matching each point cloud with historical local point clouds. Considering that there is a certain degree of relationship between the degradation environment and the confidence level of radar point cloud data, the matching of each point cloud with historical local point clouds is an important parameter when judging whether the environment has degraded. This helps to improve the accuracy of the judgment, thereby ensuring the accuracy and robustness of pose determination.
[0064] In some embodiments, the first relative pose includes a first relative translation and a first relative rotation, and the second relative pose includes a second relative translation and a second relative rotation. Determining the second degradation factor among multiple degradation factors based on the difference between the first relative pose of the IMU data and the second relative pose of the radar point cloud data within a first time period includes: determining the translational residual between the first and second relative translations; determining the rotational residual between the first and second relative rotations; and determining the second degradation factor based on the translational and rotational residuals.
[0065] For example, the pose at the current moment is predicted by pre-integrating the IMU data, and the first relative pose quantity (also known as the first relative pose change quantity) of the IMU data in the first time period is predicted. At the same time, the second relative pose quantity (also known as the second relative pose change quantity) of the radar data in the first time period is determined. Then, the residual (including translation residual and rotation residual) between the lidar matched pose (second relative pose quantity) and the IMU pre-integrated pose (first relative pose quantity) is calculated. This residual is used as the second degradation factor. The larger the second degradation factor, the greater the difference between the first relative pose quantity and the second relative pose quantity, that is, the greater the possibility that the mechanical equipment is in a degradation environment. The second degradation factor is shown in formula (3).
[0066] (3)
[0067] Where R represents the second degradation factor (residual), and the first relative pose is expressed as... , This is the first relative translation. The first relative rotation is represented as, and the second relative pose is represented as. , This is the second relative translation. This is the second relative rotation amount. To translate the residual, For rotational residuals, The weights for the translation residuals, The weights are for the rotational residuals.
[0068] By using the translational and rotational residuals between radar point cloud data and IMU data as a second degradation factor, the relationship between the differences between radar point cloud data and IMU data and the degradation environment is fully considered, ensuring the accuracy of degradation environment determination, thereby ensuring the accuracy of pose determination.
[0069] In some embodiments, the first sensing data further includes camera image data, and determining the plurality of degradation factors based on the first sensing data further includes: removing dynamic feature points from the camera image data; detecting the feature point density of the camera image data after removing dynamic feature points; and determining a third degradation factor among the plurality of degradation factors based on the feature point density.
[0070] For example, optical flow can be used to remove dynamic feature points (such as dynamic objects in the environment of mechanical equipment) from camera image data. SIFT (Scale-Invariant Feature Transform) can then be used to detect feature points in the camera image data to determine the feature point density after removing dynamic feature points. The detected feature point density is then defined as the third degradation factor, which can be used... The third degradation factor is shown in formula (4).
[0071] (4)
[0072] in, The number of detected feature points is represented by W, the width of the camera image is represented by H, and the height of the camera image is represented by H.
[0073] By taking into account the relationship between the degraded environment and the density of feature points, and by fully considering the degradation of the environment, pose determination can be performed more accurately.
[0074] T E T represents the degradation threshold corresponding to the first degradation factor. R T represents the degradation threshold corresponding to the second degradation factor. F This represents the degradation threshold corresponding to the third degradation factor. If the first degradation factor E is greater than T... E The second degradation factor R is greater than T. R And the third degradation factor Greater than T F This indicates that the mechanical equipment is currently in a degraded environment.
[0075] The following describes how to determine the degradation threshold corresponding to each degradation factor based on the historical degradation threshold corresponding to each degradation factor, using formulas (5) to (7).
[0076] (5)
[0077] (6)
[0078] (7)
[0079] in, This represents the degradation threshold corresponding to the first degradation factor. This represents the mean of the historical degradation thresholds corresponding to the first degradation factor. This represents the standard deviation of the historical degradation threshold corresponding to the first degradation factor. This represents the degradation threshold corresponding to the second degradation factor. This represents the mean of the historical degradation thresholds corresponding to the second degradation factor. This represents the standard deviation of the historical degradation threshold corresponding to the second degradation factor. This represents the degradation threshold corresponding to the third degradation factor. This represents the mean of the historical degradation thresholds corresponding to the third degradation factor. denoted by , represents the standard deviation of the historical degradation threshold corresponding to the third degradation factor, and k represents the adjustment coefficient.
[0080] The following describes how to perform a first weighted fusion of radar point cloud data and IMU data to determine the confidence level of the radar point cloud data and the first fusion result, as detailed below.
[0081] In some embodiments, performing a first weighted fusion of radar point cloud data and IMU data to determine the confidence level of the radar point cloud data and the first fusion result includes: determining a first covariance matrix of the pose information of the mechanical equipment based on the IMU data; determining a first weight and a second weight of the radar point cloud data based on the first covariance matrix and the noise of the radar point cloud data; performing a first weighted fusion of the IMU data and the radar point cloud data based on the first weight and the second weight to obtain a first fusion result and a second covariance matrix of the pose information of the mechanical equipment; and determining the confidence level of the radar point cloud data based on the position variance in the second covariance matrix.
[0082] For example, an iterative error Kalman filter algorithm is used to perform a first weighted fusion of radar point cloud data and IMU data to determine the confidence level of the radar point cloud data and the first fusion result. First, the IMU data is pre-integrated to obtain the predicted pose, and the first covariance matrix of the predicted pose is determined based on the IMU data. Then, the first weighted fusion of the IMU data and radar point cloud data is performed to obtain the second covariance matrix and the first fusion result. The position variance in the diagonal data of the second covariance matrix is checked for drift (becoming abnormally large). If no drift occurs, the radar point cloud data is considered reliable; if a drift occurs, the radar point cloud data is considered unreliable.
[0083] Compared to fixed fusion weights, the first and second weights are determined and dynamically adjusted in real time based on the first covariance matrix and the noise of the radar point cloud data, ensuring the flexibility and accuracy of the fusion process, and thus ensuring the accuracy and robustness of the pose determination process.
[0084] In some embodiments, after determining the first weight and the second weight, during a second time period, the weight of the IMU data is adjusted exponentially from the first initial weight to the first weight, and the weight of the radar point cloud data is adjusted exponentially from the second initial weight to the second weight.
[0085] If the mechanical equipment is in a degraded environment, the weight of the IMU data will gradually increase, while the weight of the radar point cloud data will gradually decrease. The weight of the IMU data is adjusted exponentially from the first initial weight to the first weight, as shown in formula (8).
[0086] (8)
[0087] in, Indicates the weight of IMU data, Indicates the attenuation coefficient. This represents the target weight (i.e., the first weight) of the IMU data. The initial weight of the IMU data (i.e., the first initial weight) is represented by the first initial weight. The process of adjusting the weight of the radar point cloud data from the second initial weight to the second weight in an exponential manner is similar.
[0088] Compared to the "hard switching" fusion method of sensor data, the gradual weight adjustment method adjusts the weights within a specified transition time, thus smoothly estimating the pose. This reduces the risk of sudden changes in the pose estimation of mechanical equipment, which could cause abrupt changes in the trajectory of mechanical equipment in the point cloud map. This ensures the stability and accuracy of the first fusion result, thereby guaranteeing the accuracy and robustness of the pose determination process.
[0089] The following describes how to determine the pose information of mechanical equipment when it is in a degraded environment and the radar point cloud data is unreliable.
[0090] In some embodiments, the first sensing data further includes wheel speed odometer data. When the confidence level of the radar point cloud data is less than or equal to the confidence level threshold, the IMU data and the wheel speed odometer data are subjected to a first weighted fusion to determine a second fusion result. Based on the second fusion result, the pose information of the mechanical equipment is determined.
[0091] When radar point cloud data is unreliable, the pose information of mechanical equipment can be determined directly by fusing IMU data and wheel speed odometer data. This reduces the interference of unreliable radar point cloud data on pose information determination and ensures the accuracy and robustness of the pose determination process.
[0092] In some embodiments, during the first weighted fusion of IMU data and wheel speed odometer data, the travel speed of the mechanical equipment can be reduced to ensure the safety of the mechanical equipment during the data fusion process.
[0093] The following describes how to determine the position and orientation information of mechanical equipment in a non-degraded environment (i.e., a normal environment).
[0094] In some embodiments, if at least one degradation factor among multiple degradation factors is less than the degradation threshold corresponding to the degradation factor, a second weighted fusion is performed on at least two items in the first sensing data to obtain a third fusion result; based on the third fusion result, the pose information of the mechanical equipment is determined.
[0095] For example, the fact that machinery is in a non-degraded environment could mean that the environment in which the machinery is located has not degraded, or it could mean that the machinery has left the degraded environment.
[0096] For example, performing a second weighted fusion on at least two items in the first sensing data includes: performing a second weighted fusion on at least two items in the first sensing data based on a factor graph optimization algorithm to obtain a third fusion result. Regarding the weights of the at least two items in the first sensing data, the corresponding factors of the at least two items can be fused by calling the GTSAM factor optimization library to calculate the optimized current estimated pose. The corresponding factors include radar factors, wheel speed odometer factors, IMU pre-integration factors, camera image factors, etc.
[0097] In some embodiments, after acquiring the first sensing data, the first sensing data is time-synchronized; based on the IMU data in the time-synchronized first sensing data, the radar point cloud data in the time-synchronized first sensing data is subjected to distortion correction processing; and the first sensing data is updated based on the time-synchronized first sensing data and the distortion-corrected radar point cloud data.
[0098] In some embodiments, where the first sensing data also includes camera image data, noise processing can be applied to the camera image data in the time-synchronized first sensing data; and the first sensing data can be updated based on the noise-processed camera image data.
[0099] By performing a series of preprocessing steps, such as time synchronization, on the first sensor data, the accuracy of determining multiple degradation factors based on the first sensor data is ensured. This ensures the accuracy of the degradation environment judgment, as well as the accuracy and robustness of the pose determination.
[0100] In some embodiments, the environment in which the mechanical equipment is located is detected in real time to determine at least one of the multiple degradation factors at the current moment is less than or equal to its corresponding updated degradation threshold. If there is a degradation factor less than its corresponding updated degradation threshold, it means that the mechanical equipment is in a non-degraded environment; otherwise, it means that the mechanical equipment is still in a degraded environment.
[0101] By detecting recovery conditions in real time, the fusion method and the data to be fused can be adjusted in a timely manner according to the specific environment, ensuring the accuracy and robustness of pose determination.
[0102] The following describes the multiple modules included in the pose determination system and the correspondence between these modules and the steps in the pose determination method.
[0103] In some embodiments, the pose determination system includes a data acquisition unit and a data processing unit, wherein the data processing unit includes a data preprocessing module, a real-time degradation detection module, a layered fusion and switching module, a recovery module, and a map update module.
[0104] The data acquisition unit is used to acquire radar point cloud data, IMU data, camera image data, and wheel speed odometer data.
[0105] The data preprocessing module is used to preprocess various sensor data collected by the data acquisition unit. First, it synchronizes radar point cloud data, IMU data, camera image data, and wheel speed odometer data. Then, it uses the synchronized IMU data to perform distortion correction on the radar point cloud data and noise reduction on the camera image data.
[0106] Real-time degradation detection module: Determines whether environmental degradation has occurred based on multiple degradation factors, and uses a dynamic threshold mechanism to update the degradation threshold corresponding to each degradation factor in real time.
[0107] The layered fusion and switching module switches the fusion method and the sensor data to be fused based on the judgment result. When the environment is normal, radar point cloud data, IMU data, and wheel speed odometer data are fused based on the factor graph optimization algorithm. When the environment degrades, radar point cloud data and IMU data are fused based on the iterative error Kalman filter algorithm. The reliability of the current radar point cloud data is judged based on the predicted updated pose covariance matrix. If the radar point cloud data is unreliable, wheel speed odometer data is introduced to fuse with the IMU data to ensure the accuracy of pose estimation. To prevent pose jumps caused by instantaneous mode switching, the weights of the IMU data are progressively adjusted to achieve progressive mode switching.
[0108] Recovery detection module: It uses the updated degradation threshold to determine the degradation status of the current environment, and then decides which fusion method to use to fuse which sensor data from multiple sensor data, thereby determining the pose.
[0109] Figure 2 Schematic diagrams illustrating other embodiments of the pose determination method of this disclosure are shown.
[0110] like Figure 2 As shown, the pose determination method includes steps 210 to 260.
[0111] In step 210, radar point cloud data, IMU data, camera image data, and wheel speed odometer data are collected, and these data are time-synchronized and preprocessed.
[0112] Preprocessing includes: (1) using synchronized IMU data to perform distortion correction on radar point cloud data; and (2) performing noise correction on camera image data.
[0113] In step 220, degradation detection is performed by combining the first degradation factor, the second degradation factor, and the third degradation factor, and the degradation threshold is updated using a dynamic threshold adjustment mechanism.
[0114] In step 230, for a normal environment, radar point cloud data, IMU data, and wheel speed odometer data are fused based on a factor graph optimization algorithm.
[0115] In step 240, for the degraded environment, the iterative error Kalman filter algorithm is used to fuse radar point cloud data and IMU data to determine the confidence level of radar point cloud data and the first fusion result. During the determination process, the first weight and the second weight are progressively adjusted.
[0116] By fusing radar point cloud data and IMU data, the first covariance matrix is updated to obtain the second covariance matrix, and the confidence level of the radar point cloud data is determined based on the position variance in the second covariance matrix.
[0117] In step 250, the current environment is detected using the updated degradation threshold to determine the environment in which the mechanical equipment is located, thereby selecting a suitable fusion method for pose determination.
[0118] In step 260, the pose information is output.
[0119] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0120] Figure 3 Schematic diagrams illustrating further embodiments of the pose determination method of this disclosure.
[0121] like Figure 3 As shown, the pose determination method includes steps 310 to 390.
[0122] In step 310, first sensing data is acquired, wherein the first sensing data is sensing data input from multiple sensors.
[0123] In step 320, the first sensor data is time-synchronized and preprocessed.
[0124] In step 330, multiple degradation factors are determined based on the first sensing data, and a degradation threshold corresponding to each degradation factor is determined. The degradation threshold corresponding to each degradation factor is updated in real time.
[0125] In step 340, it is determined whether the environment in which the mechanical equipment is located has degraded. If degradation has occurred, step 350 is executed; otherwise, step 380 is executed directly.
[0126] In step 350, the fusion result of radar point cloud data and IMU data, as well as the confidence level of the radar point cloud data, are determined based on the iterative error Kalman filter algorithm. Specifically, the weights of these two types of data are adjusted using a progressive adjustment mechanism during the determination of the fusion result.
[0127] In some embodiments, after the radar point cloud data is reliable and the fusion result of the radar point cloud data and IMU data is determined, the pose information of the mechanical equipment can be determined based on the fusion result, and the pose information can be output.
[0128] In step 360, recovery condition detection is performed to determine multiple degradation factors and the corresponding degradation threshold for each degradation factor.
[0129] In step 370, it is determined whether there exists at least one degradation factor among the plurality of degradation factors that is less than or equal to its corresponding degradation threshold. If there is at least one degradation factor among the plurality of degradation factors that is less than or equal to its corresponding degradation threshold, then step 380 is executed; otherwise, step 350 is executed again.
[0130] In step 380, at least two items in the first sensing data are fused based on the factor graph optimization algorithm to obtain the fusion result.
[0131] In step 390, pose information is output.
[0132] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0133] Figure 4 Schematic diagrams showing some embodiments of the pose determination apparatus of this disclosure are provided.
[0134] like Figure 4 As shown, the pose determination device 40 includes an acquisition unit 41, a first determination unit 42, a second determination unit 43, and a third determination unit 44.
[0135] The acquisition unit 41 is configured to acquire first sensing data of the mechanical equipment, wherein the first sensing data includes radar point cloud data and inertial measurement unit (IMU) data.
[0136] The first determining unit 42 is configured to determine multiple degradation factors based on the first sensing data.
[0137] The second determining unit 43 is configured to perform weighted fusion of radar point cloud data and IMU data according to the first fusion algorithm when each of the multiple degradation factors is greater than the degradation threshold corresponding to each degradation factor, thereby determining the confidence level of the radar point cloud data and the first fusion result.
[0138] In some embodiments, the degradation threshold corresponding to each degradation factor is dynamically determined based on the mean and standard deviation of historical degradation thresholds.
[0139] The third determining unit 44 is configured to determine the pose information of the mechanical equipment based on the first fusion result when the confidence level of the radar point cloud data is greater than the confidence level threshold.
[0140] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0141] In some embodiments, the first determining unit 42 is further configured to, for radar point cloud data, determine a first degradation factor among a plurality of degradation factors based on the difference between the radar point cloud data and historical radar point cloud data; and for IMU data, determine a second degradation factor among a plurality of degradation factors based on the difference between a first relative pose of the IMU data and a second relative pose of the radar point cloud data within a first time period.
[0142] In some embodiments, the historical radar point cloud data includes historical local point clouds stitched together from multiple frames of historical radar point clouds. The first determining unit 42 is further configured to determine a first degradation factor among multiple degradation factors based on the difference between the radar point cloud data and the historical radar point cloud data, including: determining the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud based on the radar point cloud data and the historical local point cloud; determining the matching probability between each point cloud in the radar point cloud data and the historical local point cloud based on the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud; and determining the first degradation factor based on the matching probability between each point cloud in the radar point cloud data and the historical local point cloud.
[0143] In some embodiments, the first relative pose includes a first relative translation and a first relative rotation, the second relative pose includes a second relative translation and a second relative rotation, and the first determining unit 42 is further configured to determine a translational residual between the first relative translation and the second relative translation; determine a rotational residual between the first relative rotation and the second relative rotation; and determine a second degradation factor based on the translational residual and the rotational residual.
[0144] In some embodiments, the first sensing data further includes camera image data, and the first determining unit 42 is further configured to remove dynamic feature points from the camera image data; detect the feature point density of the camera image data after removing dynamic feature points; and determine a third degradation factor among a plurality of degradation factors based on the feature point density.
[0145] In some embodiments, the second determining unit 43 is further configured to: determine a first covariance matrix of the pose information of the mechanical device based on the IMU data; determine a first weight of the IMU data and a second weight of the radar point cloud data based on the first covariance matrix and the noise of the radar point cloud data; perform a first weighted fusion on the IMU data and the radar point cloud data based on the first weight and the second weight to obtain a first fusion result and a second covariance matrix of the pose information of the mechanical device; and determine the confidence level of the radar point cloud data based on the position variance in the second covariance matrix.
[0146] In some embodiments, the first sensing data further includes wheel speed odometer data, the second determining unit 43 is further configured to perform a first weighted fusion of the IMU data and the wheel speed odometer data when the confidence level of the radar point cloud data is less than or equal to a confidence threshold, and determine a second fusion result, and the third determining unit 44 is further configured to determine the pose information of the mechanical equipment based on the second fusion result.
[0147] In some embodiments, the second determining unit 43 is further configured to, after determining the first weight and the second weight, adjust the weight of the IMU data exponentially from the first initial weight to the first weight and the weight of the radar point cloud data exponentially from the second initial weight to the second weight within a second time period.
[0148] In some embodiments, the second determining unit 43 is further configured to perform a second weighted fusion on at least two items in the first sensing data to obtain a third fusion result when at least one degradation factor among the multiple degradation factors is less than the degradation threshold corresponding to the degradation factor, and the third determining unit 44 is further configured to determine the pose information of the mechanical device based on the third fusion result.
[0149] In some embodiments, the pose determination device 40 further includes a preprocessing unit configured to time-synchronize the first sensing data; perform distortion correction processing on the radar point cloud data in the time-synchronized first sensing data based on the IMU data in the time-synchronized first sensing data; and update the first sensing data based on the time-synchronized first sensing data and the distortion-corrected radar point cloud data.
[0150] In some embodiments, where the first sensing data further includes camera image data, the preprocessing unit is further configured to perform noise processing on the camera image data in the time-synchronized first sensing data; and update the first sensing data based on the noise-processed camera image data.
[0151] Figure 5 Schematic diagrams showing other embodiments of the pose determination apparatus of this disclosure are provided.
[0152] like Figure 5 As shown, the pose determination device 40 of this embodiment includes a memory 51 and a processor 52 coupled to the memory 51. The processor 52 is configured to execute the pose determination method in any of the foregoing embodiments based on instructions stored in the memory 51.
[0153] The memory 51 may include, for example, system memory, fixed non-volatile storage media, etc. The system memory stores, for example, the operating system, application programs, boot loader, and other programs.
[0154] The pose determination device 40 may also include an input / output interface 53, a network interface 54, and a storage interface 55. These interfaces 53, 54, and 55, as well as the memory 51 and processor 52, can be connected via, for example, a bus 56. The input / output interface 53 provides a connection interface for input / output devices such as a monitor, mouse, keyboard, touchscreen, microphone, and speakers. The network interface 54 provides a connection interface for various networked devices. The storage interface 55 provides a connection interface for external storage devices such as SD cards and USB flash drives.
[0155] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0156] Figure 6 Schematic diagrams illustrating some embodiments of the pose determination system of this disclosure are shown.
[0157] like Figure 6 As shown, the pose determination system 60 includes the pose determination device 40 in any of the above embodiments; and multiple sensors 61 configured to send first sensing data to the pose determination device 40.
[0158] In the above embodiments, multiple degradation factors are determined using the first sensing data of the acquired mechanical equipment. Based on these factors, it is then determined whether the mechanical equipment is in a degrading environment. If each degradation factor is greater than its corresponding degradation threshold, the mechanical equipment is considered to be in a degrading environment. Next, in determining the mechanical equipment's pose, the confidence level of the radar point cloud data and the first fusion result of the radar point cloud data and IMU data are first determined. If the confidence level of the radar point cloud data is greater than the confidence threshold, the radar point cloud data is considered reliable. The pose information of the mechanical equipment is then determined based on the first fusion result. By using multiple degradation factors to determine whether the mechanical equipment is in a degrading environment, the determination of whether the environment in which the mechanical equipment is located is a degrading environment is achieved from multiple dimensions, resulting in better accuracy and robustness in determining the environment in which the mechanical equipment is located. By studying the fusion results of radar point cloud data and IMU data of mechanical equipment in a degraded environment, the confidence level of the radar point cloud data was determined. Only when the radar point cloud data is reliable will the pose information of the mechanical equipment be determined based on the fusion result. This results in better accuracy and robustness in determining the pose of mechanical equipment in a degraded environment.
[0159] In some embodiments, a computer program product is protected, comprising a computer program or instructions that, when executed by a processor, implement the power spectral density determination method described above. The computer program product includes a computer program carried on a computer-readable medium, containing program code for performing the methods shown in the flowchart. In such embodiments, the computer program can be downloaded and installed from a network via a pose determination device, or installed from a storage device, or installed from a ROM. When the computer program is executed by a CPU, it performs the functions defined in the methods of embodiments of this disclosure.
[0160] Those skilled in the art will understand that embodiments of this disclosure can be provided as methods, systems, or computer program products. Therefore, this disclosure can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this disclosure can take the form of a computer program product embodied on one or more computer-usable non-transitory storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.
[0161] The pose determination method, apparatus, system, and program products of this disclosure have been described in detail above. To avoid obscuring the concept of this disclosure, some details known in the art have not been described. Those skilled in the art can fully understand how to implement the technical solutions disclosed herein based on the above description.
[0162] The methods and systems of this disclosure may be implemented in many ways. For example, they may be implemented by software, hardware, firmware, or any combination of software, hardware, and firmware. The above-described order of steps for the methods is for illustrative purposes only, and the steps of the methods of this disclosure are not limited to the order specifically described above unless otherwise specifically stated. Furthermore, in some embodiments, this disclosure may also be implemented as a program recorded on a recording medium, the program including machine-readable instructions for implementing the methods according to this disclosure. Thus, this disclosure also covers recording media storing programs for performing the methods according to this disclosure.
[0163] While specific embodiments of this disclosure have been described in detail by way of example, those skilled in the art should understand that the examples are for illustrative purposes only and not intended to limit the scope of this disclosure. Those skilled in the art should understand that modifications can be made to the above embodiments without departing from the scope and spirit of this disclosure. The scope of this disclosure is defined by the appended claims.
Claims
1. A pose determination method, comprising: Acquire first sensing data from mechanical equipment, wherein the first sensing data includes radar point cloud data and inertial measurement unit (IMU) data; Based on the first sensing data, multiple degradation factors are determined; If each of the plurality of degradation factors is greater than the degradation threshold corresponding to each degradation factor, the radar point cloud data and the IMU data are subjected to a first weighted fusion to determine the confidence level of the radar point cloud data and the first fusion result. If the confidence level of the radar point cloud data is greater than the confidence level threshold, the pose information of the mechanical equipment is determined based on the first fusion result.
2. The pose determination method according to claim 1, wherein, The radar point cloud data and the IMU data are subjected to a first weighted fusion to determine the confidence level of the radar point cloud data and the first fusion result, which includes: Based on the IMU data, determine the first covariance matrix of the pose information of the mechanical equipment; Based on the first covariance matrix and the noise of the radar point cloud data, determine the first weight of the IMU data and the second weight of the radar point cloud data; Based on the first weight and the second weight, the IMU data and the radar point cloud data are subjected to a first weighted fusion to obtain the first fusion result and the second covariance matrix of the mechanical equipment's pose information; The confidence level of the radar point cloud data is determined based on the position variance in the second covariance matrix.
3. The pose determination method according to claim 1, wherein, Based on the first sensing data, multiple degradation factors are identified, including: Based on the radar point cloud data, a first degradation factor is determined among the plurality of degradation factors according to the difference between the radar point cloud data and historical radar point cloud data. For the IMU data, a second degradation factor is determined based on the difference between the first relative pose of the IMU data in the first time period and the second relative pose of the radar point cloud data in the first time period.
4. The pose determination method according to claim 3, wherein, The historical radar point cloud data includes historical local point clouds stitched together from multiple frames of historical radar point clouds. Based on the differences between the radar point cloud data and historical radar point cloud data, the first degradation factor among the plurality of degradation factors is determined to include: Based on the radar point cloud data and the historical local point cloud, determine the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud; The matching probability between each point cloud in the radar point cloud data and the historical local point cloud is determined based on the minimum distance between each point cloud in the radar point cloud data and the historical local point cloud. The first degradation factor is determined based on the matching probability between each point cloud in the radar point cloud data and the historical local point cloud.
5. The pose determination method according to claim 3, wherein, The first relative pose quantity includes a first relative translation and a first relative rotation, and the second relative pose quantity includes a second relative translation and a second relative rotation. Based on the difference between the first relative pose of the IMU data and the second relative pose of the radar point cloud data within the first time period, the second degradation factor among the plurality of degradation factors is determined to include: Determine the translation residual between the first relative translation and the second relative translation; Determine the rotational residual between the first relative rotation amount and the second relative rotation amount; The second degradation factor is determined based on the translation residual and the rotation residual.
6. The pose determination method according to claim 3, wherein, The first sensing data also includes camera image data. Determining multiple degradation factors based on the first sensing data further includes: Remove dynamic feature points from the camera image data; Detect the feature point density of the camera image data after removing the dynamic feature points; Based on the feature point density, the third degradation factor among the plurality of degradation factors is determined.
7. The pose determination method according to any one of claims 1 to 6, wherein the first sensing data further includes wheel speed and odometer data, and further includes: If the confidence level of the radar point cloud data is less than or equal to the confidence threshold, the IMU data and the wheel speed odometer data are subjected to a first weighted fusion to determine a second fusion result; Based on the second fusion result, the pose information of the mechanical equipment is determined.
8. The pose determination method according to claim 2, further comprising, after determining the first weight and the second weight: During the second time period, the weight of the IMU data is adjusted exponentially from the first initial weight to the first weight, and the weight of the radar point cloud data is adjusted exponentially from the second initial weight to the second weight.
9. The pose determination method according to any one of claims 1 to 6, wherein, The degradation threshold corresponding to each degradation factor is dynamically determined based on the mean and standard deviation of historical degradation thresholds.
10. The pose determination method according to any one of claims 1 to 6, further comprising: If at least one of the degradation factors is less than the degradation threshold corresponding to the degradation factor, a second weighted fusion is performed on at least two items in the first sensing data to obtain a third fusion result. Based on the third fusion result, the pose information of the mechanical device is determined.
11. The pose determination method according to any one of claims 1 to 6, further comprising: Synchronize the first sensor data in time; Based on the IMU data in the time-synchronized first sensor data, the radar point cloud data in the time-synchronized first sensor data is subjected to distortion correction processing. The first sensor data is updated based on the time-synchronized first sensor data and the distortion-corrected radar point cloud data.
12. The pose determination method according to claim 11, further comprising, when the first sensing data further includes camera image data: Noise processing is performed on the camera image data in the time-synchronized first sensor data; The first sensing data is updated based on the noise-processed camera image data.
13. A pose determination device, comprising: The acquisition unit is configured to acquire first sensing data of the mechanical equipment, wherein the first sensing data includes radar point cloud data and inertial measurement unit (IMU) data. The first determining unit is configured to determine multiple degradation factors based on the first sensing data; The second determining unit is configured to, when each of the plurality of degradation factors is greater than the degradation threshold corresponding to each degradation factor, perform weighted fusion of the radar point cloud data and the IMU data according to the first fusion algorithm to determine the confidence level of the radar point cloud data and the first fusion result. The third determining unit is configured to determine the pose information of the mechanical equipment based on the first fusion result when the confidence level of the radar point cloud data is greater than a confidence level threshold.
14. A pose determination device, comprising: Memory; and A processor coupled to the memory, the processor being configured to execute the pose determination method of any one of claims 1 to 12 based on instructions stored in the memory.
15. A pose determination system, comprising: The pose determination device as described in claim 13 or 14; Multiple sensors are configured to send the first sensing data to the pose determination device.
16. A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the pose determination method according to any one of claims 1 to 12.
17. A computer program product comprising a computer program that, when executed by a processor, implements the pose determination method according to any one of claims 1 to 12.