A vehicle position estimation method

Abstract

The present application relates to a kind of vehicle position estimation methods, for solving the problem of inaccurate positioning caused by the precision fluctuation of navigation positioning sensor due to the complex and changeable running environment of automatic driving vehicle, improve the precision and robustness of vehicle positioning.The technical scheme of the present application is as follows: determine the sensor set used in vehicle positioning, for each sensor in the sensor set, determine the state set to be estimated;For each state in the state set, calculate the loss generated by each state estimation, and correct the loss;The positioning accuracy is calculated using the corrected loss of each state, and then a global positioning accuracy function is established, the system state that makes the global positioning accuracy function maximum is obtained as the optimal estimate value, and the optimal estimate value is used as the optimal position information of vehicle.

Description

A vehicle position estimation method Technical Field

[0001] This invention relates to the field of autonomous vehicle positioning, and more particularly to a vehicle position estimation method. Background Technology

[0002] Against the backdrop of continuous development in new-generation information technology and artificial intelligence, autonomous vehicles have become a hot topic in the research and application of intelligent transportation systems. Among them, the positioning system of autonomous vehicles can provide real-time and accurate position and pose information of the vehicle, which is a necessary prerequisite for autonomous decision-making, path planning and underlying collaborative control of autonomous vehicles. It is of great significance for ensuring the travel safety of traffic participants in the traffic environment and improving the safety of autonomous vehicles.

[0003] However, the accuracy of navigation and positioning sensors in autonomous vehicle positioning systems fluctuates due to the complex and ever-changing operating environment. Current fusion algorithms do not consider these variations in sensor accuracy, which alters the usability of positioning information. Without correcting or compensating for navigation and positioning information from faulty sensors, the multi-source sensor fusion positioning results will drift. Furthermore, the current mainstream approach uses graph optimization based on fixed-weight sliding windows to fuse multi-source sensor navigation information to meet real-time system requirements. However, fixed-weight sliding windows assign equal weights to factor nodes at different optimization iterations, leading to low positioning accuracy. Summary of the Invention

[0004] The purpose of this invention is to provide a vehicle position estimation method to solve, or at least partially solve, the problem of inaccurate positioning caused by the fluctuation in the accuracy of navigation and positioning sensors due to the complex and variable operating environment of autonomous vehicles, thereby improving the accuracy and robustness of vehicle positioning. The technical solution of this invention is as follows.

[0005] In a first aspect, the present invention proposes a vehicle position estimation method, the method comprising the following steps:

[0006] Determine the set of sensors to be used in vehicle positioning, and for each sensor in the sensor set, determine the set of states to be estimated;

[0007] For each state in the set of states, calculate the estimated loss for each state and then correct the loss.

[0008] For each state-corrected loss, the positioning accuracy is calculated, and the following global positioning accuracy function is established:

[0009]

[0010]

[0011] In the formula: D(χ) is the global cost function, C is the set of sensors, and c is an element in the set of sensors. The loss correction coefficient is used to estimate the state of sensor c at time k. To estimate the loss of sensor c at time k, X c Let f(X) be the state value to be estimated for sensor c, χ be the set of states to be estimated, and f(X) be the state value to be estimated. c ) represents the accuracy of state estimation loss for sensor c;

[0012] Obtain the system state estimate that maximizes the global positioning accuracy function, and use it as the optimal positioning value for the vehicle.

[0013] In the above technical solution, one implementation method for correcting losses includes the following steps:

[0014] The reliability of the estimate is measured by combining the confidence level with the influence level, or by using only the influence level, thereby correcting the estimation error for each state.

[0015] Influence refers to the degree to which the system state at different times affects the estimation of the current state.

[0016] In this implementation, two technical means to improve positioning accuracy are provided. On the one hand, state influence is introduced. Since the set of states to be estimated includes historical states, each historical state is estimated multiple times. As time is updated, the distance between a certain historical state and the latest state gradually increases, and the influence of the historical state on the accuracy of the latest state estimation gradually decreases. Therefore, state influence is introduced. On the other hand, not only state influence is considered, but also the reliability of the sensor is considered. The reliability is used to correct or compensate for abnormal sensor information.

