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

Abstract

The present disclosure provides a robot environment perception method, device and system based on multi-sensor fusion. The method of the present disclosure can comprise: receiving raw observation data from each sensor configured on the robot body, obtaining equivalent observation data of each sensor at the system global time based on the time drift model of each sensor and the raw observation data of each sensor, calculating a health degree index of each sensor and generating a time-varying confidence weight of each sensor according to the health degree index, determining an active sensor subset according to the health degree index of each sensor, determining a state vector of the robot system according to the time-varying confidence weight and the equivalent observation data of each sensor in the active sensor subset, and generating environment representation data based on the state vector of the robot system, the time-varying confidence weight and the equivalent observation data of each sensor. The present disclosure can effectively avoid the influence of time synchronization error and spatial registration deviation between sensors, and improve the perception accuracy.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of robots, and more particularly, to a robot environment perception method, device and system based on multi-sensor fusion. BACKGROUND

[0002] In recent years, robots are increasingly widely used in the fields of autonomous driving, unmanned delivery, service inspection, etc. As the core foundation for realizing autonomous decision-making and safe operation of robots, environment perception technology has attracted widespread attention. At present, the robot environment perception system generally adopts a multi-sensor fusion architecture, which integrates the environment feature data obtained by various sensors such as laser radar, camera, inertial measurement unit, millimeter wave radar, etc., and completes the state estimation and map construction of the system based on filtering algorithms or feature matching algorithms.

[0003] To improve the accuracy and robustness of the perception system, related technologies usually perform offline calibration before system deployment, and use a preset fixed extrinsic parameter and static time synchronization strategy to map the data from different sensors to a unified coordinate system and time reference to achieve effective fusion of multi-sensor data. However, such methods have obvious limitations in long-term actual operation: they rely on manual calibration and static parameter assumptions, and are difficult to adapt to internal and external parameter drift caused by factors such as device aging, temperature changes or mechanical vibrations, resulting in time synchronization errors and spatial registration deviations between sensors accumulating over time, and thus affecting the perception accuracy and consistency of the system.

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

[0005] To solve the above technical problems, the embodiments of the present disclosure provide a robot environment perception method, device and system based on multi-sensor fusion.

[0006] According to one aspect of the present disclosure, a robot environment perception method based on multi-sensor fusion is provided, comprising:

[0007] receiving raw observation data from each sensor configured on the robot body;

[0008] obtaining equivalent observation data of each sensor at the system global time based on the time drift model of each sensor and the raw observation data of each sensor;

[0009] calculating a health index of each sensor according to the equivalent observation data of each sensor, and generating a time-varying confidence weight of each sensor according to the health index of each sensor;

[0010] determining an active sensor subset according to the healthiness indicators of the sensors, the active sensor subset including sensors that need to participate in data fusion at a current time instant;

[0011] determining a state vector of the robot system according to the time-varying credibility weights and the equivalent observation data of the sensors in the active sensor subset;

[0012] generating environment representation data based on the state vector of the robot system, the time-varying credibility weights and the equivalent observation data of the sensors, the environment representation data including an environment grid map with updated occupancy probabilities of each cell and a set of drivable regions.

[0013] In some embodiments, the obtaining the equivalent observation data of the sensors at the system global time based on the time drift model of the sensors and the raw observation data of the sensors includes:

[0014] updating the time drift model of the sensors, the time drift model indicating a deviation relationship between the local time of the sensors and the system global time;

[0015] calculating the equivalent observation data of the sensors based on the time drift model of the sensors and the raw observation data of the sensors at a given system global time;

[0016] wherein the time drift model is defined as follows:

[0017] wherein, T i represents the sensor local time of the i th sensor a drift amount relative to the global time, is a time drift rate coefficient of the i th sensor, is an initial time bias constant of the i th sensor.

[0018] In some embodiments, the method further includes:

[0019] generating an abnormal flag signal for a sensor when one of the following conditions is met: a time consistency residual of the sensor calculated based on the equivalent observation data of the sensor exceeds a preset time consistency residual threshold; the healthiness indicator of the sensor is lower than a preset healthiness indicator threshold;

[0020] temporarily attenuating the time-varying credibility weight of the sensor according to the following formula for the sensor marked by the abnormal flag signal:

[0021] wherein, is a weight attenuation coefficient, satisfying ;​ denotes the time-varying confidence weight of the i-th sensor before attenuation, denotes the time-varying confidence weight of the i-th sensor after attenuation.

[0022] In some embodiments, the determining the activated sensor subset according to the health indicators of the sensors comprises: determining a dynamic scene proportion, an image texture complexity, and a point cloud sparsity according to the equivalent observation data of the sensors, calculating an environment complexity indicator based on the dynamic scene proportion, the image texture complexity, and the point cloud sparsity, monitoring a system resource usage state to determine a system available computing budget, recording a unit computing cost and a unit power consumption cost of each sensor, constructing an optimization model based on the health indicators, with the system available computing budget as a budget constraint, and with the environment complexity indicator and the health indicators, the computing cost, and the power consumption cost of each sensor as variables, and executing the optimization model to select sensors to form the activated sensor subset.

[0023] In some embodiments, the method further comprises: monitoring a fusion residual increment in real time, and temporarily adding a suboptimal sensor in the alternative list and re-executing the optimization model to update the activated sensor subset if the fusion residual increment exceeds a preset fusion residual increment threshold.

[0024] In some embodiments, the generating the environment representation data based on the state vector of the robot system and the equivalent observation data of the sensors comprises: updating an occupancy probability of each grid cell in an environment grid map in a logarithmic likelihood form based on the time-varying confidence weight and the equivalent observation data of each sensor, the occupancy probability representing a possibility that a corresponding position of the grid cell is occupied by an obstacle, determining a drivable region set indicating a drivable region according to a preset occupancy probability threshold and the occupancy probability of each grid cell, and generating the environment representation data, the environment representation data at least including the environment grid map with the updated occupancy probability of each grid cell and the drivable region set.

[0025] In some embodiments, the method further comprises: determining an obstacle target set according to 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 a Mahalanobis distance threshold to perform real-time tracking of a dynamic obstacle.

[0026] In some embodiments, the method further comprises: updating an extrinsic matrix and a time bias parameter of each sensor based on the equivalent observation data of the sensors and the state vector of the robot system using a Lie algebra perturbation model.

[0027] According to an aspect of the present disclosure, there is provided a robot environment perception device based on multi-sensor fusion, comprising:

