Single-antenna dynamic initialization method applied to INS / GPS integrated navigation

Abstract

A single-antenna dynamic initialization method applied to INS / GPS integrated navigation comprises the steps that when the vehicle speed is larger than 20 km / h and GPS signals are good, the course angle and the pitch angle calculated through the GPS northeast speed serve as attitude information of dynamic initialization to be used for updating an attitude matrix, appropriate observed quantities are established for Kalman filtering fusion, and the attitude matrix is calculated through the dynamic initialization. And the error of the inertial navigation system is estimated and corrected on line, so that the precision of integrated navigation is improved. The method can be applied to a single-antenna GPS / SINS inertial integrated navigation initial alignment scheme, the algorithm is easy to implement and operate, the required dynamic initialization condition is easy to meet, the method has wide practicability, and through actual sports car test verification, when the vehicle speed is larger than 20 km / h, the track angle is initialized and calibrated through GPS speed information calculation, and the accuracy of track angle calibration is greatly improved. The filter convergence speed is high, the navigation positioning precision is high, and the method has important significance on research and development of the automatic driving technology.

Description

Technical Field

[0001] The present invention relates to the field of vehicle navigation, and in particular to a method for dynamic initialization of a single antenna for INS / GPS combined navigation applied to vehicle engineering and autonomous driving Background Art

[0002] At present, various automobile manufacturers often use an INS (inertial navigation) / GPS combined navigation method for positioning in their automatic / automatic assisted driving positioning technology. This combined navigation method can not only rely on the satellite navigation system to provide accurate positioning information when the satellite signal is good, but also rely on the vehicle's own signal to calculate accurate positioning information in occasions / road conditions where the satellite signal disappears due to obstruction, such as tunnels, basements, between high-rise buildings, and dense tree-lined avenues.

[0003] The principle of INS / GPS integrated navigation is to obtain the body posture and velocity information by integrating the angular velocity and acceleration measured by the inertial measurement unit, and to infer the position information by integrating the velocity after the posture projection. However, in actual application, due to the large noise of the inertial device itself, the error generated by the integration process gradually increases, and the amount of information output by the inertial device is a relative quantity. Therefore, the integrated navigation system usually needs to use external accurate observations to initialize the integrated navigation and the Kalman filter algorithm (Kalman filter algorithm specifies the closed-loop control quality, and also specifies the controller output, overcomes the disadvantage of controller output jump, and has a better control effect) estimation to improve the accuracy of the integrated navigation attitude positioning.

[0004] In the existing automotive navigation parts market, there are two solutions, single antenna and dual antenna, to provide external observation information. The dual antenna solution provides heading angle, speed, and position information. The heading angle provided by this solution is highly accurate and precise. The principle is that since the dual antenna has the position information of two points, the direction can be easily confirmed by the straight line connecting the two points. The system does not need to move at all, and the accurate heading angle can be obtained in a static state. However, the dual antenna GPS has the disadvantages of being difficult to install (there is an error angle for the dual antenna heading angle installation), large size, and high cost. Since the single antenna solution only has the position information of one antenna (one point), and one point is non-directional, the single antenna combined navigation system must calculate the heading through the movement trajectory by moving the antenna. From the above principle, it can be judged that the single antenna solution is difficult to accurately judge the heading angle at low speed or static state, so the single antenna solution can basically only provide speed and position information, and cannot directly provide heading angle information.

[0005] After actual testing in open areas, the single-antenna combined navigation system has cumulative errors and shows a divergent trend, while the dual-antenna system shows a convergent trend within a reasonable random error. However, due to the low manufacturing cost, small size, and easy installation of the single-antenna solution, although the single antenna cannot directly provide heading angle information, there is no problem of heading angle installation error, so it can basically replace the dual antenna in terms of positioning accuracy and has become the first choice of navigation components for many automobile manufacturers.

[0006] However, with the fierce competition in automobile research and development, production and sales and customers' demand for vehicle performance, the problem that a single antenna cannot directly provide heading angle information has gradually emerged. Heading angle initialization information is crucial for combined navigation. Therefore, it has become an indispensable trend to enable a single antenna to indirectly obtain accurate heading angle information to improve the accuracy of combined attitude positioning. Summary of the invention

