Dynamic threshold and multi-sensor pose fusion method, device and equipment for phased prediction, and storage medium

Abstract

The application discloses a dynamic threshold and multi-sensor pose fusion method and device based on phased prediction, an equipment and a storage medium, relates to the technical field of data fusion, and comprises the following steps: acquiring measurement data of multiple heterogeneous sensors, prior accuracy information and recorded historical fusion results; performing phased prediction according to the historical fusion results to determine a pose prediction value; determining pose residuals between the measurement data of each sensor and the pose prediction value to obtain residual statistical characteristics; adaptively generating a dynamic discrimination threshold based on the residual statistical characteristics of each sensor, and determining effective sensors from the multiple heterogeneous sensors according to the dynamic discrimination threshold; and determining a fusion pose according to at least one of the measurement data corresponding to the effective sensors, the prior accuracy information and the pose prediction value. The application guarantees the robustness and continuity of the multi-sensor fusion pose.

Description

Technical Field

[0001] This application relates to the field of data fusion technology, and in particular to a multi-sensor pose fusion method, apparatus, device, and storage medium for dynamic thresholding and staged prediction. Background Technology

[0002] In robot end effector pose estimation, multiple heterogeneous sensors (such as encoders, laser rangefinders, and vision systems) are typically used for measurement. Weighted fusion methods are then employed to fuse these multiple measurement data into a unified pose estimation result, improving the accuracy and reliability of the pose estimation. However, during actual operation, sensors may exhibit occasional abnormal measurements, such as those caused by environmental interference, momentary occlusion, or sensor instability, leading to significant deviations from the true values. Including these abnormal measurements in the fusion calculation directly contaminates the fusion result, resulting in decreased pose estimation accuracy. Existing multi-sensor fusion methods typically rely on anomaly detection mechanisms to identify and eliminate abnormal sensors. However, the accuracy of anomaly detection is difficult to guarantee under different operating conditions. When sensor noise levels change with operating conditions, the original judgment criteria may no longer apply, easily leading to misjudging normal measurements as abnormal or missing abnormal measurements as normal, thus affecting the accuracy and continuity of the fused pose.

[0003] Therefore, the current solution has technical problems such as occasional abnormal measurements of sensors contaminating the fusion results during the multi-sensor fusion process, and difficulty in guaranteeing the accuracy of anomaly detection under different operating conditions. Summary of the Invention

[0004] The main objective of this application is to provide a method, apparatus, device, and storage medium for multi-sensor pose fusion using dynamic thresholding and staged prediction, aiming to solve the technical problem that occasional abnormal sensor measurements during multi-sensor fusion can contaminate the fusion results, and that the accuracy of anomaly detection is difficult to guarantee under different operating conditions.

[0005] To achieve the above objectives, this application proposes a multi-sensor pose fusion method based on dynamic thresholding and staged prediction. The multi-sensor pose fusion method based on dynamic thresholding and staged prediction includes: Acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors; Based on the historical fusion results, staged predictions are performed to determine the pose prediction value; Determine the pose residuals between the measurement data of each sensor and the pose prediction value, and obtain the residual statistical characteristics; Based on the residual statistical characteristics of each sensor, an adaptive dynamic anomaly threshold is generated, and a valid sensor is determined from the multi-source sensors according to the dynamic anomaly threshold. A fused pose is determined based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

[0006] In one embodiment, the step of adaptively generating a dynamic anomaly threshold based on the residual statistical characteristics of each sensor, and determining the effective sensor from the multi-source sensors according to the dynamic anomaly threshold, includes: Obtain historical residual samples and preset sample thresholds; Based on the historical residual samples, the preset sample threshold, and multiple residual statistical features, a dynamic outlier threshold is adaptively generated. The effective sensor is determined from the multi-source sensors based on the dynamic anomaly detection threshold.

[0007] In one embodiment, the adaptive generation of a dynamic anomaly threshold based on the historical residual samples, the preset sample threshold, and multiple residual statistical features includes: Obtain the preset fixed difference threshold and multiplier factor; When the historical residual sample is less than the preset sample threshold, the fixed outlier threshold is used as the dynamic outlier threshold. When the historical residual sample is greater than or equal to the preset sample threshold, a dynamic anomaly detection threshold is generated for each sensor based on the residual statistical characteristics and the multiplier factor.

[0008] In one embodiment, determining the effective sensor from the multiple heterogeneous sensors based on the dynamic anomaly threshold includes: Based on the measurement data from each sensor and the predicted pose value, determine the pose residual; Sensors whose pose residual is greater than or equal to the dynamic anomaly detection threshold are identified as abnormal sensors. The abnormal sensor is removed from the multi-channel heterogeneous sensor to obtain the effective sensor.

[0009] In one embodiment, the step of performing phased prediction based on the historical fusion results to determine the pose prediction value includes: When the number of historical fusion results is less than a preset threshold, the average value of the measurement data from each sensor is used as the pose prediction value. When the number of historical fusion results is greater than or equal to the preset number threshold, the average value of the historical fusion results within the preset sliding window is used as the pose prediction value.

[0010] In one embodiment, determining the pose residual between the measurement data of each sensor and the pose prediction value, and obtaining the residual statistical characteristics, includes: Calculate the absolute value of the difference between the measurement data of each sensor and the pose prediction value, and use the absolute value of the difference as the pose residual; Based on the pose residuals of each sensor within a preset historical window, the statistical characteristics of the residuals are determined.

[0011] In one embodiment, determining the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value includes: When the number of effective sensors is a preset value, the pose prediction value is used as the fused pose. When the number of effective sensors is not a preset value, the fused pose is determined based on the measurement data corresponding to the effective sensors and the prior accuracy information.

[0012] Furthermore, to achieve the above objectives, this application also proposes a multi-sensor pose fusion device based on dynamic thresholding and staged prediction, the multi-sensor pose fusion device comprising: The data acquisition module is used to acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors. The pose prediction module is used to perform phased prediction based on the historical fusion results and determine the pose prediction value. The residual statistics module is used to determine the pose residual between the measurement data of each sensor and the pose prediction value, and to obtain the residual statistical characteristics. The dynamic anomaly detection module is used to adaptively generate a dynamic anomaly detection threshold based on the residual statistical characteristics of each sensor, and to determine the effective sensor from the multi-source sensors according to the dynamic anomaly detection threshold. The fusion pose module is used to determine the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

[0013] Furthermore, to achieve the above objectives, this application also proposes a multi-sensor pose fusion device for dynamic thresholding and staged prediction, the device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the multi-sensor pose fusion method for dynamic thresholding and staged prediction as described above.

[0014] In addition, to achieve the above objectives, this application also proposes a storage medium, which is a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it implements the steps of the multi-sensor pose fusion method with dynamic threshold and staged prediction as described above.

[0015] In addition, to achieve the above objectives, this application also provides a computer program product, which includes a computer program that, when executed by a processor, implements the steps of the multi-sensor pose fusion method for dynamic thresholding and phased prediction as described above.

