A method and device for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data
Through the sliding window factor graph optimization method, the problem of degradation of Beidou/INS combined navigation performance in complex environments is solved, and the global optimal solution and efficient data processing of Beidou/INS combined navigation is realized, which is suitable for complex time-varying environments.
Patent Information
- Application Number
- CN202411198252.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-29
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2044-08-29
AI Technical Summary
In complex observation environments, the problem of degradation in Beidou/INS combined navigation performance is mainly due to signal occlusion interference and limited computing capabilities, resulting in poor navigation accuracy and low data processing efficiency.
The Beidou/INS data step-by-step combined navigation method optimized based on sliding window factor graph is adopted. The Beidou relative positioning solution is performed through the dynamic position of the reference target, the state parameter vector is expanded, and the state variables and residual factors to be optimized for the sliding window are constructed, combined with the IMU pre-integration factor realizes the global optimal solution for combined navigation.
It improves the timeliness and accuracy of navigation, meets the real-time and efficient data processing requirements in complex time-varying environments, and realizes the global optimal solution for Beidou/INS combined navigation.
Smart Images

Figure CN118706114B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of global satellite navigation systems, and particularly relates to the problem of reduced combined navigation performance due to signal shielding and interference and limited computing power in a complex observation environment. The invention realizes a global optimal solution of Beidou / INS combined navigation based on sliding window factor graph optimization. Background Art
[0002] Beidou / Inertial Navigation System (INS) fusion positioning has been widely used in many fields such as robots, mobile mapping, and vehicle navigation. In dynamic applications with complex environments, high-frequency dynamic positioning, and irregular motion, Beidou and INS are an excellent combination. The extended Kalman filter (EKF) is currently the most commonly used Beidou / INS combined navigation filter. However, compared with EKF, the factor graph optimization (FGO) method globally optimizes the current data and all historical data together, and can calculate the entire motion trajectory. Through multiple iterations and linearization, it can better approach the optimal solution. The sliding window can better reduce the time cost and improve the effectiveness. FGO has obvious advantages in complex environments. The Beidou / INS combined navigation in the dynamic positioning mode usually adopts a tight combination mode. By adding the ambiguity parameter to the state parameter vector, the position, velocity, attitude and other parameters are solved together with the ambiguity floating point solution. The matrix calculation is large, the effectiveness is poor, and the data processing requirements are high. In a complex environment, it even causes the navigation positioning accuracy to deteriorate and diverge. Considering the load and equipment cost of dynamic carriers, the data processing capacity is limited and cannot support efficient and fast real-time processing of tightly combined high-dimensional matrices. Therefore, how to achieve efficient data processing is an urgent problem that needs to be solved. Summary of the invention
[0003] Aiming at the problem that the Beidou / INS integrated navigation performance of a dynamic carrier is degraded under a complex observation environment, the present invention proposes a step-by-step integrated navigation method based on factor graph optimization of Beidou / INS data, comprising the following steps:
[0004] Step 1: Use the dynamic position of the reference target as the reference to perform Beidou relative positioning solution, obtain the state parameter vector of the dynamic target, expand the state parameter vector, and construct integer-cycle ambiguity-free carrier phase observation value and Doppler frequency shift observation value;
[0005] Step 2: Based on the sliding window factor graph optimization, the Beidou / INS residual factor is constructed to achieve the global optimal solution of integrated navigation, which specifically includes the following sub-steps:
[0006] Step 2.1, combining the state parameter vector expanded in step 1 to construct the state variables to be optimized of the sliding window, the state variables including the position, velocity and posture of the dynamic target at a certain moment;
[0007] Step 2.2, construct the Beidou carrier phase observation residual factor and Doppler frequency shift observation residual factor at a certain moment according to the integer ambiguity-free carrier phase observation value and Doppler frequency shift observation value in step 1;
[0008] Step 2.3, construct the IMU pre-integration factor at a certain moment;
[0009] Step 2.4, combine the Beidou carrier phase observation residual factor and Doppler frequency shift observation residual factor of step 2.2, and the IMU pre-integration factor of step 2.3 to obtain the objective function based on factor graph optimization at a certain moment, use the objective function to solve the state variables 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 is as follows:
[0011] Step 1.1: Dynamic target A uses the dynamic position of reference target B as the reference for Beidou relative positioning solution. The state parameter vector includes position, velocity and double-difference ambiguity parameters:
[0012]
[0013] in, represents the state parameter vector, and Respectively represent the position and velocity of the dynamic target A, express m Double difference ambiguity;
[0014] Step 1.2: The dynamic target A uses the INS dead reckoning position and velocity values as prior information, and solves the floating point ambiguity by combining the double-difference pseudorange, carrier phase and Doppler shift observations. The floating point ambiguity parameter vector is searched and fixed by the least squares ambiguity reduction correlation adjustment method LAMBDA to obtain the integer ambiguity fixed solution.
[0015] Step 1.3, expand the state parameter vector of step 1.1, add attitude, inertial measurement unit gyro and acceleration bias parameters, and eliminate ambiguity parameters:
[0016]
[0017] in, represents the expanded state parameter vector, , and Respectively represent the position, velocity and posture of the dynamic target A, and Indicates the zero bias of the inertial measurement unit gyro and acceleration;
[0018] Based on the pseudorange and pseudorange rate observations predicted by INS dead reckoning, the integer ambiguity fixed solution of step 1.2 is used to construct the integer ambiguity-free carrier phase and Doppler shift observations:
[0019]
[0020] In the formula, and They represent the Beidou carrier phase and Doppler frequency shift observation values respectively, represents the integer ambiguity, and They represent the pseudorange and pseudorange rate observations of INS dead reckoning, and are the direction cosine matrices representing position and velocity, respectively. Indicates wavelength.
[0021] Furthermore, in step 1.2, the formula for solving the floating point ambiguity is as follows;
[0022]
[0023] In the formula, , and denote the position, velocity and floating-point ambiguity parameter vectors respectively, and denote the double difference satellite ground distance and double difference satellite ground speed respectively, , and denote the double-difference pseudorange, carrier phase and Doppler shift observations respectively, represents the wavelength, and Direction cosines and dimensional identity matrix, and Respectively and dimensional zero matrix, , and represent double-difference pseudorange, carrier phase and Doppler shift residuals respectively.
[0024] Furthermore, in step 2.2, The construction method of the residual factor of Beidou carrier phase observation value at this moment is as follows;
[0025] Beidou dynamic position of dynamic target A and reference target B at any moment:
[0026]
[0027] in, and Respectively Beidou dynamic position of dynamic target A and reference target B at all times, and express The INS dead-reckoning positions of the dynamic target A and the reference target B at each moment, and express The arm correction coefficient matrix of the dynamic target A and the reference target B at the moment, and They are The rotation matrix from the carrier coordinate system to the navigation coordinate system of the dynamic target A and the reference target B at the moment, and The arms from the Beidou antenna to the IMU center represent the dynamic target A and the reference target B respectively;
[0028] therefore, Beidou carrier phase observation residual factor at this moment:
[0029]
[0030] in, represents the residual factor of Beidou carrier phase observation value, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the state vector, and They represent the Beidou carrier phase observation value and integer ambiguity respectively, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites.
[0031] Furthermore, in step 2.2, the Doppler shift observation residual factor is constructed as follows;
[0032] The dynamic speed of dynamic target A and reference target B at each moment:
[0033]
[0034] in, and Respectively The speed of dynamic target A and reference target B at any moment, and express INS dead reckoning speed of dynamic target A and reference target B at any moment, Represents the angular velocity of the carrier coordinate system of the dynamic target A and the reference target B relative to the Earth-fixed coordinate system, and They are The rotation matrix from the carrier coordinate system to the navigation coordinate system of the dynamic target A and the reference target B at the moment, and The arms from the Beidou antenna to the IMU center represent the dynamic target A and the reference target B respectively;
[0035] therefore, The residual factor of the Doppler frequency shift observation at time:
[0036]
[0037] in, represents the Doppler frequency shift observation residual factor, represents the original Doppler observation value, represents the state vector, and denote the wavelength and Doppler shift observations respectively, and They are The dynamic target A and the reference target B at the moment BeiDou satellite speed, and They are The dynamic target A and the reference target B at the moment Beidou satellite speed.
[0038] Furthermore, in step 2.3, The expression of the IMU pre-integration factor at time is:
[0039] =
[0040] In the formula, It represents the IMU pre-integration factor. 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 The rotation matrix from the carrier coordinate system to the navigation coordinate system at the moment, represents the acceleration due to gravity, express Time has come The time interval between moments; and Respectively Moment and The position of INS at the moment, and Respectively Moment and The speed of INS at the moment, and Respectively Moment and The attitude quaternion from the carrier coordinate system to the navigation coordinate system at the moment; , and for Time has come IMU pre-integrated observations at time instant.
[0041] Furthermore, in step 2.4, The expression of the objective function based on factor graph optimization at each moment is as follows:
[0042]
[0043] In the formula, represents the optimal state estimate, is the state variable to be optimized of the sliding window, where Respectively The state vector at time, is the sliding window size, where State variables at this moment: , , and They are The position, speed and posture at any moment, and for The zero bias of the gyro and acceleration 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 factor, Beidou carrier phase and Doppler frequency shift observation residual factors respectively, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the original Doppler observation value, represents the original observation value of INS, represents the state vector, , and are the corresponding covariance matrices respectively.
[0044] Furthermore, in step 2.4, the state variables are solved using the objective function according to the maximum a posteriori probability estimation theory to obtain the optimal state estimate.
[0045] The present invention also provides a step-by-step integrated navigation device based on factor graph optimization of Beidou / INS data, comprising:
[0046] one or more processors;
[0047] 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 a step-by-step combined navigation method based on factor graph optimization of Beidou / INS data as described in the above technical solution.
[0048] The beneficial effects produced by the present invention are:
[0049] The present invention proposes a step-by-step combined navigation method based on sliding window factor graph optimization of BeiDou / INS data, which meets the real-time and efficient processing requirements of BeiDou / INS data, realizes the global optimal solution based on sliding window factor graph optimization, has high timeliness, and is suitable for complex time-varying environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] Figure 1 It is a method flow chart of an embodiment of the present invention. DETAILED DESCRIPTION
[0051] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings.
[0052] The embodiment of the present invention provides a step-by-step integrated navigation method based on factor graph optimization of Beidou / INS data, using INS dead reckoning position and speed to assist Beidou double difference ambiguity resolution (step 1); based on sliding window factor graph optimization, Beidou / INS residual factors are constructed to achieve the global optimal solution of integrated navigation (step 2). Figure 1 , a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data provided in an embodiment of the present invention specifically includes the following steps:
[0053] Step 1: Use INS dead reckoning position and velocity to assist Beidou double-difference ambiguity resolution, including:
[0054] Step 1.1: Dynamic target A uses the dynamic position of reference target B as the reference for Beidou relative positioning solution. The state parameter vector includes position, velocity and double-difference ambiguity parameters:
[0055]
[0056] in, represents the state parameter vector, and Respectively represent the position and velocity of the dynamic target A, express m double difference ambiguity.
[0057] Step 1.2: Dynamic target A uses INS dead reckoning position and velocity values as prior information, and combines double-difference pseudorange, carrier phase and Doppler shift observations to resolve floating point ambiguities:
[0058]
[0059] In the formula, , and denote the position, velocity and floating-point ambiguity parameter vectors respectively, and denote the double difference satellite ground distance and double difference satellite ground speed respectively, , and denote the double-difference pseudorange, carrier phase and Doppler shift observations respectively, represents the wavelength, and Direction cosines and dimensional identity matrix, and Respectively and dimensional zero matrix, , and They represent double-difference pseudorange, carrier phase and Doppler shift residuals respectively. The floating-point ambiguity parameter vector is searched and fixed by the least squares ambiguity reduction and association adjustment (LAMBDA) method to obtain the integer ambiguity fixed solution.
[0060] Step 1.3: Expand the state parameter vector of step 1.1, add attitude, inertial measurement unit (IMU) gyro and acceleration bias parameters, and eliminate ambiguity parameters:
[0061]
[0062] in, represents the expanded state parameter vector, , and Respectively represent the position, velocity and posture of the dynamic target A, and Represents the zero bias of the inertial measurement unit gyro and accelerometer.
[0063] According to the "pseudorange" and "pseudorange rate" observations predicted by INS dead reckoning, the integer ambiguity fixed solution of step 1.2 is used to construct the integer ambiguity-free carrier phase and Doppler shift observations:
[0064]
[0065] In the formula, and They represent the Beidou carrier phase and Doppler frequency shift observation values respectively, represents the integer ambiguity, and They represent the “pseudorange” and “pseudorange rate” observations of INS dead reckoning, and The direction cosine matrices representing position and velocity respectively.
[0066] Step 2: Based on the sliding window factor graph optimization, the Beidou / INS residual factor is constructed to solve the optimal result of the integrated navigation, which includes:
[0067] Step 2.1: The state variable to be optimized based on the sliding window is , where the window State variables at this moment:
[0068]
[0069] in express The state variables at time, is the sliding window size, , and They are The position, speed and posture of the dynamic target A at all times, and for IMU gyro and accelerometer zero bias at this moment.
[0070] Step 2.2: Based on the integer ambiguity-free carrier phase observations and Doppler shift observations from step 1.3, construct The residual factor of Beidou carrier phase and Doppler frequency shift observation at this moment:
[0071] Beidou dynamic position of dynamic target A and reference target B at any moment:
[0072]
[0073] in, and Respectively Beidou dynamic position of dynamic target A and reference target B at all times, and express The INS dead-reckoning positions of the dynamic target A and the reference target B at each moment, and express The arm correction coefficient matrix of the dynamic target A and the reference target B at the moment, and They are The carrier coordinate system of the dynamic target A and the reference target B at the moment ( b system) to the navigation coordinate system ( n The rotation matrix of the system, and The arms representing the Beidou antenna to the IMU center of the dynamic target A and the reference target B respectively.
[0074] therefore, Beidou carrier phase observation residual factor at this moment:
[0075]
[0076] in, represents the residual factor of Beidou carrier phase observation value, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the state vector, and They represent the Beidou carrier phase observation value and integer ambiguity respectively, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites.
[0077] at the same time, The dynamic speed of dynamic target A and reference target B at each moment:
[0078]
[0079] in, and Respectively The speed of dynamic target A and reference target B at any moment, and express INS dead reckoning speed of dynamic target A and reference target B at any moment, Indicates the dynamic target A and the reference target B b relative to the Earth-centered Earth-fixed coordinate system ( e The angular velocity of the system, and They are The dynamic target A and the reference target B b Tie to n The rotation matrix of the system, and The arms representing the Beidou antenna to the IMU center of the dynamic target A and the reference target B respectively.
[0080] therefore, The residual factor of the Doppler frequency shift observation at time:
[0081]
[0082] in, represents the Doppler frequency shift observation residual factor, represents the original Doppler observation value, represents the state vector, and denote the wavelength and Doppler shift observations respectively, and They are The dynamic target A and the reference target B at the moment Beidou satellite speed, and They are The dynamic target A and the reference target B at the moment Beidou satellite speed.
[0083] Step 2.3, Build IMU pre-integration factor at time:
[0084] =
[0085] In the formula, It represents the IMU pre-integration factor. 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 rotation matrix of the system, represents the acceleration due to gravity, express Time has come The time interval between moments; and Respectively Moment and The position of INS at the moment, and Respectively Moment and The speed of INS at the moment, and Respectively Moment and time b Tie to n The attitude quaternion of the system; , and for Time has come IMU pre-integrated observations at time instant.
[0086] Step 2.4: Combine the Beidou carrier phase observation residual factor and Doppler frequency shift observation residual factor of step 2.2 and the IMU pre-integration factor of step 2.3 to obtain The objective function of factor graph optimization at each moment is:
[0087]
[0088] 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, Beidou carrier phase and Doppler frequency shift observation residual factors respectively, , and are their corresponding covariance matrices respectively.
[0089] According to the maximum a posteriori probability estimation theory, the objective function is used to solve the optimal value of the state variable to obtain the position, velocity and attitude navigation results.
[0090] 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.
[0091] The embodiment of the present invention further provides a step-by-step integrated navigation device for optimizing Beidou / INS data based on a factor graph, comprising:
[0092] one or more processors;
[0093] 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 a step-by-step combined navigation method based on factor graph optimization of Beidou / INS data as described in the above technical solution.
[0094] According to the technical solution of the present invention, the statistical results of the Beidou / INS integrated navigation test in Table 1 are obtained. The results in Table 1 show that the Beidou / INS integrated navigation based on factor graph optimization has a higher ambiguity fixation rate and higher navigation performance.
[0095] Table 1 Statistical results of Beidou / INS integrated navigation test
[0096]
[0097] 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 step-by-step integrated navigation method based on factor graph optimization of BeiDou / INS data. It is characterized in that The following steps are involved: Step 1: Use the dynamic position of the reference target as the reference to perform Beidou relative positioning solution, obtain the state parameter vector of the dynamic target, and expand the state parameter vector to construct integer-cycle ambiguity-free carrier phase observation value and Doppler frequency shift observation value; The specific implementation of step 1 is as follows: Step 1.1: Dynamic target A uses the dynamic position of reference target B as the reference for Beidou relative positioning solution. The state parameter vector includes position, velocity and double-difference ambiguity parameters: in, represents the state parameter vector, and Respectively represent the position and velocity of the dynamic target A, express m Double difference ambiguity; Step 1.2: The dynamic target A uses the INS dead reckoning position and velocity values as prior information, and solves the floating point ambiguity by combining the double-difference pseudorange, carrier phase and Doppler shift observations. The floating point ambiguity parameter vector is searched and fixed by the least squares ambiguity reduction correlation adjustment method LAMBDA to obtain the integer ambiguity fixed solution. Step 1.3, expand the state parameter vector of step 1.1, add attitude, inertial measurement unit gyro and acceleration bias parameters, and eliminate ambiguity parameters: in, represents the expanded state parameter vector, , and Respectively represent the position, velocity and posture of the dynamic target A, and Indicates the zero bias of the inertial measurement unit gyro and accelerometer; Based on the pseudorange and pseudorange rate observations predicted by INS dead reckoning, the integer ambiguity fixed solution of step 1.2 is used to construct the integer ambiguity-free carrier phase and Doppler shift observations: In the formula, and They represent the Beidou carrier phase and Doppler frequency shift observation values respectively, represents the integer ambiguity, and They represent the pseudorange and pseudorange rate observations of INS dead reckoning, and are the direction cosine matrices representing position and velocity, respectively. represents the wavelength, and denote the carrier phase and Doppler shift residuals respectively; Step 2: Based on the sliding window factor graph optimization, the Beidou / INS residual factor is constructed to achieve the global optimal solution of integrated navigation, which specifically includes the following sub-steps: Step 2.1, combining the state parameter vector expanded in step 1 to construct the state variables to be optimized of the sliding window, the state variables including the position, velocity and posture of the dynamic target at a certain moment; Step 2.2, construct the Beidou carrier phase observation residual factor and Doppler frequency shift observation residual factor at a certain moment according to the integer ambiguity-free carrier phase observation value and Doppler frequency shift observation value in step 1; Step 2.3, construct the IMU pre-integration factor at a certain moment; Step 2.4, combine the Beidou carrier phase observation residual factor and Doppler frequency shift observation residual factor of step 2.2, and the IMU pre-integration factor of step 2.3 to obtain the objective function based on factor graph optimization at a certain moment, use the objective function to solve the state variables to obtain the optimal state estimate, and thus obtain the position, velocity and attitude navigation results of the dynamic target.
2. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 1.2, the formula for solving the floating point ambiguity is as follows; In the formula, , and denote the position, velocity and floating-point ambiguity parameter vectors respectively, and denote the double difference satellite ground distance and double difference satellite ground speed respectively, , and denote the double-difference pseudorange, carrier phase and Doppler shift observations respectively, represents the wavelength, and They represent direction cosines and dimensional identity matrix, and Respectively and dimensional zero matrix, represents double-difference pseudorange.
3. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 2.2, The construction method of the residual factor of Beidou carrier phase observation value at this moment is as follows; Beidou dynamic position of dynamic target A and reference target B at any moment: in, and Respectively Beidou dynamic position of dynamic target A and reference target B at all times, and express The INS dead-reckoning positions of the dynamic target A and the reference target B at each moment, and express The arm correction coefficient matrix of the dynamic target A and the reference target B at the moment, and They are The rotation matrix from the carrier coordinate system to the navigation coordinate system of the dynamic target A and the reference target B at the moment, and The arms from the Beidou antenna to the IMU center represent the dynamic target A and the reference target B respectively; therefore, Beidou carrier phase observation residual factor at this moment: in, represents the residual factor of Beidou carrier phase observation value, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the state vector, and They represent the Beidou carrier phase observation value and integer ambiguity respectively, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites, and They are The dynamic target A and the reference target B at the moment The orbital position of BeiDou satellites.
4. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 2.2, the Doppler shift observation residual factor is constructed as follows; The dynamic speed of dynamic target A and reference target B at each moment: in, and Respectively The speed of dynamic target A and reference target B at any moment, and express INS dead reckoning speed of dynamic target A and reference target B at any moment, Represents the angular velocity of the carrier coordinate system of the dynamic target A and the reference target B relative to the Earth-fixed coordinate system, and They are The rotation matrix from the carrier coordinate system to the navigation coordinate system of the dynamic target A and the reference target B at the moment, and The arms from the Beidou antenna to the IMU center represent the dynamic target A and the reference target B respectively; therefore, The residual factor of the Doppler frequency shift observation at time: in, represents the Doppler frequency shift observation residual factor, represents the original Doppler observation value, represents the state vector, and denote the wavelength and Doppler shift observations respectively, and They are The dynamic target A and the reference target B at the moment BeiDou satellite speed, and They are The dynamic target A and the reference target B at the moment Beidou satellite speed.
5. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 2.3, The expression of the IMU pre-integration factor at time is: = In the formula, represents the IMU pre-integration factor, 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 The rotation matrix from the carrier coordinate system to the navigation coordinate system at the moment, represents the acceleration due to gravity, express Time has come The time interval between moments; and Respectively Moment and The position of INS at the moment, and Respectively Moment and The speed of INS at the moment, and Respectively Moment and The attitude quaternion from the carrier coordinate system to the navigation coordinate system at the moment; , and for Time has come IMU pre-integrated observations at time instant.
6. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 2.4, The expression of the objective function based on factor graph optimization at each moment is as follows: In the formula, represents the optimal state estimate, is the state variable to be optimized of the sliding window, where Respectively The state vector at time, is the sliding window size, where State variables at this moment: , , and They are The position, speed and posture at any moment, and for The zero bias of the gyro and acceleration 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 factor, Beidou carrier phase and Doppler frequency shift observation residual factors respectively, Represents the original observation value of Beidou Satellite Navigation System BDS, represents the original Doppler observation value, represents the original observation value of INS, represents the state vector, , and are the corresponding covariance matrices respectively.
7. According to claim 1, a method for step-by-step integrated navigation based on factor graph optimization of Beidou / INS data, Features: In step 2.4, the state variables are solved using the objective function according to the maximum a posteriori probability estimation theory to obtain the optimal state estimate.
8. A step-by-step integrated navigation device based on factor graph optimization of Beidou / INS data. It 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 a step-by-step integrated navigation method based on factor graph optimization of Beidou / INS data as described in any one of claims 1 to 7.
