An integrity monitoring method for an integrated navigation system for autonomous driving

Abstract

The present invention proposes an integrity monitoring method for a fusion navigation system for autonomous driving, including: Step 1, performing scene matching using a vehicle-mounted camera to obtain a reference position and inverting the distance from the vehicle to the feature points; Step 2, using the observation information of the vehicle-mounted lidar and machine learning algorithms to assist the global satellite navigation system in pseudo-range error correction; Step 3, tightly coupling the camera observations with the global satellite navigation system observations to construct an observation equation; Step 4, calculating the least squares residuals and constructing a test statistic for fault detection; Step 5, calculating the standard deviation of the predicted positioning error, fitting the normal distribution of the positioning error and calculating the user protection level in the horizontal direction; Step 6, comparing the obtained user protection level with the protection limit value to complete the integrity monitoring of the multi-sensor assisted global satellite navigation system for autonomous driving.

Description

Technical Field

[0001] The present invention relates to a method for monitoring the integrity of an integrated navigation system, in particular to a method for monitoring the integrity of an integrated navigation system for autonomous driving. Background Art

[0002] As the ultimate goal of the development of intelligent transportation, autonomous driving has developed rapidly in recent years. The navigation and positioning technology can provide vehicles with position, speed, and time information, and is a basic technology to support autonomous driving. As an application concerned with safety and responsibility, autonomous driving not only requires the vehicle's navigation and positioning system to provide it with high-precision positioning information, but also has extremely high requirements for the reliability of the positioning information. Integrity, as an important indicator for evaluating reliability, mainly enhances the reliability of the navigation system by monitoring and eliminating faults in the navigation system and providing timely warning services to users. Currently, the Global Navigation Satellite System (GNSS), as the most competitive positioning and navigation technology, can provide vehicles with continuous and high-precision navigation and positioning information, and can also achieve integrated navigation with other sensors such as inertial navigation and visual odometry. It has currently been widely used in the research and testing of autonomous driving vehicles. However, in order to ensure the safety of autonomous driving, it is necessary to effectively monitor the integrity of GNSS. Especially in complex urban environments, when satellite signals are easily blocked or reflected by buildings, it will cause a shortage of visible satellites and serious multipath errors, which limits the further application of GNSS in autonomous driving.

[0003] The research on GNSS integrity originated in the aviation field. The integrity monitoring of the user terminal determines the integrity of the GNSS positioning result from the user receiver itself, and the key factors are the available resources and the monitoring algorithm. It can be divided into receiver autonomous integrity monitoring and user-assisted integrity monitoring according to whether there is other navigation resource for assistance.

[0004] The Receiver Autonomous Integrity Monitoring (RAIM) algorithm is a method for testing and judging the effectiveness of positioning results through the redundant constraint relationships between different observables. Its main content includes two major parts: one is the fault detection and identification algorithm, mainly for effectively detecting and eliminating faults that affect positioning accuracy; the other is the method for determining the availability of integrity, mainly for judging whether the integrity risk borne by the positioning system at the current epoch exceeds the limit, that is, whether the integrity algorithm is available at the current epoch. In the research on satellite fault detection algorithms in the first part, representative algorithms mainly include the pseudorange comparison method, the least squares residual method, and the parity vector method. Lee proposed the pseudorange comparison method (Reference: Lee Y C. Analysis of range and position comparison methods as a means to provide GPS integrity in the user receiver [C] / Proceedings of the 42nd Annual Meeting of the Institute of Navigation. 1986, 1-4.), and the pseudorange comparison method does not rely on any gross error detection model. It only conducts fault detection by comparing the predicted observation values calculated from redundant data with the actual observation values, but this method cannot identify the faulty satellite. Parkinson proposed the least squares residual method (Reference: Parkinson B W, Axelrad P. Autonomous GPS Integrity Monitoring Using the Pseudorange Residual [J]. Navigation, 1988, 35(2): 255-274.). The least squares residual method first calculates the least squares solution of the user's position according to the GNSS linearized observation equation, and then calculates the pseudorange residual as the test statistic to judge whether the system contains faults by comparing this statistic with the threshold. On the basis of the least squares residual method, Sturza proposed the parity vector method (Reference: Sturza M A. Navigation system integrity monitoring using redundant measurements [J]. Navigation, 1988, 35(4): 483-501.). The parity vector method is to perform orthogonal triangular decomposition on the coefficient matrix of the linearized observation equation, and express the gross error of the faulty satellite as a parity vector, thereby constructing a test statistic.In the second part of the study on the integrity determination method, the determination of integrity availability is achieved by calculating whether the integrity risk of each epoch exceeds the warning limit. Since the calculation of the integrity risk of each epoch is related to the unknown probability of satellite failure, an indirect integrity determination method has emerged. Brown proposed a method of calculating the system's horizontal positioning error protection level through mathematical analysis to determine availability (reference: RGBrown and et al.Comparison of FDE and FDI RAIM Algorithms for GPS.Proceedings ofION NTM-94,1994:51~60.). The protection level calculated at each epoch is used to continuously compare with the warning limit. If the protection level exceeds the warning limit, an alarm is issued to the user. Before the protection level is lower than the warning limit, the navigation system is declared unavailable.

