TDCP / INS semi-tight integrated navigation method and device based on sliding window factor graph optimization

Through the sliding window factor graph optimized TDCP/INS semi-tight combination navigation method, the problem of weak dynamic positioning resistance in complex environments is solved, and the consistency between dynamic reference position and actual motion state is achieved and the high-frequency stability is high. It is suitable for complex Time-changing environment.

CN118732002BActive Publication Date: 2024-12-06WUHAN UNIV
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411223871.X
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-09-03
Publication Date
2024-12-06
Estimated Expiration
2044-09-03

AI Technical Summary

Technical Problem

In complex environments, the dynamic positioning resistance is weak and the position results are inconsistent with the actual motion state. The existing loose combination and tight combination methods have their own advantages but also have shortcomings, making it difficult to provide stable and high-precision dynamics in complex time-varying environments Reference position solution.

Method used

The TDCP/INS semi-tight combination navigation method based on sliding window factor graph optimization is adopted, and the position change amount calculated by INS dead is used as a prior information to assist Beidou's high-precision position change amount solving, and TDCP/ is realized through sliding window factor graph optimization INS optimal solution, combining Beidou position residual factor and inertial measurement unit pre-integrated residual factor, calculate the optimal estimate of the state vector to obtain the position, speed and attitude navigation results of the dynamic target.

Benefits of technology

The consistency between the dynamic reference position and the actual motion trajectory is achieved, providing a high-frequency and stable dynamic reference for relative positioning, taking into account the advantages of loose combination and tight combination, improving robustness, and suitable for complex time-varying environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118732002B_ABST
    Figure CN118732002B_ABST
Patent Text Reader

Abstract

The present invention proposes a TDCP / INS semi-tight integrated navigation method and device based on sliding window factor graph optimization. The method of the present invention first uses Beidou SPP positioning to perform initial alignment on INS, and INS assists in detecting cycle slips with carrier phase observations; uses carrier phase epoch-to-epoch differences to obtain TDCP observations, uses the position difference calculated by INS dead reckoning as the initial value, and jointly solves the position change between epochs, and recursively obtains the dynamic position result; combines Beidou position factors and IMU pre-integration factors to construct a factor graph, and obtains the optimal solution of position, speed and attitude through maximum a posteriori probability estimation. The present invention is based on sliding window factor graph optimization, taking into account the advantages of both loose combination and tight combination, and has great advantages in accuracy, anti-error and applicability.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the field of global satellite navigation systems, and particularly relates to the problem that dynamic positioning has weak robustness and the position result is inconsistent with the actual motion state under complex environments. The present invention takes into account the advantages of both loose combination and tight combination, and optimizes and solves the optimal solution of global combined navigation based on a sliding window factor graph. Background Art

[0002] Dynamic relative positioning technology can provide real-time high-precision relative positioning services and is widely used in multiple fields such as robots, mobile mapping, and vehicle navigation. Dynamic relative positioning technology selects a dynamic reference station as a reference, updates its own dynamic position in real time and uses it as a position reference to solve the dynamic relative position relationship. Since the base station is in motion and the dynamic position is unknown, the existing TDCP / INS method uses carrier phase epoch difference (TDCP) to solve the high-precision position change and integrates inertial navigation (INS) to obtain high-frequency and stable dynamic reference position results. This method adopts a loose combination mode, and it is difficult to ensure the stability and reliability of the results in relatively complex observation environments such as urban streets and tree occlusion. Tight combination can overcome the above defects of loose combination, and can also perform combined solution when the number of available satellites is less than 4. However, the calculation complexity of tight combination increases, the data processing requirements are high, and two heterogeneous observation data are integrated at the same time, and the risk of filtering instability is relatively high. When the number of Beidou satellites is sufficient, the results of loose combination and tight combination are consistent. Only when the INS prior information significantly improves the accuracy of the epoch difference observation value, the tight combination will be significantly better than the loose combination.

[0003] The TDCP / INS data fusion adopts the extended Kalman filter (EKF) method, which is quite mature in theory and very easy to implement. However, compared with EKF, the factor graph optimization (FGO) method globally optimizes the current data and all historical data together, and better approaches the optimal solution through multiple iterations and linearization. FGO can iteratively calculate the entire dynamic reference position trajectory to achieve global consistency of the trajectory. At the same time, the sliding window can better reduce the time cost and achieve timeliness similar to EKF. In the face of complex time-varying environments, FGO has obvious advantages. In summary, both loose combination and tight combination have their own obvious advantages and disadvantages. How to take into account the advantages of both and minimize their respective shortcomings, and study a combined navigation method based on FGO to solve the dynamic reference position is the primary problem that needs to be solved urgently. Summary of the invention

