An optimized algorithm for integrated positioning of INS and GNSS

Abstract

An optimization algorithm for integrated positioning of INS and GNSS, comprising the following steps: S1. Determine whether the accelerometer data vector has been collected completely; if not, obtain the accelerometer data value, execute continuous collection, and when the data collection is completed, proceed to S2; S2. Calculate the average vector Acc_xyz_B of the data vector based on the vehicle coordinate system and the vector Acc_xyz_N of the average velocity based on the navigation coordinate system, obtain the GPS velocity vector GPS_vel_cur_NED based on the navigation coordinate system and the corresponding timestamp GPS_stamp_cur; S3. Make a non-zero judgment on the previous GPS data. If it is zero, assign GPS_vel_cur_NED to GPS_vel_last_NED, assign the timestamp GPS_vel_cur_NED to GPS_stamp_last, and reset the acceleration-related acquisition quantity and the state control quantity; if it is non-zero, proceed to S4; S4. Calculate the acceleration value ACC_GPS based on the velocity difference and the time difference between two times; S5. Obtain the absolute value Err_Acc_and_GPS of the two error values; S6. Judge the relationship between each element in the absolute value Err_Acc_and_GPS of the accelerometer error vector in the N and E directions and the threshold Err_Acc_and_GPS_limit, and perform the corresponding algorithm. It can improve the stability and robustness of state estimation.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular, to an optimized algorithm for the integrated positioning of INS and GNSS. Background Art

[0002] At present, most positioning methods mainly rely on the Inertial Navigation System (INS), and integrate the Global Navigation Satellite System (GNSS), visual information, and radar information to estimate the position of the vehicle. Integrated positioning is an online state estimator for the attitude, speed, and position of the vehicle by obtaining information from multiple sensors and combining relevant algorithms. In the early stage, the standard Kalman filter was mostly used for the integrated positioning algorithm. Due to the requirements for the nonlinearity of the system, currently, algorithms such as the Extended Kalman Filter and Particle Filter are mainly used. The data fusion algorithm has become an important research direction.

[0003] The defects and deficiencies of the prior art are as follows: GNSS can provide rich and high-precision positioning information, but due to the influence of the surrounding terrain and obstacles, the positioning accuracy drops significantly; INS can provide high precision in a short time, but due to the accumulation of integration errors, there will be data drift phenomena in the long term. Currently, outdoor positioning generally relies on the information of INS and GNSS, and uses the Extended Kalman Filter (EKF) to achieve the positioning of the vehicle. It is found in engineering tests that the integrated positioning method of INS and GNSS based on EKF is prone to cause position spikes in situations where the vehicle passes through tunnels, etc., affecting the stability of the vehicle's control system and increasing the difficulty of the control system design. Summary of the Invention

[0004] In order to solve the related problems such as the sharp drop in GNSS accuracy in integrated positioning leading to position estimation spikes, and in line with the principles of easy implementation, good algorithm stability, and high accuracy, the present invention proposes an optimized algorithm for the integrated positioning of INS and GNSS, and the specific solution is as follows:

[0005] An optimized algorithm for the integrated positioning of INS and GNSS, comprising the following steps:

[0006] S1. Determine whether the accelerometer data vector with a first set value of length has been collected completely; if not, obtain the accelerometer data values and store them in variables Acc_standard_x, Acc_standard_y, and Acc_standard_z. Acc_standard_x, Acc_standard_y, and Acc_standard_z are the accelerometer vectors of the X-axis, Y-axis, and Z-axis with a length of the first set value based on the carrier coordinates respectively; continue the collection, and at this time, the flag bit Multi_level_data_enough is FALSE; when the data collection is completed, set the flag bit to TRUE and enter step S2;

[0007] S2. Calculate the average vector Acc_xyz_B of the data vector based on the carrier coordinate system and the average velocity vector Acc_xyz_N based on the navigation coordinate system, and obtain the GPS velocity vector GPS_vel_cur_NED based on the navigation coordinate system and the corresponding timestamp GPS_stamp_cur;

[0008] S3. Make a non-zero judgment on the previous GPS data. If it is zero, it indicates the initial stage of the algorithm. Assign GPS_vel_cur_NED to GPS_vel_last_NED, assign the timestamp GPS_stamp_cur to GPS_stamp_last, reset the acceleration-related acquisition quantities and status control quantities, and start executing from S1 again; if it is non-zero, it indicates the non-initial stage of the algorithm, and enter step S4;