[0005] User-assisted integrity monitoring refers to a method in which GNSS users use navigation resources available to the user or the surrounding area as auxiliary redundant information to estimate and judge the integrity of GNSS together with GNSS measurements or navigation solutions. In aviation applications, barometric altimeter is used to assist integrity monitoring. Now, inertial navigation-assisted integrity monitoring and visual-assisted integrity monitoring are gradually developing. The commonly used algorithm is to combine the observation equation of barometric altimeter or inertial navigation with GNSS observation information under the Kalman filter framework to obtain the integrity enhancement algorithm under dynamic conditions. Yang Tao studied a method for inertial navigation-assisted satellite fault detection and isolation based on inspection (reference: Yang Tao, Zhao Ziyang, Li Xingfei, Zhang Jiachuan. Fault detection and isolation method for inertial / satellite tightly coupled system based on χ2 inspection [J]. Firepower and Command Control, 2016, 41(02): 169-172.). This method can automatically identify single satellite faults using fault detection functions, and automatically reconstruct observation information by eliminating faulty satellites in real time. Chen Weina proposed an airborne autonomous integrity monitoring algorithm assisted by a pressure altimeter (reference: Chen Weina, Yang Zhong, Gu Shanshan, Wang Yizhi. Airborne autonomous integrity monitoring algorithm assisted by pressure altimeter [J]. Navigation and Control, 2020, 19(03): 115-121.), using the altitude information provided by the pressure altimeter to establish the system observation equation for pressure altitude assisted integrity monitoring, which can still detect and identify faulty satellites when there are 5 visible satellites. However, the Kalman filter algorithm not only uses the observation information at the current moment, but also the observation information at the historical moment, which requires large amounts of calculation and data storage. In addition, the accuracy of the pressure altimeter's height measurement is greatly affected by airflow, and the error of the inertial navigation will also diverge over time. When using navigation resources to assist GNSS integrity monitoring, the reliability of the redundant information itself will affect the integrity of GNSS.

[0006] In the complex urban environment, due to the degradation of the satellite signal quality itself, the performance of the simple RAIM algorithm will not be able to meet the performance requirements of autonomous driving navigation. Moreover, for the method of fault detection under the Kalman filtering framework by using the navigation observation information assisted by other sensors, there is also a problem that the RAIM result may be contaminated by the errors brought by other sensors.

[0007] Because the traditional integrity monitoring algorithms of user terminals are mainly for aviation users, the pseudorange observation noise is usually regarded as obeying the Gaussian distribution, and the multipath error is estimated by a model. However, in the urban canyon environment, due to signal occlusion or reflection, the observation noise does not completely obey the Gaussian distribution, and it is even more difficult to model and estimate the multipath error.

[0008] The techniques relatively close to the present invention are the fault detection method using the residual vector method and the method of using inertial navigation to assist satellite integrity monitoring under the Kalman framework. However, the performance of the RAIM algorithm based on the residual vector will decline when the number of visible satellites is insufficient in the urban environment, and the integrity monitoring performance of the method based on inertial navigation assistance will instead decline due to the cumulative error of inertial navigation. Summary of the Invention

[0009] Object of the Invention: The technical problem to be solved by the present invention is to provide an integrity monitoring method for a fusion navigation system for autonomous driving in view of the deficiencies of the prior art.

[0010] To solve the above technical problem, the present invention discloses an integrity monitoring method for a fusion navigation system for autonomous driving, including the following steps:

[0011] Step 1: Use the vehicle-mounted camera for scene matching to obtain a reference position, and invert the distance from the vehicle to the feature point from the reference position;

[0012] Step 2: Use the vehicle-mounted lidar observation information and machine learning algorithm to assist the Global Navigation Satellite System for pseudorange error correction;

[0013] Step 3: Tightly couple the camera observation quantity and the Global Navigation Satellite System observation quantity to construct an observation equation;

[0014] Step 4: Calculate the least squares residual, and construct a test statistic therefrom for fault detection;

[0015] Step 5: Calculate the standard deviation of the predicted positioning error, fit the normal distribution of the positioning error and calculate the user protection level in the horizontal direction;

[0016] Step 6: Compare the obtained user protection level with the protection limit value to complete the integrity monitoring of the fusion navigation system for autonomous driving.

[0017] In the present invention, step 1 includes: using the carrier camera to perceive and detect the surrounding environment, matching the observation results with the prior three-dimensional high-precision point cloud map, and estimating the position of the carrier in the local coordinate system; using the building boundary points and the road boundary points as feature points, and inverting the distance from the carrier reference position to the feature points based on the coordinate information of the feature points.

[0018] In the present invention, step 2 comprises:

[0019] Step 2-1, using the global satellite navigation system receiver and laser radar in the carrier to collect data in the urban canyon, using the satellite signal carrier-to-noise ratio, satellite altitude angle, and reflection intensity of the building surface related to multipath error comparison, as well as the distance from the carrier to the building as input features;

[0020] Step 2-2, select pseudorange error as the label of each sample and construct a historical training data set;