[0028] a sensor input module configured to receive raw observation data from each sensor arranged on the robot body;

[0029] a space-time adaptive synchronization module configured to obtain equivalent observation data of each sensor in the global time of the system based on the time drift model of each sensor and the raw observation data of each sensor;

[0030] a credibility dynamic evaluation module configured to calculate a health index of each sensor according to the equivalent observation data of each sensor, and generate a time-varying credibility weight of each sensor according to the health index of each sensor;

[0031] a self-organizing perception scheduling module configured to determine an active sensor subset according to the health index of each sensor, the active sensor subset containing sensors that need to participate in data fusion at the current time;

[0032] a weighted consistency fusion module configured to determine a state vector of the robot system according to the time-varying credibility weight and the equivalent observation data of each sensor in the active sensor subset;

[0033] an environment modeling and output module configured to generate environment representation data based on the state vector of the robot system, the time-varying credibility weight and the equivalent observation data of each sensor, the environment representation data including an environment grid map in which the occupancy probability of each cell is updated and a set of drivable regions.

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

[0035] The embodiments of the present disclosure can maintain the continuity and consistency of each sensor data in the same coordinate system and time reference when the device drifts or the environment changes, ensure the accuracy and long-term stability of multi-source data fusion, weaken the influence of long-term accumulation of time synchronization errors and space registration deviations between sensors, and effectively improve the perception accuracy and consistency of the robot system. BRIEF DESCRIPTION OF DRAWINGS

[0036] Figure 1 is a flowchart of a robot environment perception method based on multi-sensor fusion provided by the embodiments of the present disclosure;

[0037] Figure 2 is a structural schematic diagram of a robot environment perception device based on multi-sensor fusion provided by the embodiments of the present disclosure;

[0038] Figure 3 is a schematic structural block diagram of a robot environment perception system based on multi-sensor fusion provided by an embodiment of the present disclosure. DETAILED DESCRIPTION

[0039] In the following, example embodiments according to the present disclosure will be described in detail with reference to the accompanying drawings. It is apparent that the described embodiments are only a part of the embodiments of the present disclosure, rather than all the embodiments of the present disclosure, and it should be understood that the present disclosure is not limited to the example embodiments described herein.

[0040] Figure 1 A flowchart of a robot environment perception method based on multi-sensor fusion provided by an embodiment of the present disclosure is shown. Referring to Figure 1 The robot environment perception method based on multi-sensor fusion of the embodiment of the present disclosure includes the following steps:

[0041] Step 101, receiving raw observation data from each sensor configured on the robot body;

[0042] Step 102, obtaining equivalent observation data of each sensor at the system global time based on the time drift model of each sensor and the raw observation data of each sensor;

[0043] Step 103, calculating the health index of each sensor according to the equivalent observation data of each sensor, and generating the time-varying confidence weight of each sensor according to the health index of each sensor;

[0044] Step 104, determining an active sensor subset according to the health index of each sensor, the active sensor subset containing sensors that need to participate in data fusion at the current time;

[0045] Step 105, determining the state vector of the robot system according to the time-varying confidence weight and the equivalent observation data of each sensor in the active sensor subset;

[0046] Step 106, generating environment representation data based on the state vector of the robot system, the time-varying confidence weight and the equivalent observation data of each sensor, the environment representation data including an environment grid map with updated occupancy probability of each grid cell and a set of drivable areas.

[0047] In step 101, the raw observation data from each sensor is received and cached and time-stamped. For example, the raw observation data of each sensor can be stored in the cache area in the order of sampling time for subsequent calling.

[0048] 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.

[0049] 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.

[0050] 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.

[0051] Specifically, an exemplary implementation of step 102 may include the following steps a1 to a5:

[0052] Step a1: Update the time drift model of the sensor based on the sliding time window.

[0053] 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:

[0054] ,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.

[0055] 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 The least square fitting is performed and the fitting result is stored in the parameter buffer to update the time drift model over time. The fitting result is used to update the model parameters With That is, the slope and intercept obtained by fitting are used to replace the old parameters to achieve adaptive update of the time drift model. Here, the time sampling samples are a set of data pairs composed of observation pairs between the time stamps of each sensor and the global time synchronization reference signal in the sliding time window, including the sampling time, local time stamp, system global time and corresponding drift difference.

[0056] Step a2, based on the time drift model of each sensor and the original observation data, the equivalent observation data of each sensor is calculated under the given system global time.

[0057] Specifically, a prediction interpolation operation is performed when calculating the equivalent observation, and the prediction interpolation operation is used to reconstruct the synchronization data frame of each sensor under the unified global time for subsequent use. Here, the "synchronization data frame" refers to a data collection unit formed by observation data from different sensors under the same global time reference and the same spatial reference system after time drift model correction and spatial coordinate transformation processing. It is a kind of data structure that has been aligned in time and space, which is used to ensure that the multi-sensor perception information has consistent space-time semantics in the process of fusion, modeling and subsequent calculation.

[0058] For the sensor , if the sensor times of its last two frames of data are and , the corresponding system global times and can be calculated by the following formula:

[0059] ,

[0060] The equivalent observation data of the sensor at the global time can be determined by the following formula:

[0061] ,

[0062] wherein, and respectively represent the original observation data of the th sensor (i.e., the sensor ) at the sensor local time , represents a normalized interpolation coefficient, is an interpolation operator function.

[0063] When​ When in scalar or vector form, the interpolation operator function can be defined as linear interpolation shown in the following equation:

[0064] When represents the attitude quaternion, the interpolation operator function can adopt spherical linear interpolation (SLERP) shown in the following equation:

[0065] Wherein, is the angle between and .

[0066] Taking the observation data of the point cloud type as an example, for the observation data of the point cloud type, rigid body propagation can be performed under the constant speed assumption by using the instantaneous attitude information provided by the inertial measurement unit.

[0067] The spatial coordinate conversion of the point cloud type observation data can be calculated by the following equation:

[0068] Wherein, represents the point cloud coordinate set at the global time , and represents the rigid body transformation matrix of the sensor coordinate system relative to the robot coordinate system .

[0069] Wherein, represents the point cloud coordinate set collected by the sensor at the local time , containing a plurality of three-dimensional points . After the coordinate transformation, the set can be mapped to the point cloud set at the global time t.

[0070] Wherein, represents the rigid body transformation matrix of the sensor coordinate system relative to the robot coordinate system , including rotation and translation. The inverse matrix is used to convert the point cloud coordinates from the sensor coordinate system to the robot coordinate system

