Positioning reliability judgment method and system and medium

Abstract

The invention relates to a positioning reliability judgment method and system and a medium. The method comprises the following steps: constructing an error state kinematics model driven by an inertial measurement device; fusing the information of at least one complementary sensor with the information of the inertial measurement device, and constructing a filtering model; the uncertainty distribution of the information of each sensor and the corresponding weight are transmitted into the filtering model, and the uncertainty distribution of a final positioning result is obtained through uncertainty transmission calculation; and setting an alarm value of the current positioning point through real-time positioning information, and judging the reliability of the final positioning result according to a preset confidence accuracy threshold and the uncertainty distribution. According to the method, the problems of discontinuity, threshold jump, low upper limit of judgment precision and the like in the current judgment method can be solved.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle positioning, and in particular to a positioning reliability judgment method, system and medium. Background Art

[0002] With the progress and development of the autonomous driving industry, the level of autonomous driving has gradually evolved from L2 / L2+ to L3 / L3+ and higher levels. Autonomous driving vehicles require accurate position, speed and direction information for path planning, control, safety assurance and other functions, and high-level autonomous driving has stringent requirements on positioning accuracy, real-time performance and reliability. False positives in positioning results may lead to wrong decisions in downstream modules, resulting in traffic hazards; on the contrary, highly reliable positioning modules can provide solid protection for autonomous driving vehicles, reduce the incidence of accidents, and give full play to the safety advantages of autonomous driving over human driving.

[0003] There are many sensors that can be used to measure the position, speed, and angle changes of moving objects. The most common ones include GNSS (Global Navigation Satellite System), IMU (Inertial Measurement Unit), LiDAR, visual odometer, various cameras, various radars, etc. These sensors have their own advantages and disadvantages. For example, GNSS can stably achieve centimeter-level accuracy in open scenes after combining with RTK (Real-time Kinematic Measurement Technology), but the accuracy will be degraded in occluded environments. IMU obtains three-axis posture information through its own accelerometer and gyroscope integration, which is not affected by external interference, but the error term will accumulate. The classic approach is to combine GNSS with IMU, and through filtering or optimization algorithms, achieve the advantages and disadvantages of complementing each other, and provide a stable, continuous, and high-frequency combined navigation signal. In addition, adding the vehicle's own speed measurement signal (Odometry) and the real-time vision and pre-established map matching unit (Vision-Map Matching) can explore better positioning performance.

[0004] The accuracy of autonomous vehicle positioning describes the deviation of the positioning point given by the system from the actual position, while reliability describes the ability to characterize the size of this deviation. If the deviation may exceed the maximum acceptable limit of the current autonomous driving scenario - the alert value (Alert Limit), then corresponding measures can be taken, such as exiting the autonomous driving mode and returning to the driver-controlled mode to ensure safety. The reliability of the positioning system can only be close to 100%, and it is difficult to achieve a true 100%. Therefore, when the probability of the positioning point being located in the area within the alert value is lower than a certain requirement (such as L3 autonomous driving requires a positioning confidence accuracy of more than 99.9999%), the result can be considered unreliable and measures need to be taken.

[0005] At present, there are some evaluation methods for the reliability of multi-source integrated navigation systems, such as the risk matrix method (LSR) based on fault detection. However, this evaluation method has problems such as discontinuity, jumps at the threshold, and low upper limit of evaluation accuracy. Summary of the invention

[0006] In view of the shortcomings of the prior art described above, the present invention provides a positioning reliability judgment method, system and medium, aiming to solve the problems of discontinuity, threshold jumps, and low upper limit of judgment accuracy existing in some current traditional reliability judgment methods.

[0007] To achieve the above purpose, the present invention adopts the following technical solution.

[0008] In a first aspect, the present invention provides a method for determining positioning reliability, comprising: Construct an error-state kinematic model of the inertial measurement unit drive; fusing at least one complementary sensor information with the inertial measurement device information to construct a filtering model; The uncertainty distribution and corresponding weights of each sensor information are passed into the filtering model, and the uncertainty distribution of the final positioning result is obtained through uncertainty transfer calculation; The warning value of the current positioning point is set through real-time positioning information, and the reliability of the final positioning result is judged according to the preset confidence accuracy threshold and the uncertainty distribution.

[0009] In a possible implementation, constructing an error state kinematic model driven by an inertial measurement device includes: Construct a state vector containing displacement, velocity, angle, acceleration deviation, and angular velocity deviation; Construct an input vector containing accelerometer measurements and angular velocity meter measurements; Construct a noise vector containing velocity disturbance, angle disturbance, acceleration disturbance and angular velocity disturbance; According to Euler numerical integration formula, the error state at the previous moment, the input signal of the inertial measurement device in this interval and the noise vector are used to obtain the error state and covariance matrix at the current moment.

[0010] In a possible implementation manner, the error state kinematic model obeys a multi-dimensional normal distribution.

[0011] In a possible implementation manner, the at least one complementary sensor information includes at least one of global satellite navigation system information, vehicle odometry information, and vision-map matching information.

[0012] In a possible implementation, the filtering model is a chained error state Kalman filtering model.

[0013] In a possible implementation, constructing the filtering model includes: Establish observation equations to relate each observation signal to the system error state quantity; Perform the measurement update process of Kalman filter to obtain the updated error state and system covariance matrix; The updated error state is added back to the system state to obtain the latest estimated system state quantity.

[0014] In a possible implementation, the uncertainty distribution of the final positioning result obtained by uncertainty transfer calculation includes: The uncertainty distribution of each sensor information is represented as a multi-dimensional Gaussian distribution; The product of the probability density functions of the multi-dimensional Gaussian distribution is calculated to obtain the uncertainty distribution of the final positioning result.

[0015] In a possible implementation, setting the warning value of the current positioning point by using the real-time positioning information includes: obtaining the minimum distance of the current vehicle from the lane edge or obstacle in each direction; The minimum distance is multiplied by the safety factor to obtain the warning value of the current positioning point.