[0021] Step 2-3, use the deep neural network algorithm to mine the relationship between the input characteristic satellite signal carrier-to-noise ratio, satellite altitude angle, reflection intensity of the building surface and the distance from the carrier to the building and the output variable pseudorange error, construct the pseudorange error prediction law, and perform pseudorange error correction.

[0022] In the present invention, step 3 comprises:

[0023] Step 3-1, convert the northeast celestial coordinates of the known feature points into geocentric earth-fixed coordinates;

[0024] Step 3-2, combine the pseudo-range observed by the global satellite navigation system and the distance inverted by the camera to solve the positioning.

[0025] In the present invention, step 3-1 comprises:

[0026] The northeast celestial coordinates of the known feature points are converted to geocentric earth-fixed coordinates, and the rotation matrix is:

[0027]

[0028] Among them, L is longitude and B is latitude.

[0029] In the present invention, in step 3-2, the process of combining the pseudo-range observed by the global satellite navigation system with the distance inverted by the camera to perform positioning and solving is as follows:

[0030]

[0031] Among them, ρ G =R+δ G is the observation equation of the global satellite navigation system, ρ C =R+δ C is the camera observation equation; where ρG The GNSS observed pseudorange corrected for the pseudorange error predicted by the deep neural network; ρ C The distance inverted from the camera positioning to the feature point; R is the distance from the satellite or feature point to the vehicle; δ G Other uncompensated pseudorange errors; δ C The camera ranging error;

[0032] For the GNSS observation equation, the three-dimensional coordinates of the vehicle and the receiver clock bias are estimated as unknown parameters; the camera observation equation estimates the three-dimensional coordinates of the vehicle; the geometric distance R is expanded according to the Taylor series to obtain:

[0033]

[0034] In the formula, R0 is the approximate geometric distance from the satellite or feature point to the vehicle; (x i , y i , z i ) are the coordinates of the i-th satellite or feature point; (x0, y0, z0) are the approximate coordinates of the vehicle; (dx, dy, dz) are the increments of the vehicle coordinates; after ignoring the non-linear error terms, we get:

[0035]

[0036] In the formula, E(·) is the mathematical expectation operator; after linearizing using the Taylor series, the tightly coupled joint positioning problem of GNSS observation data and camera observation data is approximately transformed into a linear problem, and this linear system is:

[0037] ΔR = A·dX

[0038] In the formula, ΔR = R - R0 is the observation value from the GNSS or the camera; dX = [dx, dy, dz, dt] T , dz, dy, dz are the increments of the three-dimensional coordinates of the vehicle; dt is the GNSS receiver clock bias parameter; the design matrix A is as follows:

[0039]

[0040] Among them, (x0, y0, z0) are the approximate coordinates of the vehicle; (x1, y1, z1) to (x m , y m , z m ) are the coordinates of satellites 1 to m; R1 to R m are the distances from satellites 1 to m to the vehicle; (x1, y1, z1) to (x n , y n , z n ) are the coordinates of feature points 1 to n; R1 to Rn The distances from 1 to n feature points to the carrier; the four parameters dx, dy, dz, and dt to be solved correspondingly are the three-dimensional coordinate increments of the carrier and the clock error of the global satellite navigation system receiver.

[0041] In the present invention, in step 4, calculating the least squares residual and constructing a test statistic for fault detection includes fault detection and fault identification;

[0042] Among them, the fault detection method includes:

[0043] The least squares solution of the equation described in step 3-2 is:

[0044]

[0045] Among them, are the estimated values of the carrier coordinate increment and the receiver clock error;

[0046] The calculated distance residual vector is:

[0047] z = (I - A(A T A) -1 A T )ΔR

[0048] Among them, I is the identity matrix; z is the distance residual vector; the distance residual vector is calculated through the design matrix A. After obtaining the distance residual vector, the a posteriori mean square error of unit weight of the sum of squared distance residuals is:

[0049]

[0050] Among them, SSE is the sum of squared distance residuals; the a posteriori mean square error of unit weight σ is used as the statistic for fault detection; when there is no fault, the distance observation error between the satellite and the camera follows an independent and normally distributed normal distribution, with a mean of 0 and a variance of According to the statistical distribution theory, follows a chi-square distribution with degrees of freedom m + n - 4; when there is a fault, follows a chi-square distribution with degrees of freedom m + n - 4 and a non-centrality parameter of λ;

[0051] When there is no fault in the system itself, a system alarm is a false alarm. According to the given false alarm rate P fa , it is obtained that:

[0052]

[0053] Among them, H0 represents the no-fault hypothesis; t is the actual detection value; P{{t < T 2}|H0} represents the probability that there is no fault in the system itself and no fault is detected; is X with degrees of freedom m + n - 42 Distribution probability density function, determine The detection threshold T 2 , through the detection threshold T 2 The square root T of to obtain the detection threshold σ of σ T Is:

[0054]

[0055] If σ > σ T , then there is incorrect observation information in the current global satellite navigation system and camera observations.

[0056] In step 4 of the present invention, calculating the least squares residual and constructing a test statistic for fault detection includes fault detection and fault identification;

[0057] Among them, the fault identification method includes:

[0058] Constructing a fault identification test quantity d based on the least squares residual vector i :

[0059]