[0004] Aiming at the problem that dynamic positioning robustness is weak and position result is inconsistent with actual motion state in complex environment, the present invention proposes a TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization, comprising the following steps:

[0005] Step 1: Use the position change calculated by INS dead reckoning as prior information to assist Beidou in solving the high-precision position change, and recursively obtain the dynamic position of the dynamic target at different times;

[0006] Step 2: According to the recursive results of the dynamic position, the optimal solution of TDCP / INS is realized based on the sliding window factor graph optimization, which specifically includes the following sub-steps:

[0007] Step 2.1, according to the dynamic position of the dynamic target in step 1, obtain the state vector to be optimized of the sliding window, the state vector includes the position, speed and posture of the dynamic target at a certain moment;

[0008] Step 2.2, constructing the Beidou position residual factor and the inertial measurement unit pre-integration residual factor at a certain moment according to the dynamic position of the dynamic target in step 1;

[0009] Step 2.3, combine the Beidou position residual factor and the inertial measurement unit pre-integration residual factor of step 2.2 to obtain the objective function of factor graph optimization at a certain moment, use the objective function to solve the state vector to obtain the optimal state estimate, and thus obtain the position, velocity and attitude navigation results of the dynamic target.

[0010] Furthermore, the specific implementation of step 1 includes the following sub-steps:

[0011] Step 1.1, perform initial alignment of INS through Beidou SPP positioning, use the position information calculated by INS dead reckoning to detect the cycle slip of carrier phase observation value, and mark the observation value where the cycle slip occurs;

[0012] Step 1.2, calculating the difference between adjacent time points of the observation values ​​of step 1.1 that are not marked as having cycle slips, and obtaining TDCP observation values;

[0013] Step 1.3, obtain the position change between adjacent moments by subtracting the dynamic position calculated by INS dead reckoning;

[0014] Step 1.4, taking the position change in step 1.3 as the initial value, using the TDCP observation value in step 1.2, and obtaining the high-precision position change through least squares solution;

[0015] Step 1.5, according to The dynamic position of the dynamic target at each moment is obtained by recursively combining the high-precision position change in step 1.4 The next moment of the moment The dynamic position of the dynamic target at any moment.

[0016] Furthermore, the calculation formula of the TDCP observation value in step 1.2 is as follows;

[0017]

[0018] In the formula, and represents the adjacent observation time, represents the carrier phase difference observation value between epochs, that is, the TDCP observation value, represents the wavelength, represents the satellite-to-earth distance difference between epochs, represents the speed of light, Indicates the clock error;

[0019] The observation equation is further linearized as:

[0020]

[0021] In the formula, i represents the observation satellite, represents direction cosines, Indicates the position change of the dynamic target, Indicates satellite i Inter-epoch residuals;

[0022] In satellite i and j Eliminate receiver clock error by inter-difference , expressed as:

[0023]

[0024] In the formula, i and Satellite i and j , Indicates satellite i and j The difference in carrier phase difference observation values ​​between the times of express and The time difference between the moments Indicates satellite i and j The difference between the residuals between epochs.

[0025] Furthermore, in step 1.5, the high-precision position change in step 1.4 is combined to recursively obtain the next moment The dynamic position of the dynamic target at any moment is calculated as follows:

[0026]

[0027] in, express The dynamic position of the dynamic target at all times, express The dynamic position of the dynamic target at all times, express and The X-axis coordinate of the position change between moments, express and The Y-axis coordinate of the position change between moments, express and The Z-axis coordinate of the position change between moments.

[0028] Furthermore, in step 2.2, The calculation formula of the Beidou position residual factor at the moment is as follows:

[0029]

[0030] in, express Beidou position residual factor at the moment, represents the state vector, express INS dead reckoning position at the moment, express The direction corotation matrix from the carrier coordinate system to the navigation coordinate system at the moment, It is the lever arm from Beidou antenna to IMU center. express The dynamic position of the dynamic target at any moment.

[0031] Furthermore, in step 2.2, The calculation formula of the IMU pre-integration residual factor at the moment, that is, the inertial measurement unit pre-integration residual factor, is as follows:

[0032] =