[0009] S4. Calculate the acceleration value ACC_GPS based on the velocity difference and time difference between two times;

[0010] S5. Obtain the rotation matrix Tbn from the carrier coordinate system to the navigation coordinate system at the current moment, and obtain Acc_xyz_N by multiplying the rotation matrix Tbn by Acc_xyz_B; subtract the acceleration value ACC_GPS from the average acceleration Acc_xyz_N based on the navigation coordinate system; obtain the absolute value Err_Acc_and_GPS of the two error values;

[0011] S6. Set the threshold Err_Acc_and_GPS_limit for the allowable error of normal frequency fusion of the deviation between the accelerometer and the acceleration data obtained by GPS, the magnification value GPS_Fusion_K_ms for down-frequency fusion, and the magnification GPS_limit_K for the maximum allowable error limit for fusion; judge the relationship between each element in the absolute value Err_Acc_and_GPS of the accelerometer error vector in the N and E directions and the threshold Err_Acc_and_GPS_limit, and perform corresponding algorithms.

[0012] Specifically, Acc_standard_x, Acc_standard_y, and Acc_standard_z are all N-dimensional vector values.

[0013] Specifically, the first set value is 20.

[0014] Specifically, the average value vector Acc_xyz_B of the data vector based on the carrier coordinate system is (Acc_standard_ave_x, Acc_standard_ave_y, Acc_standard_ave_z), where Acc_standard_ave_x, Acc_standard_ave_y, and Acc_standard_ave_z are the average values of the accelerometers on the X-axis, Y-axis, and Z-axis based on the carrier coordinate system, respectively, and are obtained by summing the corresponding Acc_standard_x, Acc_standard_y, and Acc_standard_z and then dividing by the first set value;

[0015] The vector Acc_xyz_N of the average velocity based on the navigation coordinate system is (Acc_ave_x_n, Acc_ave_y_e, Acc_ave_y_d); Acc_ave_x_n, Acc_ave_y_e, and Acc_ave_y_d are the average values of the accelerometers in the N-direction, E-direction, and D-direction based on the navigation coordinate system obtained by the sensor accelerometer, respectively;

[0016] The velocity vector GPS_vel_cur_NED based on the navigation coordinate system is (GPS_vel_cur_N, GPS_vel_cur_E, GPS_vel_cur_D), where GPS_vel_cur_N, GPS_vel_cur_E, and GPS_vel_cur_D are the velocity values in the N-direction, E-direction, and D-direction based on the navigation coordinate system obtained by the sensor GPS, respectively.

[0017] Specifically, step S4 is specifically

[0018] The acceleration value ACC_GPS (ACC_GPS_x, ACC_GPS_y, ACC_GPS_z) in the NED coordinate system calculated from the sensor GPS speed data. The corresponding ACC_GPS_x, ACC_GPS_y, ACC_GPS_z are obtained according to ACC_GPS = (GPS_vel_cur_NED - GPS_vel_last_NED) / (GPS_stamp_cur - GPS_stamp_last). GPS_stamp_cur is the timestamp corresponding to the currently collected data; GPS_stamp_last is the timestamp corresponding to the previous set of collected data.

[0019] Specifically, the specific steps of step S5 are as follows:

[0020] S51. Obtain the rotation matrix Tbn from the vehicle coordinate system to the navigation coordinate system corresponding to the attitude in the state estimation at this moment, and rotate the acceleration value based on the vehicle coordinate system to the navigation coordinate system Acc_xyz_N = Tbn * Acc_xyz_B;

[0021] S52. Calculate the absolute value of the accelerometer error vector Err_Acc_and_GPS = abs(ACC_GPS - Acc_xyz_N) of the acceleration obtained based on the accelerometer and GPS; considering that the Z-axis data source is not GPS, so it is set as Err_Acc_and_GPS(Err_Acc_and_GPS_x, Err_Acc_and_GPS_y, 0.00).

[0022] Specifically, the judgment criterion for step S6 is:

[0023] If each element of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is within the threshold Err_Acc_and_GPS_limit, execute the standard extended Kalman filter algorithm;

[0024] If any element value of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the threshold Err_Acc_and_GPS_limit, execute the downsampling mode and set the magnification value GPS_Fusion_K_ms of the downsampling fusion;

