Sensor data fusion system, electronic device and vehicle

Abstract

Disclosed in the embodiments of the present invention are a sensor data fusion system, an electronic device and a vehicle. The system comprises: different types of vehicle-mounted sensors, filtering sub-modules, fault detection modules and a main filtering module, wherein the vehicle-mounted sensors are used for collecting motion information of a vehicle; the filtering sub-modules are used for performing filtering processing on the motion information, so as to obtain preliminary state variable estimation values after filtering; the fault detection modules are used for performing fault detection on the vehicle-mounted sensors connected to the filtering sub-modules, determining connection relationships with the main filtering module on the basis of fault detection results, and when there are connection relationships, sending to the main filtering module preliminary state variable estimation values output by corresponding filtering sub-modules; and the main filtering module is used for performing data fusion on the received preliminary state variable estimation values corresponding to the sensors, so as to obtain a fused state variable estimation value. By using the technical solution, the robustness of a sensor data fusion system for motion information estimation is improved.

Description

A sensor data fusion system, electronic device and vehicle Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and more specifically, to a sensor data fusion system, an electronic device, and a vehicle. Background Technology

[0002] In autonomous driving systems, multi-sensor fusion has become one of the mainstream technologies. Currently, multi-sensor fusion selects one sensor from those with different sampling frequencies (e.g., image sensors, inertial measurement units, wheel speed sensors, and LiDAR) as the master sensor, and the others as slave sensors. The fusion result is used to calibrate the slave sensors, ensuring that the data collected by each sensor is on the same time reference. During sensor data acquisition, the same type of vehicle motion information uses measurements from a single sensor; for example, acceleration is measured only using information from the IMU (Inertial Measurement Unit). When a sensor malfunctions, the data it collects cannot be included in the data fusion process and cannot participate in subsequent calculations. For example, if the IMU accelerometer sensor malfunctions, the acceleration information collected by the IMU cannot participate in subsequent calculations by the control and perception modules, leading to a failure of the entire system. Summary of the Invention

[0003] This invention provides a sensor data fusion system, electronic device, and vehicle to improve the robustness of motion information estimation.

[0004] The specific technical solution is as follows:

[0005] In a first aspect, embodiments of the present invention provide a sensor data fusion system, the system comprising:

[0006] Multiple sub-filter modules, multiple fault detection modules, and a main filter module, among which,

[0007] Each sub-filter module is connected to a corresponding fault detection module. For any one of the sub-filter modules, it is used to receive vehicle motion information sent by the vehicle sensor connected to the sub-filter module, and to filter the motion information to obtain the preliminary state variable estimate and the corresponding covariance information after filtering. The vehicle sensor includes multiple types and is used to collect different types of motion information.

[0008] The fault detection module is used to detect faults in the vehicle-mounted sensors connected to the sub-filter module, determine the connection relationship with the main filter module based on the fault detection results, and send the preliminary state variable estimates and covariance information output by the corresponding sub-filter module to the main filter module if a connection relationship exists. Specifically, if the fault detection result indicates that the sensor connected to the fault detection module is faulty, the connection relationship between the fault detection module and the main filter module is disconnected; if the fault detection result indicates that the sensor connected to the fault detection module is not faulty, the connection relationship between the fault detection module and the main filter module is maintained.

[0009] The main filtering module is used to fuse the preliminary state variable estimates received from each sensor to obtain fused state variable estimates.

[0010] As demonstrated by the above scheme, each sub-filter module simultaneously performs preliminary estimation of the same state variable. By fusing the preliminary state variable estimates obtained from each sub-filter module using the main filter module, the accuracy of the state variable estimation results can be improved. Furthermore, by adaptively adjusting the weight coefficients of each sensor based on the covariance of each sub-filter module, the estimation accuracy of the state variable values ​​can be further improved. In addition, by connecting a fault detection module between the sub-filter modules and the main filter module, faulty sensors can be isolated, thereby enhancing the robustness of the system's state variable estimation.

[0011] Optional, the fault detection module includes:

[0012] The first filtering unit and the second filtering unit alternately perform fault detection. When one filtering unit is performing fault detection, the other filtering unit is reset according to the fused state variable estimate fed back by the main filtering module, and then estimates the state variable after the reset.

[0013] In this embodiment, by using two filtering units to perform fault detection alternately, and by correcting the predicted state variables of the filtering units with the fused state variable estimates fed back by the main filtering module, the error of the filtering units can be suppressed from gradually increasing.

[0014] Optionally, the first or second filtering unit is specifically used for:

[0015] Fault detection is performed using the following fault detection function:

[0016] in,

[0017] Where, λ k T represents the function value of the fault detection function. DFor fault identification threshold, in, T represents the error estimate of the i-th sensor at time k. i k The covariance represents the error estimate. This represents the estimated error value at time k of the filter unit that is connected to the i-th sub-filter module. This represents the estimation error value of the i-th sub-filter module at time k. This represents the state variable of the i-th sub-filter module at time k. This represents the estimated state variable output by the i-th sub-filter module at time k. This represents the estimated state variable output by the first or second filter unit at time k.

[0018] Optionally, for each sub-filtering module, the sub-filtering module includes:

[0019] The prior state information calculation unit is used to infer the prior state variable estimate at the current moment based on the historical state variable estimate at the previous moment;

[0020] The prior covariance determination unit is used to calculate the prior covariance information of process noise;

[0021] The Kalman gain calculation unit is used to calculate the Kalman gain information of the sub-filter module based on the prior covariance information.

[0022] The state update unit is used to update the prior state variable estimate based on the Kalman gain corresponding to the sub-filter module, so as to obtain the updated preliminary state variable estimate.

[0023] The posterior covariance determination unit is used to determine the posterior covariance information based on the prior covariance information and the Kalman gain information.

[0024] Optional, a priori state information calculation unit, specifically used for:

[0025] Based on the first extreme value equation, and calculated from the historical state variable estimates of the previous time step, the prior state variable estimates of the current time step are obtained. The first extreme value equation represents the proportional relationship between the prior state variable estimate at time k and the historical state variable estimate at time k-1. The proportionality coefficient of this relationship is the system matrix corresponding to the sub-filter module. The first extreme value equation is obtained by calculating the first partial derivative of the first function with respect to the state variable at time k. The prior state variable estimate at time k in the first extreme value equation is the extreme value of the first function, where the first function is:

[0026] Where i represents the i-th sub-filter module, k represents the k-th time, and L k It's about state variables. and state variables quadratic function, It is a Gaussian function, which applies to state variables. The integral is a constant, where,

[0027] in, Let represent the state variable of the i-th sub-filter module at time k, and this state variable is the independent variable of the first function. This represents the state variable of the i-th sub-filter module at time k-1, and the value of this state variable is a quadratic function L. k The extreme value of A ii This represents the system matrix corresponding to the i-th sub-filter module, and Q represents the system noise covariance matrix. This represents the initial state variable estimate obtained by the i-th sub-filter module at time K-1. This represents the posterior covariance information obtained by the i-th sub-filter module at time k-1.

[0028] Optional, state update unit, specifically used for:

[0029] Based on the second extreme value equation, and according to the Kalman gain information corresponding to the sub-filter module, the prior state variable estimates are updated to obtain updated preliminary state variable estimates. The second extreme value equation represents the linear relationship between the updated preliminary state variable estimates at time k and the prior state variable estimates at time k. The scaling factor of this linear relationship is the Kalman gain information. The second extreme value equation is obtained by calculating the first partial derivative of the second function with respect to the state variables at time k. The preliminary state variable estimates at time k in the second extreme value equation are the extreme values ​​of the second function, where the second function is:

[0030] in, C represents the measurement information received by the i-th sub-filter module at time k. i This represents the measurement matrix of the i-th sensor. This represents the inverse of the measurement noise matrix of the i-th sensor. Let represent the state variable of the i-th sub-filter module at time k, and this state variable is the independent variable of the second function. This represents the prior state variable estimate of the i-th sub-filter module at time k. This represents the prior covariance information obtained by the i-th sub-filter module at time k, which is obtained by calculating the second-order partial derivative of the state variable at time k with respect to the first function.

[0031] Optionally, the main filtering module includes:

[0032] The weight coefficient adjustment unit is used to determine the weight coefficients corresponding to each sensor based on the posterior covariance information obtained from each sub-filter module.

[0033] The data fusion unit is used to weight the prior process covariance values ​​of the corresponding sub-filter modules based on weighting coefficients, and to perform data fusion based on the weighted covariance information of each sub-filter module and the preliminary state variable estimates of each sub-filter module to obtain the fused state variable estimates.

[0034] Wherein, the prior process covariance is:

[0035] Where k represents time, A ii I represents the system matrix corresponding to the i-th sub-filter module, Q represents the system noise covariance matrix, and I represents the system noise covariance matrix. i This represents the identity matrix corresponding to the i-th sub-filter module. This represents the globally optimal covariance estimate at time k-1.

[0036] Optional, weighting coefficient adjustment unit, specifically used for:

[0037] The weighting coefficients for each sub-filter module are determined using the following formula:

[0038] Where i represents the i-th sub-filter module, N represents the total number of sub-filter modules, and k represents the k-th time step. This represents the posterior covariance information obtained by the i-th sub-filter module at time k.

[0039] Optional, data fusion unit, specifically used for:

[0040] The estimated values ​​of the fusion state variables are calculated using the following formula:

[0041] Among them, X g This represents the estimated value of the fused state variables. C represents the preliminary state variable estimate output by the i-th sub-filter module at time k. i This represents the measurement matrix of the i-th sensor. γ represents the inverse of the measurement noise matrix of the i-th sensor. i This represents the weight coefficient corresponding to the i-th sub-filter module. Let $\mathbf{k}$ represent the globally optimal covariance estimate at time $k$, where $\mathbf{k}$. This represents the covariance of the prior process.

[0042] Optionally, the main filtering module is connected to each sub-filtering module to use the fused state variable estimate, the global optimal covariance estimate, and the weight coefficients corresponding to each sub-filtering module as feedback information, and to feed them back to each sub-filtering module to reset the state variable estimate and covariance of each sub-filtering module.

[0043] Secondly, embodiments of the present invention also provide an electronic device, the electronic device comprising: at least one processor, the processor and a memory coupled together, the memory storing a program or instructions running on the processor, the program or instructions being executed by the processor to implement the steps in the sensor data fusion system provided in any embodiment of the present invention.

[0044] Thirdly, embodiments of the present invention also provide a vehicle that includes the electronic equipment provided in any embodiment of the present invention. Attached Figure Description

[0045] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0046] Figure 1a is a schematic structural block diagram of a sensor data fusion system provided in Embodiment 1 of the present invention;

[0047] Figure 1b is a schematic diagram of the connection relationship between one of the fault detection modules and other modules in the sensor data fusion system provided in Embodiment 1 of the present invention.

[0048] Figure 2 is a structural block diagram of an electronic device provided in Embodiment 2 of the present invention. Detailed Implementation

[0049] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0050] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0051] This invention discloses a sensor data fusion system, an electronic device, and a vehicle. These are described in detail below.

[0052] Example 1

[0053] Embodiment 1 of this invention provides a sensor data fusion system, which includes multiple sub-filtering modules, multiple fault detection modules (each fault detection module is connected to a sub-filtering module), and a main filtering module. This system can be applied to the driving process of autonomous vehicles, typically in situations where the vehicle is frequently shifting gears and moving at low speeds and large angles, such as parking scenarios. In parking scenarios, the measurement information from sensors such as drive motor speed, wheel speed, and IMU acceleration, which indirectly reflect vehicle acceleration information, suffers from delays, dead zones, abrupt changes, and drift. If the system's acceleration estimation information uses only the estimation result of a single sensor, accurate acceleration information cannot be obtained when that sensor malfunctions. For the same motion information, such as acceleration or speed information, this embodiment of the invention uses a method of fusing the output results of multiple different types of sensors to obtain a globally optimal motion information estimation result. When a sensor malfunctions, the sensor data fusion system provided in Embodiment 1 of this invention can also isolate the malfunctioning sensor, thereby improving the robustness of motion information estimation, ensuring that the reliability of the system's output motion information estimation result is not affected by the failure of a single sensor.