[0016] In a possible implementation, obtaining the minimum distance of the current vehicle from the lane edge or obstacle in each direction includes: Obtain the first minimum distance by fusing the pose information and the map in real time; Acquiring the second minimum distance through visual perception; The smaller value between the first minimum distance and the second minimum distance is taken as the minimum distance.

[0017] In a possible implementation manner, determining the reliability of the final positioning result includes: Calculate the minimum interval of the positioning point in each direction that satisfies the confidence accuracy rate according to the uncertainty distribution; comparing the minimum interval with the warning value; When the minimum interval is greater than the warning value, determining that the final positioning result is unreliable; When the minimum interval is less than or equal to the warning value, it is determined that the final positioning result is reliable.

[0018] In a second aspect, the present invention provides a positioning reliability judgment system, comprising: A kinematic model building module, at least for building an error state kinematic model driven by an inertial measurement unit; A filter model building module, at least used to fuse at least one complementary sensor information with the inertial measurement device information to build a filter model; An uncertainty distribution determination module is at least used to transfer the uncertainty distribution and corresponding weights of each sensor information into the filtering model, and obtain the uncertainty distribution of the final positioning result through uncertainty transfer calculation; The reliability determination module is at least used to set the warning value of the current positioning point through real-time positioning information, and judge the reliability of the final positioning result according to a preset confidence accuracy threshold and the uncertainty distribution.

[0019] In a third aspect, the present invention provides a readable storage medium, which adopts the following technical solution: A readable storage medium stores computer instructions, and when the computer instructions are executed by a processor, a positioning reliability judgment method as described in any one of the first aspects above is implemented.

[0020] In summary, compared with the prior art, the present invention includes at least one of the following beneficial technical effects: The present invention constructs an error state kinematic model driven by an inertial measurement device, integrates information from at least one complementary sensor (such as GNSS, Odometry, Vision-Map Matching), and uses a chain filter model to achieve uncertainty transfer of multi-source sensor data, thereby accurately calculating the error distribution of the final positioning result. Compared with traditional reliability assessment methods (such as the risk matrix method), the present invention has a higher theoretical upper limit of accuracy in error modeling, overcomes the problems of threshold jumps and evaluation discontinuity, can dynamically set warning values, and perform real-time reliability judgments based on confidence accuracy, thereby significantly improving the safety and robustness of the positioning system in high-level autonomous driving scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For those skilled in the art, other drawings can be obtained based on these drawings without creative work.

[0022] Figure 1 It is a flowchart of an embodiment of the positioning reliability judgment method of the present invention.

[0023] Figure 2 It is a flowchart of another embodiment of the positioning reliability judgment method of the present invention.

[0024] Figure 3It is a flowchart of another embodiment of the positioning reliability judgment method of the present invention.

[0025] Figure 4 It is a flowchart of another embodiment of the positioning reliability judgment method of the present invention.

[0026] Figure 5 It is a flowchart of another embodiment of the positioning reliability judgment method of the present invention.

[0027] Figure 6 It is a flowchart of another embodiment of the positioning reliability judgment method of the present invention.

[0028] Figure 7 It is a flow chart of an embodiment of a method for determining positioning reliability of the present invention.

[0029] Figure 8 It is a structural diagram of a specific embodiment of the positioning reliability judgment system of the present invention. DETAILED DESCRIPTION

[0030] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application. In addition, it should be understood that the specific implementation methods described herein are only used to illustrate and explain the present application, and are not used to limit the present application.

[0031] It should be noted that the description order of the following embodiments is not intended to limit the preferred order of the embodiments of the present application. In addition, in the following embodiments, the description of each embodiment has its own emphasis, and for parts not described in detail in one embodiment, reference can be made to the relevant description of other embodiments.

[0032] The method steps described in the embodiments of the present invention may be executed in the order described in the specific implementation manner, or the execution order of each step may be adjusted according to actual needs and on the premise that the technical problem can be solved, which will not be listed here one by one.

[0033] Reference Figure 1 , an embodiment of the present invention provides a positioning reliability judgment method, including the following steps.

[0034] S1, construct the error state kinematic model of the inertial measurement unit drive.

[0035] In some embodiments, based on the output signal of the inertial measurement unit (IMU) (such as acceleration and angular velocity), a numerical integration method (such as Euler integration or fourth-order Runge-Kutta integration) is used to achieve recursive solution of the posture state; an error state vector model including three-axis position, velocity, attitude angle, and three-axis deviations of the accelerometer and gyroscope is established; an IMU noise model (such as zero bias uncertainty and random walk noise) is introduced and modeled as isotropic Gaussian white noise; a first-order Markov chain is used to represent the state transition process to describe the correlation between state variables.

[0036] In some implementations, noise parameters may be obtained through experimental calibration according to the specific model of the IMU to further optimize the accuracy of the error model.

[0037] Together, these methods support the construction of a complete error-state kinematic model, providing a basis for subsequent steps.

[0038] S2, fusing at least one complementary sensor information with the inertial measurement device information to construct a filtering model.

[0039] In some embodiments, the filtering model is a chained error state Kalman filtering model, which gradually integrates observation data of complementary sensors such as global satellite navigation system information (GNSS), vehicle odometry information, and vision-map matching information (Vision-MapMatching) with the IMU-driven error state model to construct a loosely coupled filtering system (such as GNSS / INS, Odometry / INS, Vision-Map Matching / INS).

[0040] In some embodiments, an observation equation is used to establish a relationship between sensor observables and system state quantities, and the state estimate is optimized through measurement updates.

[0041] In some implementations, Gaussian white noise is defined as the noise model of each sensor according to its characteristics, and the dynamic correction of the error state is achieved by using filter gain calculation.

