A fusion positioning method of a combination navigation system based on graph optimization

Abstract

This invention discloses a fusion positioning method for an integrated navigation system based on graph optimization, comprising: calculating IMU trajectory information to obtain the position of a first positioning vehicle at each time step; establishing a graph optimization model using the position of the first positioning vehicle at each time step as vertices; obtaining a first position transfer relationship of the positioning vehicle between adjacent first and second time steps using GNSS data, and using the first position transfer relationship as a measurement of the graph optimization model; obtaining a second position transfer relationship of the positioning vehicle between adjacent first and second time steps using IMU trajectory information, and using the second position transfer relationship as a variable to be optimized in the graph optimization model; establishing an objective function based on the measurement and the variable to be optimized, and optimizing it using the Levenberg-Marquardt algorithm; and obtaining the corresponding error correction amount when the objective function is optimized to the minimum value, and correcting the IMU data calculation accordingly.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and more specifically to a graph-optimized integrated navigation system fusion positioning method. Background Technology

[0002] Global Navigation Satellite System (GNSS) systems, as a traditional positioning technology, can provide absolute position information in most scenarios, but their application is limited in special environments such as indoors and urban canyons. Inertial Measurement Unit (IMU) data is unaffected by external signal factors, but errors can easily accumulate over long periods due to the integration calculation principle. Therefore, the fusion of GNSS and IMU systems has become the mainstream integrated navigation method.

[0003] Traditional GNSS positioning systems receive fewer satellite signals in harsh environments. Since GNSS positioning is based on the least squares principle, the fewer constraints provided by satellite signals, the lower the positioning accuracy. In extreme cases, insufficient constraints can even prevent GNSS from locating the system, leading to reduced accuracy when relying solely on the IMU system. Filtering-based methods, based on Markov probability assumptions, only consider current measurement information, leaving historical information stored in the "previous time" considered the optimal estimate by statistical principles. This results in low utilization of historical information and significant errors. Thanks to increased computing power, graph optimization algorithms establish more measurement constraints, batch estimating state information from different times, and more directly and effectively utilizing historical information, leading to higher accuracy in the optimized estimation results. However, directly performing graph optimization modeling on GNSS positioning information and IMU position trajectory information from different times merely replaces iterative filtering with large-scale least squares solving; it does not introduce more constraint information. At the same time, the positioning results of GNSS are also an estimate based on satellite pseudorange. They are processed rather than the original direct measurement information.

[0004] Therefore, how to enhance the correlation between GNSS positioning information and IMU position trajectory information in the algorithm model to improve the accuracy and effectiveness of integrated navigation system calculation has become an urgent problem to be solved. Summary of the Invention

[0005] In view of this, embodiments of the present invention provide a graph-optimized fusion positioning method for integrated navigation systems to solve the problem that existing integrated navigation systems have insufficient utilization efficiency of the correlation between GNSS positioning information and IMU position trajectory information, resulting in low positioning accuracy and effectiveness.

[0006] This invention provides a graph-optimized integrated navigation system fusion positioning method, comprising:

[0007] The IMU trajectory information is processed to obtain the position of the first positioning vehicle at each time point;

[0008] A graph optimization model is established by taking the position of the first positioning carrier at each time point as the vertex.

[0009] The first position transfer relationship of the positioning vehicle between adjacent first and second time moments is obtained through GNSS data, and the first position transfer relationship is used as a measurement for the graph optimization model.

[0010] The second position transfer relationship of the positioning vehicle between adjacent first and second time moments is obtained through IMU trajectory information, and the second position transfer relationship is used as the variable to be optimized in the graph optimization model.

[0011] The objective function is established based on the measurements and the variables to be optimized, and then optimized using the Levenberg-Marquardt algorithm.

[0012] When the objective function is optimized to its minimum value, the corresponding error correction amount is obtained, and the IMU data solution is corrected.

[0013] Optionally, before processing the IMU trajectory information to obtain the position of the first positioning vehicle at each time point, the method further includes:

[0014] By reading ephemeris data, satellite positions are matched to obtain the satellite positions of each satellite at various times in the ECEF coordinate system.

[0015] Obtain pseudorange information of each satellite at all times using GNSS data;

[0016] Based on the satellite positions and pseudorange information, coordinate transformation is performed to obtain the positions of each satellite in the northeastern sky coordinate system.

[0017] Optionally, coordinate transformation includes:

[0018] Set the origin of the northeast-central coordinate system;

[0019] Establish a coordinate system with the east of the origin as the x-axis, the north of the origin as the y-axis, and the top of the origin as the z-axis;