[0017] In the above technical solution, the sensor set consists of: {Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and Vehicle Odometer (ODO)}. The INS and ODO sensor times are synchronized with GNSS time, and the world coordinate system is used to determine the navigation coordinate system. Multi-source sensor fusion overcomes the problem of positioning result drift when abnormal information from a single sensor is present. Spatiotemporal synchronization of multiple sensors is used to ensure the accuracy of the fusion results and stable input during fusion.

[0018] In the above technical solution, the influence is either a set value or a calculated value, and the calculation steps include:

[0019] For sensor c in the sensor set, set a corresponding sliding window for it, and count the frequency of the i-th state estimate within the sliding window, denoted as .

[0020] The state estimation influence is calculated using the following function.

[0021]

[0022] In the formula, β0 is the minimum influence level set; a is the rate of change of influence.

[0023] In the above technical solution, credibility and influence are combined by multiplying credibility and influence, with credibility being a calculated value. The calculation steps include:

[0024] When the sensor is a global navigation satellite system, its reliability at time k. Calculated using the following formula:

[0025]

[0026] In the formula: G is the observation matrix, I is the identity matrix, and w k Let w be the GNSS positioning pseudorange residual vector at time k. k-1 Let be the GNSS positioning pseudorange residual vector at time k-1;

[0027] When the sensor is an inertial navigation system or a vehicle wheel speed and odometer, the reliability of its state estimation at time k is... Calculated using the following formula:

[0028]

[0029] In the formula: s is the residual between the predicted value and the measured value of the inertial navigation system or vehicle wheel speed odometer, s0 is the set residual threshold, and r is the adjustment coefficient of different sensors.

[0030] In the above technical solution, the global positioning accuracy function uses the measured value as the initial value for iteration, and improves the positioning accuracy by reducing the error between the predicted value and the measured value through the optimal iterative algorithm, thereby obtaining the optimal system state estimate.

[0031] In the above technical solution, the measurement value of the state estimation error of the global navigation satellite system at time k is obtained by noise compensation through the original position observation value of GNSS, and the prediction value is the vehicle navigation coordinate system position information after GNSS antenna mast compensation.

[0032] In the above technical solution, the inertial navigation system uses inertial pre-integration to construct the relative position, attitude and velocity of the autonomous vehicle in the time period [k-1, k], which are used as the measurement values ​​of position, attitude and velocity at time k.

[0033] In the above technical solution, the wheel odometer uses pre-integration to construct the position increment within the time interval [k-1, k], which is used as the measurement value for calculating the state estimation error at time k.

[0034] Secondly, the present invention proposes a vehicle equipped with a positioning system implemented using any of the methods described above.

[0035] Thirdly, the present invention provides a computer-readable storage medium storing a computer program that can be loaded by a processor and executed by any of the methods described above. Attached Figure Description

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

[0037] Figure 1. A schematic diagram of the method flow in a specific embodiment;

[0038] Figure 2. A schematic diagram of the influence function in a specific embodiment;

[0039] Figure 3. A schematic diagram of a sliding time window in a specific embodiment;

[0040] Figure 4. Schematic diagram of GNSS reliability function in a specific embodiment.

[0041] Figure 5. A schematic diagram of the INS / ODO confidence function in a specific embodiment. Detailed Implementation

[0042] The complexity and unpredictability of the operating environment of autonomous vehicles lead to performance variations in various navigation sensors, resulting in abnormal outputs or even failures. For example, Global Navigation Satellite Systems (GNSS) may experience decreased positioning accuracy or even rejection due to obstacle obstruction, cycle slip deviation, and multipath effects. Based on this, this invention proposes a vehicle positioning method that provides two technical means to improve positioning accuracy. Firstly, it introduces state influence. Since the set of states to be estimated includes historical states, each historical state is estimated multiple times. As time updates, the distance between a historical state and the latest state gradually increases, and the influence of the historical state on the accuracy of the latest state estimation gradually decreases; therefore, state influence is introduced. Secondly, it considers not only the influence of state estimation but also the reliability of the sensors. By using reliability to correct or compensate for abnormal sensor information, it enhances the environmental adaptability of the autonomous vehicle positioning system and improves system robustness. Specific implementation methods are as follows.