[0071] , so as to realize the spatial correspondence of the observation data at each time point in the robot system.

[0072] For non-point cloud type observation data such as image feature points, inertial measurement unit data, etc., coordinate mapping can also be performed according to the extrinsic matrix of the corresponding sensor to realize alignment of data under the same space reference. The coordinate conversion of point cloud data is used to realize geometric layer alignment, and the conversion of non-point cloud data can be used to realize the consistency of semantics and motion information.

[0073] From the above, by modeling and compensating the time drift of the sensor, the fusion error caused by asynchronous sampling and time delay of the sensor is solved.

[0074] Further, step 102 can also include converting the equivalent observation data of each sensor to a unified world coordinate system. Specifically, the equivalent observation data of each sensor can be converted to the world coordinate system through a spatial transformation matrix of the robot coordinate system to the world coordinate system. In a specific application, the converted equivalent observation data can be uniformly stored in a global observation buffer for subsequent use.

[0075] Here, the spatial transformation matrix can be defined as

[0076] , wherein is the pose transformation matrix of the robot coordinate system relative to the world coordinate system, is the extrinsic matrix of the sensor relative to the robot coordinate system, indicates the rigid body transformation matrix of the sensor coordinate system {Si} relative to the world coordinate system {W}, which is used to unify the observation data to the world coordinate system, and the matrix is obtained by multiplying the robot pose transformation matrix and the sensor extrinsic matrix .

[0077] Further, step 102 can also include detecting the continuity of the equivalent observation data of each sensor in the time domain. Specifically, the time consistency residual of each sensor is calculated, and whether the time drift of each sensor is abnormal is determined based on the time consistency residual of each sensor and a preset time consistency residual threshold.

[0078] In some examples, the time consistency residual of each sensor can be calculated by the following formula:

[0079] , wherein is a short-time prediction operator, is a detection interval time. If the time consistency residual of the i-th sensor exceeds the preset time consistency residual threshold, it is considered that the i-th sensor has a time drift anomaly, and an indication of the i-th sensor can be generated. ​​An anomaly flag signal indicating time drift anomaly in 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.

[0080] 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.

[0081] 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.

[0082] 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.

[0083] During system operation, raw observation data of sensors can be continuously received, and equivalent observation data of each sensor can be output in real time. In the embodiments of the present disclosure, by establishing a time drift model, performing prediction interpolation and spatial transformation, synchronization of different types of sensor data under the global time and spatial reference is realized, and accurate multi-source data basis is provided 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 system, spatial transformation can be completed and fixed in the initialization stage and no longer be repeatedly performed. However, in a multi-sensor fusion system, the spatial transformation step is a necessary link.

[0084] In step 103, the health index of each sensor at the current time is calculated according to the equivalent observation data of each sensor, and a time-varying credibility weight is generated to adjust the influence of each sensor on state estimation in the subsequent fusion process. That is, based on the equivalent observation data set of each sensor and the time consistency residual error of each sensor, the time-varying credibility weight of each sensor is determined. .

[0085] Specifically, the signal-to-noise ratio index, the inter-frame continuity index, the effective coverage rate index and the time consistency index of the sensor are determined based on the equivalent observation data of the sensor, the signal-to-noise ratio index, the inter-frame continuity index, the effective coverage rate index and the time consistency index of the sensor are weighted and summed to determine the health index of the sensor, and the health index of the sensor is normalized to obtain the time-varying credibility weight of the sensor. The value range of the time-varying credibility weight is between 0 and 1.

[0086] In some examples, the health index can be calculated by the following formula.

[0087] ,

[0088] wherein, are weight coefficients of the signal-to-noise ratio index, the inter-frame continuity index, the effective coverage rate index and the time consistency index respectively, and satisfy .

[0089] The signal-to-noise ratio index represents the signal-to-noise ratio index, and is used to represent the intensity stability of the sensor signal. It is the normalized result of the ratio of the effective signal amplitude to the background noise variance in the equivalent observation data.

[0090] The inter-frame continuity index represents the inter-frame continuity index, and reflects the continuity degree between adjacent time observation data. It is calculated as the Euclidean distance between the current frame equivalent observation data and the last frame equivalent observation data.

[0091] represents an effective coverage indicator, which is a ratio of the effective observation number of the sensor at the current time to the nominal observation number thereof.

[0092] represents a time consistency indicator, which can be calculated according to the time consistency residual of the sensor . Specifically, the time consistency residual of the sensor can be calculated by the following formula:

[0093] , wherein, is a time consistency decay coefficient. represents the time consistency indicator of the i-th sensor, represents the time consistency residual of the i-th sensor.

[0094] The time consistency decay coefficient can be preset according to the desired time response sensitivity of the system, or obtained through calibration in the system initialization stage. The time sampling frequency and clock stability of different sensors are different, and the decay coefficients of the sensors can be independently set . For high-frequency sensors, a smaller decay coefficient is taken to enhance the tolerance of short-time residual; for low-frequency or large-delay sensors, a larger decay coefficient is taken to enhance the abnormality detection sensitivity. If independent calibration is not performed, a unified empirical value = 0.1-0.5 can be configured.

[0095] In specific applications, the inter-frame continuity indicator , the effective coverage indicator , and the time consistency indicator are all calculated based on the equivalent observation data, and the results are not directly included in the equivalent observation data itself, but stored in the metadata field of the synchronization data frame as an auxiliary quality evaluation quantity of the observation data. When calculating the health degree indicator of the sensor, the system can generate the time-varying credibility weight of the sensor by reading the evaluation quantities in the synchronization data frame.

[0096] In some examples, the health degree indicator values of the sensors can be normalized to generate the time-varying credibility weight by the following formula.

[0097] , wherein, is the number of sensors participating in perception, that is, the total number of sensors installed on the robot body; is a small constant to prevent the denominator from being zero, represents the health degree indicator of the i-th sensor, i = 1, 2, …, M. represents the time-varying credibility weight of the i-th sensor, , the value range of which is between 0 and 1, and the time-varying credibility weight of the sensor The larger the value, the higher the reliability of the sensor at the current time.