[0020] Based on the principle of coordinate transformation, a coordinate transformation matrix is ​​established using the latitude and longitude of the origin of the ENU coordinate system;

[0021] The satellite position is transformed from the ECEF coordinate system to the Northeast Sky coordinate system using a coordinate transformation matrix.

[0022] Optionally, the IMU trajectory information is processed to obtain the position of the first positioning vehicle at each time point, including:

[0023] Based on the initial velocity information, acceleration in three directions and time information of the positioning vehicle in the IMU trajectory information, the position change of the positioning vehicle in each time period and the first positioning vehicle position at each time moment are obtained through the integration algorithm.

[0024] Optionally, the first position transfer relationship of the positioning vehicle between adjacent first and second time moments is obtained through GNSS data, including:

[0025] The positions of each satellite at each time point are obtained using GNSS data.

[0026] A pseudorange conversion is performed between the satellite position and the first positioning carrier position to obtain three pseudoranges;

[0027] Obtain the three position coordinates at the corresponding three pseudo-ranges;

[0028] Three distance data points were calculated based on the three location coordinates and the satellite position at the initial moment;

[0029] The average of the three distance data points is used as the new distance between the satellite and the positioning vehicle.

[0030] An error equation is established based on the new distance and three pseudoranges, and the position of the second positioning vehicle at each time point is estimated using the least squares method.

[0031] Optionally, an objective function is established based on the measurements and the variables to be optimized, including:

[0032] The difference between the variable to be optimized and the measurement at each time point is used as the error function;

[0033] The objective function is obtained by accumulating each error function.

[0034] Optionally, the second position transfer relationship of the positioning vehicle between adjacent first and second time moments is obtained through IMU trajectory information, and the second position transfer relationship is used as the variable to be optimized in the graph optimization model, including:

[0035] Obtain the location of the positioning vehicle from the IMU trajectory information at time k. The location of the positioning vehicle in the IMU trajectory information at time k+1. The transfer relationship between them:

[0036]

[0037] Where, x k It is represented as a three-dimensional row vector.

[0038] Optionally, it also includes:

[0039] Obtain the carrier position calculated after one fusion at time k. At time k+1 The transfer relationship between them:

[0040]

[0041] Among them, z k It is represented as a three-dimensional row vector.

[0042] Optionally, the objective function is:

[0043]

[0044] Wherein, the error function E k =x k -z k Ω is the inverse of the covariance matrix.

[0045] Optionally, when the objective function is optimized to its minimum value, the corresponding error correction amount is obtained, and the IMU data solution is corrected, including:

[0046] P I (X,Y,Z)=P I (X,Y,Z)+ΔP(ΔX,ΔY,ΔZ)

[0047] Among them, P I This represents the calculated IMU data; ΔP is the correction amount.

[0048] This invention provides a graph-optimized fusion positioning method for integrated navigation systems. In the first data fusion of GNSS and IMU, the conversion from GNSS satellite pseudorange to the distance between the satellite's initial position and the carrier compensates for situations with insufficient satellite numbers and constraints, improving GNSS accuracy and system robustness. Subsequently, a graph is re-established using IMU information, and the graph is further optimized using the data from the first fusion, resulting in a second data fusion. Compared to Kalman filtering, which is unsuitable for large-scale scenarios, graph optimization, due to its sparse bundle adjustment, is more suitable for large-scale scenarios. Furthermore, graph optimization is a global optimization method with smaller errors. Attached Figure Description

[0049] The features and advantages of the invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the invention in any way. In the drawings:

[0050] Figure 1 A flowchart of a graph-optimized integrated navigation system fusion positioning method according to an embodiment of the present invention is shown;

[0051] Figure 2 This diagram illustrates the principle of establishing virtual constraints for a graph-optimized integrated navigation system fusion positioning method according to an embodiment of the present invention.

[0052] Figure 3 A flowchart of another graph-optimized integrated navigation system fusion positioning method is shown in an embodiment of the present invention;

[0053] Figure 4 A comparison chart showing the positioning trajectories of six satellites and three satellites is provided.

[0054] Figure 5 A comparison chart of root mean square errors corresponding to different numbers of satellites is shown;

[0055] Figure 6 A comparison chart showing the positioning trajectories of six satellites and three satellites after adding virtual constraints is presented;

[0056] Figure 7 The graph shows a comparison of the root mean square error between the trajectories of three satellites with added virtual constraints and those of six satellites. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0058] This invention provides a graph-optimized fusion positioning method for integrated navigation systems, such as... Figure 1 As shown, it includes:

[0059] Step S10: Solve the IMU trajectory information to obtain the position of the first positioning carrier at each time point.

[0060] In this embodiment, based on the initial velocity information, acceleration in three directions of the coordinate system, and time information of the positioning vehicle in the IMU trajectory information, an integral algorithm is used to obtain the position change of the positioning vehicle within each time period and the first positioning vehicle position at each moment. In a specific implementation, the IMU information consists of the acceleration in three directions of the vehicle's coordinate system and time information. Based on the initial velocity information, the position change of the vehicle at each time point is calculated using integration, and then the position of the vehicle at each moment is obtained from the initial position information. k Let k be the displacement of the carrier in a certain direction at time k, t be time, and a be the acceleration of the carrier in a certain direction. Finally, the displacement and position change of the carrier during this time are determined by synthesizing the displacements in the three directions.

[0061]

[0062] Step S20: Use the position of the first positioning carrier at each time point as the vertex to establish a graph optimization model.

[0063] In this embodiment, the graph optimization model consists of vertices and edges. An edge connects several vertices, representing a relationship between them. Here, connecting two vertices is a binary edge. Figure 2 As shown, the carrier position at each time point is calculated using the resolved IMU information. A graph is constructed using the transformation relationships between two vertices as edges, serving as the model to be optimized.

[0064] Step S30: Obtain the first position transfer relationship of the positioning vehicle between adjacent first and second time moments using GNSS data, and use the first position transfer relationship as a measurement for the graph optimization model.

[0065] In this embodiment, the distance between the positioning carrier and the initial position of the corresponding satellite, combined with the original satellite data, is used as a constraint. The nonlinear least squares method is used to solve for the carrier position, and the carrier position after one fusion is obtained, which is then used for observation in the graph optimization model.

[0066] Step S40: Obtain the second position transfer relationship of the positioning vehicle between adjacent first and second time moments through IMU trajectory information, and use the second position transfer relationship as the variable to be optimized in the graph optimization model.

[0067] Step S50: Establish the objective function based on the measurements and the variables to be optimized, and optimize it using the Levenberg-Marquardt algorithm.

[0068] In this embodiment, the Levenberg-Marquardt Algorithm (LM) is used to calculate the error correction amount, and the error correction amount is added to the target to be optimized.

[0069] Step S60: When the objective function is optimized to the minimum value, obtain the corresponding error correction amount and correct the IMU data solution.

[0070] In this embodiment, the optimized IMU information is used again for pseudorange conversion, position calculation, graph optimization, and other processes until the objective function is less than the threshold, at which point the optimization ends.

[0071] This invention provides a graph-optimized fusion positioning method for integrated navigation systems. In the first data fusion of GNSS and IMU, the conversion from GNSS satellite pseudorange to the distance between the satellite's initial position and the carrier compensates for situations with insufficient satellite numbers and constraints, improving GNSS accuracy and system robustness. Subsequently, a graph is re-established using IMU information, and the graph is further optimized using the data from the first fusion, resulting in a second data fusion. Compared to Kalman filtering, which is unsuitable for large-scale scenarios, graph optimization, due to its sparse bundle adjustment, is more suitable for large-scale scenarios. Furthermore, graph optimization is a global optimization method with smaller errors.

[0072] As an optional implementation, before step S10, the following steps are also included:

[0073] By reading ephemeris data and matching satellite positions, the satellite positions of each satellite at various times in the ECEF coordinate system can be obtained.

[0074] Obtain pseudorange information for each satellite at all times using GNSS data.

[0075] Based on the satellite positions and pseudorange information, coordinate transformation is performed to obtain the positions of each satellite in the northeastern sky coordinate system.

[0076] In this embodiment, the ephemeris contains the trajectory and time of each satellite's movement, allowing for accurate determination of the satellite's position in the ECEF coordinate system at each moment. The pseudorange information of each satellite at a given moment is obtained through the GNSS system, along with the satellite number and time information corresponding to the pseudorange measured in the GNSS information. Based on the time and satellite number in the GNSS information, the satellite's position at the corresponding moment is obtained, and the satellite's position is stored in correspondence with the pseudorange for coordinate transformation to obtain the GNSS satellite's ENU (Northeastern Sky) coordinate system position.

[0077] As an optional implementation method, coordinate transformation includes:

[0078] Set the origin of the northeast-central coordinate system.

[0079] Establish a coordinate system with the east of the origin as the x-axis, the north of the origin as the y-axis, and the top of the origin as the z-axis.