[0016] One or more technical solutions proposed in this application have at least the following technical effects: By employing a phased prediction method to obtain the pose prediction value and acquiring residual statistical characteristics based on the pose residuals of each sensor, subsequent anomaly detection can use the predicted value as a reliable benchmark and the residual statistical characteristics to reflect the current noise level of the sensor. Furthermore, based on the residual statistical characteristics of each sensor, a dynamic anomaly threshold is adaptively generated, allowing the threshold to automatically adjust with the real-time noise level of each sensor. This solves the problem of misjudgment or missed judgment caused by the inability of the judgment conditions to adapt to dynamic operating conditions in existing solutions. On this basis, effective sensors are selected from multiple sensors, and the fused pose is determined by combining the measurement data of the effective sensors, prior accuracy information, and the pose prediction value. This ensures that the fusion result, while eliminating abnormal data, can fully utilize effective measurement information and prior accuracy information. Compared with existing technologies, this application solves the problems of occasional abnormal sensor measurements contaminating the fusion results during multi-sensor fusion and the difficulty in guaranteeing the accuracy of anomaly detection under different operating conditions, achieving improved robustness and continuity of the fused pose. Attached Figure Description

[0017] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0018] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a flowchart illustrating an embodiment of the multi-sensor pose fusion method for dynamic threshold and staged prediction in this application. Figure 2 This is a flowchart illustrating Embodiment 2 of the multi-sensor pose fusion method for dynamic threshold and staged prediction in this application. Figure 3 This is a flowchart illustrating Embodiment 3 of the multi-sensor pose fusion method for dynamic threshold and staged prediction in this application. Figure 4 A simplified flowchart is provided for an embodiment of the multi-sensor pose fusion method for dynamic threshold and staged prediction in this application; Figure 5 This is a schematic diagram of the module structure of the multi-sensor pose fusion device for dynamic threshold and staged prediction according to an embodiment of this application; Figure 6 This is a schematic diagram of the device structure of the hardware operating environment involved in the multi-sensor pose fusion method of dynamic threshold and staged prediction in the embodiments of this application.

[0020] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0021] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.

[0022] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.

[0023] The main solution of this application embodiment is as follows: acquiring measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors; performing staged prediction based on the historical fusion results to determine the pose prediction value; determining the pose residual between the measurement data and the pose prediction value of each sensor to obtain residual statistical characteristics; adaptively generating a dynamic anomaly threshold based on the residual statistical characteristics of each sensor, and determining the effective sensor from the multiple heterogeneous sensors according to the dynamic anomaly threshold; and determining the fused pose based on at least one of the measurement data, prior accuracy information, and pose prediction value corresponding to the effective sensor.

[0024] In the current multi-sensor fusion process, occasional abnormal sensor measurements can contaminate the fusion results, making it difficult to guarantee the accuracy of anomaly detection under different operating conditions.

[0025] This application provides a solution that obtains pose prediction values ​​through staged prediction, providing a reliable comparison benchmark independent of the current measurement for anomaly detection. Based on the pose residuals between the measured and predicted values ​​of each sensor, residual statistical characteristics are obtained, enabling the quantitative characterization of the current noise level of each sensor. The residual statistical characteristics adaptively generate a dynamic anomaly threshold, allowing the judgment threshold to automatically adjust according to the real-time noise level of each sensor, solving the problem of difficulty in guaranteeing anomaly detection accuracy under different operating conditions. Finally, effective sensors are selected from multiple sensors, and their measurement data, prior accuracy information, and pose prediction values ​​are combined to determine the fused pose. This process fully utilizes effective information while eliminating abnormal data, and uses the predicted value as a fallback in the event of a complete anomaly. Therefore, this application improves the robustness and continuity of the fused pose.

[0026] It should be noted that the executing entity in this embodiment can be a computing service device with data processing, network communication, and program execution functions, such as a tablet computer, personal computer, or mobile phone, or an electronic device or processor capable of performing the above functions. The following description uses a processor as an example to illustrate this embodiment and the subsequent embodiments. All actions involving the acquisition of signals, information, or data in this application are performed in accordance with the relevant data protection regulations of the locality and with authorization from the owner of the corresponding device.

[0027] Based on this, embodiments of this application provide a multi-sensor pose fusion method using dynamic thresholding and staged prediction, referring to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the multi-sensor pose fusion method for dynamic threshold and staged prediction in this application.

[0028] In this embodiment, the multi-sensor pose fusion method of dynamic threshold and staged prediction includes steps S10~S50: Step S10: Obtain measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors.

[0029] It should be noted that multi-channel heterogeneous sensors refer to multiple sensors installed on the robot's end effector that employ different measurement principles; measurement data refers to the time-series measurement values ​​collected by each sensor during robot operation; prior accuracy information refers to the prior measurement variance obtained by each sensor through calibration in offline mode, used to characterize the inherent measurement accuracy of each sensor; historical fusion results refer to the fused pose output values ​​recorded at past sampling times, used for pose prediction at the current moment and subsequent dynamic threshold generation.

[0030] Specifically, during the operation of the robot's end effector, pose measurement data is collected in real time by multiple heterogeneous sensors configured on the end effector, with each sensor independently outputting its measurement value. The measurement data X from the n sensors is organized into a unified measurement matrix:

[0031] Where n is the number of sensors and N is the number of sampling points. Let be the time series measurement vector of the i-th sensor. For the measurement value of the i-th sensor at time k, denoted as . .

[0032] Simultaneously, the prior measurement variance of each sensor is read from a pre-stored offline calibration database. This serves as prior accuracy information. For example, the prior measurement variance obtained from encoder calibration. Smaller, prior measurement variance obtained from visual system calibration The variance of prior measurements from each sensor is relatively large. It serves two purposes: firstly, as the basis for weighting in static weighted fusion; secondly, as the weighting factor. And after normalization, it satisfies Secondly, it serves as the weighting basis for effective sensor channels in subsequent robust fusion.

[0033] In addition, the fused pose output values ​​recorded at previous sampling times are read from the memory. This is a result of historical integration.

[0034] Understandably, since multiple heterogeneous sensors may exhibit occasional abnormal measurements during operation, directly using the raw measurement data for fusion would contaminate the fusion results with these outliers. Furthermore, the measurement accuracy of different sensors varies, and indiscriminately applying equal weights would reduce fusion accuracy. Therefore, by executing step S10 to acquire the measurement data, prior accuracy information, and historical fusion results from each sensor, unreliable fusion foundations due to unclear data sources or missing accuracy information can be avoided, thereby improving the accuracy of subsequent pose prediction and fusion calculations.

[0035] Step S20: Perform phased prediction based on the historical fusion results to determine the pose prediction value.

[0036] It should be noted that the pose prediction value refers to the estimated value obtained by calculating the pose state of the robot's end effector at the current moment based on the recorded historical fusion results.

[0037] Specifically, based on the amount of accumulated historical fusion results, different prediction methods are automatically selected to achieve phased prediction.

[0038] In the initial stage of system operation, due to the limited number of sampling times, the number of historical fusion results recorded in the memory is insufficient to support statistical prediction based on historical data. Therefore, in this stage, the measurement data output by each sensor at the current moment is directly acquired, the arithmetic mean of the measurement data is calculated, and the calculated arithmetic mean is used as the pose prediction value at the current moment.

