Multi-sensor fusion positioning method, device and equipment of vehicle and storage medium

Abstract

The invention provides a multi-sensor fusion positioning method and device of a vehicle, equipment and a storage medium, and relates to the field of data processing. The method comprises the following steps: acquiring different and independent multi-source positioning information through a plurality of sensor sub-modules; sorting the multi-source positioning information according to the size of the observation timestamps, and screening the multi-source positioning information based on a preset time delay window; performing filtering fusion calculation on the reserved multi-source positioning information to obtain first positioning state data after time delay; performing time adjustment on the time-delayed first positioning state data by using the motion observation information to obtain second positioning state data at the current system moment; and post-processing the second positioning state data to obtain stable and smooth third positioning state data.

Description

Technical Field

[0001] This application relates to the field of data processing, and in particular to a multi-sensor fusion positioning method, apparatus, computer equipment, and readable storage medium for vehicles. Background Technology

[0002] Currently, the positioning data from various sensor modules in autonomous vehicles are coupled together and sequentially fed into a filtering algorithm according to the measurement time. The optimal positioning estimate is then obtained through fusion. However, the fusion positioning methods in related technologies suffer from the following problems:

[0003] (1) The data from each sensor are interdependent, which makes the system more complex. When one sensor fails, it directly leads to positioning failure, which is not conducive to the stability of the overall system.

[0004] (2) The calculation method of multiple filters was used in the data delay processing, which increased the computational load.

[0005] (3) For some abnormal observation data, the strategy of directly discarding them is usually adopted, which reduces the utilization rate of data and is not conducive to system stability. Summary of the Invention

[0006] In view of this, this application provides a multi-sensor fusion positioning method, apparatus, computer equipment, and readable storage medium for vehicles, thereby solving the aforementioned technical problems.

[0007] In a first aspect, embodiments of this application provide a multi-sensor fusion localization method for a vehicle, including:

[0008] Different and independent multi-source positioning information is obtained through multiple sensor sub-modules;

[0009] The multi-source positioning information is sorted according to the size of the observation timestamp, and the multi-source positioning information is filtered based on a preset time delay window;

[0010] The retained multi-source positioning information is filtered and fused to obtain the first positioning state data with time delay. In the filtering and fusion calculation process, abnormal observation verification, adaptive weight calculation and zero-velocity correction are performed. Abnormal observations are identified by abnormal observation verification, and adaptive correction coefficients are obtained by adaptive weight calculation. The adaptive correction coefficients are used to correct the filtering gain matrix to reduce the weight of the abnormal observations in the state update. Zero-velocity correction includes correcting the noise floor of the inertial measurement unit in multiple sensor sub-modules.

[0011] By using motion observation information to adjust the time-delayed first positioning state data, the second positioning state data at the current system time is obtained.

[0012] The second positioning state data is post-processed to obtain stable and smooth third positioning state data.

[0013] The method described in the embodiments of this application may also have the following additional technical features:

[0014] In the above technical solution, optionally, the multiple sensor sub-modules include: an inertial measurement unit, a wheel sensor, an inertial navigation device, a visual perception sensor, and radar;

[0015] The methods for filtering and fusion calculation include:

[0016] Initialization phase: Based on the retained multi-source localization information, determine the initial state variables, including position, attitude, and velocity;

[0017] Prediction phase: Acquire the acceleration and angular velocity collected by the inertial measurement unit, and update the state variables by integrating the acceleration and angular velocity to obtain the predicted state variables;

[0018] Observation update phase: Using the Kalman filter method, different predicted state variables are filtered and updated based on the different observations. The observations include data collected by wheel sensors, inertial navigation equipment, visual perception sensors, and radar.

[0019] Optionally, in any of the above technical solutions, the method further includes:

[0020] During the observation update phase, before filtering observation updates, abnormal observation verification, adaptive weight calculation, and zero-rate correction are performed.

[0021] Optionally, in any of the above technical solutions, anomaly observation verification is performed, including:

[0022] Calculate the Mahalanobis distance of the observed innovation and determine the abnormal observation based on the Mahalanobis distance of the observed innovation. The observed innovation is the difference between the predicted state quantity and the observed quantity. If the Mahalanobis distance of the observed innovation is greater than a preset threshold, the observation corresponding to the observed innovation is determined to be an abnormal observation.

[0023] Perform adaptive weight calculation, including:

[0024] A first Mahalanobis distance threshold and a second Mahalanobis distance threshold are set, where the first Mahalanobis distance threshold is less than the second Mahalanobis distance threshold. Based on the Mahalanobis distance of the observed information, the first Mahalanobis distance threshold, and the second Mahalanobis distance threshold, an adaptive correction coefficient is calculated. The formula for calculating the adaptive correction coefficient is as follows:

[0025]

[0026] α is the adaptive correction coefficient, c0 is the first Mahalanobis distance threshold, c1 is the second Mahalanobis distance threshold, and γ is the Mahalanobis distance of the observed innovation;

[0027] The correction formula for the gain matrix of the Kalman filter method is:

[0028]

