Robot environment sensing method, device and system based on multi-sensor fusion

Abstract

The invention provides a robot environment sensing method, device and system based on multi-sensor fusion. The method disclosed by the embodiment of the invention can comprise the following steps: receiving original observation data of each sensor configured on a robot body, and obtaining equivalent observation data of each sensor under system global time based on a time drift model of each sensor and the original observation data of each sensor, health degree indexes of all the sensors are calculated, time-varying credibility weights of all the sensors are generated according to the health degree indexes, activated sensor subsets are determined according to the health degree indexes of all the sensors, and state vectors of the robot system are determined according to the time-varying credibility weights of all the sensors in the activated sensor subsets and the equivalent observation data; and generating environment representation data based on the state vector of the robot system, the time-varying credibility weight of each sensor and the equivalent observation data. According to the invention, the influence caused by the time synchronization error and the space registration deviation between the sensors can be effectively avoided, and the sensing precision is improved.

Description

Technical Field

[0001] This disclosure relates to the field of robotics, and more specifically, to a method, apparatus, and system for robot environmental perception based on multi-sensor fusion. Background Technology

[0002] In recent years, robots have been increasingly used in fields such as autonomous driving, unmanned delivery, and service inspection. Environmental perception technology, as the core foundation for enabling robots to make autonomous decisions and operate safely, has received widespread attention. Currently, robot environmental perception systems generally adopt a multi-sensor fusion architecture, which integrates environmental feature data acquired by various sensors such as LiDAR, cameras, inertial measurement units, and millimeter-wave radar, and completes system state estimation and map construction based on filtering algorithms or feature matching algorithms.

[0003] To improve the accuracy and robustness of sensing systems, related technologies typically perform offline calibration before system deployment. This involves using pre-defined fixed extrinsic parameters and static time synchronization strategies to map data from different sensors to a unified coordinate system and time reference, thereby achieving effective fusion of multi-sensor data. However, this approach has significant limitations in long-term practical operation: it relies on manual calibration and static parameter assumptions, making it difficult to adapt to drift in internal and extrinsic parameters caused by factors such as equipment aging, temperature changes, or mechanical vibration. This leads to the accumulation of time synchronization errors and spatial registration deviations between sensors over time, ultimately affecting the system's sensing accuracy and consistency.

[0004] Therefore, there is an urgent need for a new robot environmental perception scheme to solve the problem of decreased perception accuracy and insufficient consistency caused by the long-term accumulation of time synchronization errors and spatial registration deviations between sensors. Summary of the Invention

[0005] To address the aforementioned technical problems, this disclosure provides a robot environmental perception method, apparatus, and system based on multi-sensor fusion.

[0006] According to one aspect of this disclosure, a robot environment perception method based on multi-sensor fusion is provided, comprising: Receive raw observation data from various sensors configured on the robot body; Based on the time drift model of each sensor and the original observation data of each sensor, the equivalent observation data of each sensor in the global system time are obtained; The health index of each sensor is calculated based on the equivalent observation data of each sensor, and the time-varying reliability weight of each sensor is generated based on the health index of each sensor. A subset of active sensors is determined based on the health indicators of each sensor, and the subset of active sensors includes the sensors that need to participate in data fusion at the current moment; The state vector of the robot system is determined based on the time-varying confidence weights and equivalent observation data of each sensor in the activated sensor subset. Environmental representation data is generated based on the state vector of the robot system, the time-varying confidence weights of each sensor, and the equivalent observation data. The environmental representation data includes an environmental grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

[0007] In some implementations, obtaining the equivalent observation data of each sensor at the system's global time based on the time drift model of each sensor and the raw observation data of each sensor includes: Update the time drift model of each sensor, which indicates the deviation between the local time of the sensor and the global time of the system; Given the global system time, the equivalent observation data of each sensor is calculated based on the time drift model of each sensor and the raw observation data. The time drift model is defined as follows: ,in, This represents the local time of the i-th sensor. The amount of drift relative to global time. Let i be the time drift rate coefficient of the i-th sensor. Let be the initial time bias constant of the i-th sensor.

[0008] In some embodiments, the method further includes: An anomaly flag signal is generated for the sensor when one of the following conditions is met: the time consistency residual of the sensor is calculated based on the equivalent observation data of the sensor, and the time consistency residual of the sensor exceeds a preset time consistency residual threshold; or the health index of the sensor is lower than a preset health index threshold. For the sensor marked with the aforementioned anomaly flag signal, the time-varying reliability weight of the sensor is determined according to the following formula. Perform temporary attenuation: ,in, Let be the weight decay coefficient, satisfying ; This represents the time-varying confidence weight of the i-th sensor before attenuation. This represents the time-varying confidence weight of the i-th sensor after attenuation.

[0009] In some implementations, determining the active sensor subset based on the health indicators of each sensor includes: determining the proportion of dynamic scenes, image texture complexity, and point cloud sparsity based on the equivalent observation data of each sensor; calculating an environmental complexity indicator based on the proportion of dynamic scenes, image texture complexity, and point cloud sparsity; monitoring the system resource usage status to determine the available computing budget of the system; recording the unit computing cost and unit power consumption cost of each sensor; constructing an optimization model based on the health indicators, using the available computing budget of the system as a budget constraint, and using the environmental complexity indicator, the health indicators of each sensor, the computing cost, and the power consumption cost as variables; and executing the optimization model to select sensors to form the active sensor subset.

[0010] In some implementations, the method further includes: real-time monitoring of the fusion residual increment; if the fusion residual increment exceeds a preset fusion residual increment threshold, temporarily adding a suboptimal sensor to the candidate list and re-executing the optimization model solution to update the active sensor subset.

[0011] In some implementations, generating environmental representation data based on the state vector of the robot system and the equivalent observation data of each sensor includes: updating the occupancy probability of each cell in the environmental grid map in logarithmic probability form based on the time-varying confidence weights of each sensor and the equivalent observation data, wherein the occupancy probability represents the probability that the corresponding position of the cell is occupied by an obstacle; determining a set of drivable areas indicating drivable areas according to a preset occupancy probability threshold and the occupancy probability of each cell; and generating environmental representation data, wherein the environmental representation data includes at least the environmental grid map updated with the occupancy probability of each cell and the set of drivable areas.

[0012] In some implementations, the method further includes: determining an obstacle target set based on the occupancy probability of each grid cell, performing weighted multi-sensor nearest neighbor association and threshold detection on the obstacle target set, and performing observation fusion within the Mahalanobis distance gate according to the time-varying confidence weight of each sensor to perform real-time tracking of dynamic obstacles.

[0013] In some implementations, the method further includes updating the extrinsic parameter matrix and time bias parameter of each sensor using a Lie algebra perturbation model based on the equivalent observation data of each sensor and the state vector of the robot system.

[0014] According to one aspect of this disclosure, a robot environment perception device based on multi-sensor fusion is provided, comprising: The sensor input module is used to receive raw observation data from various sensors configured on the robot body; The spatiotemporal adaptive synchronization module is used to obtain the equivalent observation data of each sensor in the global system time based on the time drift model of each sensor and the original observation data of each sensor. The reliability dynamic evaluation module is used to calculate the health index of each sensor based on the equivalent observation data of each sensor, and generate the time-varying reliability weight of each sensor based on the health index of each sensor. The self-organizing perception scheduling module is used to determine the active sensor subset based on the health indicators of each sensor. The active sensor subset includes the sensors that need to participate in data fusion at the current moment. The weighted consistency fusion module is used to determine the state vector of the robot system based on the time-varying confidence weights of each sensor in the activated sensor subset and the equivalent observation data. The environment modeling and output module is used to generate environment representation data based on the state vector of the robot system, the time-varying confidence weights of each sensor, and the equivalent observation data. The environment representation data includes an environment grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

[0015] According to one aspect of this disclosure, a robot environment perception system based on multi-sensor fusion is provided, the system comprising: a sensor group and an electronic device, the sensor group comprising two or more sensors, the electronic device comprising a processor and a memory, the memory storing a computer program, the computer program causing the processor to perform the method described therein when executed by the processor.

[0016] The embodiments disclosed herein can maintain the continuous consistency of data from each sensor under the same coordinate system and time reference when the device drifts or the environment changes, ensuring the accuracy and long-term stability of multi-source data fusion, thereby reducing the impact of long-term accumulation of time synchronization errors and spatial registration deviations between sensors, and effectively improving the perception accuracy and consistency of the robot system. Attached Figure Description

[0017] Figure 1 This is a flowchart illustrating the robot environment perception method based on multi-sensor fusion provided in the embodiments of this disclosure; Figure 2 This is a schematic diagram of the structure of a robot environment perception device based on multi-sensor fusion provided in an embodiment of this disclosure; Figure 3 This is a schematic structural block diagram of a robot environmental perception system based on multi-sensor fusion provided in an embodiment of this disclosure. Detailed Implementation