[0080] Based on the principle of coordinate transformation, a coordinate transformation matrix is ​​established using the latitude and longitude of the origin of the ENU coordinate system.

[0081] The satellite position is transformed from the ECEF coordinate system to the Northeast Sky coordinate system using a coordinate transformation matrix.

[0082] As an optional implementation, step S30 includes:

[0083] The positions of each satellite at each time point are obtained using GNSS data.

[0084] A pseudorange conversion is performed between the satellite position and the first positioning carrier position to obtain three pseudoranges;

[0085] Obtain the three position coordinates at the corresponding three pseudo-ranges;

[0086] Three distance data points were calculated based on the three location coordinates and the satellite position at the initial moment;

[0087] The average of the three distance data points is used as the new distance between the satellite and the positioning vehicle.

[0088] An error equation is established based on the new distance and three pseudoranges, and the position of the second positioning vehicle at each time point is estimated using the least squares method.

[0089] In this embodiment, based on the carrier's position (X) at time k in the calculated IMU data... k ,Y k Z k ) in (Y k Z k and satellite position Solve for a pseudorange D at the current time. k The lower position

[0090]

[0091] Based on the calculated position P1 under the current pseudorange and the satellite's initial position... The new distance is calculated based on the formula for the distance between two points in three-dimensional space.