[0029] K is the gain matrix, H is the observation matrix, P is the prior covariance matrix, and A is the covariance matrix of the observed innovation, A = HPH. T +R, where R is the error matrix of the observations.

[0030] Optionally, in any of the above technical solutions, the method further includes:

[0031] When calculating the Mahalanobis distance of the observed information, it is calculated separately for each individual component of the observed quantity;

[0032] When judging anomalous observations based on the Mahalanobis distance of the observed information, if the Mahalanobis distance of the observed information of a component is greater than a preset threshold, the component is determined to be an anomalous observation.

[0033] When adjusting weights using adaptive correction coefficients, the weights of components identified as anomalous observations are adjusted.

[0034] Optionally, in any of the above technical solutions, zero-velocity correction is performed, including:

[0035] Zero-speed correction is performed based on motor speed and vehicle vibration detection.

[0036] In any of the above technical solutions, optionally, zero-speed correction is performed based on motor speed and vehicle vibration detection, including:

[0037] When the motor speed is less than the motor speed threshold, it is preliminarily determined that the vehicle is stationary;

[0038] When the vehicle is stationary and the conditions for vehicle vibration detection are met, zero-speed correction is performed using observations when the vehicle speed is 0, the vehicle acceleration is 0, and the vehicle angular velocity is 0.

[0039] When the vehicle is determined to be stationary and the conditions for vehicle vibration detection are not met, zero-speed correction is performed using the observations when the vehicle speed is 0.

[0040] The conditions for vehicle body vibration testing are as follows:

[0041] |a+g| <a0

[0042] a is the acceleration value collected by the inertial measurement unit, g is the gravitational acceleration, and a0 is the judgment threshold.

[0043] Secondly, embodiments of this application provide a multi-sensor fusion positioning device for a vehicle, comprising:

[0044] The information acquisition module is used to acquire different and independent multi-source positioning information through multiple sensor sub-modules;

[0045] The filtering module is used to sort multi-source positioning information according to the size of the observation timestamp and filter multi-source positioning information based on a preset time delay window;

[0046] The filtering and fusion module is used to perform filtering and fusion calculations on the retained multi-source positioning information to obtain the first positioning state data with time delay. In the filtering and fusion calculation process, abnormal observation verification, adaptive weight calculation, and zero-velocity correction are performed. Abnormal observations are identified through abnormal observation verification, and adaptive correction coefficients are obtained through adaptive weight calculation. The adaptive correction coefficients are used to correct the filtering gain matrix to reduce the weight of the abnormal observations in the state update. Zero-velocity correction includes correcting the noise floor of the inertial measurement units in multiple sensor sub-modules.

[0047] The time adjustment module is used to adjust the time of the time-delayed first positioning state data using motion observation information to obtain the second positioning state data at the current system time.

[0048] The post-processing module is used to post-process the second positioning state data to obtain stable and smooth third positioning state data.

[0049] Thirdly, embodiments of this application provide a computer device including a processor and a memory, the memory storing programs or instructions that can run on the processor, the programs or instructions implementing the steps of the method as described in the first aspect when executed by the processor.

[0050] Fourthly, embodiments of this application provide a readable storage medium on which a program or instructions are stored, which, when executed by a processor, implement the steps of the method as described in the first aspect.

[0051] Fifthly, embodiments of this application provide a chip including a processor and a communication interface, the communication interface being coupled to the processor, the processor being used to run programs or instructions to implement the method as described in the first aspect.

[0052] In a sixth aspect, embodiments of this application provide a computer program product stored in a storage medium, which is executed by at least one processor to implement the method as described in the first aspect.

[0053] This application's embodiments of the vehicle multi-sensor fusion positioning method, apparatus, computer equipment, and readable storage medium read measurement data from each of multiple sensor sub-modules and perform independent positioning information calculations within each sensor sub-module to obtain their respective positioning information. These different and independent positioning information constitute multi-source positioning information. The multi-source positioning information is sorted according to the size of the observation timestamp, and data with excessively high delays is discarded according to a preset delay window size. The retained sorted positioning information is fed into a filtering algorithm for filtering and fusion calculation to obtain delayed first positioning state data. During the filtering and fusion calculation process, abnormal observation verification and adaptive weight calculation are performed. Abnormal observations are identified through abnormal observation verification, and adaptive correction coefficients are obtained through adaptive weight calculation. The adaptive correction coefficients are then used to correct the filtering gain matrix, thereby reducing the weight of abnormal observations in state updates, i.e., the fusion weight. The time-delayed first positioning state data is time-adjusted using motion observation information to obtain the second positioning state data at the current system moment. The obtained positioning data undergoes post-processing such as secondary filtering, data smoothing, and interpolation to obtain a stable and smooth positioning output.

[0054] In this embodiment, the sensor data are independent of each other, reducing system complexity and avoiding positioning failures. Furthermore, compared to related technologies that directly discard some abnormal observation data, this application reduces the weight of abnormal observations in state updates, improving data utilization and enhancing system stability. Zero-velocity correction is also performed during the filtering and fusion calculation process to correct the noise floor of the inertial measurement unit. Moreover, the calculation method using multiple filters is avoided in data delay processing; instead, data delay processing based on a preset delay window and time adjustment is used, reducing the computational load. Finally, post-processing yields a stable and smooth positioning output.