[0007] In order to solve the problem that the single antenna provides speed and position information initialization and observation quantity for state estimation during vehicle movement in the single antenna solution under the prior art, and iteratively updates only by relying on the initialization attitude information calculated by the inertial device under static state, but the initialization heading angle information is inaccurate and the gyro device itself has large noise, the speed obtained by attitude projection solution is inaccurate, and the position information calculated by speed integral solution is inaccurate, resulting in low precision of combined navigation positioning. The present invention provides a single antenna dynamic initialization method applied to INS / GPS combined navigation, which can overcome the disadvantage that the single antenna cannot provide heading angle, that is, when the vehicle speed is greater than 20km / h and the GPS signal is good, the heading angle and pitch angle calculated by the GPS northeast speed are used as the attitude information for dynamic initialization to update the attitude matrix, and establish appropriate observation quantity for Kalman filter fusion, and estimate and correct the inertial navigation system error online, so as to improve the precision of combined navigation, and finally effectively solve the problem that the single antenna cannot directly provide heading angle observation quantity for combined navigation heading angle initialization.

[0008] A single antenna dynamic initialization method for INS / GPS integrated navigation of the present invention is described as follows:

[0009] 1) Power on the integrated navigation system to perform static initialization;

[0010] 2) Initialize the filter parameters and set the correlation matrix;

[0011] 3) In the above steps, Kalman filtering is used to further predict the process and calculate the state vector X and the state transfer matrix And the state vector covariance matrix P, as follows

[0012]

[0013] Where:

[0014] State transfer matrix at time k, state transfer matrix at time k+1;

[0015] X k , X k+1 : state vector at time k, state vector at time k+1;

[0016] P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1;

[0017] 4) Dynamic initialization:

[0018] This step uses the heading angle and pitch angle calculated by the GPS northeast velocity, while the roll angle uses the result of the inertial navigation solution at the previous moment, and updates the state matrix, velocity and position information;

[0019] The roll angle is the angle between the transverse axis of the carrier and the horizontal line. The navigation system in the present invention is used to identify the lateral inclination angle of the target, and its value is equal to the angle between the line perpendicular to the bow and stern line on the plane where the target object is located and its projection on the horizontal plane.

[0020] 5) In the above step 4), the filter parameters are initialized, the state vector X is set to 0, the state vector covariance matrix P is set to the initial value, the vehicle-mounted and inertial measurement unit set the error angle and odometer coefficient estimation value, the estimation values ​​of other lever arms are all set to 0, and the dynamic initialization state bit is marked;

[0021] 6) Kalman filter update process:

[0022] This step uses the state vector covariance matrix P calculated by the Kalman filter prediction process in step 3), combined with the measurement equation coefficient matrix H and the observation noise matrix R, to calculate the gain matrix K, the state vector error matrix X and the state vector covariance matrix P. The formula is as follows:

[0023]

[0024] X k+1 =X k+ K k (Z k- H k X k )

[0025] P k+1 =(IK k H k ) k

[0026] Where:

[0027] K k : gain matrix at time k;

[0028] R k : k-time observation noise matrix;

[0029] H k : k-time measurement equation coefficient matrix;

[0030] I: identity matrix;

[0031] X k , X k+1 : The state vector error matrix at time k, the state vector error matrix at time k+1;

[0032] P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1;

[0033] 7) Provide compensation feedback:

[0034] This step uses the state vector error matrix X obtained in step 6) above to compensate the navigation output result. After the state quantity is compensated, the state quantity of the corresponding state vector error matrix X is set to 0, and it is not set to 0 if there is no compensation;

[0035] 8) Iterate and update and navigate output:

[0036] After the compensation feedback process of the above step 7) is completed, the update process of a Kalman filter is completed, and the calculation of the next prediction process is carried out, and a new update process of the Kalman filter is carried out. The iterative update of steps 1) to 7) is repeated, and the Kalman filter calculation and estimation are performed to provide stable and reliable position, speed, and attitude information for the navigation output process.