[0060] Among them, z i Represents the distance residual of the i-th observation, and Q zii Represents the element corresponding to the i-th observation in the distance residual cofactor matrix. The statistic follows a standard normal distribution when there is no fault; evenly distributing the false alarm rate to m + n observations, the detection threshold of the fault identification statistic is:

[0061]

[0062] Among them, P{d i > T d} represents that the fault identification statistic d i Is greater than the threshold T d Probability; Is the probability density function of the standard normal distribution; when a fault exists, if d i > T d , it means that the i-th observation has a fault.

[0063] In step 5 of the present invention, calculating the standard deviation of the pseudorange error of m satellites predicted at the current epoch and the value of the horizontal dilution of precision of the satellite distribution, multiplying them to obtain the standard deviation σ of the predicted positioning error x , and fitting the normal distribution of the positioning error to calculate the user protection level in the horizontal direction, the method includes:

[0064] Step 5-1, in the case of no fault:

[0065] Calculating the integrity risk, the method is as follows:

[0066] P HMI,0 = P{|Δx| > HPL|H0}P{σ < σ T |H0}P{H0}

[0067] where P HMI,0 is the integrity risk assigned to the fault - free case; HPL is the user protection level in the horizontal direction; H0 is the fault - free hypothesis; Δx is the horizontal positioning error; P{|Δx| > HPL|H0} is the probability that the positioning error exceeds the user protection level under the fault - free hypothesis; P{σ < σ T |H0} is the probability that the fault detection statistic does not exceed the detection threshold under the fault - free hypothesis, that is To obtain the probability P{|Δx| > HPL|H0} that the positioning error exceeds the user protection level under the fault - free hypothesis, the method is as follows:

[0068]

[0069] Calculate the protection level by determining the probability distribution of the horizontal positioning error, and use the standard deviation σ x of the predicted positioning error as the standard deviation of the normal distribution of the positioning error, that is Δx ~ N(0, σ x ), and the user protection level under the fault - free hypothesis is obtained as follows:

[0070]

[0071] where HPL0 is the user protection level under the fault - free hypothesis; is the 1 - α1 / 2 quantile of the standard normal distribution,

[0072] Step 5 - 2, in the case of a fault:

[0073] The calculation formula for the integrity risk is:

[0074] P HMI,1 = P{|Δx| > HPL|H1}P{σ < σ T |H1}P{H1}

[0075] In the formula, P HMI,1 is the integrity risk assigned to the case of a fault; H1 is the fault hypothesis; P{|Δx| > HPL|H1} is the probability that the positioning error exceeds the user protection level under the fault hypothesis; P{σ < σ T |H1} is the probability that the fault detection statistic does not exceed the detection threshold under the fault hypothesis, that is the miss - detection rate P MD ; when a fault occurs, the probability distribution of the horizontal positioning error is Δx ~ N(μ, σ x), where μ is the fault deviation, and the minimum detectable coarse difference value under the condition that the fault detection rate is greater than or equal to 99.9% is used as the estimated value of μ.

[0076] Obtain the probability that the positioning error exceeds the user protection level under the assumption of a fault:

[0077]

[0078] Then the user protection level when there is a fault is:

[0079]

[0080] In the formula, HPL1 is the user protection level under the assumption of a fault; is the 1 - α2 / 2 quantile of the standard normal distribution, where is the element in the first column and the i - th row of matrix A; A i,2 is the element in the second column and the i - th row of matrix A.

[0081] In the present invention, step 6 includes:

[0082] Calculate the user protection level in the horizontal direction, and the method is:

[0083] HPL = max{HPL0, HPL1}

[0084] Compare this user protection level with the protection limit value. If the user protection level does not exceed the protection limit value, the integrity is available; if the user protection level exceeds the protection limit value, an alarm is given to the user, and finally, the integrity monitoring of the multi - sensor - assisted global satellite navigation system for autonomous driving is completed.

[0085] Beneficial effects:

[0086] In order to solve the problem that the performance of the original satellite integrity monitoring algorithm is insufficient due to the reduction of redundant information when used in complex environments, the present invention proposes a multi - sensor - assisted GNSS integrity monitoring method. The camera and 3D urban map navigation information are used to assist GNSS in satellite fault detection, and considering the possible unreliability of the redundant sensor observations, a two - way sensor fault detection method is designed to avoid the error transfer of other sensors to the GNSS integrity monitoring; the protection level is calculated based on the real - time predicted pseudorange error variance, and a protection level calculation method that is more suitable for the positioning error in the urban environment is constructed to achieve effective integrity monitoring.

[0087] Aiming at the integrity requirements of autonomous driving in the urban environment, the present invention designs a multi - sensor - assisted GNSS integrity monitoring method, which effectively controls the observation quality of GNSS signals by using the redundant information of other sensors and machine learning algorithms, and ensures the reliability of using satellite positioning results for autonomous driving.

[0088] In the method of the present invention, GNSS pseudorange observation and camera inversion distance are used together to establish linear equations for dual fault detection of satellites and visual feature points, and a DNN algorithm is used to predict pseudorange errors. The standard deviation of the pseudorange error is used to approximately fit the probability distribution of the positioning error and then solve the specific implementation process of the user protection level. At the same time, the problems of performance degradation when the number of visible satellites is insufficient in an urban environment and degradation of integrity monitoring performance due to the accumulated errors of inertial navigation are overcome. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments, and the above and / or other advantages of the present invention will become more clear.