[0055] The above description is only an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of this application more obvious and understandable, the following are specific embodiments of this application. Attached Figure Description

[0056] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0057] Figure 1 A flowchart illustrating a multi-sensor fusion localization method for a vehicle according to an embodiment of this application is shown;

[0058] Figure 2A schematic diagram of the multi-sensor fusion positioning logic of a vehicle according to an embodiment of this application is shown;

[0059] Figure 3 This illustration shows a schematic diagram of multi-source information delay processing for vehicles according to an embodiment of this application;

[0060] Figure 4 A schematic diagram of the filtering and fusion calculation of a vehicle according to an embodiment of this application is shown;

[0061] Figure 5 A structural block diagram of a multi-sensor fusion positioning device for a vehicle according to an embodiment of this application is shown;

[0062] Figure 6 A structural block diagram of a computer device according to an embodiment of this application is shown. Detailed Implementation

[0063] The technical solutions of the embodiments of this application will be clearly described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application are within the scope of protection of this application.

[0064] The terms "first," "second," etc., used in the specification and claims of this application are used to distinguish similar objects and not to describe a specific order or sequence. It should be understood that such use of data can be interchanged where appropriate so that embodiments of this application can be implemented in orders other than those illustrated or described herein, and the objects distinguished by "first," "second," etc., are generally of the same class and the number of objects is not limited; for example, a first object can be one or more. Furthermore, in the specification and claims, "and / or" indicates at least one of the connected objects, and the character " / " generally indicates that the preceding and following objects are in an "or" relationship.

[0065] The multi-sensor fusion positioning method, apparatus, computer equipment, and readable storage medium for vehicles provided in this application will be described in detail below with reference to the accompanying drawings and through specific embodiments and application scenarios. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0066] This application provides a multi-sensor fusion localization method for vehicles, and the vehicle in this application can be an autonomous vehicle. Figure 1 and Figure 2 As shown, the method includes:

[0067] Step 101: Obtain different and independent multi-source positioning information through multiple sensor sub-modules.

[0068] In this step, measurement data from each of the multiple sensor submodules is read in, and independent positioning information is calculated within each sensor submodule to obtain its own positioning information. These different and independent positioning information constitute multi-source positioning information.

[0069] The multiple sensor submodules include at least two of the following: an inertial measurement unit (IMU), wheel sensors, an inertial navigation system (INS), a visual perception sensor, and radar. The IMU measures data including acceleration and angular velocity; the wheel sensors measure data including velocity; the INS's measurement data consists of data from the IMU and GNSS (GNSS) positioning, including pose information; and both the visual perception sensor and radar provide pose information. Matching radar, visual information, and maps can be performed simultaneously, or either one can be chosen. The radar can be a lidar, millimeter-wave radar, or even a measurement sensor capable of providing 3D point cloud information, such as binocular vision.

[0070] It should be noted that data from wheel sensors and inertial navigation equipment can be read directly, while positioning information based on vision and radar needs to be obtained through matching and calculation with high-precision maps.

[0071] In related technologies, the fusion of radar point clouds and visual features often employs deep or tight coupling, leading to high system complexity. Furthermore, a malfunction in one sensor directly results in positioning failure, negatively impacting the overall system stability. Therefore, this application establishes independent positioning information for each element, effectively addressing these issues.

[0072] Besides relying heavily on vehicle speed and inertial measurement unit (IMU) information, this application supports any combination of other sensors, and the data from each sensor are independent of each other, reducing system complexity and facilitating rapid deployment on different vehicle models and computing platforms.

[0073] Step 102: Sort the multi-source positioning information according to the size of the observation timestamp, and filter the multi-source positioning information based on the preset time delay window.

[0074] In this step, the multi-source positioning information is sorted according to the size of the observation timestamp, and data with excessively high latency is discarded according to the preset latency window size. This latency is mainly caused by the data transmission and processing.

[0075] Transmission or processing delays can cause observations with smaller timestamps to be received later than those with larger timestamps. For example... Figure 3 As shown, a delay window t is defined during processing. delay The latest observation timestamp is t n For timestamps less than t n -tdelay The observation data (i.e., location information) is discarded directly. The remaining observation data is sorted according to the timestamp size and then fed into the filtering calculation. In this way, the filtering algorithm will always obtain t. n -t delay The location value at time t. n The state variables at any given time can be obtained by reading the measurements from the inertial measurement unit (IMU) or the trajectory and extrapolating them through the integration of angular velocity and velocity.

[0076] Related technologies employ multiple filter calculation methods for data latency processing, increasing the computational load. However, this application's embodiment avoids this increased computational load and improves processing efficiency by setting a preset latency window for data latency processing.

[0077] Step 103: Perform filtering and fusion calculations on the retained multi-source positioning information to obtain the first positioning state data with time delay. During the filtering and fusion calculation process, abnormal observation verification, adaptive weight calculation, and zero-velocity correction are performed. Abnormal observations are identified through abnormal observation verification, and adaptive correction coefficients are obtained through adaptive weight calculation. The adaptive correction coefficients are used to correct the filtering gain matrix to reduce the weight of the abnormal observations in the state update. Zero-velocity correction includes correcting the noise floor of the inertial measurement units in multiple sensor sub-modules.