[0042] In some implementations, according to actual needs, algorithms such as extended Kalman filter (EKF), unscented Kalman filter (UKF) or particle filter (PF) can be used to adapt to data fusion of nonlinear systems or complex scenarios to ensure the applicability and robustness of the filtering model.

[0043] These methods provide diverse implementation paths for achieving optimal integration of multi-source data.

[0044] S3, passing the uncertainty distribution and corresponding weight of each sensor information into the filtering model, and obtaining the uncertainty distribution of the final positioning result through uncertainty transfer calculation.

[0045] In some embodiments, according to the sensor characteristics (such as signal occlusion of GNSS, sliding error of Odometry, illumination change of Vision-Map Matching, etc.), a Gaussian white noise model or a noise model with amplitude variation is constructed, and its parameters are calibrated based on experimental data.

[0046] In some embodiments, the covariance matrix of the error state in the Kalman filter is used to dynamically update the system uncertainty distribution, and the weight of each sensor is adjusted through the Kalman gain or the confidence-based weight allocation method.

[0047] In some embodiments, during the fusion process, a product calculation method of a probability density function may be used to derive a multi-dimensional Gaussian distribution of the final positioning result, and to evaluate its mean and covariance.

[0048] In some implementations, in order to adapt to complex scenarios, an uncertainty modeling method based on Bayesian estimation can be combined to achieve uncertainty fusion of multi-source heterogeneous sensor data.

[0049] These implementations ensure that accurate and dynamic positioning error distribution is generated through weight adjustment and uncertainty propagation of multi-source data.

[0050] S4, setting a warning value of the current positioning point through real-time positioning information, and judging the reliability of the final positioning result according to a preset confidence accuracy threshold and the uncertainty distribution.

[0051] In some embodiments, map information and real-time positioning data are used in combination with visual perception results (such as lane edge or obstacle distance) to calculate the minimum distance from the vehicle to the safety boundary, and a safety factor is introduced to set a warning value.

[0052] In some implementations, based on the properties of Gaussian distribution, a distribution interval that satisfies a preset confidence accuracy threshold (eg, 99.9999%) is selected to determine the confidence range of the positioning result.

[0053] In some implementations, whether the positioning result is reliable is determined by comparing the confidence interval of the positioning result with the warning value area.

[0054] In some embodiments, such as in complex scenarios, the alert value may be dynamically adjusted, for example, optimized based on real-time parameters such as vehicle speed and road environment (tunnel or open area).

[0055] In addition, in some implementations, historical data or machine learning algorithms may be combined to predict possible positioning deviations to enhance the adaptability and accuracy of reliability assessment.

[0056] These implementations ensure accurate judgment of positioning reliability in a variety of autonomous driving scenarios.

[0057] Further, as an embodiment of the present invention, refer to Figure 2 , step S1, constructing the error state kinematic model driven by the inertial measurement device includes the following steps.

[0058] S11, construct a state vector including displacement, velocity, angle, acceleration deviation and angular velocity deviation.

[0059] Specifically, the components of the state vector are defined, where displacement, velocity and angle describe the position, motion rate and rotation attitude in the three-axis coordinate system respectively, while acceleration deviation and angular velocity deviation represent the systematic deviations of the accelerometer and gyroscope in the inertial measurement unit.

[0060] In some implementations, depending on the application scenario, different coordinate systems (such as a vehicle coordinate system, a global coordinate system, or a navigation coordinate system) may be used to represent the state vector, and the conversion between different coordinate systems may be achieved through a coordinate transformation matrix.

[0061] In some embodiments, the state vector may be further refined, for example, by introducing high-order dynamic parameters (such as acceleration change rate) or environmental interference terms (such as temperature drift) to improve model accuracy.

[0062] During initialization, the initial value of the state vector can be assigned through a static calibration experiment or the measurement value of an external auxiliary sensor (such as GNSS), thereby laying the foundation for the subsequent calculation of the error state kinematic model.

[0063] S12, construct an input vector including accelerometer measurements and angular velocity meter measurements.

[0064] In some implementations, based on an output signal of an inertial measurement unit (IMU), three-axis acceleration data provided by an accelerometer and three-axis angular velocity data provided by a gyroscope are used as main components of an input vector.

[0065] In some implementations, input data may be acquired by sampling at fixed time intervals to ensure the temporal continuity of the data.

[0066] In some embodiments, in order to improve the accuracy of the input vector, a dynamic calibration mechanism of the sensor (such as temperature compensation or nonlinear error correction) may be introduced.

[0067] In some complex scenarios, the acceleration and angular velocity can be corrected in real time by combining data from external sensors (such as GNSS or wheel speedometers).

[0068] In addition, in some implementations, the input vector may be preprocessed (such as filtering and denoising or outlier removal) according to specific application requirements to reduce the interference of noise on the kinematic model.

[0069] These implementations ensure that the input vector fully reflects the dynamic characteristics of the IMU while also having sufficient accuracy and robustness.

[0070] S13, constructing a noise vector including velocity disturbance, angle disturbance, acceleration disturbance and angular velocity disturbance.

[0071] In some embodiments, according to the characteristics of an inertial measurement unit (IMU), a zero-bias uncertainty and random walk noise model is used to model velocity disturbance, angle disturbance, acceleration disturbance, and angular velocity disturbance as independent Gaussian white noises.

[0072] In some implementations, the noise characteristics (such as covariance and spectral density) of the IMU may be calibrated experimentally to assign specific parameters to the noise vector.

[0073] In some application scenarios, the amplitude or structure of the noise model is dynamically adjusted according to different IMU types and accuracy requirements, such as introducing non-Gaussian noise or process noise terms in high dynamic environments.

[0074] In some embodiments, to improve the robustness of the noise model, environmental sensor data (such as temperature or vibration information) may be combined to optimize the real-time estimation of the disturbance noise.

[0075] In addition, in some embodiments, the noise vector can be further modified by Monte Carlo simulation or Bayesian inference method to reflect the statistical characteristics of disturbances in complex scenarios.