[0090] Figure 1 It is the overall flow chart of the present invention. DETAILED DESCRIPTION

[0091] The core content of the present invention is to use the GNSS pseudorange observation and the camera inversion distance to establish a linear equation for dual fault detection of satellite and visual feature points, and use the deep neural network (DNN) algorithm to predict the pseudorange error, and use the standard deviation of the pseudorange error to approximately fit the positioning error probability distribution to solve the specific implementation process of the user protection level.

[0092] A method for monitoring the integrity of a fusion navigation system for autonomous driving, such as Figure 1 As shown:

[0093] Step 1: Use the camera to perform scene matching to obtain the reference position, and invert the distance from the carrier to the feature point from the reference position. Use the camera to sense and detect the surrounding environment, match the observation results with the prior 3D high-precision point cloud map, and estimate the position of the carrier in the local coordinate system; use the building boundary points and road boundary points as feature points, and invert the distance from the carrier reference position to the feature point based on the coordinate information of the feature point.

[0094] Step 2: Use LiDAR (Light detection and ranging) observation information and machine learning algorithms to assist GNSS in correcting pseudorange errors.

[0095] 1) In the offline part, GNSS receivers and LiDAR are first used to collect data in urban canyons, and the satellite signal carrier-to-noise ratio, satellite altitude angle, reflection intensity of the building surface, and distance from the carrier to the building, which are related to the multipath error comparison, are used as input features.

[0096] 2) After determining the input features, the pseudorange error is selected as the label of each sample to construct the historical training dataset.

[0097] 3) Use the deep neural network algorithm to mine the relationship between the input feature satellite signal carrier-to-noise ratio, satellite elevation angle, reflection intensity of the building surface, and the distance from the carrier to the building and the output variable pseudorange error, and construct a pseudorange error prediction rule.

[0098] Step 3: Tightly couple the camera observation with the GNSS observation to construct an observation equation.

[0099] 1) Convert the north-east-down coordinates of the known feature points to the Earth-centered Earth-fixed coordinates, and the rotation matrix is

[0100]

[0101] where L is the longitude; B is the latitude.

[0102] 2) The process of joint positioning solution using the GNSS observed pseudorange and the distance inverted by the camera is as follows:

[0103]

[0104] where ρ G = R + δ G is the GNSS observation equation, ρ C = R + δ C is the camera observation equation; ρ G is the GNSS observed pseudorange corrected by the pseudorange error predicted by the deep neural network; ρ C is the distance inverted to the feature point according to the camera positioning; R is the distance from the satellite or feature point to the carrier; δ G is other uncompensated pseudorange errors; δ C is the camera ranging error.

[0105] For the GNSS observation equation, the user's three-dimensional coordinates and the receiver clock offset are estimated as unknown parameters; for the camera observation equation, only the user's three-dimensional coordinates need to be estimated without estimating the clock offset. Expand the geometric distance R according to the Taylor series, and we can get:

[0106]

[0107] where R0 is the approximate geometric distance from the satellite or feature point to the carrier; (x i , y i , z i ) are the coordinates of the i-th satellite or feature point; (x0, y0, z0) are the approximate coordinates of the carrier; (dx, dy, dz) are the increments of the carrier coordinates. After ignoring the non-linear error terms, we can get:

[0108]

[0109] In the formula, E(·) is the mathematical expectation operator. After linearization using the Taylor series, the joint positioning problem of tightly coupling GNSS observation signals and camera observation data can be approximately transformed into a linear problem, and this linear system is:

[0110] ΔR = A·dX

[0111] In the formula, ΔR = R - R0 is the observation value from GNSS or the camera; dX = [dx, dy, dz, dt] T , and dt is the GNSS receiver clock error parameter. The design matrix A is as follows:

[0112]

[0113] In the formula, (x0, y0, z0) is the approximate coordinate of the carrier; (x1, y1, z1) to (x m , y m , z m ) are the coordinates of satellites from 1 to m; R1 to R m are the distances from satellites from 1 to m to the carrier; (x1, y1, z1) to (x n , y n , z n ) are the coordinates of feature points from 1 to n; R1 to R n are the distances from feature points from 1 to n to the carrier. The four parameters dx, dy, dz, dt to be solved correspond to the user's three-dimensional coordinate increment and the GNSS receiver clock error.

[0114] Step 4: Calculate the least squares residual, and construct a test statistic for fault detection based on this.

[0115] 1) Fault detection

[0116] The least squares solution of the above equation is:

[0117]

[0118] In the formula, is the estimated value of the user's coordinate increment and the receiver clock error.

[0119] The distance residual vector is calculated as:

[0120] z = (I - A(A T A) -1 A T )ΔR

[0121] In the formula, z is the distance residual vector; I is the identity matrix. As long as the design matrix A is known, the distance residual vector can be directly calculated without calculating the least squares solution. After obtaining the distance residual vector, the a posteriori mean square error of unit weight of the sum of squared distance residuals is:

[0122]