[0098] Further, step 103 can also include automatically reducing the time-varying credibility weight of the corresponding sensor when it is detected that the health index of the sensor suddenly drops or the time consistency residual exceeds the preset time consistency residual threshold, and selectively issuing an alarm signal as needed. That is, during operation, abnormal sensors are identified based on the health index or the time consistency residual of the sensors, and the time-varying credibility weight of the abnormal sensors is automatically reduced. The alarm signal is a logical flag used for abnormal identification and management within the system, and is used to indicate that the health index or the time consistency residual of a sensor exceeds the preset time consistency residual threshold. When the flag is triggered, the system automatically reduces the time-varying credibility weight of the sensor and suspends its data from participating in fusion calculation. At the same time, the alarm signal can be used to trigger log recording, state diagnosis, or safety policy response of the upper layer system. The alarm signal is only an internal logical output and does not involve external audible and visual alarms.

[0099] Specifically, when it is detected that the time consistency residual of a sensor exceeds the preset time consistency residual threshold, the time-varying credibility weight of the sensor is automatically reduced and a re-evaluation process is triggered to realize self-diagnosis and self-repair.

[0100] Here, the "re-evaluation process" refers to the fact that after detecting an abnormal sensor, the system re-calculates the health index and the time-varying credibility weight of each sensor in the subsequent sliding time window to verify whether the abnormal state persists. If the abnormality is eliminated for a continuous number of 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 index.

[0101] When it is detected that the health index value of a sensor is lower than the preset health index threshold or the time consistency residual exceeds the preset time consistency residual threshold , an abnormal flag signal is generated to mark the abnormal sensor.

[0102] In some examples, the time-varying credibility weight of the abnormal sensor can be automatically reduced by temporarily attenuating the weight of the sensor marked with the abnormal flag signal according to the following formula:

[0103] wherein, is a weight attenuation coefficient, satisfying . represents the time-varying credibility weight of the i-th sensor before attenuation.​ denotes the decayed time-varying reliability weight of the i-th sensor.

[0104] weight decay coefficient can be used to control the weight drop rate of the abnormal sensor after detecting the abnormality, and its value range is 0 <1. In some examples, the coefficient can be set according to the observation frequency and abnormality duration characteristics of different sensors. For high-frequency and low-delay sensors such as IMUs, a smaller weight decay coefficient (such as 0.8-0.9) can be selected to avoid excessive weight decay caused by short-term jitter; for low-frequency or poor stability sensors such as vision sensors, a larger decay coefficient (such as 0.5-0.7) can be selected to speed up the abnormal isolation response. In other examples, if the system does not perform individual calibration, the weight decay coefficients of all sensors can be uniformly set to an empirical value (for example, 0.7). During operation, the weight decay coefficient can be adaptively adjusted through the statistical results of the number of abnormalities in the sliding time window to achieve dynamic balance of multi-sensor weights.

[0105] Further, the method of the embodiments of the present disclosure can also include: the time-varying reliability weight of the sensor can be restored after the abnormal state of the sensor is removed. Specifically, the health index average value of the sensor can be calculated in a fixed recovery detection window When the condition is met, the time-varying reliability weight of the sensor is restored according to the following formula.

[0106] wherein, is the normalized result corresponding to the historical health index average value, is a preset recovery threshold. denotes the time-varying reliability weight of the restored sensor.

[0107] In step 103, the health index calculation and time-varying reliability weight updating operation can be performed on all sensors once every detection period, and a data structure containing the identifiers of each sensor, the health index value, the time-varying reliability weight, and the abnormal state signal for the sensor is generated for subsequent processing.

[0108] ​Here, the detection period is a time interval parameter in the system for periodically performing health index calculation and weight update operations, which can be set according to the system clock accuracy and real-time requirement of the task. In typical applications, the detection period is in the range of 100 ms to 1 s, which is used to avoid excessive calculation load while maintaining the real-time performance of the system. The period can be a fixed value, or it can be adaptively adjusted by the system according to the sensor sampling frequency and state change rate. When each detection period arrives, the system performs health index calculation, time-varying reliability weight update, and abnormal state signal writing operations for all sensors.

[0109] In the embodiments of the present disclosure, through the health index calculation, time-varying reliability weight processing, abnormality detection and self-repair mechanism of step 103, dynamic quantification and real-time adjustment of sensor reliability can be realized, providing a reliable basis for subsequent sensor scheduling and fusion calculation.

[0110] In step 104, based on the health index of each sensor Calculation cost Power consumption cost and the current available calculation budget of the system to determine the activated sensor subset , so as to realize the on-demand perception and energy adaptive scheduling of multiple sensors under different environmental complexity, and reduce redundant calculation under the premise of ensuring the integrity of perception.

[0111] Specifically, during the operation of the system, by constructing a scheduling optimization model with environmental complexity index, calculation cost, and power consumption cost as variables, the activated sensor subset participating in fusion is dynamically selected under the premise of meeting the system calculation budget constraint.

[0112] In some examples, the process of determining the activated sensor subset in step 104 can include the following steps b1-b3:

[0113] Step b1, calculating the environmental complexity index of each sensor according to the equivalent observation data of each sensor ;

[0114] Specifically, dynamic scene proportion, image texture complexity, and point cloud sparsity and other environmental feature parameters can be calculated according to the equivalent observation data of each sensor, and the environmental complexity index can be calculated based on these environmental feature parameters.

[0115] Wherein, the environmental complexity index can be calculated by the following formula.

[0116] , wherein, indicates the dynamic scene proportion index, which can be calculated according to the pixel change ratio of moving targets in two consecutive frames of observation data or the amplitude of optical flow field change. 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.

[0117] 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.

[0118] 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

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

[0120] Specifically, the optimization model shown in the following equation is constructed:

[0121] ,

[0122] ,

[0123] 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.

[0124] 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 correction is performed to enhance the fusion proportion of high-reliability sensors in high-complexity scenarios.

[0125] where M is the number of sensors participating in perception, representing the total number of sensors that are currently accessible to the system and participate in environmental perception data collection and fusion computation. 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 state of external sensors. In typical embodiments, M includes multi-modal sensors (such as visual sensors, lidar, inertial measurement units, etc.) configured on the robot body and external collaborative sensors accessed through communication links. When the system does not enable external collaborative perception, M is the number of sensors configured on the robot body.

[0126] where the sensor The computational cost of the sensor is denoted as , with units of computational resources per second; the power consumption cost of the sensor is denoted as , with units of energy units per second. The current system available computation budget is denoted as , representing the allocatable computation resources within the time interval .

[0127] where represents the degree of insufficient perception information under the environmental complexity index , defined as follows:

[0128] where is the health index of the i-th sensor, is a small constant to prevent the denominator from being zero, is the number of sensors participating in perception, and the value of M can be found in the foregoing, which will not be repeated here. where