[0054] Among them, any one sub-filter module is used to receive vehicle motion information sent by the external vehicle sensor connected to the sub-filter module, and to filter the motion information to obtain the preliminary state variable estimate and the corresponding covariance information after filtering. The vehicle sensor includes multiple types, which are used to collect different types of motion information. Each vehicle sensor is connected to a sub-filter module.

[0055] The fault detection module is used to detect faults in the vehicle-mounted sensors connected to the sub-filter module, determine the connection relationship with the main filter module based on the fault detection results, and send the preliminary state variable estimates and covariance information output by the corresponding sub-filter module to the main filter module if a connection relationship exists. Specifically, if the fault detection result indicates that the sensor connected to the fault detection module is faulty, the connection relationship between the fault detection module and the main filter module is disconnected; if the fault detection result indicates that the sensor connected to the fault detection module is not faulty, the connection relationship between the fault detection module and the main filter module is maintained.

[0056] The main filtering module is used to fuse the preliminary state variable estimates and covariance information received from each sensor to obtain fused state variable estimates.

[0057] Specifically, Figure 1a is a structural block diagram of a sensor data fusion system provided in one embodiment of the present invention. Figure 1a shows three sub-filter modules, namely sub-filter module 121, sub-filter module 122 and sub-filter module 123, fault detection modules connected to each sub-filter module, namely sub-filter module 131, sub-filter module 132 and sub-filter module 133, and main filter module 140.

[0058] Figure 1b also shows three different types of external vehicle-mounted sensors: vehicle-mounted sensor 111, vehicle-mounted sensor 112, and vehicle-mounted sensor 113. Vehicle-mounted sensor 111 is connected to sub-filter module 121, vehicle-mounted sensor 112 is connected to sub-filter module 122, and vehicle-mounted sensor 113 is connected to sub-filter module 123. Each vehicle-mounted sensor is used to collect vehicle motion information, which is input as a state variable to the corresponding sub-filter module. The motion information includes speed and acceleration.

[0059] It should be noted that, for subsequent data fusion, for vehicle sensors with inconsistent sampling frequencies, resampling can be performed to ensure that the sampling frequencies of each sensor are consistent. Furthermore, the state equations of each vehicle sensor must also be consistent. For example, if the system state vector is defined as x(t) = [vaj]... T Where v represents velocity, a represents acceleration, and j represents the derivative of acceleration, and the system noise is w(t) ~ N(0,q), then the system state equation can be expressed by the following formula:

[0060] The system matrix F and the system noise covariance matrix q can be expressed as follows:

[0061] If we define the discretization period of the system as Δt, then according to linear system theory, the state equation of the discretized system is: X k =AX k-1 +W k-1

[0062] Among them, the discretized system noise satisfies W k-1 ~N(0,Q), the discretized system matrix A and system noise covariance matrix Q can be expressed as:

[0063] Where Δt represents the time interval between two adjacent moments, and q1, q2, and q3 are the covariances in the noise covariance matrix, respectively.

[0064] The following explanation uses acceleration information as an example to illustrate motion information. The sensors used to obtain acceleration information can be drive motor speed sensors, wheel speed sensors, and IMU acceleration sensors. It should be noted that this embodiment does not specifically limit the type of on-board sensors for motion information acquisition; correspondingly, it does not specifically limit the number of sub-filter modules connected to each on-board sensor, nor the number of fault detection modules connected to each sub-filter module.

[0065] For a drive motor speed sensor, if the measurement information of the drive motor speed sensor is defined as... Measurement noise is The discretized state equation and measurement equation for the drive motor speed subsystem are as follows:

[0066] in, A 11 =A, the measurement matrix C1 can be represented as: C1 = [1 0 0]

[0067] For wheel speed sensors, if the measurement information of the wheel speed sensor is defined as... Measurement noise is The discretized state equation and measurement equation for the wheel speed subsystem are as follows:

[0068] in, A 22 =A, the measurement matrix C2 can be represented as: C2 = [1 0 0]

[0069] For an IMU accelerometer, if the measurement information of the IMU accelerometer is defined as... Measurement noise is The discretized state equations and measurement equations of the IMU acceleration subsystem are as follows:

[0070] in, A 33 =A, the measurement matrix C3 can be represented as: C3 = [0 1 0]

[0071] In summary, while the measurement equations for each sensor differ, the system state equation remains the same. Specifically, the state equation and measurement equation of the i-th subsystem can be expressed by the following formula:

[0072] in, Let i represent the state variable of the i-th sensor at time k. Let A represent the state variable of the i-th sensor at time k-1. ii W represents the system matrix of the i-th sensor. k-1 This represents the system noise matrix at time k-1. V represents the measurement information of the i-th sensor at time k. i k Let C represent the measurement noise matrix of the i-th sensor at time k. i This represents the measurement matrix of the i-th sensor.

[0073] The functions and roles of each module in the sensor data fusion system are described in detail below:

[0074] (I) Sub-filtering module

[0075] The sub-filtering module is used to filter the state variables to obtain the preliminary estimated values ​​of the filtered state variables and the corresponding covariance information. Each sensor is connected to a corresponding sub-filtering module, and each sub-filtering module is connected to a corresponding fault detection module.

[0076] Specifically, each sub-filtering module in this embodiment can employ a Kalman filter. Each sub-filtering module includes: a priori state information calculation unit, a priori covariance determination unit, a Kalman gain calculation unit, a state update unit, and a posterior covariance determination unit.