[0123] Wherein, SSE is the sum of squared residuals of the distance. The a posteriori standard error of unit weight σ is used as the statistic for fault detection. When there is no fault, it is considered that the distance observation errors of the satellite and the camera follow an independent and normally distributed normal distribution, with a mean of 0 and a variance of According to the statistical distribution theory, will follow a chi-square distribution with degrees of freedom m + n - 4; and when there is a fault, will follow a chi-square distribution with degrees of freedom m + n - 4 and a non-centrality parameter of λ.

[0124] When there is no fault in the system itself, a system alarm is a false alarm. Given the false alarm rate from the integrity requirements of the user then there is:

[0125]

[0126] Wherein, H0 represents the no-fault hypothesis; P{{t < T 2}|H0} represents the probability that there is no fault in the system itself and no fault is detected; is the probability density function of the χ 2 distribution with degrees of freedom m + n - 4, and the detection threshold T is determined through the above formula 2 , and the detection threshold of σ can be obtained as:

[0127]

[0128] If σ > σ T , it means that there is incorrect observation information in the current GNSS and camera observations, and fault identification and elimination are required.

[0129] 2) Fault identification

[0130] Construct a fault identification test quantity d i based on the least squares residual vector:

[0131]

[0132] Wherein, z i represents the distance residual of the i-th observation, and Q zii represents the element corresponding to the i-th observation in the distance residual cofactor matrix. The statistic follows a standard normal distribution when there is no fault. Distributing the false alarm rate evenly among the m + n observations, the detection threshold of the fault identification statistic can be obtained as:

[0133]

[0134] Wherein, P{d > T d} represents the probability that the fault identification statistic is greater than the threshold; is the probability density function of the standard normal distribution. Therefore, when a fault exists, if d i >T d , it indicates that the i-th observable is faulty and needs to be excluded.

[0135] Step 5: Calculate the standard deviation of the pseudorange error of the m satellites predicted for this epoch and the value of the Horizontal Dilution of Precision (HDOP) of the satellite distribution, multiply them to obtain the standard deviation of the predicted positioning error, and use this to fit the normal distribution of the positioning error to calculate the user protection level in the horizontal direction.

[0136] 1) In the case of no fault:

[0137] The formula for the integrity risk is:

[0138] P HMI,0 = P{|Δx| > HPL|H0}P{σ < σ T |H0}P{H0}

[0139] where P HMI,0 is the integrity risk assigned to the case of no fault; HPL is the user protection level in the horizontal direction; H0 is the no-fault hypothesis; Δx is the horizontal positioning error; P{|Δx| > HPL|H0} is the probability that the positioning error exceeds the user protection level under the no-fault hypothesis; P{σ < σ T |H0} is the probability that the fault detection statistic does not exceed the detection threshold under the no-fault hypothesis, that is The probability that the positioning error exceeds the user protection level under the no-fault hypothesis can be obtained:

[0140]

[0141] To calculate the protection level, it is necessary to determine the probability distribution of the horizontal positioning error, and use the standard deviation σ x of the predicted positioning error as the standard deviation of the normal distribution of the positioning error, that is, Δx ~ N(0, σ x ), and the user protection level under the no-fault hypothesis can be obtained as follows:

[0142]

[0143] where HPL0 is the user protection level under the no-fault hypothesis; is the 1 - α1 / 2 quantile of the standard normal distribution,

[0144] 2) In the case of a fault:

[0145] The formula for the integrity risk is:

[0146] P HMI,1 = P{|Δx| > HPL|H1}P{σ < σ T |H1}P{H1}

[0147] Wherein, P HMI,1 is the integrity risk assigned to the faulty situation; H1 is the faulty hypothesis; P{|Δx| > HPL|H1} is the probability that the positioning error exceeds the user protection level under the faulty hypothesis; P{σ < σ T |H1} is the probability that the fault detection statistic does not exceed the detection threshold under the faulty hypothesis, that is, the missed detection rate P MD . When a fault occurs, the probability distribution of the horizontal positioning error is Δx ~ N(μ, σ x ), μ is the fault deviation, and the minimum detectable gross difference under the condition that the fault detection rate is greater than or equal to 99.9% is used as the estimate of μ.

[0148] The probability that the positioning error exceeds the user protection level under the faulty hypothesis can be obtained:

[0149]

[0150] Then the user protection level when a fault exists is:

[0151]

[0152] Wherein, HPL1 is the user protection level under the faulty hypothesis; is the 1 - α2 / 2 quantile of the standard normal distribution, is the element in the first column of the i - th row of matrix A; A i,2 is the element in the second column of the i - th row of matrix A.

[0153] In summary, the user protection level in the horizontal direction is HPL = max{HPL0, HPL1}. Compare the calculated user protection level with the protection limit value. If the user protection level does not exceed the protection limit value, it means that the integrity is available; if the user protection level exceeds the protection limit value, an alarm is sent to the user.

[0154] The present invention provides an idea and method for the integrity monitoring method of a fusion navigation system for autonomous driving. There are many methods and ways to specifically implement this technical solution. The above - mentioned is only the preferred embodiment of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented by existing technologies.

Claims