[0025] If any element value of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the GPS_limit_K times of the threshold Err_Acc_and_GPS_limit, then turn off the fusion positioning and only rely on the inertial navigation to maintain the relative stability of the position estimation.

[0026] Specifically, it further includes step S7, repeating S1, S2, S4, S5, and S6.

[0027] The beneficial effects of the present invention are as follows: The present invention can improve the stability and robustness of state estimation and promote the development of data fusion positioning algorithms. Description of the Drawings

[0028] Figure 1 It is a flowchart of an optimized algorithm for the fusion positioning of INS and GNSS proposed by the present invention. Specific Embodiments

[0029] As Figure 1 shown, an optimized algorithm for the fusion positioning of INS and GNSS includes the following steps:

[0030] S1. Determine whether the accelerometer data vector with a first set value of length is collected; if not, obtain the accelerometer data values and put them into variables Acc_standard_x, Acc_standard_y, and Acc_standard_z. The above three variables are all N-dimensional vector values, and continue to collect. At this time, the flag bit Multi_level_data_enough is FALSE; when the data collection is completed, set the flag bit to TRUE and enter step S2; where Acc_standard_x, Acc_standard_y, and Acc_standard_z are the accelerometer vectors of the X-axis, Y-axis, and Z-axis with a first set value of length based on the carrier coordinates; in the present invention, the accelerometer collection frequency of the vehicle is set to 100HZ, and the GPS collection frequency is 5HZ. Therefore, one frame of GPS data corresponds to 20 frames of accelerometer data, that is, the data collected by the accelerometer is set as a vector with a length of 20.

[0031] S2. Calculate the vector Acc_xyz_B (Acc_standard_ave_x, Acc_standard_ave_y, Acc_standard_ave_z) of the average velocity based on the vehicle coordinate system and the vector Acc_xyz_N (Acc_ave_x_n, Acc_ave_y_e, Acc_ave_y_d) of the average velocity based on the navigation coordinate system, and also obtain the GPS data of this acquisition period; where Acc_standard_ave_x, Acc_standard_ave_y, and Acc_standard_ave_z are the average values of the accelerometers on the X-axis, Y-axis, and Z-axis based on the vehicle coordinate system, respectively, which are obtained by summing the corresponding Acc_standard_x, Acc_standard_y, and Acc_standard_z and then dividing by the first set value; Acc_ave_x_n, Acc_ave_y_e, and Acc_ave_y_d are the average values of the accelerometers in the N direction, e direction, and D direction based on the navigation coordinate system obtained by the sensor accelerometer; the GPS data of the acquisition period includes the GPS velocity vector GPS_vel_cur_NED (GPS_vel_cur_N, GPS_vel_cur_E, GPS_vel_cur_D) based on the navigation coordinate system and the GPS velocity vector GPS_vel_last_NED (GPS_vel_cur_N, GPS_vel_cur_E, GPS_vel_cur_D) based on the navigation coordinate system. GPS_vel_last_NED and GPS_vel_cur_NED belong to the vector3f vector class, where GPS_vel_cur_N, GPS_vel_cur_E, and GPS_vel_cur_D are the velocity values in the N direction, E direction, and D direction based on the navigation coordinate system obtained by the sensor GPS, respectively;

[0032] S3. Make a non-zero judgment on the previous GPS data. If it is zero, it indicates the initial stage of the algorithm. Assign GPS_vel_cur_NED to GPS_vel_last_NED, assign the timestamp GPS_stamp_cur to GPS_stamp_last, reset the acceleration-related acquisition quantities and status control quantities, and restart from S1; if it is non-zero, it indicates the non-initial stage of the algorithm, and enter step S4;

[0033] S4. Calculate the acceleration value ACC_GPS based on the velocity difference and time difference between the two times; the specific operation is as follows:

[0034] The acceleration values ACC_GPS (ACC_GPS_x, ACC_GPS_y, ACC_GPS_z) in the NED coordinate system calculated from the sensor GPS speed data. The corresponding ACC_GPS_x, ACC_GPS_y, ACC_GPS_z are obtained according to ACC_GPS = (GPS_vel_cur_NED - GPS_vel_last_NED) / (GPS_stamp_cur - GPS_stamp_last). GPS_stamp_cur is the timestamp corresponding to the currently collected data; GPS_stamp_last is the timestamp corresponding to the previous set of collected data;