[0077] The prior state information calculation unit is used to infer the prior state variable estimate for the current moment based on the historical state variable estimate from the previous moment. Specifically, it can be calculated using the following prior state estimation formula:

[0078] in, This represents the prior state variable estimate of the i-th sub-filter module at time k. The estimated state variable of the i-th sub-filter module at time k-1, i.e. the estimated historical state variable, is represented by A, which is the system matrix.

[0079] The prior covariance determination unit is used to calculate the prior covariance information of the process noise, which can be achieved through the following prior covariance calculation formula:

[0080] in, This represents the prior covariance value obtained by the i-th sub-filter module at time k. Let A denote the posterior covariance value obtained by the i-th sub-filter module at time k. ii This represents the system matrix corresponding to the i-th sub-filter module, and Q represents the system noise covariance matrix.

[0081] The Kalman gain calculation unit is used to calculate the Kalman gain information of this sub-filter module based on the prior covariance information. Specifically, it can be implemented using the following Kalman gain calculation formula:

[0082] Among them, K k R represents the Kalman gain at time k. i Let C represent the measurement noise matrix of the i-th sub-filter module. i This represents the measurement matrix of the i-th sensor.

[0083] The state update unit is used to update the prior state variable estimates based on the Kalman gain information corresponding to the sub-filter module, obtaining the updated preliminary state variable estimates. Specifically, it can be calculated using the following state update formula:

[0084] in, This represents the initial state variable estimate output by the i-th sub-filter module at time k. This represents the measurement information of the i-th sensor at time k.

[0085] The posterior covariance determination unit is used to determine the posterior covariance information based on the prior covariance information and the Kalman gain information. Specifically, it can be implemented through the following covariance update formula:

[0086] in, This represents the posterior covariance value obtained by the i-th sub-filter module at time k.

[0087] Those skilled in the art will understand that the system state equation of a sub-filter module represents the linear relationship between the state variables at the previous time step and the state variables at the current time step. The system state equation reflects the prior result, while the actual observation data is the correction data, which is the posterior result. By continuously correcting the output using the prior and posterior results, the final output can be made closer to the true value. Here, the prior data and the actual observation data are equivalent to the prior and posterior data in Bayesian estimation. In this embodiment, based on the Bayesian criterion, the Kalman filter equation for a single sub-filter module and the Kalman filter equation for the brightness enhancement system composed of various sub-filter modules can be derived. The specific derivation process of the Kalman filter equation for a single sub-filter module is as follows:

[0088] Assume the initial confidence level of the state variables It is Gaussian distributed, and any linear transformation of a Gaussian random variable will lead to another Gaussian random variable. Therefore, the confidence level at any time k-1 is... It follows a Gaussian distribution and can be expressed as:

[0089] in, and The confidence levels at time k-1 are respectively The mean and covariance.

[0090] The confidence level for recursively solving time k based on the Bayesian criterion is:

[0091] In the confidence formula above, η is the normalization coefficient, which is a constant, and the state transition probability is... The mean is Gaussian distribution with covariance Q, measuring probability The mean is Covariance is R i The Gaussian distributions of the two can be expressed as follows:

[0092] State transition probability confidence level at time k-1 Substituting into the confidence formula above, we can obtain the result after one step (from time k-1 to time k) when the measurement vector is added. Confidence of previous state variables:

[0093] In the formula, L k It can be represented as:

[0094] Because of L k It is about The quadratic function, and also about The quadratic function, based on L k middle Construct a bivariate function by combining the quadratic and linear terms and adding a constant term. satisfy And make For about A Gaussian function, which has certain effects on the state variables. The integral is a constant, that is... Specifically, calculating L k about The first and second derivatives of the quadratic form function L can be obtained. k The extreme value of is given by the following formula:

[0095] According to L k about The first and second derivatives are constructed with the mean as... The Gaussian function with covariance Ω is:

[0096] because Includes L k middle The quadratic and linear terms make the first function in this embodiment... It can be represented as:

[0097] because The confidence formula above can be simplified to:

[0098] calculate about From the first and second derivatives, we can obtain:

[0099] Therefore, we can obtain the result after adding the measurement vector in one step. Previous confidence level The mean and covariance are as follows:

[0100] Where i represents the i-th sub-filter module, k represents the k-th time, and L k It's about state variables. and state variables quadratic function, Let represent the state variable of the i-th sub-filter module at time k, and this state variable is the independent variable of the first function. The state variable of the i-th sub-filter module at time k-1 is estimated to be a quadratic function L.k The extreme value of A ii This represents the system matrix corresponding to the i-th sub-filter module, and Q represents the system noise covariance matrix. This represents the initial state variable estimate obtained by the i-th sub-filter module at time K-1. This represents the posterior covariance information obtained by the i-th sub-filter module at time k-1.

[0101] In the above formula for the first partial derivative of the first function with respect to the state variable, The first extreme value equation in this embodiment represents the proportional relationship between the estimated value of the prior state variable at time k and the estimated value of the historical state variable at time k-1. The proportionality coefficient of this relationship is the system matrix corresponding to the sub-filter module. The meaning of the above-mentioned first-order partial derivative formula is that by calculating the first-order partial derivative of the first function with respect to the state variable at time k, the extreme value of the first function can be obtained, that is, the estimated value of the prior state variable at time k in the first extreme value equation. In this embodiment, the prior state information calculation unit can specifically calculate the estimated value of the prior state variable at the current time based on the first extreme value equation and the estimated value of the historical state variable at the previous time.

[0102] Furthermore, by using the second-order partial derivative formula of the first function with respect to the state variables, the prior covariance information at time k obtained by the i-th sub-filter module can be obtained. Right now In this embodiment, the Kalman gain calculation unit is used to calculate the Kalman gain information of the sub-filter module based on the prior covariance information, which can be specifically expressed by the following formula:

[0103] Furthermore, the probability of measurement will be... and the measurement vector after one step Previous confidence level Substituting into the confidence formula above, we can obtain the confidence level at time k as:

[0104] In the formula, the second function J k It can be represented as:

[0105] Calculate J k about From the first and second derivatives, we can obtain:

[0106] Therefore, the confidence level at time k can be obtained. The mean and covariance are:

[0107] in, C represents the measurement information received by the i-th sub-filter module at time k. i This represents the measurement matrix of the i-th sensor. This represents the inverse of the measurement noise matrix of the i-th sensor. Let represent the state variable of the i-th sub-filter module at time k, and this state variable is the independent variable of the second function. This represents the prior state variable estimate of the i-th sub-filter module at time k.

[0108] In the formula for the first-order partial derivative of the second function with respect to the state variable, This is the second extreme value equation in this embodiment. The second extreme value equation represents the linear relationship between the preliminary state variable estimate updated at time k and the prior state variable estimate at time k. The scaling factor of this linear relationship is the Kalman gain information. This second extreme value equation is obtained by calculating the first partial derivative of the state variable at time k with respect to the second function. The preliminary state variable estimate at time k in this second extreme value equation is the extreme value of the second function.

[0109] Furthermore, by calculating the second-order partial derivative of the second function with respect to the state variables, the posterior covariance information obtained by the i-th sub-filter module at time k can be obtained, i.e. In this embodiment, the posterior covariance determination unit is used to determine the posterior covariance information based on the prior covariance information and the Kalman gain information.

[0110] (II) Fault Detection Module

[0111] The fault detection module is used to detect faults in the vehicle sensors connected to the sub-filter module, determine the connection relationship with the main filter module based on the fault detection results, and send the preliminary state variable estimates and covariance information output by the corresponding sub-filter module to the main filter module if a connection relationship exists.

[0112] In this embodiment, if the fault detection result indicates that the sensor connected to the fault detection module is faulty, the connection between the fault detection module and the main filtering module is disconnected; if the fault detection result indicates that the sensor connected to the fault detection module is not faulty, the connection between the fault detection module and the main filtering module is maintained. The advantage of this configuration is that when a sensor malfunctions, other sensors can still output motion information data for data fusion, thereby obtaining a reliable motion information estimate. That is, the sensor data fusion system provided in this embodiment will not be affected by the failure of a single sensor in estimating normal motion information values. Compared to using a single sensor to collect motion data, this configuration effectively improves the robustness of motion data estimation. For example, if only an IMU accelerometer is used to collect acceleration information, and the IMU accelerometer malfunctions, the acceleration data cannot participate in subsequent calculations, which may affect the normal operation of the entire system. In this embodiment, even if the IMU accelerometer malfunctions, other sensors, such as wheel speed sensors and drive motor speed sensors, can still estimate acceleration information and input it into the main filtering module for subsequent data fusion, thereby obtaining a highly reliable and accurate acceleration estimate.

[0113] Specifically, under the condition that the fault detection result indicates that the sensor connected to the fault detection module is not faulty, as shown in Figure 1a, the switch between the fault detection module and the main filter module is in the closed state. At this time, the fault detection module 131 will output the preliminary state variable estimate from the sub-filter module 121. and the corresponding covariance information P 11 The fault detection module 132 sends the preliminary state variable estimates output by the sub-filter module 122 to the main filter module 140. and the corresponding covariance information P 22 The fault detection module 133 sends the preliminary state variable estimates output by the sub-filter module 123 to the main filter module 140. and the corresponding covariance information P 33 The signal is sent to the main filter module 140. When the fault detection result indicates that the sensor connected to the fault detection module is faulty, the fault detection module and the main filter module are no longer connected. That is, as shown in Figure 1a, the switch between the fault detection module and the main filter module is turned on, thus isolating the faulty sensor.

[0114] Furthermore, as shown in Figure 1b, each fault detection module includes: a first shadow filter unit and a second filter unit. The state equation of each filter unit is the same as that of the corresponding sub-filter module, which is X. k =AX k-1 +W k-1Two filtering units alternately perform fault detection. While one filtering unit is performing fault detection, the other filtering unit resets according to the fused state variable estimate fed back by the main filtering module. After resetting, the other filtering unit estimates the state variables, meaning the fused state variable estimate fed back by the main filtering module can be directly assigned to the filtering unit as its output state variable estimate. This state variable estimate can serve as prior data for estimating the state variables at the next time step. Compared to using a single filtering unit for fault detection, this embodiment uses two filtering units alternately for fault detection and corrects the state variable estimates obtained by the filtering units using the fused state variable estimate fed back by the main filtering module, thus suppressing the gradual increase of errors in the filtering units.

[0115] Specifically, Figure 1b shows a schematic diagram of the connection relationship between one fault detection module and other modules in the sensor data fusion system. The other fault detection modules in the sensor data fusion system have the same structure as shown in Figure 1b. As shown in Figure 1b, the fault detection module includes filter unit 1 and filter unit 2. The sub-filter module outputs preliminary state variable estimates... and its corresponding covariance information P ii The data is sent to the main filtering module, which then fuses the state estimation information and covariance information from each sub-filter module to obtain the fused state variable estimate X. g and the global optimal covariance estimate P g The main filtering module can obtain the fused state variable estimates and the global optimal covariance estimate P. g Feedback is sent to the sub-filtering module to correct its input. Furthermore, based on its connection with the fault detection module, the main filter module can fuse the estimated state variable X. g The feedback is sent to the filtering unit to correct the state estimation information of the filtering unit.

[0116] Specifically, for each filter unit, fault detection can be performed according to the following fault detection function:

[0117] in,