[0018] Hereinafter, exemplary embodiments according to the present disclosure will be described in detail with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present disclosure, and not all embodiments of the present disclosure, and it should be understood that the present disclosure is not limited to the exemplary embodiments described herein.

[0019] Figure 1 A flowchart illustrating a robot environment perception method based on multi-sensor fusion provided in an embodiment of this disclosure is shown. See also... Figure 1 The robot environment perception method based on multi-sensor fusion according to the present disclosure includes the following steps: Step 101: Receive raw observation data from the sensors configured on the robot body; Step 102: Based on the time drift model of each sensor and the original observation data of each sensor, obtain the equivalent observation data of each sensor in the global system time. Step 103: Calculate the health index of each sensor based on the equivalent observation data of each sensor, and generate the time-varying reliability weight of each sensor based on the health index of each sensor. Step 104: Determine the active sensor subset based on the health indicators of each sensor. The active sensor subset includes the sensors that need to participate in data fusion at the current moment. Step 105: Determine the state vector of the robot system based on the time-varying confidence weights of each sensor in the activated sensor subset and the equivalent observation data; Step 106: Generate environmental representation data based on the state vector of the robot system, the time-varying confidence weights of each sensor, and the equivalent observation data. The environmental representation data includes an environmental grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

[0020] In step 101, raw observation data from each sensor is received and buffered and timestamped. For example, the raw observation data from each sensor can be stored in the buffer in the order of sampling time for later retrieval.

[0021] In practical applications, the multiple sensors configured on the robot body can include, but are not limited to, at least two of the following: LiDAR, camera, millimeter-wave radar, inertial measurement unit, and ultrasonic sensor. Specifically, each sensor communicates with the system's main control unit through a data interface module. The data interface module includes a high-speed bus interface and a time synchronization interface. The system's main control unit receives the raw observation data from each sensor through this data interface module. Each sensor has a unique identifier, and the raw observation data of each sensor contains the sensor's unique identifier to distinguish its data stream source.

[0022] In step 102, equivalent observations under a unified time reference are achieved by establishing time drift models for each sensor, thereby aligning the unified time reference and spatial coordinate system of data from different types of sensors and realizing time and spatial alignment of data from different sensors.

[0023] By establishing time drift models for each sensor within a sliding time window and using these models for forward interpolation, spatiotemporal alignment of observation data from different sensors can be achieved as the robot's motion state changes. The forward interpolation employs a linear interpolation method, which involves calculating the observation value at the intermediate time based on the time interval between adjacent observations, thus achieving continuous alignment in the time domain.

[0024] Specifically, an exemplary implementation of step 102 may include the following steps a1 to a5: Step a1: Update the time drift model of the sensor based on the sliding time window.

[0025] The time drift model of a sensor indicates the deviation between its local time and the system's global time. For the i-th sensor, its time drift model is defined as: ,in, This represents the local time of the i-th sensor. Compared to the drift in the system's global time, Let i be the time drift rate coefficient of the i-th sensor. Let be the initial time bias constant of the i-th sensor.

[0026] Among them, coefficient With constant This can be achieved by fitting a sequence of time-observed samples within a sliding time window using the least squares method, and by recursively updating based on the latest sample data in each update cycle to adapt to dynamic changes caused by time drift. Specifically, it can be achieved by using continuous time-sampled pairs within the sliding time window. and A least-squares fit is performed, and the fitting result is stored in a parameter buffer to update the time-drift model over time. The fitting result is used to update the model parameters. and This involves replacing the old parameters with the fitted slope and intercept to achieve adaptive updates of the time drift model. Here, the time sampling sample is a set of data pairs consisting of observation pairs between the timestamps of each sensor and the global time synchronization reference signal within the sliding time window, including the sampling time, local timestamp, system global time, and the corresponding drift difference.

[0027] Step a2: Given the global system time, calculate the equivalent observation data of each sensor based on the time drift model of each sensor and the original observation data.

[0028] Specifically, predictive interpolation is performed when calculating equivalent observations. This predictive interpolation reconstructs the synchronized data frames of each sensor under a unified global time frame for subsequent use. Here, a "synchronized data frame" refers to a data set unit formed by observation data from different sensors under the same global time reference and the same spatial reference frame after time drift model correction and spatial coordinate transformation. It is a data structure that is aligned in both time and space to ensure that multi-sensor perception information has consistent spatiotemporal semantics during fusion, modeling, and subsequent calculations.

[0029] For sensors If the sensor times of its two most recent frames of data are respectively and The corresponding system global time and They can be calculated using the following formulas: , sensor In global time Equivalent observation data It can be determined by the following formula: , in, and They represent the first One sensor (i.e., sensor) In sensor local time The original observation data below, Represents the normalized interpolation coefficients. This is the interpolation operator function.

[0030] when When the expression is in scalar or vector form, the interpolation operator function can be defined as linear interpolation as shown in the following equation: ,when When representing attitude quaternions, the interpolation operator function can be the spherical linear interpolation (SLERP) shown below: ,in, for and The angle between them.

[0031] Taking point cloud observation data as an example, rigid extrapolation can be performed using the instantaneous attitude information provided by the inertial measurement unit under the assumption of constant velocity.

[0032] Spatial coordinate transformation of point cloud observation data can be calculated using the following formula: ,in, Indicates global time The set of point cloud coordinates below, Representing the sensor coordinate system Relative to the robot coordinate system The rigid body transformation matrix.

[0033] in, Indicates sensor Local time The collected point cloud coordinate set contains several 3D points. This set, after coordinate transformation, can be mapped to a point cloud set at global time t. .