[0035] S5. Subtract the acceleration value ACC_GPS in step S4 from the average acceleration Acc_xyz_N based on the navigation coordinate system; obtain two error values Err_Acc_and_GPS; the specific steps are as follows:

[0036] S51. Obtain the rotation matrix Tbn from the body coordinate system to the navigation coordinate system corresponding to the attitude in the state estimation at this moment, and rotate the acceleration value based on the body coordinate system to the navigation coordinate system Acc_xyz_N = Tbn * Acc_xyz_B;

[0037] S52. Calculate the absolute value of the accelerometer error vector of the accelerations obtained based on the accelerometer and GPS, Err_Acc_and_GPS = abs(ACC_GPS - Acc_xyz_N); considering that the data source of the Z-axis is not GPS, so it is set as Err_Acc_and_GPS(Err_Acc_and_GPS_x, Err_Acc_and_GPS_y, 0.00);

[0038] S6. Set the threshold Err_Acc_and_GPS_limit for the allowable error of the acceleration data fused by the accelerometer and GPS, the downsampling fusion magnification value GPS_Fusion_K_ms, and the magnification GPS_limit_K of the error limit for turning off the fusion; judge the relationship between the elements in the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions and the threshold Err_Acc_and_GPS_limit, and make adaptive actions;

[0039] If each element of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is within the threshold Err_Acc_and_GPS_limit, execute the standard extended Kalman filter algorithm;

[0040] If the value of any element of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the threshold Err_Acc_and_GPS_limit, then the frequency reduction mode is executed, and the frequency reduction fusion magnification value GPS_Fusion_K_ms is set. The Fusion_Time_Limit control obtains the effect of reducing the fusion frequency by multiplying the parameter fusion time length Fusion_Time_Limit by the parameter frequency reduction fusion magnification GPS_Fusion_K_ms. The specific parameters depend on system debugging settings;

[0041] If the value of any element of the absolute value of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the GPS_limit_K times of the threshold Err_Acc_and_GPS_limit, then the fusion positioning is turned off, and only the inertial navigation is relied on to maintain the relative stability of the position estimation.

[0042] S7. Repeat S1, S2, S4, S5, S6 to implement the multi-level fusion positioning algorithm based on INS and GNSS, and improve the accuracy and robustness of the fusion positioning.

[0043] The above realizes multi-level fusion, improves the robustness of the INS and GNSS fusion positioning, and avoids large-scale oscillations of the output position in the case of suddenly poor GPS data quality.

[0044] The present invention discloses an optimized algorithm for the fusion positioning of INS and GNSS, which is mainly applicable to a vehicle carrying at least one set of accelerometers and GPS.

[0045] The above is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution and inventive concept of the present invention, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. An optimized algorithm for integrated positioning of INS and GNSS, characterized in that, It includes the following steps: S1. Determine whether the accelerometer data vector with a first set value of length is collected completely; if not, obtain the accelerometer data values and store them in variables Acc_standard_x, Acc_standard_y, and Acc_standard_z. Acc_standard_x, Acc_standard_y, and Acc_standard_z are the accelerometer vectors of the X-axis, Y-axis, and Z-axis with a first set value of length based on the carrier coordinates respectively; continue the collection, and at this time, the flag bit Multi_level_data_enough is FALSE; when the data collection is completed, set the flag bit to TRUE and enter step S2; S2. Calculate the average vector Acc_xyz_B of the data vector based on the carrier coordinate system and the average velocity vector Acc_xyz_N based on the navigation coordinate system, obtain the GPS velocity vector GPS_vel_cur_NED based on the navigation coordinate system and the corresponding timestamp GPS_stamp_cur; S3. Make a non-zero judgment on the previous GPS data. If it is zero, it indicates the initial stage of the algorithm. Assign GPS_vel_cur_NED to GPS_vel_last_NED, assign the timestamp GPS_stamp_cur to GPS_stamp_last, reset the acceleration-related acquisition quantities and status control quantities, and start executing from S1 again; If it is non-zero, it indicates the non-initial stage of the algorithm, and enter step S4; S4. Calculate the acceleration value ACC_GPS based on the velocity difference and time difference between two times; S5. Obtain the rotation matrix Tbn from the carrier coordinate system to the navigation coordinate system at the current moment, and obtain Acc_xyz_N by multiplying the rotation matrix Tbn by Acc_xyz_B; subtract the acceleration value ACC_GPS from the average acceleration Acc_xyz_N based on the navigation coordinate system; obtain the absolute value Err_Acc_and_GPS of the two error values; S6. Set the threshold Err_Acc_and_GPS_limit for allowing errors in the normal frequency fusion of the deviation between the accelerometer and the acceleration data obtained by GPS, the reduction factor value GPS_Fusion_K_ms for the reduced-frequency fusion, and the magnification factor GPS_limit_K for the maximum allowable error limit for fusion; judge the relationship between each element in the absolute value Err_Acc_and_GPS of the accelerometer error vector in the N and E directions and the threshold Err_Acc_and_GPS_limit, and perform the corresponding algorithm; The judgment criterion for step S6 is: If each element of the absolute value Err_Acc_and_GPS of the accelerometer error vector in the N and E directions is within the threshold Err_Acc_and_GPS_limit, execute the standard extended Kalman filter algorithm; If the absolute value of any element of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the threshold Err_Acc_and_GPS_limit, execute the frequency reduction mode and set the magnification value GPS_Fusion_K_ms for frequency reduction fusion; If the absolute value of any element of the accelerometer error vector Err_Acc_and_GPS in the N and E directions is greater than the threshold Err_Acc_and_GPS_limit multiplied by GPS_limit_K, then turn off the integrated positioning and rely only on inertial navigation to maintain the relative stability of position estimation.