[0076] These methods ensure that the noise vector can truly describe the random disturbance characteristics of the IMU signal and provide reliable input for error state calculation.

[0077] S14, according to the Euler numerical integration formula, the error state at the previous moment, the input signal of the inertial measurement device in this interval, and the noise vector are used to obtain the error state and covariance matrix at the current moment.

[0078] In some implementations, the Euler integration method is used to recursively calculate the current error state by time discretizing the input signal (accelerometer and gyroscope data) of the inertial measurement device.

[0079] In some implementations, according to the dynamic characteristics of the error state model, the standard Euler integration, first-order difference integration, or second-order integration method may be selected to improve the calculation accuracy.

[0080] The noise vector is introduced into the system state transfer equation through random disturbance terms during the integration process to simulate the influence of actual noise on the error state.

[0081] The calculation of the covariance matrix is ​​based on the state transfer Jacobian matrix and the noise covariance matrix, and its value is dynamically updated using numerical methods.

[0082] In some cases of high precision requirements, improved integration methods (such as Runge-Kutta integration) or adaptive step-size integration methods can be introduced to balance computational efficiency and accuracy.

[0083] These implementations ensure that the current error state and covariance matrix can accurately reflect the dynamic changes of the system, laying the foundation for subsequent filtering steps.

[0084] Wherein, the error state kinematic model obeys multi-dimensional normal distribution.

[0085] Exemplarily, after the error state kinematic model is initialized, a frame of IMU signal is received, and the frame of IMU signal is used to implement recursive solution of the model state within the time interval.

[0086] The model's state vector, error state, IMU signal input, and disturbance noise vector can be expressed as: Among them, x is the state vector of the model, including displacement p, velocity v, angle θ, acceleration deviation a b , angular velocity deviation ω b ; δx is the error state, which contains the error value of each component in the state vector; i m is the input vector containing the accelerometer measurement value a m and the angular velocity meter's measured value ω m ; n is the noise vector, containing the velocity disturbance v n , the perturbation of angle v n , acceleration disturbance a n and the angular velocity perturbation ω n .

[0087] According to Euler's numerical integration formula, after knowing the error state at the previous moment, the input signal of the IMU in this interval, and the noise vector, we can get: where F x and F n are the Jacobian matrices of the error state and noise vector, respectively, indicating the correlation between state variables during state transfer; Q n is the covariance matrix of the perturbation noise.

[0088] Among them, F x , Fn , Q n They are: Where △t represents the time interval, R is the rotation matrix representation of the angle in the state vector, [] × To solve the antisymmetric matrix of the vector, V n , Θ n , A n ,Ω n are the integrals of the covariance of each component in the noise vector in the time interval. The covariance of each component in the noise vector can be obtained by consulting the IMU model documentation and conducting relevant experiments.

[0089] The Gaussian white noise of each vector is also Gaussian white noise after superposition. According to the formula: The error state kinematic model distribution of IMU drive can be obtained as follows: The model follows a multidimensional normal distribution. is the mean and P is the covariance matrix.

[0090] Further, as an embodiment of the present invention, refer to Figure 3 In step S2, constructing the filtering model includes the following steps.

[0091] S21, establish observation equations to associate each observation signal with the system error state quantity.

[0092] In some implementations, according to the observation characteristics of different sensors (such as GNSS provides position information, Odometry provides speed information, Vision-Map Matching provides position or angle information), relationship equations between sensor observation quantities and system state quantities are established respectively.

[0093] In some embodiments, the observation equation may be expressed as a linear or nonlinear function of the observation value, combined with a noise term to describe the sensor observation error.

[0094] In some embodiments, for the nonlinear observation equation, a Taylor expansion may be used to construct a linearized Jacobian matrix for calculation in a Kalman filter.

[0095] In some implementations, for different sensors, the observation noise may be based on a Gaussian white noise model, or a non-Gaussian noise or weighted observation model may be used in combination with actual application scenarios.

[0096] In addition, in some embodiments, the observation equation can also be dynamically adjusted according to the environment and task requirements, such as shielding certain observation quantities when the signal is lost or the quality is degraded to improve the overall filtering stability.

[0097] These implementations ensure that the observation equations can accurately relate the observed signals to the error states, providing reliable inputs for the measurement update process.

[0098] S22, performing a measurement update process of the Kalman filter to obtain an updated error state and a system covariance matrix.

[0099] In some embodiments, based on a standard Kalman filter approach, the system state estimate and the covariance matrix are modified using the measurement residuals of the observation equation and the Kalman gain.

[0100] In some embodiments, such as in nonlinear systems, an extended Kalman filter (EKF) may be used to perform measurement updates through the linearized Jacobian matrix of the observation equation, or an unscented Kalman filter (UKF) may be used to directly update the nonlinear model.

[0101] In some embodiments, multiple source sensors may be combined, and the Kalman gain may be dynamically adjusted according to weight or confidence to balance the influence of different data sources.

[0102] In some implementations, to handle the uncertainty of observation noise, an adaptive filtering technique may be used to dynamically adjust the measurement covariance according to the noise characteristics.

[0103] In some complex environments, multi-hypothesis filtering or particle filtering technology can be introduced to improve the robustness of the filter by measuring and updating multiple candidate states.

[0104] These implementations ensure that the error state and covariance matrix of the system are dynamically optimized through an accurate measurement update process, providing a reliable basis for subsequent system state estimation.

[0105] S23, adding the updated error state back to the system state to obtain the latest estimated system state quantity.

[0106] In some implementations, after each Kalman filter measurement update is completed, the updated error state quantity is directly added to the priori system state quantity to obtain the latest system state quantity.

[0107] For nonlinear systems, the state transfer function can be used to map the updated error state and then add it back to the system state to ensure the accuracy of the nonlinear model.

[0108] In the multi-source sensor fusion scenario, the error states of each source can be weighted and superimposed according to the update frequency of different sensors to update the system state.