[0078] In this step, the retained sorted positioning information is fed into a filtering algorithm for filtering and fusion calculation to obtain the delayed first positioning status data. The delay time is the size of the pre-set time delay window mentioned in step 102.

[0079] Furthermore, anomaly observation verification and adaptive weight calculation are performed during the filtering and fusion calculation process. Anomaly observations are identified through verification, and adaptive correction coefficients are calculated using adaptive weights. These coefficients are then used to correct the filter gain matrix, thereby reducing the weight of anomaly observations in state updates, i.e., the fusion weight. Compared to related technologies that directly discard some anomaly observation data, this application reduces the weight of anomaly observations in state updates, improving data utilization and enhancing system stability. Additionally, zero-velocity correction is performed during the filtering and fusion calculation process to correct the noise floor of the inertial measurement unit.

[0080] In one embodiment of this application, the method of filtering fusion calculation includes:

[0081] Initialization phase: Based on the retained multi-source localization information, determine the initial state variables, including position, attitude, and velocity;

[0082] Prediction phase: Acquire the acceleration and angular velocity collected by the inertial measurement unit, and update the state variables by integrating the acceleration and angular velocity to obtain the predicted state variables;

[0083] Observation update phase: Using the Kalman filter method, different predicted state variables are filtered and updated based on the different observations. The observations include data collected by wheel sensors, inertial navigation equipment, visual perception sensors, and radar.

[0084] In this embodiment, the filtering calculation module mainly includes three steps: initialization, prediction, and observation update, such as... Figure 4 As shown:

[0085] (1) During the initialization phase, based on the multi-source positioning information, the initial state variables such as position, attitude, velocity, and IMU noise floor are determined. After successful initialization, the prediction and observation correction loop calculations begin.

[0086] (2) During the prediction phase, the acceleration and angular velocity information of the inertial measurement unit (IMU) are read in, and the position, attitude and velocity of the system are updated by integral calculation to obtain the predicted state variables.

[0087] (3) During the observation update phase, different predicted state quantities are corrected based on the different observations.

[0088] In one embodiment of this application, the method further includes: in the observation update phase, before performing filtered observation update, performing abnormal observation verification, adaptive weight calculation, and zero-rate correction.

[0089] In this embodiment, abnormal observation verification, adaptive weight calculation, and zero-rate correction are performed during the observation update phase. After these three processing steps, the observation update calculation is performed.

[0090] The localization filtering algorithm in this application can be Kalman filtering, particle filtering, or other Bayesian filtering algorithms, but Kalman filtering is preferred.

[0091] Related technologies typically employ sliding window-based multi-frame data smoothing or particle filtering methods for localization fusion algorithms. These methods result in excessive computational load, consuming significant system computing resources on embedded platforms and impacting the real-time performance of localization output. This application avoids these problems by not using sliding window-based multi-frame data smoothing or particle filtering methods.

[0092] In one embodiment of this application, outlier observations are primarily verified using a chi-square test, wherein the verification of outlier observations includes:

[0093] Calculate the Mahalanobis distance of the observed innovation and determine the abnormal observation based on the Mahalanobis distance of the observed innovation. The observed innovation is the difference between the predicted state quantity and the observed quantity. If the Mahalanobis distance of the observed innovation is greater than a preset threshold, the observation corresponding to the observed innovation is determined to be an abnormal observation.

[0094] The formula for calculating Mahalanobis distance is:

[0095] γ=r T A -1 r(1)

[0096] A = HPH T +R(2)

[0097] Where γ is the Mahalanobis distance, r is the observation information, A is the covariance matrix of the observation information, H is the observation matrix, P is the prior covariance matrix, and R is the error matrix of the observation.

[0098] In the chi-square test, the common practice is to calculate a Mahalanobis distance using the entire observation vector. However, if some observations are valid, discarding them entirely would reduce the utilization rate of the observation data. A significant improvement in this application is to perform the chi-square test separately for each component. That is, when calculating the Mahalanobis distance of the observation innovation, it is calculated separately for each individual component of the observation, i.e., using a single component in formula (1), and only taking the diagonal values ​​of matrix A during the calculation.

[0099] In related technologies, when calculating the overall observation vector, for example, when the observed position (x, y, z) is seen, in formula (1), r is a 3×1 matrix and A is a 3×3 matrix. Substituting these values ​​into formula (1) only yields one Mahalanobis distance value. This application calculates (x, y, z) separately to obtain three Mahalanobis distances. When calculating separately, only the corresponding values ​​on the diagonal of matrix A are used. The formulas for calculating the three Mahalanobis distances are as follows: r x A 00 r x r y A 11 r y r z A 22 r z , where A 00 A 11 A 22 r is the diagonal value of matrix A. x r y r z These are the three components of the observed new information.

[0100] When identifying anomalous observations based on the Mahalanobis distance of the observed information, if the Mahalanobis distance of a component's observed information is greater than a preset threshold, the component is determined to be an anomalous observation. When adjusting the weights using adaptive correction coefficients, the weights of the components determined to be anomalous observations are adjusted.