1. An integrity monitoring method for an integrated navigation system for autonomous driving, characterized in that, It includes the following steps: Step 1: Use the vehicle-mounted camera to perform scene matching to obtain the reference position, and invert the distance from the vehicle to the feature points from the reference position; Step 2: Use the vehicle-mounted lidar observation information and machine learning algorithm to assist the global satellite navigation system in pseudorange error correction; Step 3: Tightly couple the camera observation quantity and the global satellite navigation system observation quantity to construct an observation equation; Step 4: Calculate the least squares residual, and construct a test statistic for fault detection; Step 5: Calculate the standard deviation of the predicted positioning error, fit the normal distribution of the positioning error, and calculate the user protection level in the horizontal direction; Step 6: Compare the obtained user protection level with the protection limit value to complete the integrity monitoring of the integrated navigation system for autonomous driving; Among them, Step 1 includes: Using the vehicle-mounted camera to sense and detect the surrounding environment, matching the observation results with the prior three-dimensional high-precision point cloud map, and estimating the position of the vehicle in the local coordinate system; Taking the building boundary points and road boundary points as feature points, and inverting the distance from the vehicle reference position to the feature points according to the coordinate information of the feature points; Step 2 includes: Step 2-1: Use the global satellite navigation system receiver and lidar in the vehicle to collect data in the urban canyon. Use the carrier-to-noise ratio of the satellite signal, satellite elevation angle, reflection intensity of the building surface, and the distance from the vehicle to the building related to multipath error comparison as input features; Step 2-2: Select the pseudorange error as the label of each sample to construct a historical training data set; Step 2-3: Use the deep neural network algorithm to mine the relationship between the input features of the carrier-to-noise ratio of the satellite signal, satellite elevation angle, reflection intensity of the building surface, and the distance from the vehicle to the building and the output variable of the pseudorange error, construct a pseudorange error prediction rule, and perform pseudorange error correction; Step 3 includes: Step 3-1: Convert the north-east-down coordinates of the known feature points into earth-centered earth-fixed coordinates; Step 3-2: Jointly use the global satellite navigation system observed pseudorange and the distance inverted by the camera for positioning solution.

2. The integrity monitoring method for a fusion navigation system for autonomous driving as described in claim 1, wherein, Step 3-1 includes: Convert the north-east-down coordinates of the known feature points into earth-centered earth-fixed coordinates, and the rotation matrix is: Where L is the longitude and B is the latitude.