[0118] Where, λ k T represents the function value of the fault detection function. D For fault identification threshold, in, T represents the error estimate of the i-th sensor at time k. i k The covariance represents the error estimate. This represents the estimated error value at time k of the filter unit that is connected to the i-th sub-filter module. This represents the estimation error value of the i-th sub-filter module at time k. Let represent the state variable corresponding to the i-th sub-filter module at time k. In this embodiment, the estimated value of the fused state variable output by the main filter module can be used for calculation. This represents the estimated state variable output by the i-th sub-filter module at time k. This represents the estimated state variable output by the first or second filter unit at time k.

[0119] Those skilled in the art will understand that the false detection probability and false miss probability of the first filtering unit and the second filtering unit for sensor fault detection conform to the Bayesian prior probability and posterior probability formulas. Based on this, the above fault detection function can be derived based on the Bayesian criterion. The specific derivation process is as follows:

[0120] For any sensor to be tested, assume there are two possible states, H0 and H1, where H0 represents no fault and H1 represents a fault. The error is estimated using a shadow filter. estimation error of sub-filter module Received Make the following binary assumptions:

[0121] (a) When the sensor to be tested is fault-free,

[0122] (b) When the sensor to be detected is faulty,

[0123] The above formula represents the error estimate when the sensor to be detected is faulty. The mean is μ. When the sensor to be detected is fault-free, the error estimate is... The mean of the error estimator is 0. The mean of the error estimator differs in both cases, but the covariance of the error estimator is the same, T. i k .

[0124] Based on formulas (a) and (b) above, and The following conditional probability density function can be obtained:

[0125] When β i When ∈Π0, H0 is considered true, indicating that the sensor to be detected is fault-free. This represents the false detection probability of a faulty sensor being detected; when β i ∈Π iAt that time, judge H i A value of true indicates that the sensor being tested is faulty. This represents the probability of a missed detection due to a fault in the sensor under test. Therefore, combining the probabilities of false detection and missed detection of the sensor under test, the fault probability formula can be obtained as follows:

[0126] The decision region is divided using the minimum error probability criterion, so that the integral term of the fault probability formula is within β. i The values ​​within the Π1 region are negative, thus the following fault diagnosis formula applies:

[0127] From the above fault judgment formula, it can be seen that if H0 is true, it indicates that there is no fault, which means that β... i This belongs to the non-integral region Π0, which ensures that the integral term remains negative throughout the integration region Π1. The above fault diagnosis formula can be written in the following form:

[0128] Based on the above conditional probability density function, the logarithmic function of the error estimator can be obtained:

[0129] Wherein, L(β) i The larger the value, the lower the probability of false detection and false negative detection of the sensor being detected. Therefore, the maximum likelihood estimation is used to obtain... From the above logarithmic function formula, the fault detection function of the sensor to be detected can be obtained as follows:

[0130] Therefore, the fault diagnosis criterion for the sensor to be detected is as follows:

[0131] In this embodiment, the fault detection module enables the sensor data fusion system to have fault tolerance, thereby achieving the technical effect of improving the robustness of vehicle motion information estimation.

[0132] (III) Main Filtering Module

[0133] The main filtering module is used to fuse the preliminary state variable estimates received from each sensor to obtain fused state variable estimates.

[0134] In this embodiment, the main filtering module includes a weight coefficient adjustment unit and a data fusion unit, wherein,

[0135] The weighting coefficient adjustment unit is used to determine the weighting coefficients corresponding to each sensor based on the posterior covariance information obtained from each sub-filtering module. Specifically, the weighting coefficient adjustment unit can determine the weighting coefficients corresponding to each sub-filtering module according to the following formula:

[0136] Where i represents the i-th sub-filter module, N represents the total number of sub-filter modules, and k represents the k-th time step. This represents the posterior covariance information obtained by the i-th sub-filter module at time k.

[0137] The data fusion unit is used to weight the prior process covariance values ​​of the corresponding sub-filter modules based on the weighting coefficients obtained by the weighting coefficient adjustment unit, and to perform data fusion based on the weighted covariance information of each sub-filter module and the preliminary state variable estimates of each sub-filter module to obtain the fused state variable estimates. Specifically, the data fusion unit can calculate the fused state variable estimates according to the following formula:

[0138] Among them, X g This represents the estimated value of the fused state variables. C represents the preliminary state variable estimate output by the i-th sub-filter module at time k. i This represents the measurement matrix of the i-th sensor. γ represents the inverse of the measurement noise matrix of the i-th sensor. i This represents the weight coefficient corresponding to the i-th sub-filter module. Let $\mathbf{k}$ represent the globally optimal covariance estimate at time $k$, where $\mathbf{k}$. Let the prior process covariance be:

[0139] Where k represents time, A ii I represents the system matrix corresponding to the i-th sub-filter module, Q represents the system noise covariance matrix, and I represents the system noise covariance matrix. i This represents the identity matrix corresponding to the i-th sub-filter module. This represents the globally optimal covariance estimate at time k-1.

[0140] Those skilled in the art will understand that for augmented systems constructed from multiple sensors, since the augmented system is also a linear system, the Kalman filter formula can also be derived using a Bayesian framework. Compared to other algorithms used for Kalman filter calculations, such as the least squares method, these algorithms can only satisfy the Kalman filter operation for a single filter module and cannot be applied to augmented systems. In this embodiment, based on the Bayesian criterion, not only can the prediction and update formulas for each independent Kalman filter module be derived, but a similar derivation of the Kalman filter formula can also be performed for augmented systems constructed from multiple sensors. Therefore, this embodiment, based on the Bayesian criterion, can better meet the requirements of sensor data fusion and Kalman filtering in augmented systems.

[0141] Specifically, the formulas for determining the weighting coefficients and the estimated values ​​of the fused state variables can be derived as follows:

[0142] First, the augmented system is constructed based on the state equations and measurement equations of each sub-filter module. The state equations and measurement equations are as follows:

[0143] Among them, the augmented state vector X k The augmented measurement matrix C can be represented as:

[0144] The prior covariance matrix of the augmented system is:

[0145] Taking the ji-th element from the prior covariance matrix above, we can obtain...