[0039] As the system continues to run and the sampling time increases, the number of historical fusion results recorded in the memory gradually accumulates. When the number of historical fusion results reaches or exceeds a preset threshold, it indicates that the historical data is sufficient to support reliable statistical prediction, and the system automatically switches to a prediction method based on historical fusion results. In this stage, the historical fusion results within the most recent sliding window are extracted from the memory, i.e., the fused pose output values ​​of the most recent sampling times. The arithmetic mean of these historical fusion results is calculated, and the calculated arithmetic mean is used as the pose prediction value for the current time.

[0040] Understandably, due to insufficient historical fusion sample size at the initial stage of system startup, directly using historical fusion results for prediction would lead to significant deviations in predicted values, potentially misclassifying normal measurement fluctuations as anomalies. Therefore, by executing step S20 for phased prediction, using the current average measurement value of each sensor as a transition when historical fusion results are insufficient, and switching to the historical fusion moving average as the predicted value once sufficient historical fusion results are available, the problem of misjudging anomalies due to unreliable prediction benchmarks can be avoided. This improves the accuracy of subsequent anomaly detection, allowing the pose prediction value to gradually approach the actual pose change trend as the system runs, providing a reliable comparison benchmark for residual calculation and anomaly judgment.

[0041] Step S30: Determine the pose residual between the measurement data of each sensor and the pose prediction value, and obtain the residual statistical characteristics.

[0042] It should be noted that pose residual refers to the degree of deviation between the measurement data of each sensor and the predicted pose value, and is used to measure whether the current measurement of each sensor differs significantly from the prediction benchmark. Residual statistical characteristics refer to the statistical quantities that reflect the current noise level of each sensor, obtained through statistical calculation based on the pose residual sequence of each sensor over a period of time, and are used for subsequent adaptive generation of dynamic anomaly detection thresholds.

[0043] In one feasible implementation, step S30 may include: calculating the absolute value of the difference between the measurement data of each sensor and the pose prediction value, and using the absolute value of the difference as the pose residual; and determining the residual statistical characteristics based on the pose residual of each sensor in a preset historical window.

[0044] It should be noted that the absolute value of the difference refers to the absolute value of the difference between the measured value and the predicted value, ensuring that the pose residual is always non-negative, reflecting the degree of deviation regardless of whether the measured value is higher or lower than the predicted value. The preset history window refers to a time window used to store and statistically analyze the pose residuals of each sensor over a period of time. The residual data within the window is used to calculate the statistical characteristics of the residual at the current moment.

[0045] Specifically, for each sensor, the current measurement data of the sensor is compared with the current pose prediction value sensor by sensor, and the absolute value of the difference between the two is calculated, i.e.:

[0046] in, Let be the measurement value of the i-th sensor at time k. Let k be the pose prediction value at time k. Let be the pose residual of the i-th sensor at time k. The larger the pose residual, the greater the deviation between the current measurement value of the sensor and the prediction reference, and the higher the probability of an anomaly; the smaller the pose residual, the closer the current measurement value of the sensor is to the prediction reference, and the higher the measurement reliability.

[0047] After calculating the pose residuals of each sensor at the current moment, these residual values ​​are stored in their respective historical residual records for each sensor channel. The historical residual records are maintained in the form of a preset historical window, which is a fixed-length first-in-first-out queue, typically with a window length of 20. Whenever a new pose residual is generated, it is added to the end of the window queue. If the window is full, the earliest residual data added to the window is removed. Based on the pose residual sequence within this preset historical window, the mean residual value for each sensor is calculated for each sensor channel, serving as the residual statistical feature. This residual statistical feature reflects the average noise level of the sensor over a recent period; the higher the noise level, the larger the mean residual value; the lower the noise level, the smaller the mean residual value.

[0048] It is understandable that, due to differences in measurement accuracy and noise characteristics among various sensors, and the varying noise levels of the same sensor under different operating conditions, using a uniform judgment standard to assess anomalies in all sensors could easily lead to misjudgments—being too lenient with low-noise sensors and too strict with high-noise sensors. Therefore, by executing step S30 to calculate the pose residuals of each sensor and obtaining their respective residual statistical characteristics based on a preset historical window, misjudgments caused by mismatched judgment standards with the actual noise characteristics of each sensor can be avoided. This ensures that the residual statistical characteristics accurately reflect the current noise level of each sensor, thereby improving the adaptability and accuracy of subsequent dynamic anomaly threshold generation.

[0049] Step S40: Based on the residual statistical characteristics of each sensor, an adaptive dynamic anomaly threshold is generated, and a valid sensor is determined from the multi-source sensors according to the dynamic anomaly threshold.

[0050] It should be noted that the dynamic anomaly detection threshold is an anomaly detection threshold that is automatically adjusted according to the real-time noise level of each sensor. It is used to determine whether the current measurement of each sensor has deviated abnormally. The effective sensor is the sensor whose current measurement value has not deviated abnormally after being screened by the dynamic anomaly detection threshold and can be used for subsequent fusion calculation.

[0051] Specifically, depending on the different stages of system operation, a phased strategy is adopted to adaptively generate dynamic anomaly thresholds, and their effectiveness is determined for each sensor.

[0052] First, the system retrieves recorded historical residual samples and a preset sample threshold from memory. Historical residual samples are pose residual data calculated and recorded at each past sampling time; each sensor independently maintains its own historical residual sample sequence. The preset sample threshold is a limit used to determine whether sufficient historical residual samples have accumulated; its value is related to the system's startup characteristics.

[0053] In the initial stage of system operation, the sampling time is limited, and the number of recorded historical residual samples has not yet reached the preset sample threshold, so the residual statistical characteristics have not been fully formed. At this time, a pre-set fixed anomaly detection threshold is obtained and directly used as the dynamic anomaly detection threshold for each sensor at the current moment. The fixed anomaly detection threshold can be set according to the expected noise level of each sensor. For example, the fixed threshold for the encoder can be set to be relatively small, while the fixed threshold for the vision system can be set to be relatively large.

[0054] As the system continues to operate and sampling times accumulate, once the number of recorded historical residual samples reaches or exceeds a preset sample threshold, the calculated residual statistical characteristics can stably reflect the current noise level of each sensor. At this point, a preset multiplier factor is obtained. The residual statistical characteristics are multiplied by a factor to generate the dynamic anomaly detection threshold for each sensor, i.e.:

[0055] in, Let be the dynamic anomaly detection threshold of the i-th sensor at time k. This is the length of the historical window; a typical value is 20. This indicates that the mean value of the pose residual sequence within the most recent historical window is calculated, where λ is a preset multiplier factor, typically 3, corresponding to the 3σ criterion. When the sensor noise level increases with changes in operating conditions, the mean residual value within the historical window increases accordingly, and the dynamic anomaly detection threshold is automatically raised to avoid misjudging normal noise fluctuations as abnormalities; when the noise level decreases, the threshold is automatically lowered to maintain sensitivity to abnormal deviations.