[0092] Using the same method, respectively through (X) k Z k ) and (X k ,Y k Get distance and And based on P2 and P3 and the satellite's initial position. The new distance is calculated using the formula for the distance between two points in three-dimensional space. and

[0093] By calculation The average value is used as the distance between the current carrier and the satellite's initial position.

[0094]

[0095] When multiple satellites participate in positioning simultaneously, the above process is repeated for each satellite, and the pseudorange at time k is converted into the above result using the same method. This result is used for position calculation in a single fusion operation. The distance between the resulting carrier and the corresponding positions of the multiple satellites at their initial times is calculated. An error equation was established with respect to the carrier position, and the carrier position at time k was calculated using the least squares method. This scheme utilizes IMU information to establish virtual constraints between the satellite's initial position over a period of time and the carrier's current position, which, together with the original constraints, participate in the position calculation. Specifically, the carrier's position... The transfer relationship is used as a measurement.

[0096] As an optional implementation, step S50 includes:

[0097] The difference between the variable to be optimized and the measurement at each time point is used as the error function;

[0098] The objective function is obtained by accumulating each error function.

[0099] In this embodiment, the error function is a function of the error between the measurement and the target to be optimized. In order to better correlate the relationship between the measurement and the target to be optimized, the error function is established based on the position transfer relationship between two points on the carrier.

[0100] As an optional implementation, step S40 includes:

[0101] Obtain the location of the positioning vehicle from the IMU trajectory information at time k. The location of the positioning vehicle in the IMU trajectory information at time k+1. The transfer relationship between them:

[0102]

[0103] Where, x k It is represented as a three-dimensional row vector.

[0104] As an optional implementation, it also includes:

[0105] Obtain the carrier position calculated after one fusion at time k. At time k+1 The transfer relationship between them:

[0106]

[0107] Among them, z k It is represented as a three-dimensional row vector.

[0108] As an optional implementation, the objective function is:

[0109]

[0110] Wherein, the error function E k =x k -z k Ω is the inverse of the covariance matrix.

[0111] As an optional implementation, when the objective function is optimized to its minimum value, the corresponding error correction amount is obtained, and the IMU data solution is corrected, including:

[0112] P I (X,Y,Z)=P I (X,Y,Z)+ΔP(ΔX,ΔY,ΔZ)

[0113] Among them, P I The location of the positioning vehicle is calculated from the IMU data; ΔP is the correction amount.

[0114] In a specific embodiment, such as Figure 3 As shown, the optimized IMU information is used to perform pseudorange conversion, position calculation, graph optimization, and other processes again until the objective function is less than the threshold, at which point the optimization ends.

[0115] The positioning results of the three satellites after determining the clock bias were compared with the positioning results after adding virtual constraints, and RMSE (Root Mean Square Error) was used as the error evaluation criterion. Figure 4 and Figure 5 As shown, the dashed line represents positioning by 6 satellites, and the solid line represents positioning by 3 satellites. The trajectory with only 3 satellites has a large deviation in the z-axis direction, and also has large deviations in the x and y-axis directions, and overall deviates significantly from the trajectory with 6 satellites. Figure 6 and Figure 7 As shown, the dashed line represents the positioning of 6 satellites, and the solid line represents the positioning trajectory of 3 satellites after adding virtual constraints. After adding virtual constraints, the deviation of the positioning trajectory on the z-axis has been greatly improved, and it is more consistent with the positioning trajectory of the six satellites overall.

[0116] Although embodiments of the invention have been described in conjunction with the accompanying drawings, those skilled in the art can make various modifications and variations without departing from the spirit and scope of the invention, and such modifications and variations all fall within the scope defined by the appended claims.

Claims

1. A fusion positioning method for a combined navigation system based on graph optimization, characterized in that, include: The IMU trajectory information is processed to obtain the position of the first positioning vehicle at each time point; A graph optimization model is established by taking the position of the first positioning carrier at each time point as the vertex. The first position transfer relationship of the positioning vehicle between adjacent first and second moments is obtained through GNSS data, and the first position transfer relationship is used as a measurement of the graph optimization model. The second position transfer relationship of the positioning vehicle between adjacent first and second time points is obtained through the IMU trajectory information, and the second position transfer relationship is used as the variable to be optimized in the graph optimization model, including: obtaining the position of the positioning vehicle in the IMU trajectory information at time k. The location of the positioning carrier in the IMU trajectory information at time k+1. The transfer relationship between them: , where x k Represented as a three-dimensional row vector; An objective function is established based on the measurements and the variables to be optimized, and then optimized using the Levenberg-Marquardt algorithm. When the objective function is optimized to its minimum value, the corresponding error correction amount is obtained, and the IMU data solution is corrected accordingly. It also includes: obtaining the carrier position calculated after a fusion at time k. At time k+1 The transfer relationship between them: Among them, z k Represented as a three-dimensional row vector; The objective function is: Wherein, the error function E k =x k -z k ; is the inverse of the covariance matrix.

2. The graph-optimized integrated navigation system fusion positioning method according to claim 1, characterized in that, Before processing the IMU trajectory information to obtain the first positioning carrier position at each time point, the following steps are also included: By reading ephemeris data, satellite positions are matched to obtain the satellite positions of each satellite at various times in the ECEF coordinate system. The pseudorange information of each satellite at all times is obtained through the GNSS data; Based on the satellite positions and pseudorange information, coordinate transformation is performed to obtain the positions of each satellite in the northeastern sky coordinate system.

3. The graph-optimized integrated navigation system fusion positioning method according to claim 2, characterized in that, The coordinate transformation includes: Set the origin of the northeast-central coordinate system; A coordinate system is established with the east of the origin of the coordinate system as the x-axis, the north of the origin of the coordinate system as the y-axis, and the top of the origin of the coordinate system as the z-axis. Based on the principle of coordinate transformation, a coordinate transformation matrix is ​​established using the latitude and longitude of the origin of the ENU coordinate system; The satellite position is transformed from the ECEF coordinate system to the Northeast Sky coordinate system using the coordinate transformation matrix.

4. The graph-optimized integrated navigation system fusion positioning method according to claim 1, characterized in that, The IMU trajectory information is processed to obtain the position of the first positioning vehicle at each time point, including: Based on the initial velocity information, acceleration in three directions and time information of the positioning vehicle in the IMU trajectory information, the position change of the positioning vehicle in each time period and the position of the first positioning vehicle at each time moment are obtained by the integration algorithm.

5. The graph-optimized integrated navigation system fusion positioning method according to claim 3, characterized in that, Obtaining the first position transfer relationship of the positioning vehicle between adjacent first and second time moments using the GNSS data includes: The positions of each satellite at each time point are obtained using GNSS data. A pseudorange conversion is performed between the satellite position and the first positioning vehicle position to obtain three pseudoranges; Obtain the three position coordinates corresponding to the three pseudo-ranges respectively; Three distance data points are calculated based on the three location coordinates and the satellite position at the initial moment; The average of the three distance data points is used as the new distance between the satellite and the positioning vehicle. An error equation is established based on the new distance and the three pseudoranges, and the position of the second positioning carrier at each time point is estimated by the least squares method.

6. The graph-optimized integrated navigation system fusion positioning method according to claim 1, characterized in that, Based on the measurements and the variables to be optimized, an objective function is established, including: The difference between the variable to be optimized and the measurement at each time point is used as the error function; The objective function is obtained by accumulating each of the error functions.

7. The graph-optimized integrated navigation system fusion positioning method according to claim 1, characterized in that, When the objective function is optimized to its minimum value, the corresponding error correction amount is obtained, and the IMU data solution is corrected, including: in, This is the calculated IMU data; This is a correction amount.

Images