[0033] In the formula, express IMU pre-integration residual factor at time, represents the original observation value of INS, represents the state vector; , and for Time has come IMU pre-integrated position, velocity and attitude residuals between time points; and express Time has come The IMU pre-integrated gyro and accelerometer bias residuals during time; express timeb Tie to n The directional corotation matrix of the system, represents the acceleration due to gravity, express arrive The time interval between moments; and Respectively and The location at the moment, and Respectively and The speed of time, and Respectively and time b Tie to n The attitude quaternion of the system, Represents the multiplication of quaternions; , and for Time has come IMU pre-integrated observations at time instant.

[0034] Furthermore, in step 2.3, The objective function of moment factor graph optimization is:

[0035]

[0036] In the formula, Represents the optimal state estimate, and the state vector to be optimized of the sliding window is ,in, Respectively The state vector at time, is the sliding window size, where The state vector at the moment: , , and They are The position, velocity and posture of the dynamic target at each moment, and for Gyro and accelerometer zero bias at all times; represents the minimum value of the objective function, and denote the residual and coefficient matrices of the marginalized prior constraints respectively; and They represent the IMU pre-integration residual factor and the Beidou position residual factor respectively. Represents the original observation value of Beidou Satellite Navigation System BDS, represents the original observation value of INS, represents the state vector, and is the corresponding covariance matrix.

[0037] Furthermore, in step 2.3, the state vector is solved using the objective function according to the maximum a posteriori probability estimation theory to obtain the optimal state estimate.

[0038] The present invention also provides a TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization, comprising:

[0039] one or more processors;

[0040] A storage device is used to store one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization as described in the above technical solution.

[0041] The beneficial effects produced by the present invention are:

[0042] 1. This paper proposes a TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization to ensure that the dynamic reference position is consistent with the actual motion trajectory and provide a high-frequency stable dynamic reference for relative positioning.

[0043] 2. The present invention takes into account the advantages of both loose and tight combinations of BeiDou / INS, achieves the optimal solution based on sliding window factor graph optimization, has high robustness, and is suitable for complex time-varying environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a flow chart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings.

[0046] The embodiment of the present invention provides a TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization, using INS to assist in the solution of high-precision position change (step 1); based on the sliding window factor graph optimization, TDCP / INS optimal solution is achieved (step 2). Figure 1 A TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization provided in an embodiment of the present invention specifically comprises the following steps:

[0047] Step 1: Use the position change calculated by INS dead reckoning as prior information to assist Beidou in solving the high-precision position change, including:

[0048] Step 1.1: Initially align the INS using BeiDou Navigation Satellite System (BDS) single point (SPP) positioning, use the position information calculated by INS dead reckoning to detect the cycle slips of the carrier phase observations, and mark the observations where the cycle slips occur;

[0049] Step 1.2: Calculate the difference between adjacent time points of the observation values ​​of step 1.1 that are not marked as having cycle slips, and obtain the TDCP observation value:

[0050]

[0051] In the formula, and represents adjacent observation times, represents the carrier phase difference observation value between epochs, that is, the TDCP observation value, represents the wavelength, represents the satellite-to-earth distance difference between epochs, represents the speed of light, Indicates the clock error.

[0052] The observation equation is further linearized as:

[0053]

[0054] In the formula, i represents the observation satellite, represents direction cosines, Indicates the position change of the dynamic target, Indicates satellite i Inter-epoch residuals.

[0055] In satellite i and j Eliminate receiver clock error by inter-difference , expressed as:

[0056]

[0057] In the formula, i and Satellite i and j , Indicates satellite i and j The difference in carrier phase difference observation values ​​between the times of express and The time difference between the moments Indicates satellite iand j The difference between the residuals of the epochs of ;

[0058] Step 1.3: Subtract the dynamic position calculated by INS dead reckoning to get the position change between adjacent moments.

[0059]

[0060] In the formula, and represents adjacent observation times, Indicates the change in INS position, express INS dynamic position at the moment, express INS dynamic position at the moment;

[0061] Step 1.4: Use the position change in step 1.3 as the initial value, and use the TDCP observation data in step 1.2 to obtain a high-precision position change through least squares solution. ;

[0062] Step 1.5: According to The dynamic position of the dynamic target at the moment is obtained by recursion combined with the high-precision position change in step 1.4 The dynamic position at a certain moment is calculated as follows:

[0063]