[0101] If all three observations are calculated together, an excessively large Mahalanobis distance will directly reduce the weights of all three observations. This application calculates each component separately. If an observation of a component is good, its weight can be increased; if an observation of a component is bad, its weight can be decreased. This application provides more precise control over the weights of observations, improving the utilization rate of the observation data.

[0102] In one embodiment of this application, adaptive weight calculation includes:

[0103] A first Mahalanobis distance threshold and a second Mahalanobis distance threshold are set, where the first Mahalanobis distance threshold is less than the second Mahalanobis distance threshold. Based on the Mahalanobis distance of the observed information, the first Mahalanobis distance threshold, and the second Mahalanobis distance threshold, an adaptive correction coefficient is calculated. The formula for calculating the adaptive correction coefficient is as follows:

[0104]

[0105] α is the adaptive correction coefficient, c0 is the first Mahalanobis distance threshold, c1 is the second Mahalanobis distance threshold, and γ is the Mahalanobis distance of the observed information.

[0106] Therefore, the gain matrix of the Kalman filter method is improved as follows based on the adaptive correction coefficients:

[0107]

[0108] K is the gain matrix, H is the observation matrix, P is the prior covariance matrix, and A is the covariance matrix of the observed innovation, A = HPH. T +R, where R is the error matrix of the observations.

[0109] When the observed information deviates significantly from the expectation, the gain matrix is ​​corrected by adaptive coefficients. By correcting the gain matrix, the weight of the corresponding anomalous observations in the state update can be reduced.

[0110] In one embodiment of this application, zero-speed correction includes: performing zero-speed correction based on motor speed and vehicle vibration detection.

[0111] Zero-speed correction is a process that corrects the noise bias of the inertial measurement unit (IMU) by using observation constraints that the vehicle speed and angular velocity are both zero when the vehicle is detected to be stationary, thereby improving the accuracy of system estimation.

[0112] Zero-speed detection in related technologies usually judges by using the wheel speed of the vehicle and the volatility of the inertial measurement unit (IMU) data within a time sliding window. However, it is found during the acquisition of real vehicle tests that there is a speed lag effect when using wheel speed information, and there is a 1 km / h jump in speed at low speeds, which will affect the judgment of the stationary state and cause calculation drift.

[0113] For electric drive vehicles, the rotational speed information of the motor can more timely reflect the motion state of the vehicle. Let the motor rotational speed threshold when the vehicle is stationary be r o , when the detected motor rotational speed is less than the motor rotational speed threshold r o , it can be considered that the vehicle is in a stationary state.

[0114] When performing zero-speed update, the following three observation equations are mainly used

[0115]

[0116] The above three observation equations successively represent that the vehicle speed v is 0, the vehicle acceleration is 0, and the vehicle angular velocity is 0. Among them, the calculation method of vehicle acceleration is the acceleration value a measured by the inertial measurement unit (IMU) minus the gravitational acceleration g and the base noise bias. The inertial measurement unit (IMU) usually includes two measurement devices, an accelerometer and a gyroscope. b acc represents the base noise of the accelerometer. The calculation method of vehicle angular velocity is the angular velocity value ω measured by the inertial measurement unit (IMU) minus the base noise bias. b gyro represents the base noise of the gyroscope.

[0117] For trucks or sanitation vehicle models, due to the inertia of the loaded goods or the shock absorption design of the vehicle head, there will be a phenomenon that the vehicle body is stationary but the vehicle head still shakes. Usually, the installation position of the inertial measurement unit (IMU) is in the cab. If the three observation equations of formula (5) are still used for zero-speed correction, it will cause a large error in the estimation of the base noise bias of the inertial measurement unit (IMU). At this time, a more reasonable approach is to only use the observation update when the vehicle speed is 0.

[0118] The detection of vehicle body vibration can be judged by the absolute value of the gravitational acceleration g. Assuming the judgment threshold is a0, when |a + g| < a0, it can be considered that the vehicle is completely in a stationary state. |a + g| < a0 is the vehicle body vibration detection condition, a is the acceleration value collected by the inertial measurement unit, and g is the gravitational acceleration.

[0119] In summary, in an embodiment of the present application, zero-speed correction is performed based on motor rotational speed and vehicle body vibration detection, including:

[0120] (1) First, determine whether the vehicle is stationary based on the motor speed. Specifically, if the motor speed is less than the motor speed threshold, it is preliminarily determined that the vehicle is stationary; otherwise, it is determined that the vehicle is not stationary.

[0121] (2) Then determine whether the conditions for vehicle vibration detection are met.

[0122] When the vehicle is determined to be stationary and the conditions for vehicle vibration detection are met, it indicates that the vehicle is completely stationary. Then, zero-speed correction is performed using the observations when the vehicle speed is 0, the vehicle acceleration is 0, and the vehicle angular velocity is 0. That is, zero-speed correction is performed using the three observation equations of formula (5).