[0109] In some implementations, to improve robustness, especially in the case of large noise, data smoothing technology can be introduced to filter the updated state to reduce fluctuations.

[0110] When multi-step prediction is needed, the updated state quantity can be used as the initial value of the next time step to recursively calculate the subsequent state.

[0111] These implementations ensure that the system state quantity maintains both accuracy and stability after each update, providing reliable dynamic state estimation for positioning or navigation tasks.

[0112] Exemplarily, GNSS (Global Satellite Navigation System), Odometry (the vehicle's own range or speed information), and Vision-Map Matching (vision-map matching unit) are used as sensor information complementary to IMU to construct a chain error state Kalman filter model.

[0113] Loosely coupled filtering solutions are performed for GNSS / INS, Odometry / INS, and Vision-Map Matching / INS respectively. Each observation signal can establish an observation equation with the system error state quantity: y=h(δx)+r,r~N{0,R}; Among them, r is the noise vector of the observed signal, which can generally be processed as Gaussian white noise, and R is its covariance matrix.

[0114] Then the measurement update process of Kalman filter is carried out: K=P priorestimation H T (HP priorestimation H T +R) -1 ; δx postestimation =K(yh(δx priorestimation )); P postestimation =(I-KH)P priorestimation ; Where K is the Kalman filter gain, H is the Jacobian matrix of the measurement equation for δx, and P priorestimation is the system covariance matrix before measurement update, P postestimation is the system covariance matrix after measurement update.

[0115] The updated error state δx postestimation Adding it back to the system state x, we can get the latest estimated system state, which is the output result of fusion positioning.

[0116] Further, as an embodiment of the present invention, refer to Figure 4 In step S3, the uncertainty distribution of the final positioning result obtained by uncertainty transfer calculation includes the following steps.

[0117] S31, representing the uncertainty distribution of each sensor information as a multi-dimensional Gaussian distribution.

[0118] In some embodiments, according to the characteristics of each sensor (such as positioning error of GNSS, cumulative drift of IMU, sliding deviation of Odometry, etc.), its error model is established through experimental calibration or theoretical analysis, and described as a multi-dimensional Gaussian distribution consisting of a mean and a covariance matrix.

[0119] In some embodiments, specific observation dimensions of the sensor (such as position, speed, or angle) can be modeled separately and then synthesized into an overall multi-dimensional Gaussian distribution.

[0120] In some embodiments, if certain sensor error characteristics have a non-Gaussian distribution, they may be approximated by a Gaussian mixture model (GMM) or a method based on kernel density estimation.

[0121] In some implementations, the parameters of the multi-dimensional Gaussian distribution are dynamically adjusted according to environmental conditions (such as occlusion and vibration) to reflect the characteristics of uncertainty changing with the scene.

[0122] Furthermore, in some implementations, the covariance matrix parameters of the uncertainty distribution may be optimized through statistical analysis of historical sensor data.

[0123] These implementations ensure that the error distribution of each sensor can be accurately and flexibly represented in the form of a multi-dimensional Gaussian distribution, providing a precise basis for subsequent fusion calculations.

[0124] S32, calculating the product of the probability density function of the multi-dimensional Gaussian distribution to obtain the uncertainty distribution of the final positioning result.

[0125] In some implementations, based on the mathematical properties of multi-dimensional Gaussian distribution, the fusion results of the covariance matrix and the mean vector of the error distributions of multiple sensors are directly calculated.

[0126] For the covariance matrix, the matrix inverse sum method can be used to combine the weights of each sensor to generate the fused covariance.

[0127] For the mean vector, the final fusion mean is calculated using weighted summation.

[0128] In high-dimensional scenarios, the efficiency of product calculations can be optimized through block matrix calculations or low-rank approximation techniques.

[0129] In some embodiments, if there is noise correlation, the correlation parameters between the distributions can be adjusted using a joint distribution method to ensure the accuracy of the calculation results.

[0130] In addition, in nonlinear scenarios, the product can be approximated by variational inference or Monte Carlo method to further improve the robustness of the algorithm.

[0131] These implementations ensure that the fusion results of the multi-dimensional Gaussian distribution can accurately reflect the uncertainty of multi-source data and provide reliable support for the confidence assessment of the positioning results.

[0132] For example, since the system state quantities in the recursion of the IMU and the observation of the system all satisfy the Gaussian distribution, the update of the covariance matrix of the system error state quantity is essentially the product of the multidimensional Gaussian distribution probability density function. The product of two multidimensional Gaussian distribution probability density functions still has the form of a multidimensional Gaussian distribution probability density function:

[0133] Due to the isotropy, it means that in each dimension, the variance of the distribution is the same and the dimensions are independent. Therefore, for an n-dimensional isotropic Gaussian distribution, its probability density function can be expressed as the product of n one-dimensional Gaussian distributions: ∑=σ 2 I;

[0134] At this time, the product of two multidimensional Gaussian distributions of the same dimension can be expressed as:

[0135] in: in, is the first observed component x i The observed variance at is the second observed component x i The observed variance at is the first observed component x i The mean value of is the second observed component x i The mean value of time.

[0136] It can be seen that the distribution of the product of two multi-dimensional Gaussian distributions of the same dimension is a Gaussian distribution with a compressed or amplified amplitude. The probability density integral of the distribution is not equal to the normal distribution, but its variance and mean properties do not change. This is also the property of the error state quantity distribution of the fusion positioning system.

[0137] Further, as an embodiment of the present invention, refer to Figure 5 In step S4, setting the warning value of the current positioning point through real-time positioning information includes the following steps.

[0138] S41, obtaining the minimum distance of the current vehicle from the lane edge or obstacle in each direction.

[0139] In some implementations, the shortest distance from the vehicle to the lane line or obstacle is directly calculated through geometric calculation using real-time map data and vehicle positioning information.

[0140] In some embodiments, a visual sensor (such as a camera) may be used to detect the position of lane lines or obstacles, and the distance may be determined through image processing techniques (such as edge detection or depth estimation).

[0141] In some embodiments, a laser radar may be used to scan the surrounding environment, construct a high-precision point cloud model, and obtain the minimum distance through point cloud projection and spatial distance calculation.

[0142] In some embodiments, ultrasonic sensors are incorporated to provide close-range obstacle detection data, particularly for use in low-speed scenarios.

[0143] In some embodiments, the accuracy and robustness of the minimum distance calculation can be improved by fusing multiple source sensors (such as vision, lidar, and map information) and using data association and weighting algorithms.

[0144] These methods can flexibly achieve accurate acquisition of the minimum distance according to different scenarios and sensor configurations, providing a reliable basis for subsequent warning value calculations.

[0145] S42, multiplying the minimum distance by a safety factor to obtain a warning value of the current positioning point.

[0146] In some embodiments, a fixed safety factor is set according to the vehicle operation scenario, for example, a higher safety factor is used on urban roads to adapt to complex traffic environments.

[0147] In some embodiments, the safety factor can be adjusted dynamically and optimized in real time based on vehicle speed, environmental conditions (such as weather and light), and road characteristics (such as curvature and slope).

[0148] In some embodiments, different safety factor strategies may be adopted for different levels of positioning accuracy and safety redundancy requirements in combination with the autonomous driving level requirements (such as L3 or L4).

[0149] In some embodiments, a machine learning algorithm can be used to predict potential risks using historical data and real-time sensor information, and the safety factor can be adaptively adjusted based on the risk level.

[0150] In the scenario of multi-source sensor fusion, a hierarchical weighted method is used to set different safety factors for the minimum distances in different directions to ensure that the warning value is more accurate and reliable.

[0151] These implementations ensure that the warning values ​​can adapt to complex and changing driving environments and provide strong support for positioning reliability assessment.

[0152] Furthermore, as an embodiment of the present invention, the method of obtaining the minimum distance of the current vehicle from the lane edge or obstacle in each direction includes: obtaining a first minimum distance by real-time fusion of posture information and a map; obtaining a second minimum distance by visual perception; and taking the smaller value of the first minimum distance and the second minimum distance as the minimum distance.

[0153] Specifically, by fusing the real-time positioned vehicle posture information and high-precision map data, the geometric shortest distance from the vehicle to the lane edge or obstacle is calculated to obtain the first minimum distance; through a visual perception system (such as a camera or a visual algorithm), the lane lines or obstacles in the environment are detected in real time, and the distance between the vehicle and these targets is obtained based on image processing or depth estimation technology to obtain the second minimum distance; the first minimum distance and the second minimum distance calculated by the above two methods are compared, and the smaller value is taken as the final minimum distance of the current vehicle in each direction.

[0154] This method improves the accuracy and robustness of distance calculation by combining the global accuracy of the map and the real-time dynamics of visual perception, providing a reliable basis for subsequent warning value setting.

[0155] Further, as an embodiment of the present invention, refer to Figure 6 In step S4, the process of determining the reliability of the final positioning result includes the following steps.

[0156] S43, calculating the minimum interval of the positioning point in each direction that satisfies the confidence accuracy rate according to the uncertainty distribution.

[0157] In some embodiments, the properties of the multidimensional Gaussian distribution are utilized to calculate the confidence interval range in each direction according to the covariance matrix.

[0158] In some embodiments, by setting a threshold of confidence accuracy (eg, 99.9999%), a probability region satisfying the confidence is extracted from the uncertainty distribution, for example, by calculating an ellipsoid or rectangular boundary corresponding to the probability density.

[0159] In nonlinear scenarios, the Monte Carlo sampling method can be used to simulate the uncertainty distribution and thus statistically determine the minimum interval that meets the confidence requirements.

[0160] If the uncertainty distribution of multi-source data fusion is involved, the covariance matrix can be decomposed through principal component analysis (PCA) to simplify the uncertainty distribution into the minimum interval in each main direction.

[0161] For dynamic scenes, a real-time update mechanism can also be introduced to adjust the calculation method of uncertainty distribution according to environmental changes.

[0162] These implementation methods ensure that the confidence interval can accurately reflect the uncertainty of the positioning point and provide a scientific basis for subsequent comparison with the warning value.

[0163] S44: Compare the minimum interval with the warning value.

[0164] S45, when the minimum interval is greater than the warning value, the final positioning result is determined to be unreliable; when the minimum interval is less than or equal to the warning value, the final positioning result is determined to be reliable.

[0165] The minimum interval of the positioning point that satisfies the confidence accuracy rate in each direction calculated in step S4 is compared with the warning value in each direction to determine whether the confidence interval completely falls within the warning value range; a judgment is made based on the comparison result: if the range of the minimum interval in any direction exceeds the corresponding warning value, the final positioning result is determined to be unreliable; if the minimum interval is less than or equal to the warning value in all directions, the final positioning result is determined to be reliable.

[0166] In some embodiments, to improve the accuracy of the comparison, a weighting factor may be introduced to adjust the importance of different directions, such as giving priority to the front area according to the direction of vehicle travel;

[0167] In dynamic scenarios, real-time data can be combined for recalculation and comparison to adapt to rapid changes in the environment.

[0168] For example, after chain Kalman filter calculation, the actual coordinates of the vehicle can be known through the current real-time fusion posture information and map, and then the minimum distance d between the current vehicle and the lane edge (Line Edge) or obstacle (Obstacle) in each direction can be obtained. map .

[0169] At the same time, the minimum distance d between the visual perception of the lane edge (Line Edge) or the obstacle (Obstacle) can be obtained vision .

[0170] The minimum value of the two and multiplying it by a safety factor δ (δ≤1) can be determined as the warning value area of ​​the positioning point at the current moment: AL=δ[-min{d map ,d vision},min{dmap ,d vision}].

[0171] L3 autonomous driving requires a positioning confidence accuracy rate of more than 99.9999%. If the distribution is Gaussian distribution, the proportion of Gaussian distribution 5σ is 99.99994%, which meets the requirements.

[0172] By the formula: P postestimation =(I-KH)P priorestimation The calculated measurement updated system covariance matrix P postestimation , we can get the minimum interval of the positioning point in each direction that meets the confidence accuracy:

[0173] The calculated MaxRange is presented as an ellipsoid in space. When it is compared with AL, if MaxRange is larger, the fusion positioning result is considered unreliable; otherwise, it is considered reliable.

[0174] In summary, refer to Figure 7 A specific embodiment of the positioning reliability determination method of the present invention includes: First, multi-source sensor data (GNSS, Odometry, IMU, Vision signals) are received through a synchronous interface and format converted; Next, the input data is tested for anomalies and screened to ensure data reliability; Subsequently, map data is loaded to provide geo-reference information and the system status and parameters are initialized; The initial motion state is obtained through IMU inverse calculation, and then INS / Odometry fusion, INS / GNSS fusion and INS / Vision-Map Matching fusion are performed in sequence, and the state is gradually updated through Kalman filtering; During the fusion process, the map and visual perception results are combined to set the warning value of the current positioning point, and the reliability of the positioning result is judged based on the comparison between the confidence interval and the warning value; Finally, reliable positioning result output provides support for downstream modules and realizes periodic cycle processing.

[0175] The embodiment of the present invention improves the real-time and adaptability of positioning by fusing data from multiple sensors (GNSS, Odometry, IMU, Vision), combining map information and visual perception to dynamically adjust the warning value; it uses a chained Kalman filter to achieve gradual optimization of the error state, effectively eliminating the error accumulation problem caused by a single sensor; by setting a confidence interval and comparing it with the warning value, it can dynamically judge the reliability of the positioning result, thereby enhancing the safety and stability of the autonomous driving system in complex environments. This design not only overcomes the error jump and discontinuity problems in traditional methods, but also significantly improves the positioning accuracy and robustness of the system, meeting the needs of high-level autonomous driving scenarios.

[0176] Reference Figure 8 The embodiment of the present invention also discloses a positioning reliability judgment system, which includes a kinematic model construction module 1, a filter model construction module 2, an uncertainty distribution determination module 3 and a reliability determination module 4.

[0177] The kinematic model building module 1 is at least used to build an error state kinematic model driven by an inertial measurement device.

[0178] For example, the kinematic model construction module 1 obtains the measurement data of the accelerometer and gyroscope provided by the inertial measurement unit (IMU) to construct a state vector containing displacement, velocity, angle, acceleration deviation and angular velocity deviation to describe the motion state of the system; constructs an input vector containing accelerometer measurement values ​​and angular velocity measurement values ​​to drive state update; defines a noise vector containing velocity disturbance, angle disturbance, acceleration disturbance and angular velocity disturbance, and models it as Gaussian white noise to reflect the uncertainty of the sensor; uses the Euler integral formula to recursively calculate the current error state and covariance matrix based on the error state at the previous moment and the input vector and noise vector in the current time interval; describes the system state transition process through a first-order Markov chain to ensure the timing consistency and accuracy of the state model, thereby laying the foundation for subsequent filtering fusion and uncertainty analysis.

[0179] The filter model building module 2 is at least used to fuse at least one complementary sensor information with the inertial measurement device information to build a filter model.

[0180] Exemplarily, the filter model construction module 2 constructs a chain error state Kalman filter model by acquiring data from an inertial measurement unit (IMU) and complementary sensors (such as GNSS, Odometry, and Vision-Map Matching); first, based on the observation characteristics of each sensor, an observation equation is established to associate the observation signal with the error state quantity; then, the measurement update process of the Kalman filter is used to dynamically correct the system state and covariance matrix in combination with the sensor's noise model; for nonlinear observation equations, an extended Kalman filter (EKF) or an unscented Kalman filter (UKF) can be used for linearization or direct estimation; in multi-sensor fusion, the loosely coupled filtering solutions of INS / Odometry, INS / GNSS, and INS / Vision-MapMatching are gradually realized, and the weights and fusion strategies of each sensor information are dynamically optimized; finally, the positioning state after fusion correction is output to provide accurate input for subsequent uncertainty distribution calculations and reliability judgments.

[0181] The uncertainty distribution determination module 3 is at least used to transfer the uncertainty distribution and corresponding weights of each sensor information into the filtering model, and obtain the uncertainty distribution of the final positioning result through uncertainty transfer calculation.

[0182] Exemplarily, the uncertainty distribution determination module 3 first constructs the uncertainty distribution of each sensor according to the characteristics of each sensor (such as the accuracy of the GNSS signal, the cumulative error of the IMU, the sliding deviation of the Odometry, etc.), and expresses it in the form of a multi-dimensional Gaussian distribution, including a mean and a covariance matrix; then, the uncertainty distribution of each sensor information and its corresponding weight are passed into the filtering model through the noise model of the sensor; the probability density function of the multi-dimensional Gaussian distribution is calculated, and the information of the multi-source sensors is weightedly fused through the uncertainty transfer mechanism, and the uncertainty distribution of the final positioning result is dynamically updated; during the fusion process, the covariance matrix is ​​dynamically adjusted through the state update of the Kalman filter to ensure that the uncertainty distribution reflects the latest characteristics of the sensor data; in addition, for nonlinear scenarios, the Monte Carlo method or the Gaussian mixture model (GMM) can be used for approximate calculations to ensure the accuracy of uncertainty estimation in complex environments, thereby providing a scientific basis for positioning reliability assessment.

[0183] The reliability determination module 4 is at least used to set the warning value of the current positioning point through real-time positioning information, and judge the reliability of the final positioning result according to a preset confidence accuracy threshold and the uncertainty distribution.

[0184] Exemplarily, the reliability determination module 4 dynamically calculates the warning value area of ​​the current positioning point through real-time positioning information and map data, combined with visual perception results (such as lane lines or obstacle distances), where the warning value is determined by multiplying the minimum safety distance by the safety factor; the uncertainty distribution of the final positioning result provided by the uncertainty distribution determination module 3 is used to calculate the minimum interval of the positioning point in each direction that meets the preset confidence accuracy (such as 99.9999%); the minimum interval is compared with the warning value area in each direction, and when the minimum interval is greater than the warning value, the positioning result is determined to be unreliable and safety measures are triggered; when the minimum interval is less than or equal to the warning value, the positioning result is determined to be reliable and automatic driving is allowed to continue; in dynamic scenarios, the module can also adjust the warning value and confidence threshold in real time according to vehicle speed, road conditions and environmental changes to enhance the adaptability and accuracy of positioning reliability assessment, thereby ensuring the safety and robustness of system operation.

[0185] The embodiment of the present invention also discloses a readable storage medium.

[0186] A readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the positioning reliability judgment method described in any one of the above embodiments. The computer-readable storage medium may include: any entity or device capable of carrying a computer program, a recording medium, a USB flash drive, a mobile hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), and a software distribution medium, etc. The computer program includes a computer program code. The computer program code may be in source code form, object code form, an executable file, or some intermediate form, etc. The computer-readable storage medium may include: any entity or device capable of carrying a computer program code, a recording medium, a USB flash drive, a mobile hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), and a software distribution medium, etc.