3. A method for integrity monitoring of an integrated navigation system for autonomous driving as described in claim 2, characterized in that In Step 3-2, the process of jointly using the global satellite navigation system observed pseudorange and the distance inverted by the camera for positioning solution is as follows: where ρ G = R + δ G is the observation equation of the global satellite navigation system, and ρ C = R + δ C is the observation equation of the camera; where ρ G is the observed pseudorange of the global satellite navigation system after being corrected by the pseudorange error predicted by the deep neural network; ρ C is the distance inverted from the camera positioning to the feature point; R is the distance from the satellite or feature point to the carrier; δ G is other uncompensated pseudorange errors; δ C is the ranging error of the camera; For the global satellite navigation system observation equation, estimate the vehicle three-dimensional coordinates and receiver clock offset as unknown parameters; The camera observation equation estimates the vehicle three-dimensional coordinates; Expand the geometric distance R according to the Taylor series to get: where \(R_0\) is the approximate geometric distance from the satellite or feature point to the vehicle; \((x i , y i , z i ) are the coordinates of the \(i\)-th satellite or feature point; \((x_0, y_0, z_0)\) are the approximate coordinates of the vehicle; \((dx, dy, dz)\) are the increments of the vehicle coordinates; after neglecting the non-linear error terms, we get: Where E(·) is the mathematical expectation operator; After linearization using the Taylor series, the joint positioning problem of tightly coupling the global satellite navigation system observation data and the camera observation data is approximately transformed into a linear problem, and the linear system is: ΔR = A·dX where ΔR = R - R0 is the observation value from the global satellite navigation system or the camera; dX = [dx, dy, dz, dt] T , dx, dy, and dz are the three-dimensional coordinate increments of the carrier; dt is the clock error parameter of the global satellite navigation system receiver; the design matrix A is as follows: Among them, (x0, y0, z0) are the approximate coordinates of the carrier; (x1, y1, z1) to (x m , y m , z m ) are the coordinates of 1 to m satellites; R1 to R m are the distances from 1 to m satellites to the carrier; (x1, y1, z1) to (x n , y n , z n ) are the coordinates of 1 to n feature points; R1 to R n are the distances from 1 to n feature points to the carrier; the 4 parameters dx, dy, dz, dt to be solved correspondingly are the three-dimensional coordinate increment of the carrier and the clock error of the global navigation satellite system receiver.

4. The integrity monitoring method for an integrated navigation system for autonomous driving as described in claim 3, characterized in that, In Step 4, the method of calculating the least squares residual and constructing a test statistic for fault detection includes fault detection and fault identification; Among them, the fault detection method includes: The least squares solution of the equation described in Step 3-2 is: Among them, are the carrier coordinate increment and the receiver clock error estimate; Calculate the obtained distance residual vector as: z=(I - A(A T A) -1 A T )ΔR where \(I\) is the identity matrix; \(z\) is the distance residual vector; the distance residual vector is calculated through the design matrix \(A\), and after obtaining the distance residual vector, the a posteriori standard error of unit weight of the sum of squared distance residuals is: Among them, SSE is the sum of squared residuals of the distance; the a posteriori standard error of unit weight σ is used as the statistic for fault detection; when there is no fault, the distance observation errors of the satellite and the camera follow a normal distribution with independent distribution, with a mean of 0 and a variance of According to the statistical distribution theory, follows a chi-square distribution with degrees of freedom m + n - 4; when there is a fault, follows a chi-square distribution with degrees of freedom m + n - 4 and non-centrality parameter λ; When the system itself has no faults, the system alarm is a false alarm. According to the given false alarm rate we get: Among them, H0 represents the no-fault hypothesis; t is the actual detection value; P{{t<T 2}|H0} represents the probability that the system itself has no fault and no fault is detected; is the probability density function of the χ 2 distribution with degrees of freedom m + n - 4, and determines the detection threshold T 2 , and through the square root T of the detection threshold T 2 the detection threshold σ of σ is obtained, and T is: If σ > σ T , there is incorrect observation information in the current global satellite navigation system and camera observation data.

5. A method for integrity monitoring of an integrated navigation system for autonomous driving as described in claim 4, characterized in that, In step 4, calculating the least squares residuals and constructing a test statistic for fault detection methods includes fault detection and fault identification; where the fault identification method includes: Construct the fault identification test quantity d based on the least squares residual vector i : Among them, z i represents the distance residual of the i-th observable, and Q zii represents the element corresponding to the i-th observable in the cofactor matrix of the distance residual. The statistic follows a standard normal distribution in the absence of faults. The false alarm rate is evenly distributed among m + n observables, and the detection threshold of the fault identification statistic is obtained as follows: where, P{d i > T d} represents the probability that the fault identification statistic d i is greater than the threshold T d ; is the probability density function of the standard normal distribution; when a fault exists, if d i > T d , it means that the i-th observable has a fault.

6. A method for integrity monitoring of an integrated navigation system for autonomous driving as described in claim 5, characterized in that, In step 5, calculate the standard deviation of the pseudorange error of the m satellites predicted in the current epoch and the value of the horizontal dilution of precision of the satellite distribution, and multiply them to obtain the standard deviation σ of the predicted positioning error. x , and fit the normal distribution of the positioning error to calculate the user protection level in the horizontal direction. The methods include: Step 5-1, in the case of no fault: Calculating the integrity risk, the method is as follows: P HMI,0 = P{|Δx| > HPL|H0} P{σ < σ T |H0} P{H0} where, P HMI,0 is the integrity risk assigned to the fault-free situation; HPL is the user protection level in the horizontal direction; H0 is the fault-free hypothesis; Δx is the horizontal positioning error; P{|Δx|>HPL|H0} is the probability that the positioning error exceeds the user protection level under the fault-free hypothesis; P{σ<σ T |H0} is the probability that the fault detection statistic does not exceed the detection threshold under the fault-free hypothesis, that is To obtain the probability P{|Δx|>HPL|H0} that the positioning error exceeds the user protection level under the fault-free hypothesis, the method is as follows: The protection level is calculated by determining the probability distribution of the horizontal positioning error, and the standard deviation σ of the predicted positioning error is used x As the standard deviation of the normal distribution of the positioning error, that is, Δx ~ N(0, σ x ), the user protection level under the no-fault hypothesis is obtained as follows: where HPL0 is the user protection level under the fault-free hypothesis; is the 1-α1 / 2 quantile of the standard normal distribution, Step 5-2, in the case of a fault: The calculation formula for the integrity risk is: P HMI,1 = P{|Δx| > HPL|H1} P{σ < σ T |H1} P{H1} where P HMI,1 is the integrity risk assigned to the faulty situation; H1 is the faulty hypothesis; P{|Δx|>HPL|H1} is the probability that the positioning error exceeds the user protection level under the faulty hypothesis; P{σ<σ T |H1} is the probability that the fault detection statistic does not exceed the detection threshold under the faulty hypothesis, that is, the miss detection rate P MD ; when a fault occurs, the probability distribution of the horizontal positioning error is Δx~N(μ,σ x ), μ is the fault bias, and the minimum detectable gross error value under the condition that the fault detection rate is greater than or equal to 99.9% is used as the estimate of μ; Obtaining the probability that the positioning error exceeds the user protection level under the fault hypothesis: Then the user protection level when there is a fault is: Wherein, HPL1 is the user protection level under the assumption of a fault; is the 1-α2 / 2 quantile of the standard normal distribution, where A i,1 is the element in the first column and the i-th row of matrix A; A i,2 is the element in the second column and the i-th row of matrix A.

7. A method for integrity monitoring of an integrated navigation system for autonomous driving as described in claim 6, characterized in that, Step 6 includes: Calculating the user protection level in the horizontal direction, the method is: \(HPL=\max\{HPL0, HPL1\}\) Comparing the user protection level with the protection limit value. If the user protection level does not exceed the protection limit value, the integrity is available; if the user protection level exceeds the protection limit value, an alarm is given to the user, and finally the integrity monitoring of the integrated navigation system for autonomous driving is completed.

Images