[0056] After generating the dynamic anomaly thresholds for each sensor, a validity determination is performed for each sensor individually. For each sensor, the calculated pose residual of that sensor at the current moment is used. The dynamic anomaly detection threshold corresponding to this sensor Compare. If This indicates that the deviation between the current measurement value and the pose prediction value of the sensor is within an acceptable range, thus the sensor is determined to be a valid sensor, and its measurement data is retained for subsequent fusion; if This indicates that the current measurement value of the sensor has a significant abnormal deviation, and the sensor is determined to be an abnormal sensor. It will be removed from the multi-source sensor set and will not participate in the subsequent fusion calculation.

[0057] Through the aforementioned phased adaptive threshold generation and sensor-by-sensor determination, the set of effective sensors at the current moment is obtained. .

[0058] Understandably, since the noise level of sensors is not constant during operation, and fixed thresholds cannot be adjusted according to noise changes, false positives are likely to occur when noise increases and false negatives are likely to occur when noise decreases. Therefore, by executing step S40, after sufficient historical residual samples are obtained, dynamic anomaly detection thresholds corresponding to each sensor are adaptively generated. This avoids false positives and false negatives caused by fixed thresholds being unable to adapt to dynamic operating conditions. The anomaly detection threshold is automatically adjusted according to the real-time noise level of each sensor, thereby improving the accuracy of anomaly detection and providing a reliable and effective sensor set for fusion computing.

[0059] Step S50: Determine the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

[0060] It should be noted that fused pose refers to the pose estimation result of the robot end effector at the current moment, which is the result of integrating the measurement data of effective sensors, prior accuracy information and pose prediction values.

[0061] In one feasible implementation, step S50 may include: when the number of effective sensors is a preset value, using the pose prediction value as the fused pose; when the number of effective sensors is not a preset value, determining the fused pose based on the measurement data corresponding to the effective sensors and the prior accuracy information.

[0062] It should be noted that the preset value refers to a threshold used to determine the number of valid sensors. When the number of valid sensors equals the preset value, it indicates that all sensors are judged as abnormal at the current moment, and there is no usable measurement data for fusion; when the number of valid sensors does not equal the preset value, it indicates that at least one valid sensor exists at the current moment, and weighted fusion can be performed based on the valid measurement data.

[0063] Specifically, the first step is to obtain the set of valid sensors at the current moment. Count the number of effective sensors .

[0064] When the number of effective sensors is greater than zero, meaning there is at least one effective sensor, weighted fusion is performed based on the measurement data corresponding to the effective sensors and prior accuracy information. For each effective sensor, its weight... The prior measurement variance of the sensor The reciprocal of is determined, that is:

[0065] After the weights are normalized, they satisfy the following conditions: Sensors with smaller prior measurement variance have higher measurement accuracy, correspondingly larger weights, and contribute more to the fusion result; conversely, sensors with lower accuracy have smaller weights. After determining the weights, the current measurements from the effective sensors are weighted and summed to obtain the fused pose at the current moment. :

[0066] in, Let be the measurement value of the effective sensor i at time k.

[0067] For example, when only the encoder and laser rangefinder are deemed valid among the three sensors, while the vision system is deemed abnormal, the measurement data from the vision system is discarded. Only the measurement data from the encoder and laser rangefinder are weighted by the inverse of their respective prior variances and then summed to obtain the fused pose at the current moment. Since the measurements from the abnormal sensors have been excluded from the fusion calculation, the fusion result is not contaminated by abnormal data.

[0068] When the number of valid sensors is zero, meaning all sensors are deemed abnormal, it indicates that the measurements from all sensors have deviated significantly at the current moment. Under this extreme condition, forcing the use of abnormal data for fusion will result in a fusion result that severely deviates from the true pose. Therefore, instead of performing weighted fusion calculations, the predicted pose is directly used as the fused pose output for the current moment.

[0069] Since the pose prediction is calculated based on reliable historical fusion results within a past window, using the prediction as the fusion output can maintain the continuity of pose estimation and avoid abrupt changes in the fusion results when all sensors are abnormal.

[0070] After fusing the pose output, it is recorded in memory, updating the historical fusion result sequence for use in pose prediction at the next sampling time. Simultaneously, the abnormal sensor rejection rate at the current time can be recorded. It is used for online monitoring of the health status of various sensors.

[0071] Understandably, during multi-sensor fusion, if sensors with occasional abnormal measurements are not removed, their abnormal values ​​will directly contaminate the weighted fusion result. Furthermore, if all sensors are simultaneously disturbed and become abnormal, forced fusion will cause the fusion result to deviate significantly from the true value. Therefore, by executing step S50, when valid sensors exist, weighted fusion is performed using the inverse of the prior variance, fully utilizing valid measurement information and the accuracy differences between sensors. When all sensors are abnormal, the predicted value is used as a fallback output. This avoids the fusion result being completely contaminated by abnormal data or experiencing abrupt changes, thus ensuring the robustness and continuity of the fused pose and providing reliable pose estimation results for the robot actuator.

[0072] This embodiment provides a multi-sensor pose fusion method based on dynamic thresholding and staged prediction. By acquiring measurement data, prior accuracy information, and historical fusion results from multiple heterogeneous sensors, it provides a reliable comparison benchmark for anomaly detection based on staged prediction. The method calculates residual statistical characteristics of each sensor's pose residual within a preset historical window to quantify real-time noise levels. Then, based on these residual statistical characteristics, it adaptively generates a dynamic anomaly threshold and filters effective sensors. Effective sensors are fused using a weighted average of their prior variances, and in cases of full anomaly, the predicted value is used as a fallback output. This embodiment solves the problems of occasional sensor anomalies contaminating the fusion results during multi-sensor fusion and the difficulty in guaranteeing anomaly detection accuracy under different operating conditions, thus improving the robustness and continuity of the fused pose.

[0073] Based on the first embodiment of this application, in the second embodiment of this application, the content that is the same as or similar to that in the first embodiment described above can be referred to the above description, and will not be repeated hereafter. Based on this, please refer to... Figure 2 Step S40 may include steps S41 to S43: Step S41: Obtain historical residual samples and preset sample thresholds.

[0074] It should be noted that historical residual samples refer to the pose residual data calculated and recorded at each sampling time in the past. Each sensor independently maintains its own historical residual sample sequence, and the historical residual samples of each sensor channel do not interfere with each other. The preset sample threshold is a threshold used to determine whether there are enough historical residual samples accumulated. Its value determines when to switch from a fixed outlier threshold to an adaptive dynamic outlier threshold.

[0075] Specifically, the system reads the historical residual records corresponding to each sensor from the memory, obtaining the accumulated pose residual sequence as historical residual samples. Simultaneously, a preset sample threshold is read from the preset parameter storage area. This preset sample threshold can be set according to the sampling frequency and sensor characteristics during the system startup phase. If the current sampling time is early and the number of historical residual samples is small, it indicates that the system is still in the initial startup phase and the residual statistical characteristics have not yet stabilized. If the number of historical residual samples has reached or exceeded the preset sample threshold, it indicates that the historical residual data has been sufficiently accumulated, and the residual statistical characteristics can reliably reflect the current noise level of the sensor.