[0043] Figure 1 is a flowchart illustrating the method implementation process. The operations in the flowchart may not be performed in a specific order. Instead, they may be performed in reverse order or simultaneously. Furthermore, one or more additional operations can be added to the flowchart. One or more operations can be removed from the flowchart.

[0044] S10. Determine the set of sensors used in vehicle positioning, and for each sensor in the sensor set, determine the set of states to be estimated.

[0045] In one implementation of this step, the determined sensor set is: {Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), Vehicle Wheel Speed ​​Odometer (ODO)}. However, only one or two of these sensors may be used, and there is no limit to the number of the same type of sensor.

[0046] When using two or more types of sensors, a time synchronization issue arises during sensor acquisition. Taking a sensor set {Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and Vehicle Odometer (ODO)} as an example, the GNSS sampling time is high-precision GNSS time, maintained jointly by the navigation satellite and atomic clocks, while the INS and ODO sampling times are local time. The GNSS receiver acquires navigation signals from navigation satellites or ground base stations, obtaining the vehicle's current position information in the geocentric coordinate system (in this embodiment, latitude, longitude, and altitude information in the WGS-84 coordinate system represent the vehicle's position). The INS receives acceleration and angular velocity information from the INS sensor coordinate system relative to the geocentric inertial coordinate system, and through inertial calculations, obtains the vehicle's position, velocity, and attitude information relative to the previous moment in the INS coordinate system. The ODO receives encoder pulse data from the wheels and calculates the vehicle's position increment relative to the previous moment in the ODO coordinate system. The inconsistency in the coordinate systems of the spatial information from the various sensors makes them incomparable and inconvenient for final vehicle positioning estimation.

[0047] By synchronizing the time information of each sensor, the time systems of all sensors to be fused are unified, ensuring the accuracy of the fusion results. Furthermore, unifying the navigation information calculated by multiple sensors into a single navigation coordinate system facilitates the comparison and calculation of positioning information.

[0048] Taking a sensor suite {Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and Vehicle Odometer (ODO)} as an example, hardware time synchronization is performed on the three types of sensors. All sensor time systems are synchronized to GNSS time. By combining the 1PPS trigger signal of the GNSS receiver with the acquisition time interface signals of other sensors, the GNSS time of each sensor at the sampling completion moment can be obtained, synchronizing the sampling timestamps of INS and ODO. The world coordinate system is used as the navigation coordinate system. The initial GNSS positioning result, after compensation by the GNSS antenna mast, is used as the origin 0 of the navigation coordinate system. The x-axis is the tangent to the east-pointing parallel of latitude, the y-axis is the tangent to the north-pointing meridian of longitude, and the z-axis is perpendicular to the XOY plane, forming a right-handed coordinate system. The navigation and positioning results of GNSS, INS, and ODO are projected onto the navigation coordinate system (n-system) to form the positional constraint information of each sensor on the carrier under a unified coordinate system.

[0049] INS state at time k in the navigation coordinate system An update can be represented as:

[0050]

[0051] Where n is the navigation coordinate system and b is the vehicle coordinate system. Let n be the state variable of the carrier navigation coordinate system at time k-1. g represents the increment of the carrier's navigation state per unit time. n Let Δt be the gravity vector in the navigation coordinate system n, and Δt be the increment per unit time. Let be the rotation matrix of the carrier coordinate system b relative to the navigation coordinate system n at time k-1.

[0052] The state update of GNSS in the navigation coordinate system can be expressed as:

[0053]