[0123] If the vehicle is determined to be stationary and the conditions for vehicle vibration detection are not met, it indicates that the vehicle is not completely stationary. In this case, zero-speed correction is performed using the observations when the vehicle speed is 0.

[0124] By using the above methods, we can ensure that the noise floor bias estimation of the inertial measurement unit (IMU) is more accurate and reduce errors.

[0125] Step 104: Use motion observation information to adjust the time of the first positioning state data that is delayed in time, and obtain the second positioning state data at the current system time.

[0126] In this step, the motion observation information (i.e., measured values) for positioning is calculated based on the inertial measurement unit (IMU) or the flight track. The first positioning state data is then extrapolated to the positioning state at the current system time, which is the second positioning state data. For example, extrapolation is performed by integrating angular velocity and velocity, thus achieving time adjustment.

[0127] Step 105: Post-process the second positioning state data to obtain stable and smooth third positioning state data.

[0128] In this step, the obtained positioning data undergoes post-processing such as secondary filtering, data smoothing, and interpolation to obtain a stable and smooth positioning output.

[0129] In this embodiment, the sensor data are independent of each other, reducing system complexity and avoiding positioning failures. Furthermore, compared to related technologies that directly discard some abnormal observation data, this application reduces the weight of abnormal observations in state updates, improving data utilization and enhancing system stability. Zero-velocity correction is also performed during the filtering and fusion calculation process to correct the noise floor of the inertial measurement unit. Moreover, the calculation method using multiple filters is avoided in data delay processing; instead, data delay processing based on a preset delay window and time adjustment is used, reducing the computational load. Finally, post-processing yields a stable and smooth positioning output.

[0130] As a specific implementation of the aforementioned multi-sensor fusion localization method for vehicles, this application provides a multi-sensor fusion localization device for vehicles. For example... Figure 5 As shown, the multi-sensor fusion positioning device 500 for the vehicle includes: an information acquisition module 501, a parameter acquisition module, and a cleaning control module.

[0131] Among them, the information acquisition module 501 is used to acquire different and independent multi-source positioning information through multiple sensor sub-modules;

[0132] The filtering module 502 is used to sort the multi-source positioning information according to the size of the observation timestamp and filter the multi-source positioning information based on a preset time delay window.

[0133] The filtering and fusion module 503 is used to perform filtering and fusion calculations on the retained multi-source positioning information to obtain the first positioning state data with time delay. In the filtering and fusion calculation process, abnormal observation verification, adaptive weight calculation, and zero-velocity correction are performed. Abnormal observations are identified through abnormal observation verification, and adaptive correction coefficients are obtained through adaptive weight calculation. The adaptive correction coefficients are used to correct the filtering gain matrix to reduce the weight of the abnormal observations in the state update. Zero-velocity correction includes correcting the noise floor of the inertial measurement units in multiple sensor sub-modules.

[0134] The time adjustment module 504 is used to adjust the time of the time-delayed first positioning state data using motion observation information to obtain the second positioning state data at the current system time.

[0135] The post-processing module 505 is used to post-process the second positioning state data to obtain stable and smooth third positioning state data.

[0136] Furthermore, the multiple sensor sub-modules include: inertial measurement unit, wheel sensor, inertial navigation device, visual perception sensor, and radar;

[0137] The methods for filtering and fusion calculation include:

[0138] Initialization phase: Based on the retained multi-source localization information, determine the initial state variables, including position, attitude, and velocity;

[0139] Prediction phase: Acquire the acceleration and angular velocity collected by the inertial measurement unit, and update the state variables by integrating the acceleration and angular velocity to obtain the predicted state variables;

[0140] Observation update phase: Using the Kalman filter method, different predicted state variables are filtered and updated based on the different observations. The observations include data collected by wheel sensors, inertial navigation equipment, visual perception sensors, and radar.

[0141] Furthermore, the filtering and fusion module 503 is also used to: perform abnormal observation verification, adaptive weight calculation, and zero-rate correction before filtering observation updates during the observation update phase.

[0142] Further, anomaly observation verification is performed, including:

[0143] Calculate the Mahalanobis distance of the observed innovation and determine the abnormal observation based on the Mahalanobis distance of the observed innovation. The observed innovation is the difference between the predicted state quantity and the observed quantity. If the Mahalanobis distance of the observed innovation is greater than a preset threshold, the observation corresponding to the observed innovation is determined to be an abnormal observation.

[0144] Perform adaptive weight calculation, including:

[0145] A first Mahalanobis distance threshold and a second Mahalanobis distance threshold are set, where the first Mahalanobis distance threshold is less than the second Mahalanobis distance threshold. Based on the Mahalanobis distance of the observed information, the first Mahalanobis distance threshold, and the second Mahalanobis distance threshold, an adaptive correction coefficient is calculated. The formula for calculating the adaptive correction coefficient is as follows:

[0146]

[0147] α is the adaptive correction coefficient, c0 is the first Mahalanobis distance threshold, c1 is the second Mahalanobis distance threshold, and γ is the Mahalanobis distance of the observed innovation;

[0148] The correction formula for the gain matrix of the Kalman filter method is:

[0149]