[0064] in, express The dynamic position of the dynamic target at the moment of observation, express The dynamic position of the dynamic target at all times, express and The X-axis coordinate of the position change between moments, express and The Y-axis coordinate of the position change between moments, express and The Z-axis coordinate of the position change between moments.

[0065] Step 2: According to the dynamic position recursion results, the optimal solution of TDCP / INS is realized based on the sliding window factor graph optimization, which includes:

[0066] Step 2.1: The state vector to be optimized based on the sliding window is , where the window The state vector at the moment:

[0067]

[0068] in express The state vector at time, is the sliding window size, , and They are The position, speed and posture of dynamic targets at all times, and for The gyro and accelerometer bias of the inertial measurement unit (IMU) at all times.

[0069] Step 2.2: Based on the dynamic position result of step 1.5, construct Beidou position residual factor and IMU pre-integration factor at this moment:

[0070] Beidou position residual factor at the moment:

[0071]

[0072] in, express Beidou position residual factor at the moment, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the state vector, express INS dead reckoning position at the moment, express The carrier coordinate system at the moment ( b system) to the navigation coordinate system ( n The directional corotation matrix of the system, It is the lever arm from Beidou antenna to IMU center. express The dynamic position of the dynamic target at any moment.

[0073] at the same time, IMU pre-integration factor at time:

[0074] =

[0075] In the formula, express IMU pre-integration factor at time, represents the original observation value of INS, represents the state vector; , and for Time has come IMU pre-integrated position, velocity and attitude residuals between time points; and express Time has come The IMU pre-integrated gyro and accelerometer bias residuals during time; express time b Tie to n The directional corotation matrix of the system, represents the acceleration due to gravity, express arrive The time interval between moments; and Respectively and The location at the moment, and Respectively and The speed of time, and Respectively and time b Tie to n The attitude quaternion of the system, Represents the multiplication of quaternions; , and for Time has come IMU pre-integrated observations at time instant.

[0076] Step 2.3: Combine the Beidou position residual factor and IMU pre-integration factor in step 2.2 to obtain The objective function of moment factor graph optimization is:

[0077]

[0078] In the formula, represents the optimal state estimate, represents the minimum value of the objective function, and denote the residual and coefficient matrices of the marginalized prior constraints, respectively. and They represent the IMU pre-integration factor and Beidou position residual factor respectively. and is its corresponding covariance matrix.

[0079] According to the maximum a posteriori probability estimation theory, the objective function is used to solve the optimal value of the state vector to obtain the position, velocity and attitude navigation results.

[0080] In specific implementation, the above process can use computer software technology to realize automatic operation of the process, and the system device that runs the method process of the present invention should also be within the protection scope of the present invention.

[0081] On the other hand, an embodiment of the present invention also provides a TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization, including:

[0082] one or more processors;

[0083] A storage device is used to store one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization as described in the above technical solution.

[0084] According to the technical solution of the present invention, the statistical results of the TDCP / INS semi-tight integrated navigation test in Table 1 are obtained. The results in Table 1 show that the position, speed and attitude accuracy of the TDCP / INS semi-tight integrated navigation based on factor graph optimization is higher.

[0085] Table 1 Statistical results of TDCP / INS semi-tight integrated navigation test

[0086]

[0087] The specific embodiments described herein are merely examples of the spirit of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in similar ways, but they will not deviate from the spirit of the present invention or exceed the scope defined by the appended claims.

Claims

1. A TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization is characterized in that: The following steps are involved: Step 1: Use the position change calculated by INS dead reckoning as prior information to assist Beidou in solving the high-precision position change, and recursively obtain the dynamic position of the dynamic target at different times; Step 2: According to the recursive results of the dynamic position, the optimal solution of TDCP / INS is realized based on the sliding window factor graph optimization, which specifically includes the following sub-steps: Step 2.1, according to the dynamic position of the dynamic target in step 1, obtain the state vector to be optimized of the sliding window, the state vector includes the position, speed and posture of the dynamic target at a certain moment; Step 2.2, constructing the Beidou position residual factor and the inertial measurement unit pre-integration residual factor at a certain moment according to the dynamic position of the dynamic target in step 1; Step 2.3, combine the Beidou position residual factor and the inertial measurement unit pre-integration residual factor of step 2.2 to obtain the objective function of factor graph optimization at a certain moment, use the objective function to solve the state vector to obtain the optimal state estimate, and thus obtain the position, velocity and attitude navigation results of the dynamic target.

2. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 1 is characterized in that: The specific implementation of step 1 includes the following sub-steps: Step 1.1, perform initial alignment of INS through Beidou SPP positioning, use the position information calculated by INS dead reckoning to detect the cycle slip of carrier phase observation value, and mark the observation value where the cycle slip occurs; Step 1.2, calculating the difference between adjacent time points of the observation values ​​of step 1.1 that are not marked as having cycle slips, and obtaining TDCP observation values; Step 1.3, obtain the position change between adjacent moments by subtracting the dynamic position calculated by INS dead reckoning; Step 1.4, taking the position change in step 1.3 as the initial value, using the TDCP observation value in step 1.2, and obtaining the high-precision position change through least squares solution; Step 1.5, according to The dynamic position of the dynamic target at each moment is obtained by recursively combining the high-precision position change in step 1.4 The next moment of the moment The dynamic position of the dynamic target at any moment.

3. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 2 is characterized in that: The calculation formula of TDCP observation value in step 1.2 is as follows; In the formula, and represents adjacent observation times, represents the carrier phase difference observation value between epochs, that is, the TDCP observation value, represents the wavelength, represents the satellite-to-earth distance difference between epochs, represents the speed of light, Indicates the clock error; The observation equation is further linearized as: In the formula, i represents the observation satellite, represents direction cosines, Indicates the position change of the dynamic target, Indicates satellite i Inter-epoch residuals; In satellite i and j Eliminate receiver clock error by inter-difference , expressed as: In the formula, i and Satellite i and j , Indicates satellite i and j The difference in carrier phase difference observation values ​​between the times of express and The time difference between the moments Indicates satellite i and j The difference between the residuals between epochs.

4. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 2 is characterized in that: In step 1.5, the next moment is obtained by recursively combining the high-precision position change in step 1.4 The dynamic position of the dynamic target at any moment is calculated as follows: in, express The dynamic position of the dynamic target at all times, express The dynamic position of the dynamic target at all times, express and The X-axis coordinate of the position change between moments, express and The Y-axis coordinate of the position change between moments, express and The Z-axis coordinate of the position change between moments.

5. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 1, characterized in that: In step 2.2, The calculation formula of the Beidou position residual factor at the moment is as follows: in, express Beidou position residual factor at the moment, represents the state vector, express INS dead reckoning position at the moment, express The direction corotation matrix from the carrier coordinate system to the navigation coordinate system at the moment, It is the lever arm from Beidou antenna to IMU center. express The dynamic position of the dynamic target at any moment.

6. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 1 is characterized in that: In step 2.2, The calculation formula of the IMU pre-integration residual factor at the moment, that is, the inertial measurement unit pre-integration residual factor, is as follows: = In the formula, express IMU pre-integration residual factor at time, represents the original observation value of INS, represents the state vector; , and for Time has come IMU pre-integrated position, velocity and attitude residuals between time points; and express Time has come The IMU pre-integrated gyro and accelerometer bias residuals during time; express time b Tie to n The directional corotation matrix of the system, represents the acceleration due to gravity, express arrive The time interval between moments; and Respectively and The location at the moment, and Respectively and The speed of time, and Respectively and time b Tie to n The attitude quaternion of the system, Represents the multiplication of quaternions; , and for Time has come IMU pre-integrated observations at time instant.

7. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 1, characterized in that: In step 2.3, The objective function of moment factor graph optimization is: In the formula, Represents the optimal state estimate, and the state vector to be optimized of the sliding window is ,in, Respectively The state vector at time, is the sliding window size, where The state vector at the moment: , , and They are The position, velocity and posture of the dynamic target at each moment, and for Gyro and accelerometer zero bias at all times; represents the minimum value of the objective function, and denote the residual and coefficient matrices of the marginalized prior constraints respectively; and They represent the IMU pre-integration residual factor and the Beidou position residual factor respectively. Represents the original observation value of Beidou Satellite Navigation System BDS, represents the original observation value of INS, represents the state vector, and is the corresponding covariance matrix.

8. The TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization according to claim 1, characterized in that: In step 2.3, the state vector is solved using the objective function according to the maximum a posteriori probability estimation theory to obtain the optimal state estimate.

9. A TDCP / INS semi-tight integrated navigation device based on sliding window factor graph optimization is characterized in that: include: one or more processors; A storage device for storing one or more programs, when the one or more programs are executed by the one or more processors, enables the one or more processors to implement the TDCP / INS semi-tight integrated navigation method based on sliding window factor graph optimization as described in any one of claims 1 to 8.