[0146] Using the variance upper bound technique, we can obtain:

[0147] Wherein, the magnification factor γ1…γ N The following conditions must be met:

[0148] Therefore, the amplified prior covariance can be obtained as:

[0149] That is, the diagonal elements in the prior covariance matrix represent the prior covariance values ​​obtained by each sub-filter module, and the off-diagonal elements are zero.

[0150] By comparing the ji-th element in the prior covariance matrix with the amplified prior covariance formula, it can be seen that after adopting the variance upper bound technique, the time updates of each sub-filter module can be performed independently, meaning the time updates of each sub-filter module are uncorrelated. This is combined with the sub-filter module covariance matrix reset method. Process noise variance reset method Q = γ i Q, we can obtain:

[0151] For augmented systems, if only one measurement value is considered The state quantity measurement is updated as follows:

[0152] in, and Further decomposed into

[0153] Therefore, the state update formula for the augmented system can be obtained as follows:

[0154] Consider the i-th measurement value For the measurement update of the j-th state variable, we can obtain the following from the state update formula of the augmented system:

[0155] For augmented systems, only one measurement value is considered. The covariance measurement is updated as follows:

[0156] Taking the jl-th element, we can obtain the formula for the posterior covariance of the jl-th element:

[0157] Based on the amplified prior covariance formula above, we can see that: ① When j ≠ i, the state update formula of the augmented system describes the i-th measurement value. The measurement update for the j-th state variable is simplified to That is, the i-th measurement value ① It will not cause a measurement update for the j-th state variable; ② When jl≠ii, the posterior covariance formula of the jl-th element describes the i-th measurement value. The jl-th element of the covariance measurement update is simplified to That is, the i-th measurement value No measurement updates are performed on the correlation terms between local filters. In summary, after adopting the variance upper bound technique, the measurement updates of each sub-filter module can be performed independently, meaning the measurement updates of each sub-filter module are uncorrelated. Therefore, the covariance update formula for each sub-filter module in the augmented system can be obtained as follows:

[0158] The covariance of the globally optimal estimate can be expressed as:

[0159] If the state equations of each sub-filter are consistent, then... The formula for the globally optimal covariance can be obtained as follows:

[0160] The augmented system, as shown in the formula below, is constructed by combining the independent measurement information of each sub-filter module. Combining this augmented system, the globally optimal covariance formula reveals that the global filter combines the independent measurement information of each sub-filter module to achieve optimal measurement updates. Therefore, assuming the state equations of each sub-filter module are consistent, the fused global filter is indeed optimal after adopting the information allocation principle.

[0161] The globally optimal estimated state variables, i.e., the estimated values ​​of the fused state variables in this embodiment, can be calculated using the following formula:

[0162] From the above formula for the state variables of the global optimal estimation, we can see that the state estimate of each sub-filter module is... In the globally optimal estimated state variable X g The proportion in the sub-filter module is positively correlated with its allocation coefficient. Therefore, to improve the globally optimal estimated state variable X g The accuracy must be such that the state estimate of the high-precision sub-filter module is accurate. Having a large allocation coefficient Covariance reflects the accuracy of the state estimate of the corresponding sub-filter module: a larger covariance indicates worse accuracy of the state estimate of the corresponding sub-filter module, and the allocation coefficient of the sub-filter module should be decreased; conversely, the allocation coefficient should be increased. Therefore, the strategy for adaptively adjusting the allocation coefficient of the sub-filter module based on covariance is as follows:

[0163] In summary, the globally optimal estimated state variables and covariance can be obtained based on the state variables and covariance of each sub-filter module.

[0164] In summary, the Bayesian criterion can better meet the requirements of sensor data fusion and Kalman filtering in augmented systems.

[0165] Furthermore, after the main filter module obtains the globally optimal estimated state variables and covariance, it will then fuse the estimated state variables, the globally optimal covariance, and the weight coefficients corresponding to each sub-filter module as feedback information, and feed them back to each sub-filter module to reset the estimated state variables and covariance of each sub-filter module. Specifically, as shown in Figure 1a, the main filter module 140 will fuse the estimated state variables X... g The global optimal covariance estimate P g The weight coefficient γ1 corresponding to sub-filter module 121 is fed back to sub-filter module 121 to reset the state variable estimates and covariance of sub-filter module 121. Furthermore, the fused state variable estimates X are... g The global optimal covariance estimate P g The weight coefficient γ2 corresponding to sub-filter module 122 is fed back to sub-filter module 122 to reset the state variable estimates and covariance of sub-filter module 122. The state variable estimates X are then fed back to sub-filter module 122. g The global optimal covariance estimate P g The weight coefficient γ3 corresponding to the sub-filter module 123 is fed back to the sub-filter module 123 to reset the state variable estimate and covariance of the sub-filter module 123.

[0166] This embodiment employs the aforementioned feedback method, enabling each sub-filter module to use the global optimal estimate as prior information and perform state estimation at the next moment, thereby obtaining more accurate state variable estimates. In other words, by adopting the above technical solution, the accuracy of motion information estimation can be improved, such as improving the accuracy of acceleration or velocity estimation, thus providing a reliable data foundation for vehicle speed control.

[0167] It should be noted that when a fault detection module detects a fault in a connected vehicle sensor, the connection between the main filter module and the fault detection module is disconnected. At this time, the main filter module can set the weight coefficient corresponding to the faulty sensor to zero, which is equivalent to turning on the switch between the main filter module and the sub-filter module as shown in Figure 1a, thereby isolating the faulty sensor.

[0168] The sensor data fusion system provided in this embodiment, as shown in the above scheme, allows each sub-filter module to simultaneously perform preliminary estimation of the same state variable. By fusing the preliminary state variable estimates obtained from each sub-filter module using the main filter module, the accuracy of the state variable estimation results can be improved. Furthermore, by adaptively adjusting the weight coefficients of each sensor based on the covariance of each sub-filter module, the estimation accuracy of the state variable values ​​can be further improved. In addition, by connecting a fault detection module between the sub-filter modules and the main filter module, faulty sensors can be isolated, thereby improving the robustness of the system's state variable estimation.