[0129] is the trade-off coefficient of the optimization objective, representing the importance of environmental information, computation resources, and power consumption constraints, respectively. In specific applications, the value of can be set offline or adjusted online adaptively according to task real-time performance, computation load, and energy consumption budget. Specifically, the optimization model is solved within each scheduling period, which can be completed through a heuristic greedy algorithm, and the specific process includes the following three steps:

[0130] First, arrange all sensors in descending order according to the proportion value

[0131] . Second, select the sensors in order and accumulate the computation cost

[0132] Until the system's available computing budget is reached. ;

[0133] The third step is to select the sensors to form an active sensor subset. .

[0134] 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.

[0135] Real-time monitoring of fusion residual increment Integrating residual increments Defined as follows:

[0136] ,

[0137] 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.

[0138] 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.

[0139] In typical applications It can be obtained from the following formula:

[0140] ,in, These are sensor observations. This is the output for fusion estimation.

[0141] Specifically, the system triggers a sensor dynamic reconfiguration mechanism. The system forms the aforementioned candidate list according to the health index and power consumption cost of the sensors not selected into the active sensor subset in the previous round of optimization solution. The sensors in the candidate list are all available but not activated at the current time. In the dynamic correction, the system selects an inferior sensor (i.e., the sensor with a score value less than the current activation threshold) from the candidate list to temporarily join the active sensor subset to form a new temporary active sensor subset. To verify the impact of the adjustment on the system performance, the optimization model needs to be re-executed based on the new temporary active sensor subset to update the optimal balance state of each trade-off item and ensure that the aforementioned computational budget constraint still holds. If the fusion residual increment of the system after solving is within the range of the fusion residual increment threshold Return to the fusion residual increment threshold , the selected inferior sensor is confirmed to be an effective 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.

[0142] In a specific application, the optimization process of step 104 can be periodically executed during operation, and the active sensor subset can be recalculated immediately when triggered by detection events such as sensor abnormalities or sudden changes in environmental complexity indicators.

[0143] In step 104, after determining the active sensor subset, the output data includes but is not limited to the active state vector of each sensor, the current environmental complexity indicator, the available computational budget of the system, and the active sensor subset. Among them, the available computational budget of the system represents the computational resource budget that can be allocated by the system at the current time, and the active sensor subset represents the activated sensor set obtained by solving the optimization model.

[0144] In the embodiments of the present disclosure, the active sensor subset is determined by constructing an optimization model containing environmental complexity and resource constraints, which realizes the on-demand activation and redundancy control of the sensors. The active sensor subset and the aforementioned time-varying confidence weight can make the system maintain stable environmental perception ability under the condition of limited computational resources.

[0145] In step 105, the weighted consistency fusion of multi-source data is performed under the active sensor subset, that is, the data of the activated sensors are subjected to state estimation and information fusion. Specifically, based on the time-varying confidence weight of each sensor in the active sensor subset and the equivalent observation data , the state vector and the covariance matrix of the robot system are determined.

[0146] In some examples, the process of determining the state vector and the covariance matrix of the robot system in step 105 can include the following steps c1-c3:

[0147] Step cl, establish system state model and define observation relationship.

[0148] Here, the system state model is used to describe the motion and pose change relationship of the robot system over time series, and its state equation can represent the system dynamics evolution, and the observation equation is used to associate the sensor observation data and the system state variable.

[0149] Specifically, the state vector of the robot system is defined as , and the state vector is , which contains position, velocity and attitude variables, and can be represented as follows:

[0150] , wherein represents the position vector, represents the velocity vector, represents the attitude Euler angle quaternion parameter.

[0151] Specifically, the observation equation of each sensor is defined as , which can be represented as follows:

[0152] , wherein is the observation vector of the th sensor, is the observation model function of the th sensor, is the measurement noise term, which is assumed to follow a zero-mean Gaussian distribution. The covariance matrix can be estimated by statistical estimation of historical observation data of the sensor or online recursive estimation, and its amplitude can be adjusted in real time in combination with the health index to reflect the change of observation accuracy.

[0153] Step c2, perform weighted least squares solution according to the time-varying credibility weight of each sensor to determine the state increment.

[0154] Specifically, a weighted objective function is constructed, and the minimization of the weighted objective function is solved by iterative optimization to determine the state increment .

[0155] In some examples, the weighted objective function can be represented as follows:

[0156] , wherein . is the covariance matrix of the sensor . represents the observation vector after time synchronization and spatial registration, and is the input observation model function. The actual measurement input of the sensor corresponds to the original observation data.the observation model function of the sensor , the observation noise covariance matrix of the sensor , which can be obtained from the measurement error statistics of the sensor in the calibration stage and dynamically adjusted according to the time-varying credibility weight during operation.

[0157] In some examples, a weighted least squares solution method containing an M-estimation kernel function can be used, and the measurement information matrix is dynamically adjusted according to the time-varying credibility weight of each sensor in the fusion process to reduce the influence of abnormal observations on the state estimation result. The measurement information matrix refers to the inverse matrix of the observation noise covariance matrix, which is used for quantitative description of the observation credibility in the weighted least squares estimation. The system can dynamically adjust the diagonal elements of the matrix according to the time-varying credibility weight of each sensor to achieve abnormal observation suppression.

[0158] In some examples, in step 105, an M-estimation kernel function can be introduced into the weighted objective function to modify the weighted residual term, so as to adjust the weighting mechanism of the weighted objective function when there are abnormal values in the observation data, and reduce the influence of abnormal observations on the solution result. When the time consistency residual or health index of the sensor observation data is lower than a preset threshold, it is considered that there is an abnormal value. The system modifies the weighted objective function by introducing the M-estimation kernel function in this case to reduce the influence of abnormal observations on the state estimation.

[0159] The weighted objective function with the M-estimation kernel function is expressed as follows:

[0160] ,

[0161] wherein the kernel function is a kind of robust weighting function, which is used for nonlinear suppression of observation residuals in the weighted least squares estimation, thereby reducing the influence of abnormal observations on the estimation result, satisfying and having a suppression effect on large residuals. the observation vector of the sensor . the observation model function of the sensor , which can be obtained according to the imaging geometry model, the ranging model of the sensor or through a data-driven learning method. the observation noise covariance matrix, which can be determined according to the measurement error statistics of the sensor in the calibration stage or the real-time estimation result in operation, the time-varying credibility weight of the sensor .

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