[0187] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0188] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, apparatus or device (such as a computer-based system, a system including a processing module, or other system that can fetch instructions from an instruction execution system, apparatus or device and execute instructions), or used in combination with these instruction execution systems, apparatuses or devices.

[0189] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for determining positioning reliability, characterized in that: include: Construct an error-state kinematic model of the inertial measurement unit drive; fusing at least one complementary sensor information with the inertial measurement device information to construct a filtering model; The uncertainty distribution and corresponding weights of each sensor information are passed into the filtering model, and the uncertainty distribution of the final positioning result is obtained through uncertainty transfer calculation; The warning value of the current positioning point is set through real-time positioning information, and the reliability of the final positioning result is judged according to the preset confidence accuracy threshold and the uncertainty distribution.

2. The positioning reliability determination method according to claim 1, characterized in that: The construction of the error state kinematic model driven by the inertial measurement device includes: Construct a state vector containing displacement, velocity, angle, acceleration deviation, and angular velocity deviation; Construct an input vector containing accelerometer measurements and angular velocity meter measurements; Construct a noise vector containing velocity disturbance, angle disturbance, acceleration disturbance and angular velocity disturbance; According to Euler numerical integration formula, the error state at the previous moment, the input signal of the inertial measurement device in this interval and the noise vector are used to obtain the error state and covariance matrix at the current moment.