[0076] Understandably, due to the limited sampling time during the initial system startup, historical residual samples are insufficient. If residual statistical characteristics are directly calculated and dynamic anomaly thresholds are generated based on a small number of residual samples, the instability of the statistics themselves will cause the thresholds to fluctuate wildly, thus reducing the accuracy of anomaly detection. Therefore, by executing step S41 to obtain historical residual samples and preset sample thresholds, it is possible to clearly distinguish whether the system is currently in the startup phase with insufficient sample accumulation or the steady-state phase with sufficient samples, thereby providing a basis for selecting a threshold generation strategy in stages.

[0077] Step S42: Based on the historical residual samples, the preset sample threshold, and multiple residual statistical features, adaptively generate a dynamic anomaly detection threshold.

[0078] In one feasible implementation, step S42 may include: obtaining a preset fixed anomaly threshold and a multiplier factor; when the historical residual sample is less than the preset sample threshold, using the fixed anomaly threshold as a dynamic anomaly threshold; when the historical residual sample is greater than or equal to the preset sample threshold, generating a dynamic anomaly threshold corresponding to each sensor based on the residual statistical characteristics and the multiplier factor.

[0079] Obtain the preset fixed outlier threshold and multiplier factor. The fixed outlier threshold can be set independently according to the expected noise level of each sensor. For example, the fixed outlier threshold for the encoder can be set to a smaller value, while the fixed outlier threshold for the vision system can be set to a larger value. The multiplier factor is used to amplify the residual statistical characteristics into the outlier threshold, with a typical value of 3.

[0080] The number of historical residual samples is compared with a preset sample threshold. If the number of historical residual samples is less than the preset sample threshold, it indicates that the system is still in the initial startup stage, the historical residual data accumulation of each sensor channel is insufficient, and the residual statistical characteristics cannot yet stably reflect the true noise level of the sensor. In this case, the preset fixed anomaly detection threshold is directly used as the dynamic anomaly detection threshold for each sensor at the current moment. Each sensor can use its own independent fixed anomaly detection threshold to adapt to the inherent accuracy differences of different sensors.

[0081] When the number of historical residual samples is greater than or equal to a preset sample threshold, it indicates that sufficient historical residual data has been accumulated, and the residual statistical characteristics can stably reflect the current noise level of each sensor. At this point, a dynamic anomaly detection threshold is generated for each sensor based on the residual statistical characteristics and a multiplier factor. Specifically, the residual statistical characteristics of each sensor are multiplied by a preset multiplier factor to obtain the dynamic anomaly detection threshold for each sensor. Since the residual statistical characteristics of each sensor are calculated independently and are different from each other, the generated dynamic anomaly detection thresholds are also independent of each other. When the noise level of a sensor increases with changes in operating conditions, its residual statistical characteristics increase accordingly, and the dynamic anomaly detection threshold is automatically raised to relax the judgment criteria and avoid misjudging normal noise fluctuations as abnormalities; when the noise level decreases, the residual statistical characteristics decrease accordingly, and the dynamic anomaly detection threshold is automatically lowered to tighten the judgment criteria and maintain sensitivity to abnormal deviations. Thus, the anomaly detection threshold achieves adaptive adjustment according to the real-time noise level of the sensor.

[0082] Understandably, since a fixed anomaly threshold cannot be adjusted dynamically according to changes in sensor noise levels, it is easy to misjudge normal fluctuations as abnormalities when noise increases, and easy to miss true anomalies as normalities when noise decreases. Therefore, by executing step S42, a fixed anomaly threshold is used as a transition when historical residual samples are insufficient, and a dynamic anomaly threshold is adaptively generated based on residual statistical characteristics and multiplier factors after historical residual samples are sufficient. This avoids the problem of fixed thresholds being unable to adapt to dynamic operating conditions, allowing the anomaly threshold to automatically adjust according to the real-time noise levels of each sensor, thereby improving the accuracy of anomaly detection.

[0083] Step S43: Determine the valid sensor from the multi-source sensors according to the dynamic anomaly detection threshold.

[0084] In one feasible implementation, step S43 may include: determining the pose residual based on the measurement data of each sensor and the pose prediction value; identifying sensors whose pose residual is greater than or equal to the dynamic anomaly threshold as abnormal sensors; and removing the abnormal sensors from the multi-channel heterogeneous sensors to obtain valid sensors.

[0085] Specifically, for each sensor, the absolute value of the difference between the sensor's current measurement and the predicted pose is calculated to obtain the pose residual of the sensor at that current moment. The pose residual reflects the degree to which the sensor's current measurement deviates from the prediction reference; the larger the residual value, the more severe the deviation.

[0086] The pose residuals of each sensor are compared sensor-by-sensor with the corresponding dynamic anomaly threshold. If the pose residual is less than the dynamic anomaly threshold, it indicates that the deviation between the current measurement value and the prediction reference of the sensor is within the allowable range and no significant anomaly has occurred. The sensor is then determined to be a valid sensor and retained in the set of valid sensors. If the pose residual is greater than or equal to the dynamic anomaly threshold, it indicates that the current measurement value of the sensor has deviated significantly beyond the allowable range. The sensor is then determined to be an abnormal sensor and removed from the set of multi-channel heterogeneous sensors.

[0087] For example, at a certain sampling moment, if the pose residual of the encoder among the three sensors is less than its corresponding dynamic anomaly threshold, the encoder is determined to be a valid sensor; if the pose residual of the laser rangefinder is less than its corresponding dynamic anomaly threshold, the laser rangefinder is determined to be a valid sensor; if the pose residual of the vision system increases significantly due to instantaneous occlusion, exceeding or equaling its corresponding dynamic anomaly threshold, the vision system is determined to be an abnormal sensor and is discarded. Ultimately, the valid sensor set at that moment only includes the encoder and the laser rangefinder; abnormal data from the vision system will not be included in subsequent fusion calculations.

[0088] Understandably, due to the different noise characteristics of each sensor and the varying measurement deviations of the same sensor at different times, if all sensor measurement data are included in the subsequent fusion calculation without differentiation, abnormal measurement values ​​will directly contaminate the fusion result, leading to significant deviations in pose estimation. Therefore, by executing step S43, the validity of each sensor is determined sensor by sensor based on its corresponding dynamic anomaly threshold. Abnormal sensors with pose residuals greater than or equal to the threshold are removed, and only valid sensors with pose residuals less than the threshold are retained. This avoids abnormal measurement data from entering the subsequent fusion calculation, thereby ensuring the reliability of the fusion data source.

[0089] This embodiment provides a multi-sensor pose fusion method based on dynamic threshold and staged prediction. When historical residual samples are insufficient, a fixed anomaly threshold is used as a transition. After sufficient historical residual samples are available, dynamic anomaly thresholds corresponding to each sensor are adaptively generated based on residual statistical characteristics and multiplier factors. The validity of each sensor is determined according to the dynamic anomaly threshold, and abnormal sensors are removed while valid sensors are retained. This allows the anomaly determination threshold to be automatically adjusted according to the real-time noise level of the sensor, improving the accuracy of anomaly determination under different operating conditions.