[0163] ,

[0164] 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.

[0165] In some examples, state increment It can be expressed as the following formula:

[0166] ,

[0167] in, Indicates the first The Jacobian matrix of observations from each sensor, , Indicates the first The weighted observation residual vector of each sensor.

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

[0169] Specifically, it can be based on the following formula according to the state increment. Update the state vector:

[0170] 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.

[0171] 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. :

[0172] ,

[0173] in, a time-varying confidence weight for the i-th sensor, denotes the Jacobian matrix of the observation model with respect to the state variable, and Rᵢ(t) is the observation noise covariance matrix.

[0174] Secondly, the equivalent information matrix is calculated based on the following formula The covariance matrix of the robot system is calculated.

[0175] In a specific application, during the operation of the system, step 105 can perform an iterative solving process at a fixed frequency to ensure that the state estimation maintains time continuity and information consistency under the condition of multi-sensor heterogeneous input. After each calculation period, step 105 can output a fusion result containing the state vector the covariance matrix and the fusion residual statistics, wherein the fusion residual statistics ΔR(t) reflect the overall consistency of the multi-sensor weighted fusion, and can be used as the confidence basis for updating the environment state.

[0176] In the embodiments of the present disclosure, by introducing the time-varying confidence weight and the kernel function correction in step 105, the weighted fusion of multi-sensor data under the unified space-time reference is realized, which can maintain the numerical stability and consistency of the state estimation in the presence of observation abnormalities.

[0177] In step 106, based on the system state vector x(t) and the covariance matrix calculated in step 105, the environment representation data can be generated based on the fusion result of step 105. The environment representation data can be used for navigation, obstacle avoidance, and path planning. Specifically, the occupancy probability of each cell in the environment grid map is updated in the form of logarithmic ratio, and the drivable area is determined and the free space set is output. Here, the free space set refers to the set of space units in the environment grid map that are not occupied by obstacles or dynamic targets and are available for the robot to pass through.

[0178] Specifically, based on the state vector the covariance matrix and the equivalent observation data of each sensor , the environment grid map is updated, the drivable area set is determined, and the environment update state flag is generated. The activated sensor subset is the set of sensors participating in fusion determined in step 104 according to the calculation resource constraint. The environment update state flag can be used to indicate whether the occupancy state of each grid cell changes in the current period, and its value can be "0" indicating no change, "1" indicating new occupancy, and "-1" indicating release.

[0179] Specifically, the exemplary implementation process of step 106 can include the following steps d1-d2:

[0180] Step d1, establish and maintain the environment grid map, update the occupancy probability of each grid cell in the environment grid map in the form of logarithmic odds based on the time-varying credibility weight and equivalent observation data of each sensor.

[0181] Firstly, a discrete three-dimensional grid structure is used to model the environment space to construct the environment grid map, in which the entire environment space is divided into a set of equidistant elements Each grid cell in the set of equidistant elements corresponds to an occupancy probability , which represents the possibility of the position being occupied by an obstacle.

[0182] Secondly, to avoid numerical instability caused by direct multiplication of probabilities, the occupancy log-odds of each grid is updated in the form of log-odds.

[0183] The log-odds is defined as follows:

[0184] wherein is the occupancy log-odds of grid cell .

[0185] Whenever new observations arrive, the occupancy probability of each grid cell is updated in the form of log-odds, and its update equation is as follows:

[0186] ,

[0187] wherein is the time-varying credibility weight of sensor , is the equivalent observation data of sensor , is the observation increment function of sensor to grid cell , which is defined as follows:

[0188] After the system state vector x(t) is calculated in step 105, the observation data Zᵢ(t) can be aligned in coordinates and compensated in space and time based on the state estimation result, and then the update parameters Locc and Lfree are calculated, and the environment grid occupancy state is updated;

[0189] ,

[0190] wherein represents the occupancy increment , and represents the free increment. and are occupancy update parameters, which can be obtained through experimental calibration according to the ranging accuracy of the system sensor and the environmental noise characteristics.

[0191] Finally, based on the occupancy log-likelihood of each updated grid, the occupancy probability of each grid is obtained by the conversion relationship between log-likelihood and probability of the following formula .

[0192] wherein, represents the occupancy probability of the grid , is the occupancy log-likelihood of the grid .

[0193] Step d2, according to the preset occupancy probability threshold and the occupancy probability of each grid, a drivable region set indicating the drivable region is determined.

[0194] Specifically, the drivable region set is generated based on the occupancy probability of each grid. The occupancy probability threshold is set to be , when the grid is marked as free space; when , the grid is marked as an obstacle, and the drivable region set is formed using the grid marked as free space.

[0195] The drivable region set is defined as follows:

[0196] wherein, represents the set of all grid units in the environment grid map, represents the drivable region set, which is a subset of grid units that meet the occupancy probability threshold condition in the current period.

[0197] In specific applications, the drivable region set 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.

[0198] Step d3, generating environment representation data for calling by path planning and motion control of the robot.

[0199] Here, the environment representation data can at least include the environment grid map in which the occupancy probability of each grid is updated and the drivable region set.

[0200] Further, step 106 can further include: calculating an environment modeling confidence index according to the covariance matrix of the robot system, the environment modeling confidence index being used to reflect the reliability of the current modeling result.

[0201] Specifically, the environment modeling confidence index is defined as follows:

[0202] wherein, denotes the matrix trace operation. The environment modeling confidence indicator Γ(t) ranges from 0 to 1, which is used to reflect the modeling reliability of the current environment grid map and the set of feasible regions.

[0203] If the environment modeling confidence indicator Γ(t) is less than a preset confidence threshold Γ_min, the system suspends the map writing operation and requests to re-execute step 105 to recalculate the state vector and the covariance matrix of the robot system. The map writing operation is used to write the fused occupancy grid data into the global map storage unit, and the writing is suspended when insufficient confidence is detected to prevent unreliable data from polluting the map.

[0204] The environment representation data obtained in step 106 can be uniformly stored as a data structure M(t), including the environment grid map, the set of feasible regions Ω(t), and the environment modeling confidence indicator Γ(t).

[0205] The update flag is used to indicate whether there is an occupancy state or confidence change in the current period, and its value is 1 for update and 0 for keeping.

[0206] During the operation of the system, the map updating operation can be periodically performed, and the position information of the state vector x(t) is aligned in the global map coordinate system, thereby realizing the unification of local observation and global map.