[0054] Where n is the navigation coordinate system and e is the geocentric coordinate system. The GNSS positioning result in navigation coordinate system n at time k. Let be the transformation matrix of the geocentric coordinate system e relative to the navigation coordinate system n. The GNSS positioning result at time k in the Earth-centered Earth-fixed coordinate system e is shown. This is the positioning result of the GNSS at time 0 in the geocentric coordinate system e after system initialization.

[0055] The state update of ODO in the navigation coordinate system can be represented as:

[0056]

[0057] Where n is the navigation coordinate system, o is the wheeled odometer coordinate system, and b is the vehicle coordinate system. This represents the position update result of ODO in the navigation coordinate system n at time k. This represents the position information of the vehicle in the navigation coordinate system at time k-1. Let be the rotation matrix of the vehicle coordinate system n relative to the navigation coordinate system b at time k-1. Let be the rotation matrix of the wheel odometer coordinate system o relative to the vehicle coordinate system b. The measured values ​​of the carrier velocity of ODO from time k-1 to time k.

[0058] For each sensor, a set of states to be estimated is determined. A state is the sensor's location information output at a specific moment; it can be a single value or a set of values, depending on the sensor. The data information in the state set is set according to requirements, and the time period can include not only the current moment but also past moments. In the latter case, the sensor state is estimated over a period of time. The state outputs of different sensors may or may not be at the same time, and the output interval between two states of a single sensor may be the same as or different from that of other sensors. Therefore, when using multiple sensors, the size of the state set determined for each sensor can be the same or different.

[0059] S20. For each state in the state set, calculate the estimated loss for each state and correct the loss.

[0060] In one implementation, the sum of squared errors for each state estimate is calculated as the loss generated by each state estimate.

[0061] Will Let X be the measured value of the state of sensor c at time k. c Let k be the state value to be estimated for sensor c. Then, the state estimation error function of sensor c at time k is denoted as... The loss of sensor c at time k can be expressed as:

[0062] ∑ c，k Let be the covariance matrix of sensor c at time k.

[0063] The sensor state error estimation function for the above sensor set is given as an example below.

[0064] (2.1.1) Error function of inertial navigation system

[0065] First, all INS state variables are unified into a navigation coordinate system. Then, the difference between the predicted state and the measured state, which includes information such as the carrier's position, is constructed to narrow the gap between the two through an optimal iterative algorithm.

[0066] The accelerometer and gyroscope of the INS acquire the specific force and angular velocity information of the carrier in inertial space, respectively. The measurement model is as follows:

[0067]

[0068] Among them, b a (t) and b g (t) represents the zero bias of the accelerometer and gyroscope at time t, respectively. and Let f be the original observations of the specific force and angular velocity at time t. t and ω t These are the observed specific force and angular velocity at time t after zero bias compensation.

[0069] To adapt to the high-frequency characteristics of INS and ensure the real-time performance of the multi-source sensor fusion localization algorithm, inertial pre-integration is used to construct the relative position, attitude, and velocity constraints of the autonomous vehicle over a period of time. Furthermore, using pre-integration as a measurement of the INS output state avoids the correlation between state constraints and initial state quantities, reducing repeated integration caused by motion state updates during optimization, thus improving the algorithm's real-time performance. The INS pre-integration model is shown below:

[0070]

[0071] in, The values ​​represent the pre-integration of the position, velocity, and attitude of the autonomous vehicle in the carrier coordinate system b from time k-1 to time k. Let be the attitude quaternion at time t relative to time k-1 in the carrier coordinate system b.

[0072] The error expression based on INS pre-integration is:

[0073] Where X represents the system state variable. This represents the measurement information of each sensor from time k-1 to time k in the carrier coordinate system b. Let g represent the position residual, velocity residual, attitude residual, accelerometer zero-bias residual, and gyroscope zero-bias residual from time k-1 to time k in the carrier coordinate system b, respectively. Δt is the unit time increment, and g is the position residual, velocity residual, attitude residual, accelerometer zero-bias residual, and gyroscope zero-bias residual. n Let g be the gravity vector in the navigation coordinate system n. Let n be the rotation quaternion of the navigation coordinate system n relative to the vehicle coordinate system b at time k-1. Let n be the position at time k in the navigation coordinate system. Let n be the position at time k-1 in the navigation coordinate system. Let n be the velocity at time k in the navigation coordinate system. Let n be the velocity at time k-1 in the navigation coordinate system. Let be the attitude quaternion at time k in the navigation coordinate system n. Let be the attitude quaternion at time k-1 in the navigation coordinate system, [·] xyz This indicates that the imaginary part of the quaternion is returned. Representing quaternion multiplication, The accelerometer bias is zero at time k. The accelerometer bias is zero at time k-1. The gyroscope has zero bias at time k. The gyroscope has zero bias at time k-1.

[0074] (2.1.2) Error function of wheeled odometer

[0075] Wheel-mounted odometers can provide the vehicle's speed information, and the vehicle's speed model in its navigation coordinate system is as follows:

[0076]

[0077] in, Let ε be the velocity of the vehicle at time t in the navigation coordinate system n. v For the linear velocity measurement noise of wheel odometers, s o For the scale factor of the wheel odometer, Let be the rotation matrix of the wheel odometer coordinate system o relative to the navigation coordinate system n at time t. This is the speed measurement value of the wheel odometer.

[0078] Because the high-frequency output characteristics of the wheel speedometer make it impossible to estimate the odometer state for each frame using all odometer measurement information, a wheel odometer pre-integration can be used to replace the original state derivation algorithm. According to the principles of vehicle kinematics, the wheel odometer pre-integration only constitutes the position increment constraint within a relative time interval and is independent of the initial position state. Its pre-integration expression is:

[0079]

[0080] in, Let be the rotation matrix of the vehicle coordinate system b relative to the navigation coordinate system n at time t. Let be the rotation matrix of the wheel odometer coordinate system o relative to the vehicle coordinate system b.

[0081] Based on the pre-integration form of the wheeled odometer, an error function is constructed for the ODO that includes the predicted state and the measured state, incorporating vehicle position information. Its expression is as follows:

[0082]

[0083] Where X is the system state variable. This represents the measurement information of each sensor from time k-1 to time k in the carrier coordinate system b. Download the position information of the navigation coordinate system n at time k. Download the position information of the navigation coordinate system n at time k-1. Let n be the rotation matrix of the navigation coordinate system n relative to the vehicle coordinate system b at time k-1.

[0084] (2.1.3) Global Navigation Satellite System Error Function

[0085] GNSS can obtain the position measurement value of the vehicle through calculation, and its measurement equation is:

[0086]

[0087] in, p is the GNSS position measurement at time k after noise compensation; k The original GNSS position observations at time k; This refers to the measurement noise for GNSS position. GNSS error is defined as the difference between the carrier's navigation coordinate system position information (compensated by the GNSS antenna mast) and the GNSS positioning measurement value in the navigation coordinate system, expressed as:

[0088]

[0089] in, This represents the position information of the carrier in the navigation coordinate system n at time k. Let be the rotation matrix of the vehicle coordinate system b relative to the navigation coordinate system n at time k. This refers to the GNSS boom information in the carrier coordinate system b.

[0090] Using the error function given above, the state estimation error can be calculated, and thus the estimated loss can be calculated.

[0091] In one implementation, the steps of correcting the loss include:

[0092] The reliability of the estimate is measured by combining the confidence level with the influence level, or by using only the influence level, thereby correcting the estimation error for each state.

[0093] Influence refers to the degree to which the system state at different times affects the estimation of the current state.

[0094] In one implementation, the influence metric of the state estimate can be a set value or a calculated value. One calculation implementation involves: for sensor c in the sensor set, setting a corresponding sliding window, counting the frequency of the i-th state estimate within the sliding window, and denoting it as... The state estimation influence is calculated using the following function.

[0095]

[0096] In the formula, β0 is the minimum set influence; a is the rate of change of influence. In one embodiment, β0 is 0.1 and a is 1.2. The influence measurement function is shown in Figure 2.