[0034] in, Represents the sensor coordinate system { The rigid body transformation matrix relative to the robot coordinate system {B}, including rotation and translation. Its inverse matrix... Used to change the point cloud coordinates from the sensor coordinate system { The coordinates are transformed to the robot coordinate system {B}, thereby realizing the spatial correspondence of the observation data at each time point in the robot system.

[0035] In this article, symbols This represents the local time sampling point of the sensor, and the symbol t represents the global time of the system, used to represent the synchronization result at a unified moment.

[0036] For observation data that is not a point cloud, such as image feature points or inertial measurement unit data, coordinate mapping can also be performed based on the extrinsic parameter matrix of the corresponding sensor to achieve data alignment under the same spatial reference. Coordinate transformation of point cloud data is used to achieve geometric layer alignment, while transformation of non-point cloud data can be used to achieve consistency between semantic and motion information.

[0037] Based on the above, by modeling and compensating for the time drift of the sensor, the fusion error caused by asynchronous sensor sampling and time delay can be resolved.

[0038] Furthermore, step 102 may also include: transforming the equivalent observation data of each sensor to a unified world coordinate system. Specifically, the equivalent observation data of each sensor can be transformed to the world coordinate system using a spatial transformation matrix from the robot coordinate system to the world coordinate system. In practical applications, the transformed equivalent observation data can be uniformly stored in a global observation cache for subsequent use.

[0039] Here, the spatial transformation matrix can be defined as ,in, Robot coordinate system The attitude transformation matrix relative to the world coordinate system. This is the extrinsic parameter matrix of the sensor relative to the robot's coordinate system. This represents the rigid body transformation matrix of the sensor coordinate system {Si} relative to the world coordinate system {W}, used to unify the observed data to the world coordinate system. This matrix is ​​derived from the robot pose transformation matrix. With sensor extrinsic matrix Multiply them to get the result.

[0040] Furthermore, step 102 may also include: detecting the continuity of equivalent observation data of each sensor in the time domain. Specifically, calculating the time consistency residual of each sensor, and determining whether the time drift of each sensor is abnormal based on the time consistency residual of each sensor and a preset time consistency residual threshold.

[0041] In some examples, the time consistency residuals of each sensor can be calculated using the following formula: ,in, For short-time prediction operators, This is the detection interval. If the first... Time consistency residual of each sensor If the time consistency residual exceeds the preset threshold, then the first... The sensor exhibits a time drift anomaly, which can generate an indication of the first sensor. An anomaly flag signal indicating abnormal time drift of the first sensor is provided for subsequent use. If the first... If the time consistency residual of a sensor is less than or equal to the aforementioned time consistency residual threshold, then it is determined that the sensor does not have a time drift anomaly.

[0042] Among them, the time consistency residual threshold The time consistency residual threshold can be preset or dynamically calculated based on the sampling frequency, signal delay characteristics, and system clock stability of each sensor. Different time consistency residual thresholds can be used for different types of sensors. With a fixed threshold, the time consistency residual threshold... This can be obtained through experimental calibration or system initialization parameter settings. In the case of an adaptive threshold, the system can dynamically adjust based on the statistical variance of the residuals within the sliding time window. If the time consistency residual of the i-th sensor... Exceeding its corresponding time consistency residual threshold If so, it is determined that the sensor has a time drift anomaly.

[0043] In step 102, the equivalent observation data for each sensor may include, but is not limited to, observation values, global timestamps, spatial coordinate markers, and synchronization status flags. The synchronization status flag indicates whether the data from each sensor in the current data frame has achieved temporal and spatial alignment. When the temporal consistency residuals of all sensors... Less than its corresponding time consistency residual threshold When the spatial coordinate markers are consistent, the synchronization status flag is set to a valid value of "1"; if any sensor fails to meet the synchronization condition, the synchronization status flag is set to "0", and the system can selectively suspend the frame fusion operation based on the value of the synchronization status flag. The synchronization status flag provides a basis for judging the validity of frame-level data during system operation, preventing asynchronous data from being misused for state estimation.

[0044] Among them, spatial coordinate labels can be used to identify the coordinate system type corresponding to sensor observation data, including the sensor coordinate system { The system uses the robot coordinate system {B} and the world coordinate system {W}. During data synchronization and spatial transformation, the system uses these spatial coordinate markers to determine the reference coordinate system of the currently observed data, ensuring that all data is processed under the same spatial reference in subsequent fusion stages.

[0045] During system operation, raw observation data from sensors can be continuously received, and equivalent observation data from each sensor can be output in real time. In this embodiment, by establishing a time drift model, performing predictive interpolation, and spatial transformation, synchronization of data from different types of sensors under a global time and spatial reference is achieved, providing a precise multi-source data foundation for subsequent processing. In specific applications, in some low-precision or single-sensor application scenarios, if all observation data are collected in the same installation reference frame, spatial transformation can be completed and fixed during the initialization phase and will not be repeated. However, in multi-sensor fusion systems, the spatial transformation step is a necessary step.

[0046] In step 103, the health index of each sensor at the current time is calculated based on the equivalent observation data of each sensor, and a time-varying confidence weight is generated to adjust the influence of each sensor on the state estimation during subsequent fusion. That is, based on the equivalent observation dataset of each sensor... and its time consistency residuals Determine the time-varying reliability weights of each sensor .

[0047] Specifically, based on the equivalent observation data of the sensor, the sensor's signal-to-noise ratio (SNR), inter-frame coherence, effective coverage, and temporal consistency indices are determined. These indices are then weighted and summed to determine the sensor's health index. After normalizing the sensor's health index, the time-varying reliability weight is obtained. The time-varying reliability weight ranges from 0 to 1.

[0048] In some examples, the health index can be calculated using the following formula.

[0049] , in, These are the weighting coefficients for the signal-to-noise ratio, inter-frame coherence, effective coverage, and temporal consistency metrics, respectively, satisfying... .

[0050] The signal-to-noise ratio (SNR) is a metric used to characterize the strength and stability of sensor signals. It is the normalized result of the ratio of the effective signal amplitude to the variance of the background noise in equivalent observation data.

[0051] It represents the inter-frame coherence index, reflecting the degree of continuity between observation data at adjacent time points, and is calculated as the Euclidean distance between the equivalent observation data of the current frame and the equivalent observation data of the previous frame.

[0052] The effective coverage index refers to the ratio of the number of effective observations of a sensor at a given moment to its nominal number of observations.

[0053] The time consistency index can be represented by the time consistency residual of the sensor. It can be calculated. Specifically, it can be calculated using the following formula: ,in, This is the time-consistency decay coefficient. This represents the time consistency index of the i-th sensor. This represents the time consistency residual of the i-th sensor.

[0054] Time consistency decay coefficient The time response sensitivity can be preset according to the system's desired sensitivity, or obtained through calibration during system initialization. Different sensors have different time sampling frequencies and clock stability, and their respective attenuation coefficients can be set independently. For high-frequency sensors, a smaller attenuation coefficient is used to enhance the tolerance for short-time residuals; for low-frequency sensors or sensors with large delays, a larger attenuation coefficient is used to enhance the sensitivity for anomaly detection. If independent calibration has not been performed, a uniform empirical value can be used. Configure it to a value of 0.1 to 0.5.

[0055] In practical applications, the inter-frame coherence index Effective coverage index and time consistency index All values ​​are calculated based on equivalent observation data. The results are not directly included in the equivalent observation data itself, but are stored as supplementary quality assessment quantities in the metadata field of the synchronization data frame. When calculating the sensor's health indicators, the system can generate the sensor's time-varying reliability weights by reading these assessment quantities from the synchronization data frame.

[0056] In some examples, the health metric values ​​of each sensor can be normalized using the following formula to generate time-varying confidence weights.

[0057] ,in, The number of sensors involved in perception, that is, the total number of sensors installed on the robot body. To prevent tiny constants with a denominator of zero, Let represent the health index of the i-th sensor, i=1,2,…,M. This represents the time-varying confidence weight of the i-th sensor. The value ranges from 0 to 1, and the sensor's The higher the value, the higher the reliability of the sensor at the current moment.

[0058] Furthermore, step 103 may also include: when a sudden drop in the health index of a sensor or a time consistency residual exceeding a preset time consistency residual threshold is detected, automatically reducing the time-varying reliability weight of the corresponding sensor, and selectively issuing an alarm signal as needed. That is, during operation, abnormal sensors are identified based on their health index or time consistency residual, and their time-varying reliability weight is automatically reduced for abnormal sensors. The alarm signal is a logical flag used internally by the system for anomaly identification and management, indicating that the health index or time consistency residual of a certain sensor exceeds a preset time consistency residual threshold. When this flag is triggered, the system automatically reduces the time-varying reliability weight corresponding to that sensor and suspends its data from participating in fusion calculations. Simultaneously, this alarm signal can be used to trigger log recording, status diagnosis, or security policy responses from upper-level systems. The alarm signal is only an internal logical output and does not involve external audible or visual alarms.

[0059] Specifically, when the time consistency residual of a certain sensor is detected to exceed the preset time consistency residual threshold, the time-varying reliability weight of the sensor is automatically reduced and a re-evaluation process is triggered to achieve self-diagnosis and self-repair.

[0060] Here, "reassessment process" refers to the system recalculating the health indicators of each sensor within a subsequent sliding time window after detecting an abnormal sensor. and time-varying credibility weight This is to verify whether the abnormal state persists. If the abnormality is eliminated within a certain number of consecutive periods, the system automatically restores the normal weight of the sensor. This process does not re-execute data fusion, but only periodically recalculates the sensor health indicators.

[0061] When the health index value of a certain sensor is detected to be lower than the preset health index threshold. or time-consistent residuals Exceeding the preset time consistency residual threshold At that time, an anomaly flag signal is generated to mark the sensor as abnormal.

[0062] In some examples, the time-varying confidence weight of anomaly sensors can be automatically reduced by weighting sensors marked with anomaly flag signals according to the following formula. Perform temporary attenuation.

[0063] ,in, Let be the weight decay coefficient, satisfying . This represents the time-varying confidence weight of the i-th sensor before attenuation. This represents the time-varying confidence weight of the i-th sensor after attenuation.

[0064] Weight decay coefficient This can be used to control the rate at which the weight decreases after an anomaly is detected by an anomaly sensor; its value range is 0 < <1. In some examples, the coefficient The weight attenuation coefficient can be set separately according to the observation frequency and anomaly persistence characteristics of different sensors. For high-frequency, low-latency sensors such as IMUs, a smaller weight attenuation coefficient (e.g., 0.8–0.9) can be selected to avoid excessive weight attenuation due to short-term jitter; for low-frequency or less stable sensors such as vision sensors, a larger attenuation coefficient (e.g., 0.5–0.7) can be selected to accelerate anomaly isolation response. In other examples, if the system does not perform individual calibration, the weight attenuation coefficient of each sensor can also be set. The weight is uniformly set to an empirical value (e.g., 0.7). During operation, this weight decay coefficient... The weights of multiple sensors can be dynamically balanced by adaptively adjusting the statistical results of the number of anomalies within the sliding time window.

[0065] Furthermore, the method in this embodiment may further include: restoring the time-varying confidence weight of the sensor after the abnormal state of the sensor is resolved. Specifically, this can be done within a fixed recovery detection window. Mean of health index of internal statistical sensors When the conditions are met When the time-varying confidence weight of the sensor is restored, the following formula is used.

[0066] ,in, This is the normalized result corresponding to the historical mean of the health index. This is the preset recovery threshold. This represents the time-varying confidence weight of the recovered sensor.

[0067] In step 103, a health index calculation and time-varying confidence weight update operation can be performed on all sensors once in each detection cycle, and a data structure containing each sensor identifier, health index value, time-varying confidence weight, and abnormal state signal for the sensor can be generated for subsequent processing.

[0068] Here, the detection period is a parameter used in the system to periodically perform health index calculations and weight updates. Its value can be set according to the system clock accuracy and task real-time requirements. In typical applications, the detection period ranges from 100ms to 1s, which is used to avoid excessive computational load while maintaining system real-time performance. This period can be a fixed value or can be adaptively adjusted by the system based on the sensor sampling frequency and state change rate. At the end of each detection period, the system performs a health index calculation, a time-varying reliability weight update, and an abnormal state signal writing operation for all sensors.

[0069] In this embodiment of the disclosure, through the calculation of health index, processing of time-varying reliability weight, and anomaly detection and self-repair mechanism in step 103, the dynamic quantification and real-time adjustment of sensor reliability can be realized, providing a reliable foundation for subsequent sensor scheduling and fusion calculation.

[0070] In step 104, based on the health indicators of each sensor... Computational cost Power consumption cost and the current system's available computing budget Determine the active sensor subset This enables on-demand sensing and adaptive energy consumption scheduling of multiple sensors under varying environmental complexities, reducing redundant computation while ensuring the integrity of sensing.

[0071] Specifically, during system operation, a scheduling optimization model is constructed with environmental complexity indicators, computational cost, and power consumption cost as variables. Under the premise of meeting the system's computational budget constraints, the sensors that need to participate in the fusion are dynamically selected to form an active sensor subset.

[0072] In some examples, the process of determining the subset of active sensors in step 104 may include steps b1 to b3 as follows: Step b1: Calculate the environmental complexity index of each sensor based on the equivalent observation data of each sensor. ; Specifically, environmental feature parameters such as the proportion of dynamic scenes, image texture complexity, and point cloud sparsity can be calculated based on the equivalent observation data of each sensor, and the environmental complexity index can be calculated comprehensively based on these environmental feature parameters.

[0073] The environmental complexity index can be calculated using the following formula.

[0074] ,in, This indicator represents the proportion of dynamic scenes and can be calculated based on the percentage change in pixels of moving targets or the magnitude of changes in the optical flow field between two consecutive frames of observation data. An index representing image texture complexity can be obtained based on the gray-level co-occurrence matrix entropy or gradient variance. The sparsity index of point clouds can be calculated by inversely proportional to the number of point clouds per unit volume. These are the weighting coefficients for each component of environmental complexity, satisfying... . This represents an indicator of environmental complexity.

[0075] In a typical embodiment, Its value can be determined during the offline calibration phase of the system, or it can be dynamically adjusted during runtime based on the sensor's health indicators. For example, when the visual sensor has a high weight, it can be appropriately increased. To highlight the impact of image texture complexity; when the LiDAR data density is high, increase This reflects the contribution of point cloud sparsity to environmental complexity. For example, if adaptive adjustment is not enabled, the default setting can be: =0.3.

[0076] Step b2: Monitor system resource usage to determine the available computing budget and record the unit computing cost for each sensor. and cost per unit power consumption

[0077] Specifically, in step b3, an optimization model is constructed using the available computing budget of the system as the budget constraint and the environmental complexity index, the health index of each sensor, the computing cost, and the power consumption cost as variables. The optimization model is then solved to select sensors to form an active sensor subset.

[0078] Specifically, the optimization model shown in the following equation is constructed: , , in, , representing the sensor activation vector; Indicates activation of the first One sensor, This indicates that the sensor is turned off. The number of sensors involved in the sensing process. This represents an indicator of environmental complexity.

[0079] Environmental complexity index The calculation results can be used as weighting adjustment factors in subsequent sensor activation decisions. In one embodiment, the system can, according to... The value of affects the time-varying reliability weight of each sensor. Normalization corrections are performed to enhance the fusion ratio of high-reliability sensors in highly complex scenarios.

[0080] Where M represents the number of sensors participating in perception, indicating the total number of sensors that the system can currently access and participate in environmental perception data acquisition and fusion calculation. In specific applications, the system can dynamically adjust the set of sensors participating in perception according to task requirements, and the value of M can change in real time with the access status of external sensors. In a typical embodiment, M includes multimodal sensors (such as vision sensors, LiDAR, inertial measurement units, etc.) configured on the robot body and external collaborative sensors accessed via communication links. When the system does not enable external collaborative perception, M is simply the number of sensors configured on the robot body.

[0081] Among them, sensors The computational cost is denoted as The unit is computing resources per second; sensors The power consumption cost is denoted as The unit is energy units per second. The current system's available computational budget is... Indicates the time interval Internally allocated computing resources.

[0082] in, Indicating environmental complexity index The degree of insufficient perceived information is defined by the following formula: ,in, For the first Health indicators of individual sensors To prevent tiny constants with a denominator of zero, The value of M represents the number of sensors involved in the sensing process, as discussed earlier and will not be repeated here.

[0083] in, The tradeoff coefficients for optimizing the objective represent the importance of environmental information, computing resources, and power consumption constraints, respectively. In practical applications, The value can be set offline or adjusted online adaptively based on the task's real-time requirements, computing load, and energy consumption budget.

[0084] Specifically, the optimization model is solved within each scheduling cycle. This solution can be achieved using a heuristic greedy algorithm, and the process includes the following three steps: The first step is to calculate the proportional value. Arrange all sensors from largest to smallest; The second step is to select sensors sequentially and accumulate the computational costs. Until the system's available computing budget is reached. ; The third step is to select the sensors to form an active sensor subset. .

[0085] Furthermore, step 104 may also include: performing deactivation rollback upon detecting a sudden increase in system perception error (i.e., fusion residual increment). Specifically, the system performs deactivation rollback based on the sensor's health indicators. With power consumption cost The system calculates activation priorities, prioritizing sensors with high health metrics and low power consumption for fusion. When the fusion residual increment exceeds a certain threshold, the system performs a feedback activation rollback operation to suppress computational resource fluctuations caused by overactivation. For example, when the fusion residual increment rate exceeds a threshold, sensors with lower activation status are rolled back sequentially according to their priority to stabilize the system's computational load.

[0086] Real-time monitoring of fusion residual increment Integrating residual increments Defined as follows: , in, For the first The current fusion residual of each sensor. When Exceeding the preset threshold for fusion residual increment If necessary, a suboptimal sensor is temporarily added to the candidate list, and the optimization model is re-executed to update the active sensor subset, thereby enhancing system observability. After the rollback operation is complete, the updated active subset... Replace the current subset and continue participating in the fusion.

[0087] The fusion residual of the i-th sensor at time t can be calculated from the Euclidean distance between the equivalent observation data of the sensor and the fusion estimation result of the system.

[0088] In typical applications It can be obtained from the following formula: ,in, These are sensor observations. This is the output for fusion estimation.

[0089] Specifically, the system triggers a dynamic sensor reconfiguration mechanism. Based on sensors not selected for the active sensor subset in the previous optimization solution, the system generates a candidate list according to a comprehensive score of their health indicators and power consumption costs. Sensors in the candidate list are currently available but not yet activated. During dynamic correction, the system selects a suboptimal sensor (i.e., a sensor with a score lower than the current activation threshold) from the candidate list and temporarily adds it to the active sensor subset, forming a new temporary active sensor subset. To verify the impact of this adjustment on system performance, the optimization model needs to be re-executed based on the new temporary active sensor subset to update the optimal balance of each trade-off and ensure that the aforementioned computational budget constraints still hold. If the fusion residual increment of the system after the solution... Regression to the fusion residual increment threshold If the selected suboptimal sensor is within the specified range, it is confirmed that the selected suboptimal sensor is a valid supplement, and the active sensor subset is updated to the temporary sensor subset; otherwise, it is restored to the active sensor subset before the supplement.

[0090] In practical applications, the optimization process of step 104 can be executed periodically during operation, and the active sensor subset can be recalculated immediately when a detection event is triggered, such as sensor malfunction or sudden change in environmental complexity indicators.

[0091] In step 104, after determining the subset of activated sensors, the output data includes, but is not limited to, the activation state vector of each sensor, the current environmental complexity index, the system's available computational budget, and the subset of activated sensors. The system's available computational budget represents the computational resource budget that the system can allocate at the current moment, and the subset of activated sensors represents the set of activated sensors obtained based on the optimization model.

[0092] In this embodiment of the disclosure, an optimization model containing environmental complexity and resource constraints is constructed to determine the subset of active sensors, thereby realizing on-demand activation and redundancy control of sensors. The active sensor subset, in conjunction with the aforementioned time-varying reliability weights, enables the system to maintain stable environmental perception capabilities under conditions of limited computing resources.

[0093] In step 105, weighted consistency fusion of multi-source data is performed on the activated sensor subset; that is, state estimation and information fusion are performed on the data from the activated sensors. Specifically, this is based on the sensor activation subset. Time-varying reliability weights of each sensor and equivalent observation data Determine the state vector and covariance matrix of the robot system.

[0094] In some examples, the process of determining the state vector and covariance matrix of the robot system in step 105 may include the following steps c1 to c3: Step c1: Establish the system state model and define the observation relationships.

[0095] Here, the system state model is used to describe the relationship between the motion and attitude changes of the robot system over time. Its state equation can characterize the dynamic evolution of the system, and the observation equation is used to correlate the observation data of each sensor with the system state variables.

[0096] Specifically, the state vector of the robot system is defined as follows: The state vector is It includes variables such as position, velocity, and attitude, and can be expressed as follows: ,in, Represents a position vector. Represents the velocity vector. The attitude Euler angles are represented as quaternion parameters.

[0097] Specifically, the observation equations for each sensor are defined as follows: It can be expressed as the following formula: ,in, For the first The observation vector of each sensor, For the first The observation model function for each sensor. To measure the noise term, it is assumed to follow a zero-mean Gaussian distribution. Covariance matrix. It can be obtained through statistical estimation or online recursive estimation of historical sensor observation data, and its amplitude can be adjusted in real time in conjunction with health indicators to reflect changes in observation accuracy.

[0098] Step c2 involves performing a weighted least squares solution based on the time-varying reliability weights of each sensor to determine the state increment.

[0099] Specifically, a weighted objective function is constructed, and its minimization is achieved through iterative optimization to determine the state increment. .

[0100] In some examples, the weighted objective function can be expressed as follows: ,in, . For sensors The covariance matrix. This represents the observation vector after time synchronization and spatial registration, and serves as the input observation model function. The actual measurement input corresponds one-to-one with the original observation data. For sensors The observation model function, For sensors The observation noise covariance matrix can be obtained from the measurement error statistics of the sensor during the calibration phase, and then adjusted according to the time-varying reliability weights during operation. Dynamic adjustment.

[0101] In some examples, a weighted least squares solution method with an M-estimation kernel function can be used. During the fusion process, the measurement information matrix is ​​dynamically adjusted according to the time-varying confidence weights of each sensor to reduce the impact of anomalous observations on the state estimation results. The measurement information matrix is ​​the inverse of the observation noise covariance matrix, used to quantify the observation confidence in weighted least squares estimation. The system can dynamically adjust the diagonal elements of this matrix according to the time-varying confidence weights of each sensor to achieve anomalous observation suppression.

[0102] In some examples, step 105 can be introduced into the weighted objective function. Estimating kernel function This is used to correct the weighted residual term, adjusting the weighting mechanism of the weighted objective function when outliers exist in the observation data, thus reducing the impact of abnormal observations on the solution results. When the temporal consistency residual or health index of the sensor observation data is lower than a preset threshold, it is considered an outlier. In this case, the system introduces an M-estimation kernel function to correct the weighted objective function, thereby reducing the impact of abnormal observations on state estimation.

[0103] Introduction Estimating kernel function The weighted objective function is expressed as follows: , Among them, kernel function This is a robust weighting function used to nonlinearly suppress observation residuals in weighted least squares estimation, thereby reducing the impact of outlier observations on the estimation results and satisfying the following conditions: It also has a suppressive effect on large residuals. For sensors The observation vector. For sensors The observation model function can be obtained based on the sensor's imaging geometry model, ranging model, or through data-driven learning methods. To observe the noise covariance matrix, it can be determined based on the measurement error statistics of the sensor during the calibration phase or the real-time estimation results during operation. For sensors The time-varying credibility weight.

[0104] For example, the kernel function can be a Huber function, represented as follows.

[0105] , in, This is the threshold parameter of the kernel function. After introducing the kernel function, when the residual... Exceed The corresponding weighted value will be automatically reduced. r represents the weighted residual term, i.e., the observation error after covariance weighting. When introducing the kernel function... Subsequently, smaller residuals are weighted using quadratic terms, while larger residuals are weighted using linear terms, thereby adaptively reducing the impact of outlier observations. Threshold parameter It can be adaptively set based on the variance range of the observed residuals. This parameter controls the switching between the linear and saturated regions, thereby balancing robustness and estimation accuracy. After introducing this function, small residuals are weighted by squares, and large residuals are weighted by linearity, thus adaptively reducing their impact on state estimation when outlier observations exist.

[0106] In some examples, state increment It can be expressed as the following formula: , in, Indicates the first The Jacobian matrix of observations from each sensor, , Indicates the first The weighted observation residual vector of each sensor.

[0107] Step c3: Update the state vector based on the state increment.

[0108] Specifically, it can be based on the following formula according to the state increment. Update the state vector: Furthermore, in step 105, the covariance matrix can be calculated and the equivalent information matrix updated. Covariance matrix As a measure of fusion uncertainty, it can be used to evaluate modeling confidence in subsequent processing.

[0109] First, based on the linearization result of the weighted least squares estimation, the equivalent information matrix Λ(t) is calculated: The equivalent information matrix is ​​calculated using the following formula based on the weighted solution result. : , in, Let be the time-varying confidence weight of the i-th sensor. Rᵢ(t) represents the Jacobian matrix of the observation model with respect to the state variables, and Rᵢ(t) is the observation noise covariance matrix.

[0110] Secondly, based on the following formula using the equivalent information matrix Calculate the covariance matrix of the robot system.

[0111] In practical applications, during system operation, step 105 can perform an iterative solution process at a fixed frequency to ensure that the state estimation maintains temporal continuity and information consistency under multi-sensor heterogeneous input conditions. After each calculation cycle, step 105 can output a state vector. covariance matrix The fusion results of the fusion residual statistics are also included. Among them, the fusion residual statistics ΔR(t) reflects the overall consistency of the multi-sensor weighted fusion and can be used as the basis for the confidence level of environmental state updates.

[0112] In this embodiment of the disclosure, step 105 introduces time-varying confidence weights and kernel function corrections to achieve weighted fusion of multi-sensor data under a unified spatiotemporal reference, which can maintain the numerical stability and consistency of state estimation in the presence of observational anomalies.

[0113] In step 106, the system state vector x(t) and covariance matrix are calculated in step 105. Based on the fusion results from step 105, environmental representation data can be generated, which can be used for navigation, obstacle avoidance, and path planning. Specifically, the occupancy probability of each cell in the environmental grid map is updated in logarithmic probability form, while the drivable area is determined and the free space set is output. Here, the free space set refers to the set of spatial units in the environmental grid map that are not occupied by obstacles or dynamic targets and are accessible to the robot.

[0114] Specifically, based on state vectors covariance matrix and equivalent observation data from each sensor The process involves updating the environmental grid map, determining the set of drivable areas, and generating an environmental update status flag. The activated sensor subset is the set of sensors participating in the fusion process determined in step 104 based on computational resource constraints. The environmental update status flag indicates whether each grid cell has undergone a change in occupancy status within the current period; its value can be "0" for no change, "1" for new occupancy, and "-1" for release.

[0115] Specifically, an exemplary implementation of step 106 may include the following steps d1 to d2: Step d1: Establish and maintain an environmental grid map. Update the occupancy probability of each cell in the environmental grid map in logarithmic probability based on the time-varying confidence weights of each sensor and equivalent observation data.

[0116] First, a discrete 3D grid structure is used to model the environmental space to construct an environmental grid map. In the environmental grid map, the entire environmental space is divided into sets of equally spaced elements. Each cell in an equally spaced element set (j=1,2,…,N) corresponds to an occupancy probability This is used to indicate the possibility that the location is occupied by an obstacle.

[0117] Secondly, to avoid numerical instability caused by directly using probability multiplication, the logarithmic probability of each grid cell is updated using logarithmic probability.

[0118] Logarithmic odds are defined as follows: ,in, For grid element The logarithmic probability of occupancy.

[0119] Whenever a new observation arrives, the occupancy probability of each cell is updated in logarithmic form, with the update equation as follows: , in, For sensors Time-varying credibility weights For sensors Equivalent observation data, For sensors For gate element The observation increment function is defined as follows: After calculating the system state vector x(t) in step 105, the observed data Zᵢ(t) can be aligned and spatiotemporally compensated based on the state estimation result, and then the update parameters Locc and Lfree can be calculated and the environmental grid occupancy state can be updated. , in, Indicates the increase in share. , indicating the idle increment. and To obtain updated parameters, they can be obtained through experimental calibration based on the ranging accuracy of the system sensors and the environmental noise characteristics.

[0120] Finally, based on the updated log-occupancy probability of each cell, the occupancy probability of each cell is obtained through the conversion relationship between log-probability and probability using the following formula. .

[0121] ,in, Represents grid element The probability of occupancy, For grid element The logarithmic probability of occupancy.

[0122] Step d2: Determine the set of drivable regions that indicate drivable regions based on the preset occupancy probability threshold and the occupancy probability of each cell.

[0123] Specifically, a set of drivable regions is generated based on the occupancy probability of each cell. An occupancy probability threshold is set. ,when At that time, the gate element Mark as free space; when At that time, the gate element Mark obstacles and use grid cells marked as free space to form a set of drivable areas.

[0124] The set of drivable areas is defined as follows: ,in, This represents the set of all raster cells in an environmental raster map. This represents the set of feasible driving regions, which is a subset of grid cells that meet the occupancy probability threshold condition within the current period.

[0125] In practical applications, the set of drivable areas can be output in the form of a three-dimensional point set or a two-dimensional projection, providing data support for robot navigation and path planning.

[0126] Step d3 generates environmental representation data for use in the robot's path planning and motion control.

[0127] Here, the environmental representation data may include at least an environmental grid map updated with the occupancy probability of each cell and the set of drivable areas.

[0128] Furthermore, step 106 may also include: calculating an environmental modeling confidence index based on the covariance matrix of the robot system, wherein the environmental modeling confidence index is used to reflect the reliability of the current modeling results.

[0129] Specifically, environmental modeling confidence index Defined as follows: ,in, This represents the matrix trace operation. The environmental modeling confidence index Γ(t) ranges from 0 to 1 and is used to reflect the modeling reliability of the current environmental raster map and feasible region set.

[0130] If the environmental modeling confidence index Γ(t) is less than the preset confidence threshold Γ_min, the system pauses the map writing operation and requests to re-execute step 105 to recalculate the robot system's state vector and covariance matrix. The map writing operation is used to write the fused occupied grid data into the global map storage unit. When insufficient confidence is detected, the writing is paused to prevent unreliable data from polluting the map.

[0131] The environmental representation data obtained in step 106 can be uniformly stored as a data structure M(t), including an environmental grid map, a feasible region set Ω(t), and an environmental modeling confidence index Γ(t).

[0132] The update flag is used to indicate whether there is an occupied state or a change in confidence level in the current period. Its value is 1 to indicate an update and 0 to indicate a hold.

[0133] During system operation, map update operations can be performed periodically, and the position information of the state vector x(t) can be used to align the coordinates in the global map coordinate system, thereby achieving the unification of local observation and global map.

[0134] By jointly controlling the occupancy logarithmic probability and confidence index, continuous modeling and dynamic updating of the environment are achieved, providing a unified and quantitative environmental modeling foundation for robot navigation, obstacle detection, and path planning.

[0135] In this embodiment of the disclosure, by adopting a log-probability occupancy update mechanism and output control based on confidence (i.e., environment modeling confidence index), continuous modeling and real-time updating of the environment are achieved, which can provide a unified and quantitative environmental description basis for the robot system to perform subsequent dynamic obstacle detection and path planning.

[0136] Furthermore, step 106 may also include: determining the obstacle target set based on the occupancy probability of each grid cell, performing weighted multi-sensor nearest neighbor association and threshold detection on the obstacle target set, and performing observation fusion within the Mahalanobis distance gate according to the time-varying confidence weight of each sensor to perform real-time tracking of dynamic obstacles.

[0137] After completing environmental modeling and occupancy probability updates, the system can detect potential movable targets by observing changes in grid occupancy probabilities over continuous time. Specifically, it detects, associates, and tracks the status of movable targets identified in the environmental modeling results. The initial identification of movable targets is based on the significance of changes in grid occupancy probabilities over time. This provides temporal status information of dynamic obstacles during real-time operation.

[0138] First, based on the raster map matrix with the occupancy probability of each cell updated. and state vector By analyzing the changes in occupancy probability at consecutive time intervals, a dynamic region candidate set is extracted. .

[0139] The dynamic region determination condition is as follows: ,in, For grid element The change in occupancy probability between adjacent time points The dynamic region detection threshold can be adaptively set according to the sensor frame rate and the rate of environmental change. The set of gate elements that meet the criteria is clustered into connected components to form a preliminary obstacle candidate set. : ,in, This represents the number of obstacles currently detected. This represents the k-th obstacle.

[0140] Secondly, establish the observation correspondence of the same obstacle target among multi-sensor observations in order to determine the pairing observation set.

[0141] Specifically, for any two sensor observation sets and Calculate the Mahalanobis distance between observations to obtain the paired observation set: , in, and The first With the The first sensor detected the first With the The observation vector of each obstacle Denotes the joint observation covariance matrix. It is obtained by weighted superposition of the noise covariance matrices observed by the two sensors, and satisfies the following equation: ,in, , The first The observation noise covariance matrix of j sensors. If so, then the two observation sets are determined to correspond to the same target. The threshold distance, representing the correlation between observations, is used to determine whether observations from different sensors correspond to the same obstacle target. Indicates the first The first sensor The observation data and the j-th sensor's data Mahalanobis distance between observation data.

[0142] Third, based on the paired observation set, the state vector of each dynamic obstacle is estimated and its trajectory is predicted.

[0143] Specifically, each obstacle The state vector is defined as follows: ,in, Indicates the coordinates of the obstacle's position. This represents the velocity component of the obstacle.

[0144] The obstacle state update equation is as follows: , The observation equation is as follows: ,in, Here is the state transition matrix. H is the control input matrix, and H is the observation matrix; and Let process noise and observation noise be represented respectively, both being zero-mean Gaussian white noise, with covariance matrices of respectively. and . This is a system control input used to describe external influences (such as acceleration control or model correction terms); if there is no explicit control input, G can be set to 0.

[0145] Fourth, the obstacle state can be updated using a weighted multi-sensor Kalman filter method.

[0146] Specifically, the steps for predicting and updating the obstacle state are as follows: Prediction can be achieved using the following formula: , , The update can be achieved using the following formula: , , , in, This is the updated obstacle state vector. This is the weighted gain matrix. Here is the updated covariance matrix, where, is the fusion gain matrix, used to weight and fuse multi-sensor observations to minimize estimation errors; its calculation is based on the principle of weighted Kalman filtering. The updated state estimation covariance matrix is ​​obtained by correcting the predicted covariance based on observations, and reflects the uncertainty in obstacle state estimation.

[0147] The above algorithm uses time-varying reliability weights among different sensors. By achieving observation fusion, the stability of dynamic obstacle trajectory estimation can be improved.

[0148] Finally, manage the set of obstacle trajectories in time series. .

[0149] The obstacle trajectory set consists of several target state sequences, which can be expressed as follows: , in, in, This represents the number of obstacles detected at time t; Let represent the state sequence of the k-th obstacle from the initial time to the current time t.

[0150] In each cycle, the trajectory continuity is detected when the target is in continuous motion. If no match is found within a frame, the corresponding trajectory is automatically terminated and resources are released. For newly detected targets that are not matched, a new trajectory is established. Create a new trajectory entry and initialize the state. This is the trajectory loss threshold, used to determine whether obstacles persist. When continuous... If no valid observation match is obtained for a frame, it is assumed that the target has left the detection range or has been occluded, and the corresponding trajectory is terminated. The value can be adaptively set according to the system frame rate and obstacle speed, with a typical value of 3 to 5 frames.

[0151] Through the aforementioned processing, obstacle data can be obtained, which may include, but is not limited to, a set of obstacle states. obstacle velocity estimation set and obstacle trajectory set The data is provided to the upper-level motion planning module in a structured format with timestamps. Here, all obstacle data can be accompanied by a state confidence index, which can be calculated from the Kalman filter and the trace of the covariance matrix, and used for subsequent path risk assessment.

[0152] In this embodiment of the disclosure, real-time identification and trajectory estimation of dynamic obstacles are achieved through target detection based on occupancy changes, multi-sensor Mahalanobis distance correlation and weighted Kalman state update, providing continuous environmental dynamic information for robot path planning and obstacle avoidance control.

[0153] Further, see Figure 1 The method of this disclosure embodiment may further include: step 107, updating the extrinsic parameter matrix and time bias parameter of each sensor based on the equivalent observation data of each sensor and the state vector of the robot system using the Lie algebra perturbation model, so as to maintain spatial consistency and temporal consistency among multiple sensors.

[0154] Specifically, based on the equivalent observation data of each sensor and the state vector of the robot system Determine the extrinsic parameter matrix of each sensor and time offset parameters The updated value.

[0155] First, during the system deployment phase, initial external parameters and initial time bias parameters are provided for each sensor.

[0156] The extrinsic parameter matrix for each sensor is defined as follows: , in, , Representing the sensor coordinate system Relative to the robot coordinate system The rotation matrix, This represents the translation vector. Initial extrinsic parameters can be obtained through static calibration or structured light alignment and stored in matrix form in the robot system parameter storage area.

[0157] Secondly, the extrinsic parameter correction and time bias correction are jointly estimated. Specifically, equivalent observation data from each sensor and the state vector of the robot system are collected within a sliding time window to construct a joint error function. This joint error function is then iteratively minimized using the Gauss-Newton method or Lie algebra optimization to obtain the extrinsic parameter correction vector. With time offset correction value .

[0158] The joint error function is as follows: , in, For the first Time offset correction for each sensor. This represents the state vector of the robot system. Indicates the time offset of the i-th sensor The corrected observation vector is used to compensate for the asynchronous error between the sensor sampling time and the system master clock. Indicates sensor The observation model function, The joint calibration error function is used to measure the total sum of squared residuals between multi-sensor observation data and system state estimates. Let the extrinsic perturbation vector be of Lie algebra form, defined as for For the rotational disturbance component, This represents the translational disturbance component. The mapping operator from Lie algebras to Lie groups is defined as follows: , in, Represents the vector The generated antisymmetric matrix. This mapping is used to accumulate extrinsic parameter corrections into the pose matrix in each iteration, thus achieving pose update.

[0159] Finally, the extrinsic parameter matrix and time bias parameter are updated based on the extrinsic parameter correction and time bias correction.

[0160] Specifically, the updated extrinsic parameter matrix can be calculated using the following formula: , The time bias parameter update formula is as follows: ,in, Update the step size coefficient for time bias. This represents the time offset correction value. Indicates the time interval for parameter updates. Represents the extrinsic parameter correction vector The Lie algebra matrix form.

[0161] In practical applications, after each calibration cycle, the updated extrinsic parameter matrix and time offset parameters can be stored in the robot system parameter register to ensure that the latest parameters are used subsequently.

[0162] Furthermore, step 107 may also include: verifying the stability and consistency of the updated parameters.

[0163] Specifically, after each calibration cycle, the residual consistency index of each sensor is calculated. The residual consistency index can be obtained by the following formula: , in, Indicates the first The residual consistency index of each sensor This represents the state vector of the robot system. Indicates the first Time deviation parameter values ​​of each sensor Indicates the first The extrinsic parameter matrix of each sensor, This represents the observation vector output by the i-th sensor after time bias β(t) correction, used to compensate for the time asynchrony error between the sensor's sampling time and the system clock. This represents the observation model function for the i-th sensor. Let x(t) be the extrinsic parameter matrix of the i-th sensor relative to the robot's body coordinate system. This matrix is ​​used to transform the system state vector x(t) from the body coordinate system to the sensor coordinate system before calculating the theoretical observations. If the residual consistency index of a certain sensor Exceeding the set residual consistency index threshold If this happens, the system can revert to the previous calibration parameters and mark the sensor as entering the recalibration queue. Among these parameters, the residual consistency index threshold... This represents the upper limit used to determine the validity of sensor calibration results. When the sensor's residual consistency index... Less than or equal to the residual consistency index threshold This indicates that the sensor's calibration results are stable within the current period and the observation residuals are within an acceptable range. In this case, the system maintains the current extrinsic parameter matrix. and time offset parameters The status remains unchanged; we will only continue to track and verify subsequent data. If the conditions are met for multiple consecutive periods... If the calibration result is negative, the calibration result is determined to be "passed consistency verification".

[0164] Furthermore, the residual consistency index of each sensor can be tracked during multiple consecutive calibrations. The changing trend of the residual consistency index of a certain sensor rate of change The convergence threshold of the residual consistency index is lower than the preset threshold. This confirms that the extrinsic parameter matrix and time bias parameter of the sensor are stable. The residual consistency index of a certain sensor... rate of change The convergence threshold of the residual consistency index is higher than the preset threshold. This indicates that the sensor's calibration results have not yet converged, exhibiting slight instability or drift. In this situation, the system will continue monitoring, not immediately locking the parameters, but instead extending the observation window to collect more data and perform weighted smoothing of the sensor's residuals. If the values ​​remain above the convergence threshold, a recalibration process can be triggered to update the results.

[0165] In practical applications, the joint calibration of the extrinsic parameter matrix and the time offset parameter can be performed at a low frequency during system operation and can be triggered periodically. After each execution, a parameter update signal can be sent to the robot system's scheduler to reload the latest time offset parameters and update the extrinsic parameter matrix used during coordinate transformation.

[0166] The processing result in step 107 may include, but is not limited to, the corrected set of extrinsic parameter matrices. Time offset parameter set and the set of residual consistency indicators The corrected set of extrinsic parameter matrices Includes the corrected extrinsic parameter matrices of each sensor installed on the robot body, and the set of time offset parameters. Includes the time offset parameters of each sensor set on the robot body, and a set of residual consistency indicators. This includes residual consistency metrics for all sensors installed on the robot. All parameter updates can be timestamped and saved in a structured format for subsequent system recovery and fault diagnosis.

[0167] In this embodiment, the synchronous adaptive correction of the external participation time bias is achieved by minimizing the joint error based on Lie algebra perturbation, so that the multiple sensors of the robot system maintain a unified spatiotemporal alignment relationship during long-term operation, which can provide parameter consistency guarantee for the continuous and stable operation of the system.

[0168] The method of this disclosure embodiment has at least the following beneficial effects: 1) By using external parameters and time offset joint calibration, the joint calibration of time and space parameters of the multi-sensor system during operation is realized. It can automatically correct time errors and external parameter errors when the equipment drifts or the environment changes, thereby maintaining the continuous consistency of data from each sensor under the same coordinate system and time reference, and ensuring the accuracy and long-term stability of multi-source information fusion.

[0169] 2) By calculating indices such as sensor signal health, covariance consistency, and time deviation in real time, the covariance consistency index is used to evaluate the degree of matching between the observation data and the estimation results of each sensor, and the time deviation index is used to reflect the stability of the observation time synchronization. The working status of each sensor is dynamically quantified and evaluated, and the evaluation results are transformed into weight parameters for weighted fusion, so that the system can automatically adjust the sensing strategy according to the sensor reliability, thereby achieving an adaptive response to changes in data quality.

[0170] 3) The system automatically selects an active subset of sensors based on multi-dimensional information such as environmental complexity, computing resources, and sensor status, enabling the system to dynamically balance sensing accuracy and computing load in different scenarios, and achieve self-adjustment of resource allocation and efficient collaboration of multi-source sensing.