[0169] Example 2

[0170] Figure 2 is a structural block diagram of an electronic device provided in Embodiment 2 of the present invention. As shown in Figure 2, the electronic device includes:

[0171] At least one processor (a processor 520 is shown in Figure 2),

[0172] The processor 520 is coupled to the memory 510, which stores a program or instructions that run on the processor 520. When the processor 520 executes the program or instructions, it implements the steps in the sensor data fusion system provided in any embodiment of the present invention.

[0173] Based on the above embodiments, another embodiment of the present invention provides a vehicle that includes the electronic equipment provided in the above embodiments.

[0174] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0175] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0176] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A sensor data fusion system, characterized in that, include: Multiple sub-filter modules, multiple fault detection modules, and a main filter module, among which, Each sub-filter module is connected to a corresponding fault detection module. For each sub-filter module, it is used to receive vehicle motion information sent by the external vehicle sensor connected to the sub-filter module, and to filter the motion information to obtain the preliminary state variable estimate and the corresponding covariance information after filtering. The types of vehicle sensors include multiple types, which are used to collect different types of motion information. The fault detection module is used to detect faults in the external vehicle-mounted sensors connected to the sub-filter module, determine the connection relationship with the main filter module based on the fault detection results, and send the preliminary state variable estimates and covariance information output by the corresponding sub-filter module to the main filter module if a connection relationship exists. Specifically, if the fault detection result indicates that the sensor connected to the fault detection module is faulty, the connection between the fault detection module and the main filter module is disconnected; if the fault detection result indicates that the sensor connected to the fault detection module is not faulty, the connection between the fault detection module and the main filter module is maintained. The main filtering module is used to fuse the preliminary state variable estimates and covariance information received from each sensor to obtain fused state variable estimates.

2. The system according to claim 1, characterized in that, The fault detection module includes: The first filtering unit and the second filtering unit alternately perform fault detection. When one of the filtering units is performing fault detection, the other filtering unit is reset according to the fused state variable estimate fed back by the main filtering module, and the state variable is estimated after the reset.

3. The system according to claim 2, characterized in that, The first or second filtering unit is specifically used for: Fault detection is performed using the following fault detection function: in, Where, λ k T represents the function value of the fault detection function. D For fault identification threshold, in, Let represent the error estimate of the i-th sensor at time k. The covariance represents the error estimate. This represents the estimated error value at time k of the filter unit that is connected to the i-th sub-filter module. This represents the estimation error value of the i-th sub-filter module at time k. This represents the state variable of the i-th sub-filter module at time k. This represents the estimated state variable output by the i-th sub-filter module at time k. This represents the estimated state variable output by the first or second filter unit at time k.

4. The system according to any one of claims 1-3, characterized in that, For each sub-filter module, the sub-filter module includes: The prior state information calculation unit is used to infer the prior state variable estimate at the current moment based on the historical state variable estimate at the previous moment; The prior covariance determination unit is used to calculate the prior covariance information of process noise; The Kalman gain calculation unit is used to calculate the Kalman gain information of the sub-filter module based on the prior covariance information. The state update unit is used to update the prior state variable estimate based on the Kalman gain corresponding to the sub-filter module, so as to obtain the updated preliminary state variable estimate. The posterior covariance determination unit is used to determine the posterior covariance information based on the prior covariance information and the Kalman gain information.

5. The system according to any one of claims 1-4, characterized in that, The main filtering module includes: The weight coefficient adjustment unit is used to determine the weight coefficients corresponding to each sensor based on the posterior covariance information obtained from each sub-filter module. The data fusion unit is used to weight the prior process covariance values ​​of the corresponding sub-filter modules based on the weighting coefficients, and to perform data fusion based on the weighted covariance information of each sub-filter module and the preliminary state variable estimates of each sub-filter module to obtain fused state variable estimates. Wherein, the prior process covariance is: Where k represents time, A ii I represents the system matrix corresponding to the i-th sub-filter module, Q represents the system noise covariance matrix, and I represents the system noise covariance matrix. i This represents the identity matrix corresponding to the i-th sub-filter module. This represents the globally optimal covariance estimate at time k-1.

6. The system according to claim 5, characterized in that, The weighting coefficient adjustment unit is specifically used for: The weighting coefficients for each sub-filter module are determined using the following formula: Where i represents the i-th sub-filter module, N represents the total number of sub-filter modules, and k represents the k-th time step. This represents the posterior covariance information obtained by the i-th sub-filter module at time k.

7. The system according to claim 5, characterized in that, The data fusion unit is specifically used for: The estimated values ​​of the fusion state variables are calculated using the following formula: Among them, X g This represents the estimated value of the fused state variables. C represents the preliminary state variable estimate output by the i-th sub-filter module at time k. i This represents the measurement matrix of the i-th sensor. γ represents the inverse of the measurement noise matrix of the i-th sensor. i This represents the weight coefficient corresponding to the i-th sub-filter module. Let $\mathbf{k}$ represent the globally optimal covariance estimate at time $k$, where $\mathbf{k}$. This represents the covariance of the prior process.

8. The system according to any one of claims 5 to 7, characterized in that, The main filtering module is connected to each sub-filtering module and is used to feed back the fused state variable estimate, the global optimal covariance estimate, and the weight coefficients corresponding to each sub-filtering module as feedback information to each sub-filtering module in order to reset the state variable estimate and covariance of each sub-filtering module.

9. An electronic device, characterized in that, include: At least one processor coupled to a memory storing a program or instructions that run on the processor, the program or instructions, when executed by the processor, implement the steps in the sensor data fusion system as claimed in any one of claims 1 to 8.

10. A vehicle, characterized in that, The vehicle includes the electronic equipment as described in claim 9.

Images