[0207] Through joint control of the occupancy logit and the confidence indicator, continuous modeling and dynamic updating of the environment are realized, and a unified and quantitative environment modeling basis is provided for robot navigation, obstacle detection, and path planning.

[0208] In the embodiments of the present disclosure, by using the occupancy update mechanism in the form of logit and the output control based on confidence (i.e., the environment modeling confidence indicator), continuous modeling and real-time updating of the environment are realized, and a unified and quantitative environment description basis can be provided for the robot system to perform subsequent dynamic obstacle detection and path planning.

[0209] Further, step 106 can further include: determining a set of obstacle targets according to the occupancy probability of each grid cell, performing weighted multi-sensor nearest neighbor association and threshold detection on the set of obstacle targets, and performing observation fusion according to the time-varying confidence weight of each sensor within the Mahalanobis distance threshold to perform real-time tracking of dynamic obstacles.

[0210] After the environment modeling and the occupancy probability updating, the system can detect potential movable targets by the change of the occupancy probability of the grid at continuous time. Specifically, the movable targets identified in the environment modeling results are detected, associated and tracked, wherein the preliminary identification of the movable targets is based on the significance of the change of the occupancy probability of the grid over time. Thus, the timing state information of the dynamic obstacles is provided in the real-time running process.

[0211] Firstly, based on the grid map matrix of the updated occupancy probability of each grid and the state vector , the dynamic region candidate set is extracted by analyzing the change of the occupancy probability at continuous time.

[0212] The dynamic region determination condition is as follows:

[0213] wherein, is the occupancy probability change of the grid at adjacent time, is the dynamic region detection threshold, which can be adaptively set according to the sensor frame rate and the environment change speed. The grid set satisfying the condition is clustered by the connected domain to form the preliminary obstacle candidate set :

[0214] wherein, is the current number of detected obstacles, represents the kth obstacle.

[0215] Secondly, the observation corresponding relationship of the same obstacle target is established between the multi-sensor observations to determine the paired observation set.

[0216] Specifically, for any two observation sets of sensors and , the Mahalanobis distance between the observations is calculated to obtain the paired observation set:

[0217] ,

[0218] wherein, and are the observation vectors of the first and the second obstacle detected by the first and the second sensor, represents the joint observation covariance matrix, and the joint observation covariance matrix is obtained by weighted superposition of the observation noise covariance matrices of the two sensors, that is, it satisfies the following formula:

[0219] where, , are the observation covariance matrices of the i-th and j-th sensor, respectively. If , it is determined that the two observation sets correspond to the same target. denotes the threshold distance for observation association, which is used to determine whether the observations from different sensors correspond to the same obstacle target, denotes the i-th observation data of the i-th sensor, denotes the i-th observation data of the j-th sensor.

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

[0221] Specifically, the state vector of each obstacle is defined as follows:

[0222] where, denotes the obstacle position coordinates, denotes the velocity components of the obstacle.

[0223] The obstacle state update equation is as follows:

[0224] ,

[0225] The observation equation is as follows:

[0226] where, is the state transition matrix, is the control input matrix, and H is the observation matrix. and denote the process noise and observation noise, respectively, both of which are zero-mean Gaussian white noise with covariance matrices and , respectively. is the system control input term, which is used to describe external influences (such as acceleration control or model correction terms); if there is no explicit control input, G = 0.

[0227] Fourth, the weighted multi-sensor Kalman filtering method can be used to update the obstacle state.

[0228] Specifically, the prediction and update steps of the obstacle state are as follows:

[0229] Prediction can be achieved by the following formula:

[0230] ​​​ ,

[0231] ,

[0232] The update can be achieved using the following formula:

[0233] ,

[0234] ,

[0235] ,

[0236] 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.

[0237] 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.

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

[0239] The obstacle trajectory set consists of several target state sequences, which can be expressed as follows:

[0240] ,

[0241] 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.

[0242] 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 of T can be adaptively set according to the system frame rate and the speed of the obstacle, and a typical value is 3-5 frames.

[0243] Through the foregoing processing, obstacle data can be obtained, which can include but is not limited to an obstacle state set a speed estimation set of the obstacle and an obstacle trajectory set , and is provided to an upper motion planning module in a structured format identified by a time stamp. Here, all the obstacle data can be attached with a state confidence index, which can be calculated from Kalman filtering and the trace value of a covariance matrix, for subsequent path risk assessment.

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

[0245] Further, referring to Figure 1 , the method of the embodiments of the present disclosure can further include: step 107, based on the equivalent observation data of each sensor and the state vector of the robot system, updating the extrinsic parameter matrix and the time bias parameter of each sensor by using a Lie algebra disturbance model, to maintain the spatial consistency and time consistency among the multiple sensors.

[0246] Specifically, based on the equivalent observation data of each sensor and the state vector of the robot system determining the updated value of the extrinsic parameter matrix and the time bias parameter of each sensor.

[0247] First, in the system deployment phase, an initial extrinsic parameter and an initial time bias parameter are provided for each sensor.

[0248] The extrinsic parameter matrix of each sensor is defined as follows:

[0249] ,

[0250] wherein , represents a rotation matrix of the sensor coordinate system relative to the robot coordinate system , and represents a translation vector. The initial extrinsic parameter can be obtained by static calibration or structured light alignment, and is stored in the form of a matrix in the robot system parameter storage area.

[0251] 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 .

[0252] The joint error function is as follows:

[0253] ,

[0254] 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.

[0255] 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:

[0256] ,

[0257] 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.

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

[0259] Specifically, the updated extrinsic parameter matrix can be calculated using the following formula:

[0260] ,

[0261] The formula for updating the time bias parameter is as follows:

[0262] ,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.

[0263] 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.

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

[0265] 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:

[0266] ,

[0267] 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.

[0268] 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".

[0269] 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 value. 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.

[0270] 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.

[0271] 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.

[0272] 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.

[0273] The method of this disclosure embodiment has at least the following beneficial effects:

[0274] 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.

[0275] 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.

[0276] 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.

[0277] 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.

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

[0279] 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.

[0280] The sensor input module 201 is used to receive raw observation data from various sensors configured on the robot body;

[0281] 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.

[0282] The credibility dynamic evaluation module 203 is configured to calculate a health index of each sensor according to the equivalent observation data of the sensor, and generate a time-varying credibility weight of each sensor according to the health index of the sensor.

[0283] The self-organizing perception scheduling module 204 is configured to determine an active sensor subset according to the health index of each sensor, the active sensor subset including sensors that need to participate in data fusion at a current time point.