[0171] 4) The state estimation method based on dynamic weights and robust kernel functions can effectively suppress the influence of sensor data anomalies or noise, maintain the continuity and numerical stability of state estimation, and achieve consistent fusion and reliable output of multi-source observation data.

[0172] Figure 2 A schematic diagram of the structure of the device provided in an embodiment of this disclosure is shown.

[0173] See Figure 2 The robot environment perception device based on multi-sensor fusion provided in this disclosure embodiment may include: a sensor input module 201, a spatiotemporal adaptive synchronization module 202, a reliability dynamic evaluation module 203, a self-organizing perception scheduling module 204, a weighted consistency fusion module 205, and an environment modeling and output module 206. The sensor input module 201 is used to receive raw observation data from various sensors configured on the robot body; The spatiotemporal adaptive synchronization module 202 is used to obtain the equivalent observation data of each sensor in the global system time based on the time drift model of each sensor and the original observation data of each sensor. The reliability dynamic evaluation module 203 is used to calculate the health index of each sensor based on the equivalent observation data of each sensor, and generate the time-varying reliability weight of each sensor based on the health index of each sensor. The self-organizing perception scheduling module 204 is used to determine an active sensor subset based on the health indicators of each sensor, wherein the active sensor subset includes the sensors that need to participate in data fusion at the current moment. The weighted consistency fusion module 205 is used to determine the state vector of the robot system based on the time-varying confidence weights of each sensor in the activated sensor subset and the equivalent observation data. The environment modeling and output module 206 is used to generate environment representation data based on the state vector of the robot system, the time-varying confidence weights of each sensor and the equivalent observation data. The environment representation data includes an environment grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