[0090] Based on the first and second embodiments of this application, in this third embodiment, the content that is the same as or similar to that in embodiments one and two above can be referred to the above description, and will not be repeated hereafter. Based on this, please refer to... Figure 3 Step S20 may include steps S21 to S22: Step S21: When the number of historical fusion results is less than a preset threshold, the average value of the measurement data of each sensor is used as the pose prediction value.

[0091] It should be noted that the preset quantity threshold is a limit used to determine whether the historical fusion results have accumulated sufficiently. When the number of recorded historical fusion results is less than this threshold, it indicates that the system is still in the initial stage of startup, and the historical fusion samples are not yet sufficient to support reliable statistical predictions.

[0092] Specifically, in the initial stage of system operation, the number of historical fusion results recorded in the memory is relatively small. At this time, the measurement data output by each sensor at the current moment is directly acquired. Calculate the arithmetic mean of all sensor measurements and use this arithmetic mean as the pose prediction value for the current moment. :

[0093] Where n is the number of sensors. Let be the measurement value of the i-th sensor at time k. This method does not rely on historical fusion results and can immediately output usable predicted values ​​when the system is just started and historical data is insufficient, ensuring that the anomaly detection and fusion process can operate normally from the first sampling time.

[0094] Understandably, due to insufficient historical fusion sample size in the early stages of system startup, if statistical prediction is forcibly performed based on a very small amount of historical data, the resulting prediction values ​​may have significant deviations, leading to the misjudgment of normal measurement fluctuations as abnormalities during subsequent anomaly detection. Therefore, by executing step S21, using the average measurement value of each sensor as the pose prediction value when historical fusion results are insufficient, the misjudgment problem caused by unreliable prediction benchmarks can be avoided, providing a stable and usable prediction benchmark for the system startup phase.

[0095] Step S22: When the number of historical fusion results is greater than or equal to the preset number threshold, the average value of the historical fusion results within the preset sliding window is used as the pose prediction value.

[0096] It should be noted that the preset sliding window refers to the time window used to capture the most recent historical fusion results. The window length can be set according to the system's operating characteristics, with a typical value of 10. When the number of historical fusion results reaches or exceeds the preset threshold, the system enters a steady-state operation phase, where historical fusion data has been sufficiently accumulated, and predictions can be made based on the moving average of historical fusion results.

[0097] Specifically, as the system continues to run, the number of historical fusion results recorded in the memory increases continuously. When the number is greater than or equal to a preset threshold, the most recent historical fusion result within a preset sliding window is retrieved from the memory. The fused pose output value at each sampling time point Calculate the arithmetic mean of these historical fusion results and use it as the pose prediction value at the current moment. :

[0098] in, To preset the sliding window length, This represents the fused pose output value at time j.

[0099] Understandably, since the historical fusion results in the steady-state stage have accumulated sufficiently and possess high reliability, continuing to use the measurement mean as the prediction value would introduce current measurement noise and reduce the stability of the prediction benchmark. Therefore, by executing step S22, switching to moving average prediction based on historical fusion results after sufficient historical fusion results have been obtained, the interference of measurement noise on the prediction benchmark can be avoided, making the pose prediction value closer to the actual motion trend, thereby further improving the accuracy of subsequent residual calculation and anomaly detection.

[0100] This embodiment provides a multi-sensor pose fusion method with dynamic threshold and staged prediction. When historical fusion results are insufficient, the average value of each sensor measurement is used as the pose prediction value. When historical fusion results are sufficient, the average value of historical fusion results within a preset sliding window is used as the pose prediction value. This achieves a smooth transition of the pose prediction value from the startup stage to the steady-state stage, so that the prediction benchmark can gradually approach the real pose change trend as the system runs, providing a reliable comparison benchmark for anomaly detection.

[0101] For example, to help understand the implementation process of the multi-sensor pose fusion method for dynamic threshold and staged prediction obtained in combination with the above embodiments, please refer to... Figure 4 , Figure 4A simplified flowchart of a multi-sensor pose fusion method based on dynamic thresholding and staged prediction is provided, specifically: First, measurement data from multiple heterogeneous sensors mounted on the robot's end effector are acquired, with each sensor independently outputting its current pose measurement. Simultaneously, the prior measurement variance of each sensor is read from a pre-stored offline calibration database as prior accuracy information. The measurement data is used for subsequent residual calculation and fusion, while the prior accuracy information is used for weighted fusion of the effective sensors.

[0102] Subsequently, the system automatically selects different prediction methods to generate the pose prediction value for the current moment based on the number of recorded historical fusion results. Specifically, in the early stages of system operation, when the number of historical fusion results is insufficient, the arithmetic mean of the measurement data from each sensor at the current moment is used as the pose prediction value; when the number of historical fusion results accumulates sufficiently and reaches or exceeds a preset threshold, the system automatically switches to a steady-state prediction method, using the average of historical fusion results within a preset sliding window as the pose prediction value.

[0103] After obtaining the pose prediction value, for each sensor, the current measurement data is compared with the pose prediction value, and the absolute value of the difference between the two is calculated as the pose residual of that sensor at the current moment. After calculating the pose residual of each sensor, it is stored in the respective historical residual record of each sensor channel, providing a data basis for the subsequent generation of dynamic thresholds.

[0104] Next, the system generates dynamic anomaly thresholds for each sensor in stages based on the accumulation of historical residual samples. In the early stages of system operation, when the number of historical residual samples is insufficient, a preset fixed anomaly threshold is used as the judgment threshold for each sensor. When sufficient historical residual samples have been accumulated, the system generates corresponding adaptive dynamic anomaly thresholds for each sensor by multiplying the residual statistical characteristics of each sensor within a preset historical window, i.e., the historical residual mean, by a preset multiplier factor.

[0105] After generating the dynamic anomaly threshold, the system compares the pose residual of each sensor at the current moment with its corresponding dynamic anomaly threshold sensor by sensor to determine whether the residual is less than the threshold. When the pose residual of a sensor is less than the corresponding dynamic anomaly threshold, it indicates that the deviation of the current measurement value of the sensor from the prediction benchmark is within the allowable range and no significant anomaly has occurred. The sensor is determined to be valid and retained, and its measurement data will participate in subsequent fusion calculations. When the pose residual is greater than or equal to the corresponding dynamic anomaly threshold, it indicates that the current measurement value of the sensor has deviated significantly beyond the allowable range. The sensor is determined to be abnormal and discarded, and its measurement data will not participate in subsequent fusion calculations.

[0106] After determining the validity and anomaly of all sensors, the system counts the number of valid sensors at the current moment and determines the fused pose based on whether a valid sensor exists. If at least one valid sensor exists, the weights of each channel in the valid sensor set are determined by normalizing the inverse of the prior variance. Sensors with smaller prior variances have higher measurement accuracy and receive larger weights. The current measurements of each valid sensor are then weighted and summed to obtain the fused pose at the current moment. If all sensors are determined to be anomalies, i.e., no valid sensors exist, a full anomaly protection mechanism is triggered. Weighted fusion is not performed, and the current pose prediction value is directly used as the fused output.