[0150] K is the gain matrix, H is the observation matrix, P is the prior covariance matrix, and A is the covariance matrix of the observed innovation, A = HPH. T +R, where R is the error matrix of the observations.

[0151] Furthermore, the filtering and fusion module 503 is specifically used for:

[0152] When calculating the Mahalanobis distance of the observed information, it is calculated separately for each individual component of the observed quantity;

[0153] When judging anomalous observations based on the Mahalanobis distance of the observed information, if the Mahalanobis distance of the observed information of a component is greater than a preset threshold, the component is determined to be an anomalous observation.

[0154] When adjusting weights using adaptive correction coefficients, the weights of components identified as anomalous observations are adjusted.

[0155] Furthermore, zero-velocity correction is performed, including:

[0156] Zero-speed correction is performed based on motor speed and vehicle vibration detection.

[0157] In any of the above technical solutions, optionally, zero-speed correction is performed based on motor speed and vehicle vibration detection, including:

[0158] When the motor speed is less than the motor speed threshold, it is preliminarily determined that the vehicle is stationary;

[0159] When the vehicle is stationary and the conditions for vehicle vibration detection are met, zero-speed correction is performed using observations when the vehicle speed is 0, the vehicle acceleration is 0, and the vehicle angular velocity is 0.

[0160] When the vehicle is determined to be stationary and the conditions for vehicle vibration detection are not met, zero-speed correction is performed using the observations when the vehicle speed is 0.

[0161] The conditions for vehicle body vibration testing are as follows:

[0162] |a+g| <a0

[0163] a is the acceleration value collected by the inertial measurement unit, g is the gravitational acceleration, and a0 is the judgment threshold.

[0164] The multi-sensor fusion positioning device 500 for vehicles in this application embodiment can be a computer device or a component within a computer device, such as an integrated circuit or a chip. The multi-sensor fusion positioning device 500 for vehicles provided in this application embodiment can achieve... Figure 1 The various processes implemented in the multi-sensor fusion localization method for vehicles will not be described again here to avoid repetition.

[0165] This application also provides a computer device, such as... Figure 6 As shown, the computer device 600 includes a processor 601 and a memory 602. The memory 602 stores programs or instructions that can run on the processor 601. When the program or instructions are executed by the processor 601, they implement the various steps of the above-described multi-sensor fusion positioning method embodiment for vehicles and achieve the same technical effect. To avoid repetition, they will not be described again here.

[0166] The memory 602 can be used to store software programs and various data. The memory 602 may primarily include a first storage area for storing programs or instructions and a second storage area for storing data. The first storage area may store the operating system, application programs or instructions required for at least one function (such as sound playback, image playback, etc.). Furthermore, the memory 602 may include volatile memory or non-volatile memory, or both. The non-volatile memory may be read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), or flash memory. Volatile memory can be random access memory (RAM), static random access memory (SRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM), and direct memory bus RAM (DRRAM). The memory 602 in this embodiment includes, but is not limited to, these and any other suitable types of memory.

[0167] Processor 601 may include one or more processing units; optionally, processor 601 integrates an application processor and a modem processor, wherein the application processor mainly handles operations involving the operating system, user interface, and applications, and the modem processor mainly handles wireless communication signals, such as a baseband processor. It is understood that the aforementioned modem processor may also not be integrated into processor 601.

[0168] This application also provides a readable storage medium storing a program or instructions. When the program or instructions are executed by a processor, they implement the various processes of the above-described multi-sensor fusion positioning method for vehicles and achieve the same technical effect. To avoid repetition, they will not be described again here.

[0169] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element. Furthermore, it should be noted that the scope of the methods and apparatuses in the embodiments of this application is not limited to performing functions in the order shown or discussed, but may also include performing functions substantially simultaneously or in the reverse order, depending on the functions involved. For example, the described methods may be performed in a different order than described, and various steps may be added, omitted, or combined. Additionally, features described with reference to certain examples may be combined in other examples.

[0170] The embodiments of this application have been described above with reference to the accompanying drawings. However, this application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of this application without departing from the spirit and scope of the claims, and all of these forms are within the protection scope of this application.

Claims

1. A multi-sensor fusion positioning method of a vehicle, characterized by, The method comprises the following steps: acquiring different and independent multi-source positioning information through multiple sensor sub-modules; sorting the multi-source positioning information according to the size of the observation time stamp, and filtering the multi-source positioning information based on a preset time delay window; filtering and fusing the retained multi-source positioning information to obtain time-delayed first positioning state data; wherein, during the filtering and fusing process, abnormal observation checking, adaptive weight calculation and zero speed correction are performed, the abnormal observation checking is used to determine abnormal observation, the adaptive weight calculation is used to obtain an adaptive correction coefficient, and the adaptive correction coefficient is used to correct the filtering gain matrix to reduce the weight of the abnormal observation in state updating, and the zero speed correction includes correcting the noise of the inertial measurement unit in the multiple sensor sub-modules; adjusting the time-delayed first positioning state data using motion observation information to obtain second positioning state data at the current system time; post-processing the second positioning state data to obtain stable and smooth third positioning state data.