[0174] Furthermore, the spatiotemporal adaptive synchronization module 202 can be specifically used to: update the time drift model of each sensor, wherein the time drift model indicates the deviation relationship between the local time of the sensor and the global time of the system; and, given the global time of the system, calculate the equivalent observation data of each sensor based on the time drift model of each sensor and the original observation data. The time drift model is defined as follows: ,in, This represents the local time of the i-th sensor. The amount of drift relative to global time. Let i be the time drift rate coefficient of the i-th sensor. Let be the initial time bias constant of the i-th sensor.

[0175] Furthermore, the reliability dynamic evaluation module 203 can also be used to generate an anomaly flag signal for the sensor when one of the following conditions is met: the time consistency residual of the sensor is calculated based on the equivalent observation data of the sensor, and the time consistency residual of the sensor exceeds a preset time consistency residual threshold; the health index of the sensor is lower than a preset health index threshold; for the sensor marked by the anomaly flag signal, the time-varying reliability weight of the sensor is adjusted according to the following formula. Perform temporary attenuation: ,in, Let be the weight decay coefficient, satisfying ; This represents the time-varying confidence weight of the i-th sensor before attenuation. This represents the time-varying confidence weight of the i-th sensor after attenuation.

[0176] Furthermore, the self-organizing perception scheduling module 204 can be specifically used to: determine the proportion of dynamic scenes, image texture complexity, and point cloud sparsity based on the equivalent observation data of each sensor; calculate the environmental complexity index based on the proportion of dynamic scenes, image texture complexity, and point cloud sparsity; monitor the system resource usage status to determine the available computing budget of the system; record the unit computing cost and unit power consumption cost of each sensor; construct an optimization model based on the health index, using the available computing budget of the system as the budget constraint, and using the environmental complexity index, the health index of each sensor, the computing cost, and the power consumption cost as variables; and execute the optimization model solution to select sensors to form the active sensor subset.