[0037] According to the single antenna dynamic initialization method applied to INS / GPS integrated navigation described in the present invention, it is characterized in that the static initialization in step 1) is judged in a manner that if there is no satellite GPS signal, the static initialization is judged to be incomplete, until the satellite signal is good, it is judged that the static initialization is completed, and the next step is performed.

[0038] According to the single antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention, it is characterized in that the dynamic initialization of step 4) determines whether the condition for dynamic initialization is met when the GPS is fixed and the vehicle speed is greater than 20km / h.

[0039] GPS fixed solution means that the receiver position is fixed during the observation process, the integer ambiguity has been resolved and the measurement has been initialized.

[0040] The above dynamic initialization requires the carrier speed to be above 20km / h. This is based on the analysis of the data collected from the sports car. When the speed is greater than 20km / h and the signal is good, the heading angle (also called track angle) calculated by the GPS east and north speed is more accurate than the benchmark. The specific analysis after actual measurement is as follows:

[0041] 1. When the vehicle speed is low or close to 0, the heading angle provided by the true value device is significantly different from the fixed solution track angle, and there is a jump point phenomenon.

[0042] 2. When the vehicle speed is less than 20km / h, the error between the track angle under the fixed solution and the true value equipment heading angle fluctuates between -12° and 12°, which is a large error;

[0043] 3. When the vehicle speed is greater than 20km / h, the error tends to be stable and close to 0. The track angle when the vehicle speed is greater than 20km / h is more accurate using the fixed solution.

[0044] The reason for the above analysis is that the single-antenna GPS does not directly provide real-time heading information. Generally, the attitude information is obtained indirectly by calculating the velocity information output by the GPS. The calculated attitude is actually the rotation angle of the carrier's velocity vector relative to the geographic coordinate system. There are differences in the sideslip angle and angle of attack from the Euler attitude angle, so it is called the attitude angle. When the carrier performs coordinated motion, the pseudo attitude angle has the same performance as the attitude angle, so the pseudo attitude angle can be used as the initial attitude angle to align the strapdown inertial navigation system. Assuming that the velocity components in the geographic coordinate system output by the GPS are V e 、V n 、V u , then the heading angle ψ (also called track angle) and pitch angle θ in the pseudo attitude angle are:

[0045] ψ=arctan(V e / V n )

[0046]

[0047] When the vehicle is moving, it is difficult to obtain a relatively accurate roll angle, so the roll angle can be directly used as the roll angle solution result in static state or initialized to 0. However, the attitude information calculated by the speed provided by the single-antenna GPS when the vehicle is stationary or the speed is too low is inaccurate, so dynamic initialization requires the carrier speed to be above 20km / h.

[0048] The invention discloses a single-antenna dynamic initialization method applied to INS / GPS integrated navigation. The principle of the method is that: according to the information provided by the single-antenna GPS, the attitude, speed and position are dynamically initialized, and the inaccurate heading angle and the horizontal angle (pitch angle and roll angle information) calculated by using the accelerometer information are used as the initial value of the attitude angle. However, as the observation quantity is established and the Kalman filter is estimated, the error gradually converges, and the integrated navigation system can obtain more accurate attitude, speed and position information. The technical method of the invention has important significance and value for the rapid convergence of the single-antenna initialization.

[0049] The following is obtained by using a single antenna dynamic initialization method for INS / GPS integrated navigation of the present invention:

[0050] Beneficial effects:

[0051] 1. The single-antenna dynamic initialization method for INS / GPS integrated navigation of the present invention can be applied to the initial alignment scheme of single-antenna GPS / SINS inertial integrated navigation. The algorithm is easy to operate, the required dynamic initialization conditions are easy to meet, and the method has wide practicability.

[0052] 2. The single-antenna dynamic initialization method for INS / GPS integrated navigation of the present invention has been verified by actual sports car tests. When the vehicle speed is greater than 20 km / h, the track angle calculated using GPS speed information is used for initialization calibration. The filter converges quickly and the navigation positioning accuracy is high.

[0053] 3. The single-antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention is of great significance to the research and development of autonomous driving technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 A specific flow chart of a single antenna dynamic initialization method applied to INS / GPS integrated navigation of the present invention;