2. The multi-sensor fusion positioning method of claim 1, wherein, The multiple sensor sub-modules include an inertial measurement unit, a wheel sensor, an inertial navigation device, a visual perception sensor and a radar. The filtering and fusing method comprises the following steps: an initialization stage: determining state quantities at an initial time according to the retained multi-source positioning information, the state quantities including position, attitude and speed; a prediction stage: acquiring acceleration and angular velocity collected by the inertial measurement unit, and updating the state quantities by integrating the acceleration and the angular velocity to obtain predicted state quantities; an observation updating stage: performing filtering observation updating on different predicted state quantities based on different observation data collected by the wheel sensor, the inertial navigation device, the visual perception sensor and the radar through the Kalman filtering method.

3. The multi-sensor fusion positioning method of claim 2, wherein, The method further comprises the following steps: in the observation updating stage, before performing the filtering observation updating, performing abnormal observation checking, adaptive weight calculation and zero speed correction.

4. The multi-sensor fusion positioning method of claim 1, wherein, The abnormal observation checking comprises the following steps: calculating the Mahalanobis distance of the observation innovation, and determining the abnormal observation according to the Mahalanobis distance of the observation innovation, wherein the observation innovation is the difference between the predicted state quantity and the observation, and if the Mahalanobis distance of the observation innovation is greater than a preset threshold, the observation corresponding to the observation innovation is determined as the abnormal observation; The adaptive weight calculation comprises the following steps: setting a first Mahalanobis distance threshold and a second Mahalanobis distance threshold, the first Mahalanobis distance threshold being smaller than the second Mahalanobis distance threshold, and calculating the adaptive correction coefficient according to the Mahalanobis distance of the observation innovation, the first Mahalanobis distance threshold and the second Mahalanobis distance threshold; wherein the calculation formula of the adaptive correction coefficient is: α is the adaptive correction coefficient, c0 is the first Mahalanobis distance threshold, c1 is the second Mahalanobis distance threshold, and γ is the Mahalanobis distance of the observation innovation; The correction formula of the gain matrix of the Kalman filtering method is: K is a gain matrix, H is an observation matrix, P is a prior covariance matrix, A is a covariance matrix of observation innovation, A = HPH T + R, R is an error matrix of observation.

5. The multi-sensor fusion positioning method of claim 4, wherein, The method further comprises the following steps: when calculating the Mahalanobis distance of the observation innovation, the Mahalanobis distance of each component of the observation is calculated respectively; when determining the abnormal observation according to the Mahalanobis distance of the observation innovation, if the Mahalanobis distance of the observation innovation of a component is greater than a preset threshold, the component is determined as the abnormal observation. When the adaptive correction coefficient is used to adjust the weight, the weight of the component determined as the abnormal observation is adjusted.

6. The multi-sensor fusion positioning method of claim 3, wherein, The zero speed correction is performed, including: The zero speed correction is performed based on the motor speed and the vehicle body vibration detection.

7. The multi-sensor fusion positioning method of claim 6, wherein, The zero speed correction is performed based on the motor speed and the vehicle body vibration detection, including: When the motor speed is less than a motor speed threshold, it is preliminarily determined that the vehicle is in a stationary state; When it is determined that the vehicle is in a stationary state and the vehicle body vibration detection condition is met, the observation when the vehicle speed is 0, the vehicle acceleration is 0 and the vehicle angular speed is 0 is used for the zero speed correction; When it is determined that the vehicle is in a stationary state and the vehicle body vibration detection condition is not met, the observation when the vehicle speed is 0 is used for the zero speed correction; The vehicle body vibration detection condition is: |a+g|<a0 a is an acceleration value collected by an inertial measurement unit, g is a gravity acceleration, and a0 is a judgment threshold.

8. A multi-sensor fusion positioning device of a vehicle, characterized by, It includes: An information acquisition module is configured to acquire different and independent multi-source positioning information through multiple sensor sub-modules; A screening module is configured to sort the multi-source positioning information according to the size of the observation time stamp, and screen the multi-source positioning information based on a preset time delay window; A filter fusion module is configured to perform filter fusion calculation on the retained multi-source positioning information to obtain time-delayed first positioning state data; wherein, during the filter fusion calculation, abnormal observation checking, adaptive weight calculation and zero speed correction are performed, the abnormal observation checking is used to determine abnormal observation, the adaptive weight calculation is used to obtain an adaptive correction coefficient, the adaptive correction coefficient is used to correct a filter gain matrix to reduce the weight of the abnormal observation in state updating, and the zero speed correction includes correcting the noise of an inertial measurement unit in the multiple sensor sub-modules; A time adjustment module is configured to perform time adjustment on the time-delayed first positioning state data using motion observation information to obtain second positioning state data at a current system time; A post-processing module is configured to perform post-processing on the second positioning state data to obtain stable and smooth third positioning state data.

9. A computer device, comprising: The program or instruction is executed by the processor to implement the steps of the multi-sensor fusion positioning method of the vehicle according to any one of claims 1 to 8.

10. A readable storage medium, on which a program or instructions are stored, characterized in that, The program or instruction is executed by the processor to implement the steps of the multi-sensor fusion positioning method of the vehicle according to any one of claims 1 to 8.

Images