[0177] Furthermore, the self-organizing perception scheduling module 204 can also be used to: monitor the fusion residual increment in real time; if the fusion residual increment exceeds a preset fusion residual increment threshold, temporarily add a suboptimal sensor to the candidate list and re-execute the optimization model solution to update the active sensor subset.

[0178] Furthermore, the environment modeling and output module 206 can be specifically used to: update the occupancy probability of each cell in the environment grid map in logarithmic probability form based on the time-varying confidence weights of each sensor and equivalent observation data, wherein the occupancy probability represents the possibility that the corresponding position of the cell is occupied by an obstacle; determine a set of drivable areas indicating drivable areas according to a preset occupancy probability threshold and the occupancy probability of each cell; and generate environment representation data, wherein the environment representation data includes at least the environment grid map updated with the occupancy probability of each cell and the set of drivable areas.

[0179] Furthermore, the robot environment perception device based on multi-sensor fusion may also include: an obstacle detection module 207, which is used to determine the set of obstacle targets based on the occupancy probability of each grid element, perform weighted multi-sensor nearest neighbor association and threshold detection on the set of obstacle targets, and perform observation fusion according to the time-varying confidence weight of each sensor within the Mahalanobis distance gate to perform real-time tracking of dynamic obstacles.

[0180] Furthermore, the robot environment perception device based on multi-sensor fusion may also include: a parameter update module 208, which is used to update the extrinsic parameter matrix and time bias parameter of each sensor based on the equivalent observation data of each sensor and the state vector of the robot system using a Lie algebra perturbation model.