[0107] Finally, the system uses the weighted fusion result or the pose prediction value after backtracking in the case of all anomalies as the final fused pose output for the current moment, which is then used for pose estimation of the robot's end effector. After the current moment is processed, the system waits for the next sampling moment, acquires new measurement data, and then repeats the above process until all sampling points are processed, forming a complete closed loop from multi-sensor data input to robust fused pose output.

[0108] It should be noted that the above examples are only for understanding this application and do not constitute a limitation on the multi-sensor pose fusion method of dynamic threshold and staged prediction of this application. Any simple transformations based on this technical concept are within the protection scope of this application.

[0109] This application also provides a multi-sensor pose fusion device based on dynamic thresholding and staged prediction. Please refer to [link / reference]. Figure 5 The dynamic threshold and staged prediction multi-sensor pose fusion device includes: The data acquisition module 10 is used to acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors. Pose prediction module 20 is used to perform phased prediction based on the historical fusion results and determine the pose prediction value. The residual statistics module 30 is used to determine the pose residual between the measurement data of each sensor and the pose prediction value, and to obtain the residual statistical characteristics. The dynamic anomaly detection module 40 is used to adaptively generate a dynamic anomaly detection threshold based on the residual statistical characteristics of each sensor, and to determine the effective sensor from the multi-source sensors according to the dynamic anomaly detection threshold. The fusion pose module 50 is used to determine the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

[0110] The multi-sensor pose fusion device with dynamic thresholding and staged prediction provided in this application, employing the multi-sensor pose fusion method with dynamic thresholding and staged prediction in the above embodiments, can solve the technical problem that occasional abnormal sensor measurements during multi-sensor fusion can contaminate the fusion results, and the accuracy of anomaly detection is difficult to guarantee under different operating conditions. Compared with the prior art, the beneficial effects of the multi-sensor pose fusion device with dynamic thresholding and staged prediction provided in this application are the same as those of the multi-sensor pose fusion method with dynamic thresholding and staged prediction provided in the above embodiments, and other technical features in the multi-sensor pose fusion device with dynamic thresholding and staged prediction are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0111] In one embodiment, the dynamic anomaly detection module 40 is further configured to acquire historical residual samples and preset sample thresholds; Based on the historical residual samples, the preset sample threshold, and multiple residual statistical features, a dynamic outlier threshold is adaptively generated. The effective sensor is determined from the multi-source sensors based on the dynamic anomaly detection threshold.

[0112] In one embodiment, the dynamic anomaly detection module 40 is further configured to obtain a preset fixed anomaly detection threshold and a multiplier factor; When the historical residual sample is less than the preset sample threshold, the fixed outlier threshold is used as the dynamic outlier threshold. When the historical residual sample is greater than or equal to the preset sample threshold, a dynamic anomaly detection threshold is generated for each sensor based on the residual statistical characteristics and the multiplier factor.

[0113] In one embodiment, the dynamic anomaly detection module 40 is further configured to determine the pose residual based on the measurement data from each sensor and the pose prediction value; Sensors whose pose residual is greater than or equal to the dynamic anomaly detection threshold are identified as abnormal sensors. The abnormal sensor is removed from the multi-channel heterogeneous sensor to obtain the effective sensor.

[0114] In one embodiment, the pose prediction module 20 is further configured to use the average value of the measurement data of each sensor as the pose prediction value when the number of historical fusion results is less than a preset number threshold. When the number of historical fusion results is greater than or equal to the preset number threshold, the average value of the historical fusion results within the preset sliding window is used as the pose prediction value.

[0115] In one embodiment, the residual statistics module 30 is further configured to calculate the absolute value of the difference between the measurement data of each sensor and the pose prediction value, and use the absolute value of the difference as the pose residual. Based on the pose residuals of each sensor within a preset historical window, the statistical characteristics of the residuals are determined.

[0116] In one embodiment, the fusion pose module 50 is further configured to use the pose prediction value as the fusion pose when the number of effective sensors is a preset value; When the number of effective sensors is not a preset value, the fused pose is determined based on the measurement data corresponding to the effective sensors and the prior accuracy information.

[0117] This application provides a multi-sensor pose fusion device with dynamic thresholding and staged prediction. The multi-sensor pose fusion device with dynamic thresholding and staged prediction includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the multi-sensor pose fusion method with dynamic thresholding and staged prediction in the above embodiment 1.

[0118] The following is for reference. Figure 6 This document illustrates a structural schematic of a multi-sensor pose fusion device suitable for implementing dynamic thresholding and staged prediction in the embodiments of this application. The multi-sensor pose fusion device for dynamic thresholding and staged prediction in the embodiments of this application may include, but is not limited to, mobile terminals such as mobile phones, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PMPs (Portable Media Players), and in-vehicle terminals (e.g., in-vehicle navigation terminals), as well as fixed terminals such as digital TVs and desktop computers. Figure 6 The illustrated multi-sensor pose fusion device with dynamic threshold and staged prediction is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0119] like Figure 6As shown, the multi-sensor pose fusion device for dynamic thresholding and staged prediction may include a processing unit 1001 (e.g., a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes according to a program stored in ROM (Read Only Memory) 1002 or a program loaded from storage device 1003 into RAM (Random Access Memory) 1004. RAM 1004 also stores various programs and data required for the operation of the multi-sensor pose fusion device for dynamic thresholding and staged prediction. The processing unit 1001, ROM 1002, and RAM 1004 are interconnected via bus 1005. Input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to I / O interface 1006: input devices 1007 including, for example, touchscreens, touchpads, keyboards, mice, image sensors, microphones, accelerometers, gyroscopes, etc.; output devices 1008 including, for example, LCDs (Liquid Crystal Displays), speakers, vibrators, etc.; storage devices 1003 including, for example, magnetic tapes, hard disks, etc.; and communication devices 1009. Communication device 1009 allows the multi-sensor pose fusion device for dynamic thresholding and staged prediction to wirelessly or wiredly communicate with other devices to exchange data. Although the figure shows a multi-sensor pose fusion device for dynamic thresholding and staged prediction with various systems, it should be understood that it is not required to implement or possess all the systems shown. More or fewer systems can be implemented alternatively.

[0120] Specifically, according to the embodiments disclosed in this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this application include a computer program product comprising a computer program carried on a 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, or installed from storage device 1003, or installed from ROM 1002. When the computer program is executed by processing device 1001, it performs the functions defined in the methods of the embodiments disclosed in this application.

[0121] The multi-sensor pose fusion device with dynamic thresholding and staged prediction provided in this application, employing the multi-sensor pose fusion method with dynamic thresholding and staged prediction in the above embodiments, can solve the technical problem that occasional abnormal sensor measurements during multi-sensor fusion can contaminate the fusion results, and the accuracy of anomaly detection is difficult to guarantee under different operating conditions. Compared with the prior art, the beneficial effects of the multi-sensor pose fusion device with dynamic thresholding and staged prediction provided in this application are the same as those of the multi-sensor pose fusion method with dynamic thresholding and staged prediction provided in the above embodiments, and other technical features in this multi-sensor pose fusion device with dynamic thresholding and staged prediction are the same as those disclosed in the previous embodiment method, and will not be repeated here.

[0122] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0123] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0124] This application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, the computer-readable program instructions being used to execute the multi-sensor pose fusion method for dynamic threshold and staged prediction in the above embodiments.