[0055] Figure 2 It is a schematic diagram of the error between the heading angle of the integrated navigation system without dynamic initialization scheme and the true heading angle;

[0056] Figure 3 A schematic diagram of the horizontal distance error between the uninitialized solution position and the true value device position of the integrated navigation system;

[0057] Figure 4 Schematic diagram of the heading angle error between the dynamic initialization scheme and the true heading angle error of the integrated navigation system;

[0058] Figure 5 This is a schematic diagram of the heading angle error and the true heading angle error at the initialization moment of the dynamic initialization scheme of the integrated navigation system. DETAILED DESCRIPTION

[0059] The following further describes the technical means, creative features, objectives and effects achieved by a single-antenna dynamic initialization method applied to INS / GPS integrated navigation of the present invention in conjunction with the accompanying drawings and embodiments.

[0060] Example

[0061] like Figure 1 As shown, a single antenna dynamic initialization method applied to INS / GPS integrated navigation is as follows:

[0062] 1) Power on the integrated navigation system to perform static initialization;

[0063] 2) Initialize the filter parameters and set the correlation matrix;

[0064] 3) In the above steps, Kalman filtering is used to further predict the process and calculate the state vector X and the state transfer matrix And the state vector covariance matrix P, as follows

[0065]

[0066]

[0067] Where:

[0068] State transfer matrix at time k, state transfer matrix at time k+1;

[0069] X k , X k+1 : state vector at time k, state vector at time k+1;

[0070] P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1;

[0071] 4) Dynamic initialization:

[0072] This step uses the heading angle and pitch angle calculated by the GPS northeast velocity, while the roll angle uses the result of the inertial navigation solution at the previous moment, and updates the state matrix, velocity and position information;

[0073] The roll angle is the angle between the transverse axis of the carrier and the horizontal line. The navigation system in the present invention is used to identify the lateral inclination angle of the target, and its value is equal to the angle between the line perpendicular to the bow and stern line on the plane where the target object is located and its projection on the horizontal plane.

[0074] 5) In the above step 4), the filter parameters are initialized, the state vector X is set to 0, the state vector covariance matrix P is set to the initial value, the vehicle-mounted and inertial measurement unit set the error angle and odometer coefficient estimation value, the estimation values ​​of other lever arms are all set to 0, and the dynamic initialization state bit is marked;

[0075] 6) Kalman filter update process:

[0076] This step uses the state vector covariance matrix P calculated by the Kalman filter prediction process in step 3), combined with the measurement equation coefficient matrix H and the observation noise matrix R, to calculate the gain matrix K, the state vector error matrix X and the state vector covariance matrix P. The formula is as follows:

[0077]

[0078] X k+1 =X k+ K k (Z k- H k X k )

[0079] P k+1 =(IK k H k ) k

[0080] Where:

[0081] K k : gain matrix at time k;

[0082] P k : k-time observation noise matrix;

[0083] H k : k-time measurement equation coefficient matrix;

[0084] I: identity matrix;

[0085] X k , X k+1 : state vector at time k, state vector at time k+1;

[0086] P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1;

[0087] 7) Provide compensation feedback:

[0088] This step uses the state vector error matrix X obtained in step 6) above to compensate the navigation output result. After the state quantity is compensated, the state quantity of the corresponding state vector error matrix X is set to 0, and it is not set to 0 if there is no compensation;

[0089] 8) Iterate and update and navigate output:

[0090] After the compensation feedback process of the above step 7) is completed, the update process of a Kalman filter is completed, and the calculation of the next prediction process is carried out, and a new update process of the Kalman filter is carried out. The iterative update of steps 1) to 7) is repeated, and the Kalman filter calculation and estimation are performed to provide stable and reliable position, speed, and attitude information for the navigation output process.

[0091] The static initialization of step 1) is judged as follows: if there is no satellite GPS signal, it is judged that the static initialization is not completed, until the satellite signal is good, it is judged that the static initialization is completed and the next step is performed.

[0092] Step 4) of dynamic initialization determines whether the conditions for dynamic initialization are met when the GPS solution is fixed and the vehicle speed is greater than 20 km / h.