[0181] In practical applications, robot environmental perception devices based on multi-sensor fusion can be implemented as software, hardware, or a combination of both. In some examples, robot environmental perception devices based on multi-sensor fusion can be implemented as follows: Figure 3 The electronic device in the system shown or deployed in the electronic device.

[0182] Figure 3 A schematic structural diagram of a robot environmental perception system based on multi-sensor fusion, provided in an embodiment of this disclosure, is shown. See also... Figure 3 The robot environment perception system based on multi-sensor fusion provided in this disclosure embodiment may include: a sensor group 301 and an electronic device 302. The sensor group 301 includes two or more sensors, and the electronic device 302 includes a processor and a memory. The memory stores a computer program, and when the computer program is run by the processor, the processor executes the aforementioned robot environment perception method based on multi-sensor fusion.

[0183] In some examples, sensor group 301 may include at least two of the following: lidar, camera, millimeter-wave radar, inertial measurement unit, and ultrasonic sensor. Figure 3 In the example, the robot environment perception system based on multi-sensor fusion includes a lidar 3011, a camera 3012, a millimeter-wave radar 3013, an inertial measurement unit 3014, and an ultrasonic sensor 3015.

[0184] Electronic device 302 may include one or more processors and memory.

[0185] A processor can be a central processing unit (CPU) or other form of processing unit with data processing and / or instruction execution capabilities, and can control other components in an electronic device to perform desired functions.