2. An optimization algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that Acc_standard_x, Acc_standard_y, and Acc_standard_z are all N-dimensional vector values.

3. An optimization algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that, The first set value is 20.

4. An optimization algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that, The average value vector Acc_xyz_B of the data vector based on the vehicle coordinate system is (Acc_standard_ave_x, Acc_standard_ave_y, Acc_standard_ave_z), where Acc_standard_ave_x, Acc_standard_ave_y, and Acc_standard_ave_z are the average values of the accelerometers on the X-axis, Y-axis, and Z-axis based on the vehicle coordinate system, respectively, and are obtained by summing the corresponding Acc_standard_x, Acc_standard_y, and Acc_standard_z and then dividing by the first set value; The vector Acc_xyz_N of the average velocity based on the navigation coordinate system is (Acc_ave_x_n, Acc_ave_y_e, Acc_ave_y_d); Acc_ave_x_n, Acc_ave_y_e, and Acc_ave_y_d are the average values of the accelerometers in the N, E, and D directions based on the navigation coordinate system obtained by the sensor accelerometer, respectively; The velocity vector GPS_vel_cur_NED based on the navigation coordinate system is (GPS_vel_cur_N, GPS_vel_cur_E, GPS_vel_cur_D), where GPS_vel_cur_N, GPS_vel_cur_E, and GPS_vel_cur_D are the velocity values in the N, E, and D directions based on the navigation coordinate system obtained by the sensor GPS, respectively.

5. An optimization algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that, Step S4 is specifically The acceleration value ACC_GPS (ACC_GPS_x, ACC_GPS_y, ACC_GPS_z) in the NED coordinate system calculated from the sensor GPS speed data. The corresponding ACC_GPS_x, ACC_GPS_y, ACC_GPS_z are obtained according to ACC_GPS = (GPS_vel_cur_NED - GPS_vel_last_NED) / (GPS_stamp_cur - GPS_stamp_last). GPS_stamp_cur is the timestamp corresponding to the currently collected data; GPS_stamp_last is the timestamp corresponding to the previous set of collected data.

6. An optimization algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that The specific steps of step S5 are as follows: S51. Obtain the rotation matrix Tbn from the body coordinate system to the navigation coordinate system corresponding to the attitude in the state estimation at this moment, and rotate the acceleration value based on the body coordinate system to the navigation coordinate system Acc_xyz_N = Tbn * Acc_xyz_B; S52. Calculate the absolute value of the accelerometer error vector of the accelerations obtained based on the accelerometer and GPS, Err_Acc_and_GPS = abs(ACC_GPS - Acc_xyz_N); Considering that the data source of the Z-axis is not GPS, it is set as Err_Acc_and_GPS (Err_Acc_and_GPS_x, Err_Acc_and_GPS_y, 0.00).

7. An optimized algorithm for INS and GNSS integrated positioning according to claim 1, characterized in that It also includes step S7, repeating S1, S2, S4, S5, S6.

Images