[0093] A single-antenna dynamic initialization method for INS / GPS integrated navigation of the present invention is compared with a scheme that does not use dynamic initialization. The scheme that does not use dynamic initialization uses the single-antenna speed position information under static conditions, and the heading angle is set to an initial value of 0. The position error caused by the static initialization heading angle error, the single-antenna solution result with incorrect static initialization heading angle, after actual vehicle testing, differs from the driving path used as a reference by about 37 meters, deviates too much from the lane line, and cannot provide safe and reliable navigation positioning for the vehicle. Figure 2 As shown in the figure, the dynamic initialization scheme is not adopted. The heading angle error is 100° due to inaccurate initialization, and the heading angle error converges slowly and has a large error. In addition, Figure 3 As shown in the figure, due to the large heading angle error, the velocity projection solution error is large, which in turn affects the position error. The maximum horizontal distance error between its position and the true value equipment reaches 157m. The horizontal distance error of the position is large and the error convergence time is slow.

[0094] The above-mentioned integrated navigation system that does not adopt the dynamic initialization scheme is activated after the static initialization inaccurate information is started. The performance of the unlocked sections such as underground garages and tunnels is expressed in the form of error rate as follows:

[0095] Horizontal distance error before leaving the warehouse: The horizontal distance error between the combined navigation output position and the true value device output position at the previous moment (one IMU (inertial measurement unit) sampling time) using the GPS observation at the first time point before leaving the warehouse.

[0096] Loss of lock distance: the distance traveled by the combined navigation system during the period from entering the warehouse to leaving the warehouse.

[0097] Error rate: outbound horizontal distance error / unlocking distance*100%, please refer to the following table 1 for details:

[0098] Road Section Loss of lock distance (m) Horizontal distance error (m) Lateral error(m) Longitudinal error(m) Error rate 1 301.8931158 23.36284838 7.111290845 22.25426312 7.73％ 2 296.9877278 8.606866885 8.21677185 2.561799749 2.89％ 3 298.5495903 6.095227576 6.078420663 0.452328914 2.04％ 4 299.4918085 6.678238291 6.463637694 1.679361317 2.23％ 5 301.8931158 23.36284838 7.111290845 22.25426312 7.74％

[0099] Table 1- Statistics of lock loss errors for non-dynamic initialization schemes

[0100] The single-antenna dynamic initialization method for INS / GPS combined navigation of the present invention is used. Its static initialization still uses the initial value 0 set by the heading angle. The speed position information provided by the single antenna is immediately dynamically initialized once the dynamic initialization condition is met to initialize the filter and its parameters and related error values.

[0101] After actual vehicle testing, the path using dynamic initialization is compared with the single antenna solution result path with incorrect initial static initialization heading angle. The path using dynamic initialization basically coincides with the reference path with a small difference, and is all within the lane line, which can provide safe and reliable navigation positioning for the vehicle.

[0102] like Figure 4 As shown in the figure, taking the static starting point at t=20507s as an example, the vehicle is powered on and starts running. At this time, the heading angle used in the static initialization is the manually set 0° initial value. Compared with the true value device, the heading angle error is large, with a difference of 100°. At t=20519.6s, the dynamic initialization begins, and the heading angle error is around 0.39°, and the error begins to decrease and slowly converges. Figure 5 As shown in the figure, although the initial heading angle error is large, resulting in a large error in the velocity projection solution, the position error will also increase before dynamic initialization, with the maximum error reaching 15.28539m. However, after dynamic initialization, the position error is immediately reduced to about 0.023m.

[0103] The integrated navigation system using the single-antenna dynamic initialization method for INS / GPS integrated navigation of the present invention performs dynamic initialization calibration after the static initialization inaccurate information is started. The performance of the unlocked sections such as underground garages and tunnels is expressed in the form of error rate as shown in Table 2 below:

[0104]

[0105]

[0106] Table 2 - Statistics of loss of lock error in dynamic initialization scheme