[0186] The memory may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and a processor may execute the program instructions to implement the methods of the various embodiments of this disclosure described above and / or other desired functions.

[0187] Depending on the specific application, electronic devices may also include any other suitable components.

[0188] In addition to the methods and apparatus described above, embodiments of this disclosure may also be computer program products comprising computer program instructions that, when executed by a processor, cause the processor to perform the steps of the methods according to various embodiments of this disclosure as described in the "Exemplary Methods" section above.

[0189] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this disclosure. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on a user's computing device, partially on a user's computing device, as a standalone software package, partially on a user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0190] Furthermore, embodiments of this disclosure may also be computer-readable storage media having computer program instructions stored thereon, which, when executed by a processor, cause the processor to perform the steps in the methods according to various embodiments of this disclosure described in the "Exemplary Methods" section above.

[0191] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may be, for example, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof.

[0192] The above description has been given for purposes of illustration and description. Furthermore, this description is not intended to limit the embodiments of this disclosure to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations therein.

Claims

1. A robot environment perception method based on multi-sensor fusion, characterized in that, include: Receive raw observation data from various sensors configured on the robot body; Based on the time drift model of each sensor and the original observation data of each sensor, the equivalent observation data of each sensor in the global system time are obtained; The health index of each sensor is calculated based on the equivalent observation data of each sensor, and the time-varying reliability weight of each sensor is generated based on the health index of each sensor. A subset of active sensors is determined based on the health indicators of each sensor, and the subset of active sensors includes the sensors that need to participate in data fusion at the current moment; The state vector of the robot system is determined based on the time-varying confidence weights and equivalent observation data of each sensor in the activated sensor subset. Environmental representation data is generated based on the state vector of the robot system, the time-varying confidence weights of each sensor, and the equivalent observation data. The environmental representation data includes an environmental grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

2. The method according to claim 1, characterized in that, The equivalent observation data of each sensor at the system's global time, obtained based on the time drift model of each sensor and the raw observation data of each sensor, includes: Update the time drift model of each sensor, which indicates the deviation between the local time of the sensor and the global time of the system; Given the global system time, the equivalent observation data of each sensor is calculated based on the time drift model of each sensor and the raw observation data. The time drift model is defined as follows: ， in, This represents the local time of the i-th sensor. The amount of drift relative to global time. Let i be the time drift rate coefficient of the i-th sensor. Let be the initial time bias constant of the i-th sensor.

3. The method according to claim 1, characterized in that, The method further includes: An anomaly flag signal is generated for the sensor when one of the following conditions is met: the time consistency residual of the sensor is calculated based on the equivalent observation data of the sensor, and the time consistency residual of the sensor exceeds a preset time consistency residual threshold; or the health index of the sensor is lower than a preset health index threshold. For the sensor marked with the aforementioned anomaly flag signal, the time-varying reliability weight of the sensor is determined according to the following formula. Perform temporary attenuation: ， in, Let be the weight decay coefficient, satisfying ; This represents the time-varying confidence weight of the i-th sensor before attenuation. This represents the time-varying confidence weight of the i-th sensor after attenuation.

4. The method according to claim 1, characterized in that, The step of determining the active sensor subset based on the health indicators of each sensor includes: The proportion of dynamic scenes, image texture complexity, and point cloud sparsity are determined based on the equivalent observation data of each sensor, and the environmental complexity index is calculated based on the proportion of dynamic scenes, image texture complexity, and point cloud sparsity. Monitor system resource usage to determine the available computing budget for the system, and record the unit computing cost and unit power consumption cost of each sensor; Based on the health index, an optimization model is constructed with the available computing budget of the system as the budget constraint, and the environmental complexity index, the health index of each sensor, the computing cost, and the power consumption cost as variables. The optimization model is then executed to select sensors to form the active sensor subset.

5. The method according to claim 4, characterized in that, The method further includes: Real-time monitoring of fusion residual increments; If the fusion residual increment exceeds a preset fusion residual increment threshold, a suboptimal sensor is temporarily added to the candidate list and the optimization model is re-executed to update the active sensor subset.

6. The method according to claim 1, characterized in that, The generation of environmental representation data based on the state vector of the robot system and the equivalent observation data of each sensor includes: Based on the time-varying confidence weights of each sensor and equivalent observation data, the occupancy probability of each cell in the environmental grid map is updated in logarithmic probability form, whereby the occupancy probability represents the probability that the corresponding position of the cell is occupied by an obstacle. The set of drivable regions indicating drivable regions is determined based on a preset occupancy probability threshold and the occupancy probability of each cell. Generate environment representation data, which includes at least the environment raster map updated with the occupancy probability of each raster and the set of drivable areas.

7. The method according to claim 6, characterized in that, The method further includes: determining the obstacle target set based on the occupancy probability of each grid cell, performing weighted multi-sensor nearest neighbor association and threshold detection on the obstacle target set, and performing observation fusion according to the time-varying confidence weight of each sensor within the Mahalanobis distance gate to perform real-time tracking of dynamic obstacles.

8. The method according to claim 1, characterized in that, The method further includes updating the extrinsic parameter matrix and time bias parameter of each sensor based on the equivalent observation data of each sensor and the state vector of the robot system using a Lie algebra perturbation model.

9. A robot environmental perception device based on multi-sensor fusion, characterized in that, The robot environment perception device based on multi-sensor fusion includes: The sensor input module is used to receive raw observation data from various sensors configured on the robot body; The spatiotemporal adaptive synchronization module is used to obtain the equivalent observation data of each sensor in the global system time based on the time drift model of each sensor and the original observation data of each sensor. The reliability dynamic evaluation module is used to calculate the health index of each sensor based on the equivalent observation data of each sensor, and generate the time-varying reliability weight of each sensor based on the health index of each sensor. The self-organizing perception scheduling module is used to determine the active sensor subset based on the health indicators of each sensor. The active sensor subset includes the sensors that need to participate in data fusion at the current moment. The weighted consistency fusion module is used to determine the state vector of the robot system based on the time-varying confidence weights of each sensor in the activated sensor subset and the equivalent observation data. The environment modeling and output module is used to generate environment representation data based on the state vector of the robot system, the time-varying confidence weights of each sensor, and the equivalent observation data. The environment representation data includes an environment grid map updated with the occupancy probability of each grid cell and a set of drivable areas.

10. A robot environmental perception system based on multi-sensor fusion, characterized in that, The system includes: a sensor group and an electronic device, the sensor group including two or more sensors, the electronic device including a processor and a memory, the memory storing a computer program, the computer program causing the processor to perform the method as described in any one of claims 1-8 when executed by the processor.

Images