[0097] The sliding window used in the aforementioned influence calculation can also meet the requirements for real-time vehicle positioning. Referring to Figure 3, as observation information accumulates over time, too many state estimates will increase the computational burden, while too few state estimates may affect positioning accuracy. By setting a sliding window and adjusting its size according to computational capabilities, estimation is performed only on the state nodes within the sliding window, thus meeting the real-time vehicle positioning requirements.

[0098] In one implementation, the sliding window size is set to 20 seconds. The first state node in this sliding window is no longer estimated at 21 seconds. As the vehicle moves, the more times the motion state nodes within the sliding window are estimated, the higher the estimation accuracy of the corresponding state variables will be. Therefore, an influence factor is set to correct the error of this node.

[0099] As a further improvement, not only is the influence of state node estimation measured, but also the reliability of its error. Since the error in state node estimation is caused by the positioning accuracy or anomalies of the sensors, measuring the reliability of the error can reduce the impact of sensor-related errors. The reliability can be a set value or a calculated value. During calculation, a reliability model can be established for each sensor. The following example shows the reliability models established for the sensors in the aforementioned sensor set.

[0100] (2.2.1) GNSS Reliability Model

[0101] Due to the uncertainty of the operating environment of autonomous vehicles, GNSS positioning accuracy often degrades or even fails due to factors such as obstacle obstruction, cycle slip deviation, and multipath effects. To improve the positioning robustness of autonomous vehicles, they should possess GNSS autonomous integrity monitoring capabilities and outlier removal functions in complex environments.

[0102] A GNSS receiver needs at least four satellites to receive radio signals from navigation satellites in order to meet the requirements for calculating the carrier's position. In order to enable autonomous integrity monitoring of the GNSS receiver, redundant observations from more navigation satellites are needed to eliminate abnormal observations.

[0103] To determine the reliability of pseudorange observations received from navigation satellites via GNSS, a pseudorange least squares residual discrimination algorithm is used. The reliability of the positioning result is determined based on the magnitude of the pseudorange residual. The positioning pseudorange residual can be expressed as:

[0104] w k+1 =(IG(G T G) -1 G T )w k

[0105] Where: w is the pseudorange residual vector, I is the identity matrix, and G is the observation matrix, whose expression is:

[0106]