[0107] Through the comparison results of the above two schemes, it can be seen that the loss-of-lock accuracy after dynamic initialization is basically lower than 5‰. Compared with not performing dynamic initialization, the path accuracy of only static initialization is improved by an order of magnitude. It can be seen that the single-antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention has a very significant effect on improving the accuracy of the path.

[0108] However, those skilled in the art should recognize that the above embodiments are only used to illustrate the present application, and are not used to limit the present application. As long as they are within the spirit of the present application, any changes or modifications to the above embodiments will fall within the scope of the claims of the present application.

[0109] The single-antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention can be applied to the initial alignment scheme of GPS / SINS inertial combined navigation of the single antenna, the algorithm is easy to operate, the required dynamic initialization conditions are easy to meet, and the method has wide practicability; the single-antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention is verified by actual sports car test, and when the vehicle speed is greater than 20km / h, the track angle calculated by GPS speed information is used for initialization calibration, the filter convergence speed is fast, and the navigation positioning accuracy is high; the single-antenna dynamic initialization method applied to INS / GPS combined navigation of the present invention has important significance for the research and development of automatic driving technology, and is suitable for the field of single-antenna dynamic initialization of INS / GPS combined navigation.

Claims

1. A single-antenna dynamic initialization method for INS / GPS integrated navigation, including a single-antenna integrated navigation system, which is specifically as follows: 1) Power on the integrated navigation system to perform static initialization; 2) Initialize the filter parameters and set the correlation matrix; 3) In the above steps, Kalman filtering is used to further predict the process and calculate the state vector X and the state transfer matrix And the state vector covariance matrix P, as follows Where: State transfer matrix at time k, state transfer matrix at time k+1; X k , X k+1 : state vector at time k, state vector at time k+1; P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1; 4) Dynamic initialization: This step uses the heading angle and pitch angle calculated by the GPS northeast velocity, while the roll angle uses the result of the inertial navigation solution at the previous moment, and updates the state matrix, velocity and position information; 5) In the above step 4), the filter parameters are initialized, the state vector X is set to 0, the state vector covariance matrix P is set to the initial value, the vehicle-mounted and inertial measurement unit set the error angle and odometer coefficient estimation value, the estimation values ​​of other lever arms are all set to 0, and the dynamic initialization state bit is marked; 6) Kalman filter update process: This step uses the state vector covariance matrix P calculated by the Kalman filter prediction process in step 3), combined with the measurement equation coefficient matrix H and the observation noise matrix R, to calculate the gain matrix K, the state vector error matrix X and the state vector covariance matrix P. The formula is as follows: X k+1 =X k +K k (Z k -H k X k ) P k+1 =(I-K k H k )P k Where: K k : gain matrix at time k; R k : k-time observation noise matrix; H k : k-time measurement equation coefficient matrix; I: identity matrix; X k , X k+1 : The state vector error matrix at time k, the state vector error matrix at time k+1; P k , P k+1 : The covariance matrix of the state vector at time k, the covariance matrix of the state vector at time k+1; 7) Provide compensation feedback: This step uses the state vector error matrix X obtained in step 6) above to compensate the navigation output result. After the state quantity is compensated, the state quantity of the corresponding state vector error matrix X is set to 0, and it is not set to 0 if there is no compensation; 8) Iterate and update and navigate output: After the compensation feedback process of the above step 7) is completed, the update process of a Kalman filter is completed, and the calculation of the next prediction process is carried out, and a new update process of the Kalman filter is carried out. The iterative update of steps 1) to 7) is repeated, and the Kalman filter calculation and estimation are performed to provide stable and reliable position, speed, and attitude information for the navigation output process.

2. A single antenna dynamic initialization method for INS / GPS integrated navigation as claimed in claim 1, characterized in that: The static initialization of step 1) is judged as follows: if there is no satellite GPS signal, it is judged that the static initialization is not completed, until the satellite signal is good, it is judged that the static initialization is completed and the next step is performed.

3. A single antenna dynamic initialization method for INS / GPS integrated navigation as claimed in claim 1, characterized in that: The dynamic initialization of step 4) determines whether the dynamic initialization is achieved when the GPS is fixed and the vehicle speed is greater than 20 km / h.

Images