[0125] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, RAM (Random Access Memory), ROM (Read Only Memory), EPROM (Erasable Programmable Read Only Memory or Flash Memory), optical fibers, CD-ROM (CD-Read Only Memory), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0126] The aforementioned computer-readable storage medium may be included in the multi-sensor pose fusion device for dynamic thresholding and staged prediction; or it may exist independently and not be assembled into the multi-sensor pose fusion device for dynamic thresholding and staged prediction.

[0127] The aforementioned computer-readable storage medium carries one or more programs that, when executed by a multi-sensor pose fusion device for dynamic thresholding and phased prediction, cause the multi-sensor pose fusion device for dynamic thresholding and phased prediction to: acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors; perform phased prediction based on the historical fusion results to determine the pose prediction value; determine the pose residual between the measurement data and the pose prediction value of each sensor to obtain residual statistical characteristics; adaptively generate a dynamic anomaly threshold based on the residual statistical characteristics of each sensor, and determine a valid sensor from the multiple heterogeneous sensors based on the dynamic anomaly threshold; and determine the fused pose based on at least one of the measurement data, prior accuracy information, and pose prediction value corresponding to the valid sensor.

[0128] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof, including 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 LAN (Local Area Network) or WAN (Wide Area Network)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0129] 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 application. 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.

[0130] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.

[0131] The readable storage medium provided in this application is a computer-readable storage medium that stores computer-readable program instructions (i.e., a computer program) for executing the above-described multi-sensor pose fusion method based on dynamic thresholding and staged prediction. This solves the technical problem that occasional abnormal sensor measurements during multi-sensor fusion can contaminate the fusion results, and that the accuracy of anomaly detection is difficult to guarantee under different operating conditions. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as those of the multi-sensor pose fusion method based on dynamic thresholding and staged prediction provided in the above embodiments, and will not be elaborated upon here.

[0132] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the multi-sensor pose fusion method with dynamic threshold and staged prediction as described above.

[0133] The computer program product provided in this application can solve the technical problem that occasional abnormal sensor measurements during multi-sensor fusion can contaminate the fusion results, and the accuracy of anomaly detection is difficult to guarantee under different operating conditions. Compared with the prior art, the beneficial effects of the computer program product provided in this application are the same as those of the multi-sensor pose fusion method with dynamic threshold and staged prediction provided in the above embodiments, and will not be repeated here.

[0134] The above description is only a part of the embodiments of this application and does not limit the patent scope of this application. All equivalent structural transformations made under the technical concept of this application and using the contents of the specification and drawings of this application, or direct / indirect applications in other related technical fields, are included in the patent protection scope of this application.

Claims

1. A multi-sensor pose fusion method based on dynamic thresholding and staged prediction, characterized in that, The method includes: Acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors; Based on the historical fusion results, staged predictions are performed to determine the pose prediction value; Determine the pose residuals between the measurement data of each sensor and the pose prediction value, and obtain the residual statistical characteristics; Based on the residual statistical characteristics of each sensor, an adaptive dynamic anomaly threshold is generated, and a valid sensor is determined from the multi-source sensors according to the dynamic anomaly threshold. A fused pose is determined based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

2. The method as described in claim 1, characterized in that, The process of adaptively generating a dynamic anomaly threshold based on the residual statistical characteristics of each sensor, and determining the effective sensor from the multi-source sensors according to the dynamic anomaly threshold, includes: Obtain historical residual samples and preset sample thresholds; Based on the historical residual samples, the preset sample threshold, and multiple residual statistical features, a dynamic outlier threshold is adaptively generated. The effective sensor is determined from the multi-source sensors based on the dynamic anomaly detection threshold.

3. The method as described in claim 2, characterized in that, The adaptive generation of a dynamic anomaly threshold based on the historical residual samples, the preset sample threshold, and multiple residual statistical features includes: Obtain the preset fixed difference threshold and multiplier factor; When the historical residual sample is less than the preset sample threshold, the fixed outlier threshold is used as the dynamic outlier threshold. When the historical residual sample is greater than or equal to the preset sample threshold, a dynamic anomaly detection threshold is generated for each sensor based on the residual statistical characteristics and the multiplier factor.

4. The method as described in claim 2, characterized in that, The step of determining the effective sensor from the multiple heterogeneous sensors based on the dynamic anomaly threshold includes: Based on the measurement data from each sensor and the predicted pose value, determine the pose residual; Sensors whose pose residual is greater than or equal to the dynamic anomaly detection threshold are identified as abnormal sensors. The abnormal sensor is removed from the multi-channel heterogeneous sensor to obtain the effective sensor.

5. The method as described in claim 1, characterized in that, The step of performing phased prediction based on the historical fusion results to determine the pose prediction value includes: When the number of historical fusion results is less than a preset threshold, the average value of the measurement data from each sensor is used as the pose prediction value. When the number of historical fusion results is greater than or equal to the preset number threshold, the average value of the historical fusion results within the preset sliding window is used as the pose prediction value.

6. The method as described in claim 1, characterized in that, The step of determining the pose residual between the measurement data of each sensor and the pose prediction value, and obtaining the residual statistical characteristics, includes: Calculate the absolute value of the difference between the measurement data of each sensor and the pose prediction value, and use the absolute value of the difference as the pose residual; Based on the pose residuals of each sensor within a preset historical window, the statistical characteristics of the residuals are determined.

7. The method according to any one of claims 1-6, characterized in that, Determining the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value includes: When the number of effective sensors is a preset value, the pose prediction value is used as the fused pose. When the number of effective sensors is not a preset value, the fused pose is determined based on the measurement data corresponding to the effective sensors and the prior accuracy information.

8. A multi-sensor pose fusion device for dynamic thresholding and staged prediction, characterized in that, The device includes: The data acquisition module is used to acquire measurement data, prior accuracy information, and recorded historical fusion results from multiple heterogeneous sensors. The pose prediction module is used to perform phased prediction based on the historical fusion results and determine the pose prediction value. The residual statistics module is used to determine the pose residual between the measurement data of each sensor and the pose prediction value, and to obtain the residual statistical characteristics. The dynamic anomaly detection module is used to adaptively generate a dynamic anomaly detection threshold based on the residual statistical characteristics of each sensor, and to determine the effective sensor from the multi-source sensors according to the dynamic anomaly detection threshold. The fusion pose module is used to determine the fused pose based on at least one of the measurement data corresponding to the effective sensor, the prior accuracy information, and the pose prediction value, so as to perform pose estimation of the robot actuator through the fused pose.

9. A multi-sensor pose fusion device based on dynamic thresholding and staged prediction, characterized in that, The device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the multi-sensor pose fusion method for dynamic thresholding and staged prediction as described in any one of claims 1 to 7.

10. A storage medium, characterized in that, The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, it implements the steps of the multi-sensor pose fusion method for dynamic threshold and staged prediction as described in any one of claims 1 to 7.

Images