3. The positioning reliability determination method according to claim 2, characterized in that: The error state kinematic model obeys multi-dimensional normal distribution.

4. The positioning reliability determination method according to claim 1, characterized in that: The at least one complementary sensor information includes at least one of global satellite navigation system information, vehicle odometry information, and vision-map matching information.

5. The method for determining the reliability of fusion positioning according to claim 1, characterized in that: The filtering model is a chain error state Kalman filtering model.

6. The positioning reliability determination method according to claim 5, characterized in that: The construction of the filtering model includes: establishing an observation equation to associate each observation signal with a system error state quantity; Perform the measurement update process of Kalman filter to obtain the updated error state and system covariance matrix; The updated error state is added back to the system state to obtain the latest estimated system state quantity.

7. The positioning reliability determination method according to claim 1, characterized in that: The uncertainty distribution of the final positioning result obtained by uncertainty transfer calculation includes: The uncertainty distribution of each sensor information is represented as a multi-dimensional Gaussian distribution; The product of the probability density functions of the multi-dimensional Gaussian distribution is calculated to obtain the uncertainty distribution of the final positioning result.

8. The positioning reliability determination method according to claim 1, characterized in that: The step of setting the warning value of the current positioning point by using the real-time positioning information includes: Get the minimum distance between the current vehicle and the lane edge or obstacle in each direction; The minimum distance is multiplied by the safety factor to obtain the warning value of the current positioning point.