[0107] Where, ρ i For pseudorange information of satellite i, (x i y i z i (x) represents the position information of satellite i, where i takes the value [1 2 3 4], (x) i y i z i () represents the receiver's location information.

[0108] This invention sets the sum of squares of the pseudorange residual vector as a reliability measure for the GNSS sensor. As the pseudorange residual increases, it can be assumed that there is abnormal information in the GNSS measurement, and the reliability of the GNSS state estimation should be reduced. By setting a misalignment threshold for the pseudorange residual vector at the current time, abnormal navigation satellite observation information whose pseudorange residual does not meet the requirements is eliminated. Therefore, the reliability model of GNSS state estimation based on pseudorange residual at time k is constructed as follows, and its function graph is shown in Figure 4:

[0109]

[0110] (2.2.2) INS and ODO credibility models

[0111] Autonomous vehicles typically employ MEMS inertial navigation systems. In the complex environment in which autonomous vehicles actually operate, the performance of MEMS-based INS can fluctuate in accuracy due to changes in the environment. For example, INS cannot effectively measure when an autonomous vehicle is traveling at a constant speed in a straight line.

[0112] Both INS and ODO sensors acquire vehicle position information through trajectory extrapolation. To address abnormal INS and ODO measurement information in different operating environments, this invention designs a dynamic weighting function to mitigate the impact of abnormal INS and ODO measurements on the multi-source sensor fusion positioning system, thereby improving the system's positioning robustness.

[0113] Due to the high-frequency characteristics of INS and ODO, autonomous vehicles are generally in a relatively stable motion state within the sensor data acquisition interval. The current state prediction value can be obtained from the state measurement value of the previous moment. Assuming that the vehicle's motion state does not change during the INS and ODO sensor data acquisition interval, the reliability of INS and ODO is dynamically determined by the residual between the predicted value and the measured value. The expression is as follows, and the function graph is shown in Figure 5:

[0114]

[0115] Where s is the residual between the sensor's predicted value and the measured value, s0 is the reasonable threshold for the residual, that is, when the residual between the predicted value and the measured value is less than s0, it is considered to be within a reasonable range of variation, which is determined according to the sensor with different precision; r is the adjustment coefficient for different sensors.

[0116] Thus, the credibility model of each sensor in the example sensor set is obtained. The credibility of each sensor is calculated using the credibility model, and the credibility of the sensor is used as the credibility of the state estimation of that sensor.

[0117] The loss of the state estimate is corrected by using the influence of each state estimate, that is, using the influence as a correction coefficient for the loss; or the loss of the state estimate is corrected by using the influence and confidence of each state estimate, that is, using the product of the influence and confidence as a correction coefficient for the loss.

[0118] The loss caused by the state estimation error of sensor c at time k can be expressed as: Let the state estimation loss correction coefficient of sensor c at time k be denoted as... The loss after each state correction can be obtained as follows:

[0119] S30. Calculate the positioning accuracy using the loss corrected for each state, and then establish the following global positioning accuracy function:

[0120]

[0121]

[0122] In the formula:

[0123] In the formula: D(χ) is the global cost function, C is the set of sensors, and c is an element in the set of sensors. The loss correction coefficient is used to estimate the state of sensor c at time k. To estimate the loss of sensor c at time k, X c Let f(X) be the state value to be estimated for sensor c, χ be the set of states to be estimated, and f(X) be the state value to be estimated. c ) represents the accuracy of state estimation loss for sensor c.

[0124] S40. Obtain the system state estimate that maximizes the global positioning accuracy function, and use it as the optimal positioning value for the vehicle.

[0125] The global localization accuracy function described above is the objective optimization function. For ease of calculation, we take its logarithm and then multiply both sides by -2 to obtain a new objective optimization function as follows:

[0126]

[0127] In the formula: S c Let the set of times for state estimation of sensor c be . Let c be the state estimation loss correction coefficient for sensor c at time k, where C is the set of sensors and c is an element in the sensor set.

[0128] In this form, This can be viewed as a global loss function; minimizing it yields the optimal vehicle positioning information. A nonlinear optimization method is used to obtain the optimal estimate of the navigation state of the autonomous vehicle.

[0129]

[0130] In one implementation, the state parameter estimation in the above equation is transformed into a least-squares problem, and the Gauss-Newton nonlinear optimization method is used as the optimal iterative algorithm to solve the objective function, thereby obtaining the optimal navigation estimate of the carrier. During the implementation of the method of this invention, it can be seen that:

[0131] (1) The method of the present invention not only considers the impact of sensor anomalies on positioning accuracy, but also considers the increase in computational load due to the increase in historical information, which affects the influence of state estimation. By introducing a sensor state estimation influence metric, the state estimation error at different times can be adaptively corrected.

[0132] (2) The method of this invention is universally applicable. Sensors can be selected from the sensor set according to the actual situation, and can be a single type or a single sensor. Although a single sensor may have estimation errors due to anomalies, these errors can be corrected through state estimation influence and confidence metrics, thereby reducing the impact of sensor anomalies. The confidence metric function can eliminate the effect of sensor outliers. Multiple or combined sensors can also be used to fully utilize the advantages of each sensor, since the probability of all sensors being abnormal simultaneously is relatively low, which can improve the robustness of the positioning.

[0133] In another embodiment, a system implemented by the method of the embodiments is configured in an autonomous vehicle to improve vehicle positioning accuracy.

[0134] Through the above description of the embodiments, those skilled in the art can clearly understand that the method of this disclosure can be implemented by means of software plus necessary general-purpose hardware, and of course, it can also be implemented by special-purpose hardware, including application-specific integrated circuits, dedicated CPUs, dedicated memory, dedicated components, etc. Generally, any function performed by a computer program can be easily implemented by corresponding hardware, and the specific hardware structure used to implement the same function can also be diverse, such as analog circuits, digital circuits, or dedicated circuits. However, for the purposes of this disclosure, software program implementation is more often a preferred implementation method.