[0284] The weighted consistency fusion module 205 is configured to determine a state vector of the robot system according to the time-varying credibility weight and the equivalent observation data of each sensor in the active sensor subset.

[0285] The environment modeling and output module 206 is configured to generate environment representation data based on the state vector of the robot system, the time-varying credibility weight and the equivalent observation data of each sensor, the environment representation data including an environment grid map in which an occupancy probability of each grid cell is updated and a set of drivable regions.

[0286] Further, the space-time adaptive synchronization module 202 can be specifically configured to update a time drift model of each sensor, the time drift model indicating a deviation relationship between a local time of the sensor and a system global time; and calculate the equivalent observation data of each sensor based on the time drift model of the sensor and the original observation data given the system global time.

[0287] The time drift model is defined as follows:

[0288] , wherein, represents a sensor local time of the i th sensor a drift amount relative to the global time, is a time drift rate coefficient of the i th sensor, is an initial time bias constant of the i th sensor.

[0289] Further, the credibility dynamic evaluation module 203 can also be configured to generate an abnormal flag signal for a sensor when one of the following conditions is met: a 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 with the abnormal flag signal, the time-varying credibility weight of the sensor is temporarily attenuated according to the following formula:

[0290] , wherein, is a weight attenuation coefficient, satisfying ;​ denotes the time-varying confidence weight of the i-th sensor before attenuation, denotes the time-varying confidence weight of the i-th sensor after attenuation.

[0291] Further, the self-organizing perception scheduling module 204 can be specifically configured to: determine a dynamic scene proportion, an image texture complexity, and a point cloud sparsity according to the equivalent observation data of each sensor, calculate an environment complexity index based on the dynamic scene proportion, the image texture complexity, and the point cloud sparsity; monitor a system resource usage state to determine a system available computing budget, and record a unit computing cost and a unit power consumption cost of each sensor; based on the health index, construct an optimization model with the system available computing budget as a budget constraint and with the environment complexity index and the health index, the computing cost, and the power consumption cost of each sensor as variables, and execute the optimization model to solve to select sensors to form the active sensor subset.

[0292] Further, the self-organizing perception scheduling module 204 can also be configured to: monitor a fusion residual increment in real time, and if the fusion residual increment exceeds a preset fusion residual increment threshold, temporarily add an inferior sensor in an alternative list and re-execute the optimization model to solve to update the active sensor subset.

[0293] Further, the environment modeling and output module 206 can be specifically configured to: update an occupancy probability of each cell in an environment grid map in a logarithmic likelihood form based on the time-varying confidence weight and the equivalent observation data of each sensor, the occupancy probability representing a possibility that a corresponding position of the cell is occupied by an obstacle; determine a drivable region set indicating a drivable region according to a preset occupancy probability threshold and the occupancy probability of each cell; and generate environment representation data, the environment representation data at least including the environment grid map with the occupancy probability of each cell updated and the drivable region set.

[0294] Further, the robot environment perception device based on multi-sensor fusion can also include: an obstacle detection module 207 configured to determine an obstacle target set according to the occupancy probability of each cell, perform weighted multi-sensor nearest neighbor association and threshold detection on the obstacle target set, and perform observation fusion according to the time-varying confidence weight of each sensor within a Mahalanobis distance threshold to perform real-time tracking of a dynamic obstacle.

[0295] Further, the robot environment perception device based on multi-sensor fusion can also include: a parameter updating module 208 configured to update an extrinsic matrix and a time bias parameter of each sensor by using a Lie algebra disturbance model based on the equivalent observation data of each sensor and a state vector of a robot system.

[0296] In a specific application, the robot environment perception device based on multi-sensor fusion can be implemented as software, hardware or a combination of both. In some examples, the robot environment perception device based on multi-sensor fusion can be implemented as the following Figure 3 The electronic device in the system shown or deployed in the electronic device.

[0297] Figure 3 A schematic structural diagram of the robot environment perception system based on multi-sensor fusion provided by the embodiments of the present disclosure is shown. Referring to Figure 3 The robot environment perception system based on multi-sensor fusion provided by the embodiments of the present disclosure can include a sensor group 301 and an electronic device 302, the sensor group 301 includes two or more sensors, the electronic device 302 includes a processor and a memory, the memory stores a computer program, and the computer program causes the processor to execute the foregoing robot environment perception method based on multi-sensor fusion when the computer program is run by the processor.

[0298] In some examples, the sensor group 301 can include at least two of a lidar, a camera, a millimeter wave radar, an inertial measurement unit, and an ultrasonic sensor. Figure 3 In an 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.

[0299] The electronic device 302 can include one or more processors and a memory.

[0300] The processor can be a central processing unit (CPU) or other form of processing unit that has data processing capability and / or instruction executing capability, and can control other components in the electronic device to perform desired functions.

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

[0302] According to specific application circumstances, the electronic device can further include any other appropriate components.

[0303] In addition to the method and the device described above, embodiments of the present disclosure can also be a computer program product including computer program instructions, which, when executed by a processor, cause the processor to perform the steps of the methods according to various embodiments of the present disclosure described in the above "Exemplary Methods" section of this specification.

[0304] The computer program instructions can be written in any combination of one or more programming languages, including an object oriented programming language such as Java, C++, etc., and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code can execute entirely on the user's computing device, partly on the user's device, as a stand-alone software package, partly on the user's computing device and partly on a remote computing device or entirely on the remote computing device or server.

[0305] In addition, embodiments of the present disclosure can also be a computer readable storage medium, having stored thereon computer program instructions which, when executed by a processor, cause the processor to perform the steps of the methods according to various embodiments of the present disclosure described in the above "Exemplary Methods" section of this specification.

[0306] The computer readable storage medium can be any combination of one or more non-transitory media. The non-transitory medium can be a non-transitory signal medium or a non-transitory storage medium. The non-transitory storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples (a non-exhaustive list) of the non-transitory storage medium include the following: an electrical connection having one or more wires, a portable disc, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0307] The above description is given for illustrative and descriptive purposes. Furthermore, this description is not intended to limit embodiments of the present disclosure to the forms disclosed herein. Although several example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, changes, additions and sub-combinations thereof.

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 time of the system 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 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 solved to select sensors to form the active sensor subset. 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. Also 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.

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: 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.

4. 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 log probability form, where 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.

5. The method according to claim 4, 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.

6. 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.

7. 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.

8. 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 types of 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-6 when executed by the processor.

Images