9. The positioning reliability determination method according to claim 8, characterized in that: The method of obtaining the minimum distance of the current vehicle from the lane edge or obstacle in each direction includes: Obtain the first minimum distance by fusing the pose information and the map in real time; Acquiring the second minimum distance through visual perception; The smaller value between the first minimum distance and the second minimum distance is taken as the minimum distance.

10. The positioning reliability determination method according to claim 1, characterized in that: The determining the reliability of the final positioning result includes: Calculate the minimum interval of the positioning point in each direction that satisfies the confidence accuracy rate according to the uncertainty distribution; comparing the minimum interval with the warning value; When the minimum interval is greater than the warning value, determining that the final positioning result is unreliable; When the minimum interval is less than or equal to the warning value, it is determined that the final positioning result is reliable.

11. A positioning reliability judgment system, characterized in that: The system comprises: A kinematic model building module, at least for building an error state kinematic model driven by an inertial measurement unit; A filter model building module, at least used to fuse at least one complementary sensor information with the inertial measurement device information to build a filter model; An uncertainty distribution determination module is at least used to transfer the uncertainty distribution and corresponding weights of each sensor information into the filtering model, and obtain the uncertainty distribution of the final positioning result through uncertainty transfer calculation; The reliability determination module is at least used to set the warning value of the current positioning point through real-time positioning information, and judge the reliability of the final positioning result according to a preset confidence accuracy threshold and the uncertainty distribution.

12. A readable storage medium, characterized in that: The readable storage medium stores computer instructions, and when the computer instructions are executed by a processor, the positioning reliability judgment method according to any one of claims 1 to 10 is implemented.

Images