[0135] Although embodiments of the present invention have been described above in conjunction with the accompanying drawings, the present invention is not limited to the specific embodiments and application fields described above. The specific embodiments described above are merely illustrative and instructive, and not restrictive. Those skilled in the art can make many other forms based on the guidance of this specification and without departing from the scope of protection of the claims of the present invention, and all of these are within the scope of protection of the present invention.

Claims

1. A vehicle position estimation method, characterized in that, The method includes the following steps: determining the set of sensors used in vehicle positioning, the sensor set being {Global Navigation Satellite System (GNSS), Inertial Navigation System (INS), and Vehicle Wheel Speed ​​Odometer (ODO)}; synchronizing the INS and ODO sensor times to GNSS time; and using the world coordinate system as the navigation coordinate system; and for each sensor in the sensor set, determining the set of states to be estimated. For each state in the state set, calculate the loss generated by each state estimate and correct the loss. The steps include: measuring the confidence of the estimate in combination with its influence, or measuring only its influence, to correct the error of each state estimate; where influence is the degree of impact of the system state at different times on the current state estimate, and influence can be a set value or a calculated value. The calculation steps include: for sensor c in the sensor set, setting a corresponding sliding window for it, counting the frequency of the i-th state estimate within the sliding window, and denoting it as... The state estimation influence is calculated using the following function. ： In the formula, The minimum influence setting; The rate of change of influence; combining credibility and influence involves multiplying credibility and influence, with credibility being a calculated value. The calculation steps include: when the sensor is a global navigation satellite system, its credibility at time k. Calculated using the following formula: In the formula: Let I be the observation matrix, and let I be the identity matrix. Let k be the GNSS positioning pseudorange residual vector at time k. Let $k$ be the GNSS positioning pseudorange residual vector at time $k-1$. When the sensor is an inertial navigation system and the vehicle is a wheel speed-odometer, the reliability of its state estimation at time $k$ is... Calculated using the following formula: In the formula: This refers to the residual between the predicted and measured values ​​of an inertial navigation system or vehicle wheel speed odometer. For the set residual threshold, The adjustment coefficients are for different sensors; the positioning accuracy is calculated for the loss after correction for each state, and then the following global positioning accuracy function is established: In the formula: Let C be the global cost function, C be the set of sensors, and c be the element in the set of sensors. The loss correction coefficient is used to estimate the state of sensor c at time k. Estimate the loss of sensor c at time k. Let c be the state value to be estimated. The accuracy of the state estimation loss of sensor c is calculated; the system state estimate that maximizes the global positioning accuracy function is obtained and used as the optimal positioning value for the vehicle.

2. The method according to claim 1, characterized in that, The global positioning accuracy function uses the measured value as the initial value for iteration. It improves the positioning accuracy by reducing the error between the predicted value and the measured value through the optimal iterative algorithm, thereby obtaining the optimal system state estimate.

3. The method according to claim 2, characterized in that, The measurement value used for calculating the state estimation error of the global navigation satellite system at time k is obtained by noise compensation from the raw position observations of GNSS, and the prediction value is the vehicle navigation coordinate system position information after GNSS antenna mast compensation.

4. The method according to claim 2, characterized in that, The inertial navigation system uses inertial pre-integration to construct the relative position, attitude, and velocity of the autonomous vehicle over the time interval [k-1,k], which are then used as the measurements of position, attitude, and velocity at time k.

5. The method according to claim 2, characterized in that, The wheeled odometer uses pre-integration to construct the position increment within the time interval [k-1,k], which is then used as the measured value for calculating the state estimation error at time k.

6. A computer-readable storage medium, characterized in that: The computer program is stored that can be loaded by a processor and executed according to any one of claims 1 to 5.

Images