A multi-constraint fused factor graph integrated navigation method

Abstract

This invention discloses a multi-constraint fusion factor graph integrated navigation method, comprising at least the following steps: S1: configuring initial navigation parameters and factor graph framework, constructing an initial factor graph and prior factors; S2: collecting and preprocessing GNSS observation data, IMU inertial data (1) and wheel odometry data (2), constructing GNSS measurement factors; S3: performing CoreNav fusion (6) on multiple constraints, generating CoreNav multi-constraint edge factors (10); S4: adding prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors (10) to the initial factor graph, constructing a multi-constraint fusion factor graph; S5: performing optimization solution on the multi-constraint fusion factor graph, outputting the optimized navigation state. This invention can fuse multiple constraints and GNSS / INS in the factor graph, improving the accuracy, robustness, and real-time performance of integrated navigation.

Description

Technical Field

[0001] This invention relates to the field of integrated navigation technology, and more specifically to a factor graph integrated navigation method with multi-constraint fusion. Background Technology

[0002] The statements herein are provided merely as background information in connection with this application and do not necessarily constitute prior art.

[0003] Inertial navigation and satellite navigation combined positioning technology is a core technology in fields such as wheeled robots, autonomous vehicles, and intelligent transportation platforms, directly determining the continuity, accuracy, and robustness of vehicle positioning. Current mainstream combined navigation schemes are mostly based on loose or tight combinations of GNSS (Global Navigation Satellite System) and INS (Inertial Navigation System). While this can meet basic positioning needs in open environments, in scenarios where GNSS signals are easily blocked or subject to multipath interference, such as urban canyons, tunnels, forests, and transitions between indoor and outdoor environments, the accuracy of satellite positioning will drop sharply or even fail completely. Pure INS inertial navigation suffers from error accumulation; the zero-bias errors of gyroscopes and accelerometers will continuously diverge, resulting in positioning deviations and failing to meet the requirements for long-term, high-precision navigation. Factor graphs, as an efficient probabilistic graphical model for state estimation, can transform multi-source observations and constraints into factor nodes, achieving global optimal state solutions through nonlinear optimization, and have been gradually applied in the field of combined navigation. However, existing factor graph-based integrated navigation methods rely on a single source of constraints, mostly depending solely on GNSS observations to construct the factor graph. This results in insufficient constraints, a lack of deep fusion of multi-dimensional information from multiple sensors, and high computational complexity and poor real-time performance in continuous navigation scenarios, making them unsuitable for the application requirements of vehicle and robotic platforms. Therefore, a lightweight, multi-constraint fusion integrated navigation method based on factor graphs is urgently needed.

[0004] Prior art document (202210095337.X) discloses a navigation method and apparatus for a GNSS / INS integrated system. The method includes: step S1, constructing zero-velocity detection statistics for the GNSS / INS integrated system; step S2, constructing a factor graph of the GNSS / INS integrated system, including INS nodes and GNSS nodes, wherein the factor graph represents the system state variables of the GNSS / INS integrated system at various measurement times; step S3, determining the vehicle's travel state based on the zero-velocity detection statistics: when the vehicle is stationary, constructing zero-velocity correction factor nodes based on the factor graph, and using the zero-velocity correction factor nodes to locate the vehicle to obtain its position and velocity; when the vehicle is not stationary, locating the vehicle based on the factor graph to obtain its position and velocity. The technical solution in this prior art document adds the vehicle velocity measurement value from the GNSS receiver as a detection quantity. When the vehicle is detected to be at zero velocity, it does not add new factor nodes, does not perform multi-constraint correction, and does not perform model lightweighting, resulting in low accuracy, robustness, and real-time performance. Summary of the Invention

[0005] A brief overview of this application is provided below to offer a basic understanding of certain aspects thereof. It should be understood that this overview is not an exhaustive summary of the application. It is not intended to identify key or essential parts of the application, nor is it intended to limit its scope. Its purpose is merely to present certain concepts in a simplified form as a prelude to the more detailed description that follows.

[0006] This invention addresses the problems of existing integrated navigation technologies, such as low anti-interference capability in complex environments, significant zero drift, poor system robustness, single constraint conditions, large computational load, and poor real-time performance. It provides a factor graph integrated navigation method that integrates multiple constraints and GNSS / INS in the factor graph, thereby improving navigation accuracy, robustness, and real-time performance.

[0007] This invention provides a factor graph fusion navigation method with multiple constraints, comprising at least the following steps: S1: Configure initial navigation parameters and factor graph framework, and construct initial factor graph and prior factors; S2: Collect and preprocess GNSS observation data, IMU (gyroscope and accelerometer) inertial data and wheel odometry data to construct GNSS measurement factors; S3: Perform CoreNav (core navigation) fusion on multiple constraints to generate CoreNav multi-constraint edge factors; S4: Add prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors to the initial factor graph to construct a multi-constraint fusion factor graph; S5: Perform optimization on the multi-constraint fusion factor graph and output the optimized navigation state. An initial factor graph and prior factors are constructed by configuring initial navigation parameters. GNSS measurement factors are constructed by collecting GNSS observation data, IMU inertial data, and wheeled odometer data. Multiple constraints are fused using CoreNav to generate CoreNav multi-constraint edge factors. A multi-constraint fusion factor graph is jointly constructed and optimized, achieving tight fusion of multi-source information and multiple constraints, improving the robustness of navigation in complex scenarios. The optimal navigation state is obtained globally by globally optimizing GNSS observation data, IMU inertial data, and wheeled odometer data within the factor graph framework, improving positioning accuracy. Simultaneously, optimizing the multi-constraint fusion factor graph saves computing power and reduces computation time, enhancing the system's real-time performance.

[0008] The initial navigation parameters include initial position, initial attitude, initial state vector, covariance matrix, factor graph parameters, and initial error state vector.

[0009] The data includes GNSS observation data, IMU inertial data, and wheel odometer data, including GNSS pseudorange, carrier phase measurements, IMU angular velocity and acceleration, and wheel speed encoder data.

[0010] Optimizing navigation status includes the vehicle's position, velocity, attitude, clock deviation, and tropospheric delay.

[0011] The multi-constraint fusion factor graph combined navigation method described in this invention, as a preferred embodiment, includes wheel odometer constraints, zero-speed update constraints, and non-holonomic constraints. By fusing wheel odometer, zero-speed update, and non-holonomic constraints to generate CoreNav multi-constraint edge factors, combined with the factor graph, global optimization of multi-source information can be achieved. This aims to suppress the accumulation of inertial navigation errors and improve the positioning robustness and accuracy in GNSS-constrained scenarios, meeting the high-precision navigation requirements of wheeled vehicles.

[0012] The multi-constraint fusion factor graph combination navigation method of the present invention, as a preferred embodiment, includes the following steps in step S3: S31: Based on IMU inertial data and wheeled odometer data, determine the attitude error, velocity error, position error, and sensor zero bias; S32: Construct an error state vector consisting of attitude error, velocity error, position error, and sensor bias, where the error state vector is defined as: x_err=[δψ_nbᵀ,δv_ebᵀ,δp_bᵀ,b_aᵀ,b_gᵀ]ᵀ Where x_err is the error state vector, δψ_nb is the attitude error, δv_eb is the velocity error, δp_b is the position error, b_a is the accelerometer zero bias, and b_g is the gyroscope zero bias. S33: Based on measurement innovations corresponding to zero-speed update constraints, nonholonomic constraints, and wheel odometer constraints, the error state vector is corrected; S34: Generate CoreNav multi-constraint edge factors based on the corrected error state vector.

[0013] By constructing an error state vector that includes attitude error, velocity error, position error, accelerometer bias, and gyroscope bias, and correcting it based on measurement innovations such as zero-velocity update constraints, nonholonomic constraints, and wheeled odometer constraints, and by utilizing the inherent physical constraints of the wheeled vehicle, the cumulative error of the inertial navigation system can be estimated and compensated, thereby suppressing positioning drift caused by sensor bias. Furthermore, the reliability of relative motion state calculation is improved, which in turn improves the accuracy and confidence of CoreNav multi-constraint edge factors, enhances the global optimality of navigation state estimation, and is applicable to complex scenarios such as GNSS signal obstruction and multipath interference.

[0014] The multi-constraint fusion factor graph combined navigation method of the present invention, as a preferred embodiment, includes the following steps for correcting the error state vector based on the measurement innovation corresponding to the zero-rate update constraint: S321: Detect motion status; S322: When the motion state is stationary, the measurement innovation corresponding to the zero-rate update constraint satisfies the following formula: δz_Z,k=[-v̂_ebᵀ,-ω̂_ibᵀ]ᵀ_k Where δz_Z,k represents the measurement innovation corresponding to the zero-velocity update constraint, δv_eb represents the velocity error, ω_ib represents the angular velocity, and k is the sampling time index, indicating the kth sampling time in the discrete time step, corresponding to the kth iteration in the EKF state update cycle. By detecting the motion state of the carrier, the measurement innovation of the zero-velocity update constraint is used to correct the error state vector when stationary, which can suppress the accumulation of errors in inertial navigation during stationary periods, quickly correct velocity and attitude deviations, provide reliable constraints for the factor graph, and enhance navigation stability.

[0015] Where δz_Z≈H_Z·x_err, H_Z is the measurement matrix of the zero-rate update constraint.

[0016] Among them, the carrier can be judged to be stationary based on the variance of IMU acceleration and gyroscope output and the magnitude of wheel speed; when the variance is less than a preset threshold and the wheel speed is close to zero, the carrier is judged to be stationary.

[0017] Specifically, when the motion state is non-stationary, low-deterministic process noise constraints are added to enable zero-rate update constraints to propagate in the factor graph.

[0018] The factor graph combined navigation method with multi-constraint fusion described in this invention, as a preferred embodiment, innovates in the measurement of nonholonomic constraints by utilizing the constraint that wheeled robots cannot slide laterally, which satisfies the following formula: δz_RC,k=-[0,0,1;0,1,0]·(C_nᵇb·v_eb-ω_ib×L_rb)_k Where δz_RC,k represents the measurement innovation corresponding to the nonholonomic constraint, C_nᵇb is the direction cosine matrix of the vehicle body system, v_eb is the velocity calculated by INS, ω_ib is the angular velocity, ω_ib×L_rb is the lever compensation of the wheeled odometer, and k is the sampling time number, representing the kth sampling time in the discrete time step, corresponding to the kth iteration in the EKF state update cycle. By constructing the measurement innovation using the nonholonomic constraint that the wheeled vehicle cannot slide laterally, and combining the direction cosine matrix transformation with lever compensation, lateral and vertical velocity drift can be suppressed, and navigation errors can be corrected.

[0019] Where δz_RC,k≈H_RC·x_err, H_RC is the measurement matrix of the nonholonomic constraint.

[0020] The multi-constraint fusion factor graph combined navigation method described in this invention, as a preferred embodiment, includes wheel odometer constraints corresponding to measurement innovations such as longitudinal velocity, lateral velocity, vertical velocity, and heading angular velocity satisfying the following formula: δz_O=[v_lon,O-v_lon,i,-v_lat,i,-v_ver,i,ψ̇_nb,O-ψ̇_nb,i·cosθ_nb]ᵀ Where δz_O represents the measurement innovation corresponding to the wheeled odometer constraint, v_lon,O represents the longitudinal velocity measured by the wheeled odometer, v_lon,i represents the longitudinal velocity calculated by the INS, -v_lat,i represents the lateral / vertical velocity measured by the wheeled odometer, -v_ver,i represents the lateral / vertical velocity calculated by the INS, ψ_nb,O represents the heading angle measured by the odometer, ψ_nb,i represents the heading angle calculated by the INS, and cosθ_nb represents the pitch angle constraint.

[0021] By constructing an innovative wheel-based odometer measurement system that integrates speed and heading, the odometer observations and inertial calculations can be compared and corrected in real time to compensate for longitudinal speed and heading deviations. Furthermore, by combining pitch angle constraints, the accuracy of attitude estimation can be improved, thereby enhancing the continuity and reliability of the navigation system.

[0022] Where δz_O≈H_O·x_err, H_O is the measurement matrix constrained by the wheel odometer.

[0023] Among them, based on the measurement innovations corresponding to zero-speed update constraints, nonholonomic constraints, and wheel odometer constraints, the error state vector is corrected to satisfy the following formula: K=P·H^T·(H·P·H^T+R)^(-1); x_err = x_err + K·δz; P = (IK·H)·P; Where K is the Kalman gain, H is the measurement matrix corresponding to δz_Z, δz_RC, and δz_O, x_err is the error state vector, and P is the covariance of x_err.

[0024] The multi-constraint fusion factor graph combined navigation method of this invention, as a preferred embodiment, incorporates prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors into an initial factor graph to construct a multi-constraint fusion factor graph. This construction includes the following steps: substituting CoreNav multi-constraint edge factors as between-factors into the initial factor graph; GNSS measurement factors connecting state nodes and GNSS measurement values; prior factors representing prior knowledge of the initial state; and the multi-constraint fusion factor graph satisfying the following formula: X̂=argmin_X{∑_i||x_0-x_i||²_Σ+∑_j||(x_j-x_{j-1})-Δ||²_Λ+∑_k||z_k-h_k(x_k)||²_Ξ} Where ||x_0-x_{prior}||²_Σ is the cost function of the prior factor, ||(x_j-x_{j-1})-Δ||²_Λ is the cost function of the between-factor, and ∑_k||z_k-h_k(x_k)||²_Ξ is the cost function of the GNSS measurement factor.

[0025] By constructing a unified multi-constraint fusion factor graph using prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors in the form of "between-factor," and achieving global optimization through a cost function, the goal of multi-constraint fusion of initial state constraints, relative motion constraints at adjacent times, and GNSS observation constraints is achieved. This unified optimization of various information sources, including GNSS, IMU, wheeled odometer, zero-velocity update constraints, and incomplete constraints, within the factor graph framework fully integrates multi-source information, achieving tight coupling between INS / wheeled odometer and GNSS multi-source information. It avoids estimation bias caused by single information sources or local constraints, effectively improving the accuracy, consistency, and stability of navigation state estimation, and enhancing the reliability of positioning in complex scenarios.

[0026] The multi-constraint fusion factor graph combination navigation method described in this invention, as a preferred embodiment, uses CoreNav multi-constraint edge factors that satisfy the following formula: ψ_CN:||(x_j-x_{j-1})-Δ_CN||²_Λ_CN Where Δ_CN is the relative displacement estimate output by CoreNav, and Λ_CN is the corresponding covariance matrix.

[0027] By constructing a cost function from the relative displacement estimate output by CoreNav and the corresponding covariance matrix, the constraint confidence of adjacent navigation states can be quantified, providing relative motion constraints for global optimization of the factor graph, improving the rationality of constraints and the consistency of estimation, further suppressing inertial drift, and improving the positioning accuracy and robustness of the method.

[0028] The high-precision real state obtained by CoreNav fusion is obtained by subtracting the nominal state X_nom from the IMU integral using the corrected x_err, which satisfies the following formula: X_real = X_nom − x_err; where X_real is the real state, X_nom is the nominal state obtained by IMU integration, and x_err is the corrected error state vector. That is: X_{CN,k}=X_{nom,k}−x_{err,k}; where X_{CN,k} is the CoreNav output at time k.

[0029] The calculation of Δ_CN satisfies the following formula: Δ_{CN,k}=X_{CN,k}⊖X_{CN,k−1}; ⊖ represents the state difference operation, Δ_CN is the relative displacement estimate output by CoreNav, and Λ_CN is the corresponding covariance matrix.

[0030] Specifically, Δ_CN and Λ_CN are encapsulated into a Between-factor of the factor graph, connecting two adjacent variable nodes X_{k-1} and X_k in the factor graph.

[0031] The multi-constraint fusion factor graph combination navigation method of the present invention, as a preferred embodiment, includes the following steps in step S5: S51: The iSAM2 incremental smoothing and graphing algorithm is used to convert the multi-constraint fusion factor graph into a Bayesian tree structure according to the preset elimination order. S52: Perform incremental updates on the Bayesian tree based on newly arrived measurement data; S53: Implement loop closure detection and correction based on Bayesian tree structure, update the estimated values ​​of all state nodes, and obtain the optimal state estimate; S54: Based on the optimal state estimate, output the optimized navigation state.

[0032] By using the iSAM2 algorithm to transform the factor graph into a Bayesian tree structure and performing incremental updates only on newly added measurements to achieve loop closure correction, the computational load during continuous navigation can be reduced, the computation time required can be shortened, and the real-time performance of navigation can be improved. At the same time, the optimal state estimate is obtained through global state update, and the optimized navigation state is output based on the optimal state estimate, which can ensure the consistency and stability of navigation results and is suitable for scenarios with high real-time requirements.

[0033] This invention also provides a multi-constraint fusion factor graph combined navigation system, which, as a preferred embodiment, includes an initialization module, a parameter acquisition module, a multi-constraint fusion module, a factor graph fusion module, and an optimization solution module, wherein: The initial module configures the initial navigation parameters and factor graph framework, determines the initial factor graph and prior factors, and transmits the initial factor graph and prior factors to the factor graph fusion module. The parameter acquisition module is used to acquire and preprocess GNSS observation data, IMU inertial data and wheeled odometer data, and transmit the GNSS observation data, IMU inertial data and wheeled odometer data to the factor graph fusion module, and transmit the IMU inertial data and wheeled odometer data to the multi-constraint fusion module. The multi-constraint fusion module receives IMU inertial data and wheeled odometer data transmitted from the parameter acquisition module. Based on the IMU inertial data, wheeled odometer data, zero-speed update constraints and nonholonomic constraints, it performs CoreNav fusion to generate CoreNav multi-constraint edge factors, which are then transmitted to the factor graph fusion module. The factor graph fusion module receives CoreNav multi-constraint edge factors transmitted by the multi-constraint fusion module, GNSS observation data, IMU inertial data, and wheeled odometer data collected by the parameter acquisition module, and initial factor graphs and prior factors transmitted by the initialization module. It is used to construct GNSS measurement factors based on GNSS observation data, IMU inertial data, and wheeled odometer data, add prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors to the initial factor graph, construct a multi-constraint fusion factor graph, and transmit the multi-constraint fusion factor graph to the optimization solution module. The optimization solution module receives the multi-constraint fusion factor map transmitted by the factor map fusion module, performs optimization solution on the multi-constraint fusion factor map, and outputs the optimized navigation state.

[0034] The parameter acquisition module includes a GNSS receiver, an IMU (gyroscope and accelerometer), and a wheeled odometer encoder.

[0035] The present invention has the following beneficial effects: (1) This invention integrates multiple information sources such as GNSS, IMU, wheel odometry, zero-speed update constraints and non-holonomic constraints into a unified optimization under the factor graph framework. This fully integrates multi-source information and achieves tight coupling of multi-source information between INS / wheel odometry and GNSS. It avoids estimation bias caused by a single information source or local constraints, effectively improves the accuracy, consistency and stability of navigation state estimation, and enhances the reliability of positioning in complex scenarios. (2) By introducing zero-speed update constraints into the multi-constraint fusion factor graph, the present invention propagates static constraint information to the entire factor graph, which can suppress the accumulation of errors in inertial navigation, quickly correct speed and attitude deviations, provide reliable constraints for the factor graph, and enhance navigation stability. (3) By constructing CoreNav multi-constraint edge factors, this invention achieves the purpose of multi-constraint fusion of initial state constraints, relative motion constraints at adjacent times and GNSS observation constraints, and optimizes various information such as GNSS, IMU, wheeled odometer, zero-speed update constraints and incomplete constraints in a unified way under the factor graph framework. It can fully integrate multi-source information and realize the tight coupling of multi-source information of INS / wheeled odometer and GNSS. (4) By using the iSAM2 algorithm to update the global state, this invention can reduce the amount of computation during continuous navigation, reduce the time required for computation, improve the real-time performance of navigation, and at the same time ensure the consistency and stability of navigation results, making it suitable for scenarios with high real-time requirements. (5) This invention achieves tight fusion of multi-source information and multi-constraint by constructing a multi-constraint fusion factor graph and optimizing the solution, so that it can still maintain high positioning accuracy when GNSS signals are interfered with, and improve the robustness of navigation in complex scenarios.

[0036] These and other advantages of this application will become more apparent from the following detailed description of preferred embodiments in conjunction with the accompanying drawings. Attached Figure Description

[0037] Figure 1 This is a flowchart of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 2 This is a flowchart of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 3 This is a flowchart of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 4 This is a flowchart of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 5 This is an architecture diagram of CoreNav multi-sided fusion factor according to a multi-constraint fusion factor graph combination navigation method based on this application; Figure 6 This is a comparison chart of ENU direction errors for a noisy dataset of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 7 This is a 3DRMSE comparison chart of a noisy dataset for a multi-constraint fusion factor graph combined navigation method according to this application; Figure 8 This is a comparison chart of the maximum 3D error of a noisy dataset for a multi-constraint fusion factor graph combined navigation method according to this application; Figure 9 This is a 2D trajectory comparison diagram of a multi-constraint fusion factor graph combined navigation method according to this application; Figure 10 This is a system architecture diagram of a multi-constraint fusion factor graph combined navigation system according to this application; Figure 11 This is a simulated multipath noise histogram of a multi-constraint fusion factor graph integrated navigation system according to this application.

[0038] It should be noted that the accompanying drawings are not necessarily drawn to scale, but are shown only in a schematic manner without affecting the reader's understanding.

[0039] Explanation of reference numerals in the attached figures: 1. IMU inertial data; 2. Wheel odometry data; 3. Zero-velocity update constraint; 4. Nonholonomic constraint; 5. Detecting motion state; 6. CoreNav fusion; 7. Attitude error; 8. Velocity error; 9. Position error; 10. CoreNav multi-constraint edge factor; 11. Navigation using standard GNSS factor graph; 12. Navigation using multi-constraint fusion factor graph considering only zero-velocity update constraint; 13. Navigation using multi-constraint fusion factor graph; 14. Time; 15. Eastward error; 16. Northward error; 17. Alternating current error; 18. Test dataset; 19. 3D RMSE; 20. Test dataset; 21. Maximum 3D error; 22. DGPS reference trajectory; 23. Starting point; 24. End point; 25. Eastward position; 26. Northward position; 27. Initial module; 28. Parameter acquisition module; 29. ​​Multi-constraint fusion module; 30. Factor graph fusion module; 31. Optimization solution module; 32. Frequency; 33. Pseudorange error; 34. Carrier phase error. Detailed Implementation Example 1

[0040] like Figure 1 As shown, a multi-constraint fusion factor graph combined navigation method includes at least the following steps: S1: Configure initial navigation parameters and factor graph framework, and construct initial factor graph and prior factors; initial navigation parameters include initial position, initial attitude, initial state vector, covariance matrix, factor graph parameters and initial error state vector; S2: Collect and preprocess GNSS observation data, IMU inertial data 1 and wheeled odometer data 2 to construct GNSS measurement factors; GNSS observation data, IMU inertial data 1 and wheeled odometer data 2 include GNSS pseudorange, carrier phase measurement values, IMU angular velocity and acceleration and wheel speed encoder data; S3: Perform CoreNav fusion on multiple constraints 6 to generate CoreNav multi-constraint edge factors 10; the multiple constraints include wheel odometry constraints, zero-speed update constraints 3, and non-holonomic constraints 4; such as Figure 2 As shown, step S3 includes the following steps: S31: Based on IMU inertial data 1 and wheeled odometer data 2, determine attitude error 7, velocity error 8, position error 10, and sensor zero bias; S32: Construct an error state vector consisting of attitude error 7, velocity error 8, position error 10, and sensor zero bias, where the error state vector is defined as: x_err=[δψ_nbᵀ,δv_ebᵀ,δp_bᵀ,b_aᵀ,b_gᵀ]ᵀ Where x_err is the error state vector, δψ_nb is the attitude error 7, δv_eb is the velocity error 8, δp_b is the position error 10, b_a is the accelerometer zero bias, and b_g is the gyroscope zero bias; S33: Based on the measurement innovation corresponding to zero-speed update constraint 3, nonholonomic constraint 4 and wheel odometer constraint, the error state vector is corrected; like Figure 3 As shown, the steps for correcting the error state vector based on the measurement innovation corresponding to zero-rate update constraint 3 include: S321: Detect motion state 5; S322: When the motion state is stationary, the measurement innovation corresponding to zero-velocity update constraint 3 satisfies the following formula: δz_Z,k=[-v̂_ebᵀ,-ω̂_ibᵀ]ᵀ_k Where δz_Z,k is the measurement innovation corresponding to zero-velocity update constraint 3, δv_eb is the velocity error 8, ω_ib is the angular velocity, k is the sampling time number, representing the kth sampling time of the discrete time step, corresponding to the kth iteration in the EKF state update cycle.

[0041] Where δz_Z≈H_Z·x_err, H_Z is the measurement matrix of zero-rate update constraint 3.

[0042] Among them, the carrier can be judged to be stationary based on the variance of IMU acceleration and gyroscope output and the magnitude of wheel speed; when the variance is less than a preset threshold and the wheel speed is close to zero, the carrier is judged to be stationary.

[0043] When the motion state is non-stationary, a low-deterministic process noise constraint is added to enable the zero-rate update constraint 3 to propagate in the factor graph.

[0044] The measurement innovation corresponding to nonholonomic constraint 4 is to utilize the constraint that wheeled robots cannot slide laterally, satisfying the following formula: δz_RC,k=-[0,0,1;0,1,0]·(C_nᵇb·v_eb-ω_ib×L_rb)_k Where δz_RC,k is the measurement innovation corresponding to nonholonomic constraint 4, C_nᵇb is the direction cosine matrix of the vehicle body system, v_eb is the velocity calculated by INS, ω_ib is the angular velocity, ω_ib×L_rb is the lever arm compensation of the wheel odometer, and k is the sampling time number, representing the kth sampling time of the discrete time step, corresponding to the kth iteration in the EKF state update cycle.

[0045] Where δz_RC≈H_RC·x_err, H_RC is the measurement matrix of nonholonomic constraint 4.

[0046] The measurement innovations corresponding to the constraints of wheeled odometers include longitudinal velocity, lateral velocity, vertical velocity, and yaw angular velocity satisfying the following formulas: δz_O=[v_lon,O-v_lon,i,-v_lat,i,-v_ver,i,ψ̇_nb,O-ψ̇_nb,i·cosθ_nb]ᵀ Where δz_O represents the measurement innovation corresponding to the wheeled odometer constraint, v_lon,O represents the longitudinal velocity measured by the wheeled odometer, v_lon,i represents the longitudinal velocity calculated by the INS, -v_lat,i represents the lateral / vertical velocity measured by the wheeled odometer, -v_ver,i represents the lateral / vertical velocity calculated by the INS, ψ_nb,O represents the heading angle measured by the odometer, ψ_nb,i represents the heading angle calculated by the INS, and cosθ_nb represents the pitch angle constraint.

[0047] Where δz_O≈H_O·x_err, H_O is the measurement matrix constrained by the wheel odometer.

[0048] S34: Generate CoreNav multi-constraint edge factor 10 based on the corrected error state vector; Among them, based on the measurement innovations corresponding to zero-speed update constraint 3, nonholonomic constraint 4, and wheel odometer constraint, the error state vector is corrected to satisfy the following formula: K = P·H^T·(H·P·H^T+R)^(-1) x_err=x_err+K·δz P=(IK·H)·P Where K is the Kalman gain, H is the measurement matrix corresponding to δz_Z, δz_RC, and δz_O, x_err is the error state vector, and P is the covariance of x_err.

[0049] S4: Add the prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factor 10 to the initial factor graph to construct a multi-constraint fusion factor graph; The steps to construct a multi-constraint fusion factor graph by adding prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors 10 to the initial factor graph include: Substituting the CoreNav multi-constraint edge factor 10 as the Between-factor into the initial factor graph, the GNSS measurement factor connects the state nodes and the GNSS measurements, and the prior factor represents prior knowledge of the initial state. The multi-constraint fusion factor graph satisfies the following formula: X̂=argmin_X{∑_i||x_0-x_i||²_Σ+∑_j||(x_j-x_{j-1})-Δ||²_Λ+∑_k||z_k-h_k(x_k)||²_Ξ} Where ||x_0-x_{prior}||²_Σ is the cost function of the prior factor, ||(x_j-x_{j-1})-Δ||²_Λ is the cost function of the between-factor, and ∑_k||z_k-h_k(x_k)||²_Ξ is the cost function of the GNSS measurement factor.

[0050] CoreNav multi-constraint edge factor 10 satisfies the following formula: ψ_CN:||(x_j-x_{j-1})-Δ_CN||²_Λ_CN Where Δ_CN is the relative displacement estimate output by CoreNav, and Λ_CN is the corresponding covariance matrix.

[0051] The high-precision true state after CoreNav fusion is obtained by subtracting the nominal state X_nom of the IMU integral from the corrected x_err, satisfying the following formula: X_real=X_nom−x_err Where X_real is the real state, X_nom is the nominal state obtained by IMU integration, and x_err is the corrected error state vector; Right now: X_{CN,k}=X_{nom,k}−x_{err,k} Where X_{CN,k} is the CoreNav output at time k.

[0052] The calculation of Δ_CN satisfies the following formula: Δ_{CN,k}=X_{CN,k}⊖X_{CN,k−1} ⊖ represents the state difference operation, Δ_CN is the relative displacement estimate output by CoreNav, and Λ_CN is the corresponding covariance matrix.

[0053] Specifically, Δ_CN and Λ_CN are encapsulated into a Between-factor of the factor graph, connecting two adjacent variable nodes X_{k-1} and X_k in the factor graph.

[0054] S5: Perform optimization solution on the multi-constraint fusion factor graph and output the optimized navigation state; the optimized navigation state includes the vehicle's position, velocity, attitude, clock deviation and tropospheric delay; like Figure 4 As shown, step S5 includes the following steps: S51: The iSAM2 incremental smoothing and graphing algorithm is used to convert the multi-constraint fusion factor graph into a Bayesian tree structure according to the preset elimination order. S52: Perform incremental updates on the Bayesian tree based on newly arrived measurement data; S53: Implement loop closure detection and correction based on Bayesian tree structure, update the estimated values ​​of all state nodes, and obtain the optimal state estimate; S54: Output the optimized navigation state based on the optimal state estimate. Example 2

[0055] The following example, using a test conducted by the inventor in the field with a wheeled robot as the carrier, will provide a more specific and detailed supplement to one or more embodiments mentioned above.

[0056] The inventors conducted three sets of field tests: Test 1, Test 2, and Test 3. The tests used a four-wheeled robot platform equipped with a GNSS receiver, an ADIS-16495 IMU, and a wheel encoder. The reference trajectory was provided by carrier phase differential GPS (DGPS). Additionally, to simulate an urban canyon environment, 2% multipath noise was added to the data.

[0057] The experimental site was an open off-road environment with undulating terrain, sparse vegetation, and no tall buildings to block the view. The test platform was a four-wheel differential steering robot equipped with Novatel GNSS, ADIS-16495 IMU and wheel speed encoder.

[0058] The working conditions for the three sets of field tests (Test 1, Test 2, and Test 3) are as follows: Test 1: The driving distance was 671m, the number of stops was 9, and the route was mainly straight with a few gentle turns; Test 2: The driving distance was 652m, the number of stops was 19, and the route included multiple sharp turns and was highly dynamic. Test 3: The driving distance was 663m, and the number of stops was 20. The route was relatively flat, but the number of stops was the highest. The multipath noise is a discriminator model based on satellite elevation angle, which is randomly applied to about 2% of the data points.

[0059] like Figure 11 As shown, the statistical distribution of the added multipath noise exhibits obvious heavy-tailed characteristics, with the pseudorange multipath error on the order of meters and the carrier phase multipath error on the order of centimeters.

[0060] like Figure 5 As shown, CoreNav multi-constraint edge factor 10 is constructed in the multi-constraint fusion factor graph.

[0061] Construct standard GNSS factor maps, multi-constraint fusion factor maps considering only zero-rate update constraints, and multi-constraint fusion factor maps respectively.

[0062] like Figure 6 As shown, in a multipath noise environment, large error spikes appear when using standard GNSS factor map navigation, while using multi-constraint fusion factor map navigation that only considers zero-rate update constraints or using multi-constraint fusion factor map navigation can effectively suppress these abnormal errors and show stronger robustness.

[0063] like Figure 7 As shown, when multipath noise is present, the comparison of 3D RMSE on the noise dataset shows that navigation using a multi-constraint fusion factor graph performs best, especially in Test 2, where the 3D RMSE is only 1.16m, which is significantly better than 1.90m when using a standard GNSS factor graph navigation and 1.52m when using a multi-constraint fusion factor graph navigation that only considers zero-rate update constraints.

[0064] like Figure 8 As shown, the comparison of the maximum 3D errors in the noisy dataset demonstrates a significant advantage in suppressing anomalous errors when using a multi-constraint fusion factor map. In Test 1, the maximum error when navigating using the standard GNSS factor map reached 67.81m, while the error using the multi-constraint fusion factor map was only 7.29m, a reduction of approximately 89%.

[0065] like Figure 9 As shown in the 2D trajectory comparison in Test 1, the standard GNSS factor map navigation showed insufficient accuracy in dynamic scenarios, with significant trajectory deviations occurring during sharp turns at 200-300m eastward and 300-350m northward. However, when using the multi-constraint fusion factor map navigation, the positioning trajectory was almost indistinguishable from the black reference trajectory to the naked eye throughout the entire process. This demonstrates that the method of this invention, under conditions such as straight lines and sharp turns, achieves a high degree of consistency between the positioning trajectory and the actual driving trajectory when using multi-constraint fusion factor map navigation, with no significant drift. Example 3

[0066] like Figure 10 As shown, a multi-constraint fusion factor graph integrated navigation system includes an initialization module 27, a parameter acquisition module 28, a multi-constraint fusion module 29, a factor graph fusion module 30, and an optimization solution module 31. The initialization module 27 configures initial navigation parameters and a factor graph framework, determines the initial factor graph and prior factors, and transmits the initial factor graph and prior factors to the factor graph fusion module 30. The parameter acquisition module 28 acquires and preprocesses GNSS observation data, IMU inertial data, and wheeled odometer data, and transmits these data to the factor graph fusion module 30. It also transmits the IMU inertial data and wheeled odometer data to the multi-constraint fusion module 29. The multi-constraint fusion module 29 receives the IMU inertial data and wheeled odometer data transmitted from the parameter acquisition module 28, and performs optimization based on the IMU inertial data, wheeled odometer data, zero-velocity update constraints, and nonholonomic constraints. The eNav fusion module generates CoreNav multi-constraint edge factors, which are then transmitted to the factor graph fusion module 30. The factor graph fusion module 30 receives the CoreNav multi-constraint edge factors transmitted by the multi-constraint fusion module 29, GNSS observation data, IMU inertial data, and wheel odometry data collected by the parameter acquisition module 28, and the initial factor graph and prior factors transmitted by the initialization module 27. It constructs GNSS measurement factors based on the GNSS observation data, IMU inertial data, and wheel odometry data, adds the prior factors, GNSS measurement factors, and CoreNav multi-constraint edge factors to the initial factor graph, constructs a multi-constraint fusion factor graph, and transmits the multi-constraint fusion factor graph to the optimization solution module 31. The optimization solution module 31 receives the multi-constraint fusion factor graph transmitted by the factor graph fusion module 30, performs optimization on the multi-constraint fusion factor graph, and outputs the optimized navigation state.

[0067] The parameter acquisition module 28 includes a GNSS receiver, an IMU (gyroscope and accelerometer), and a wheel odometer encoder.

[0068] When using a multi-constraint fusion factor map, under multipath interference, the pseudorange error is on the order of meters and the carrier phase error is on the order of centimeters, effectively offsetting the impact of GNSS multipath errors and significantly improving the system's positioning robustness and accuracy in complex scenarios.

[0069] Regarding the embodiments of this application, it should also be noted that, without conflict, the embodiments of this application and the features in the embodiments can be combined with each other to obtain new embodiments.

[0070] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. The scope of protection of this application shall be determined by the scope of the claims.

Claims

1. A multi-constrained fused factor graph integrated navigation method, characterized in that, At least the following steps are included: S1: Configure initial navigation parameters and factor graph framework, and construct initial factor graph and prior factors; S2: Collect and preprocess GNSS observation data, IMU inertial data (1) and wheeled odometer data (2) to construct GNSS measurement factors; S3: Perform CoreNav fusion on multiple constraints (6) to generate CoreNav multi-constraint edge factors (10). S4: Add the prior factors, the GNSS measurement factors, and the CoreNav multi-constraint edge factors (10) to the initial factor graph to construct a multi-constraint fusion factor graph; S5: Perform optimization on the multi-constraint fusion factor graph and output the optimized navigation state.

2. The multi-constrained fused factor graph integrated navigation method according to claim 1, wherein, The multiple constraints include wheel odometer constraints, zero-speed update constraints (3), and non-holonomic constraints (4).

3. The multi-constrained fused factor graph integrated navigation method according to claim 2, wherein, Step S3 includes the following steps: S31: Based on the IMU inertial data (1) and the wheel odometer data (2), determine the attitude error (7), velocity error (8), position error (10), and sensor zero bias; S32: Construct an error state vector consisting of the attitude error (7), the velocity error (8), the position error (10), and the sensor zero bias, wherein the error state vector is: x_err=[δψ_nbᵀ,δv_ebᵀ,δp_bᵀ,b_aᵀ,b_gᵀ]ᵀ; Where x_err is the error state vector, δψ_nb is the attitude error (7), δv_eb is the velocity error (8), δp_b is the position error (10), b_a is the accelerometer zero bias, and b_g is the gyroscope zero bias; S33: Based on the measurement innovation corresponding to the zero-speed update constraint (3), the non-holonomic constraint (4) and the wheel odometer constraint, the error state vector is corrected; S34: Generate the CoreNav multi-constraint edge factor (10) based on the corrected error state vector.

4. The multi-constrained fused factor graph integrated navigation method according to claim 3, wherein, The steps for correcting the error state vector based on the measurement innovation corresponding to the zero-rate update constraint (3) include: S321: Detect motion state (5); S322: When the motion state is a stationary state, the measurement innovation corresponding to the zero-velocity update constraint (3) is: δz_Z,k=[-v̂_ebᵀ,-ω̂_ibᵀ]ᵀ_k; Where δz_Z,k is the measurement innovation corresponding to the zero-speed update constraint (3), δv_eb is the velocity error (8), ω_ib is the angular velocity, and k is the sampling time number, representing the kth sampling time of the discrete time step.

5. The multi-constrained fused factor graph integrated navigation method according to claim 3, wherein, The measurement innovation corresponding to the nonholonomic constraint (4) is to utilize the constraint that wheeled robots cannot slide laterally to satisfy the following formula: δz_RC,k=-[0,0,1;0,1,0]·(C_nᵇb·v_eb-ω_ib×L_rb)_k; Wherein, δz_RC,k is the measurement innovation corresponding to the nonholonomic constraint (4), C_nᵇb is the direction cosine matrix of the vehicle body system, v_eb is the velocity calculated by INS, ω_ib is the angular velocity, ω_ib×L_rb is the lever arm compensation of the wheel odometer, and k is the sampling time number, representing the kth sampling time of the discrete time step.

6. The multi-constrained fused factor graph integrated navigation method according to claim 3, wherein, The measurement innovations corresponding to the wheeled odometer constraints include longitudinal velocity, lateral velocity, vertical velocity, and yaw angular velocity, which satisfy the following formula: δz_O=[v_lon,O-v_lon,i,-v_lat,i,-v_ver,i,ψ̇_nb,O-ψ̇_nb,i·cosθ_nb]ᵀ; Wherein, δz_O is the measurement innovation corresponding to the wheel odometer constraint, v_lon,O is the longitudinal velocity measured by the wheel odometer, v_lon,i is the longitudinal velocity calculated by the INS, -v_lat,i is the lateral / vertical velocity measured by the wheel odometer, -v_ver,i is the lateral / vertical velocity calculated by the INS, ψ_nb,O is the heading angle measured by the odometer, ψ_nb,i is the heading angle calculated by the INS, and cosθ_nb is the pitch angle constraint.

7. The multi-constrained fused factor graph integrated navigation method according to claim 3, wherein, The process of constructing a multi-constraint fusion factor graph by adding the prior factors, the GNSS measurement factors, and the CoreNav multi-constraint edge factors (10) to the initial factor graph includes the following steps: The CoreNav multi-constraint edge factor (10) is substituted into the initial factor graph as a Between-factor. The GNSS measurement factor connects the state node and the GNSS measurement value. The prior factor represents prior knowledge of the initial state. The multi-constraint fusion factor graph satisfies the following formula: X̂=argmin_X{∑_i||x_0-x_i||²_Σ+∑_j||(x_j-x_{j-1})-Δ||²_Λ+∑_k||z_k-h_k(x_k)||²_Ξ}; Where, ||x_0-x_{prior}||²_Σ is the cost function of the prior factor, ||(x_j-x_{j-1})-Δ||²_Λ is the cost function of the Between-factor, and ∑_k||z_k-h_k(x_k)||²_Ξ is the cost function of the GNSS measurement factor.

8. The multi-constrained fused factor graph integrated navigation method according to claim 7, wherein, The CoreNav multi-constraint edge factor (10) satisfies the following formula: ψ_CN:||(x_j-x_{j-1})-Δ_CN||²_Λ_CN; Where Δ_CN is the relative displacement estimate output by CoreNav, and Λ_CN is the corresponding covariance matrix.

9. The multi-constrained fused factor graph integrated navigation method of claim 1, wherein, Step S5 includes the following steps: S51: Using the iSAM2 incremental smoothing and graphing algorithm, the multi-constraint fusion factor graph is converted into a Bayesian tree structure according to a preset elimination order; S52: Perform incremental updates on the Bayesian tree based on newly arrived measurement data; S53: Based on the Bayesian tree structure, loop closure detection and correction are implemented, the estimated values ​​of all state nodes are updated, and the optimal state estimate is obtained; S54: Output the optimized navigation state based on the optimal state estimate.

10. A factor graph fusion navigation system with multiple constraints, characterized in that, It includes an initialization module (27), a parameter acquisition module (28), a multi-constraint fusion module (29), a factor graph fusion module (30), and an optimization solution module (31), wherein: The initial module (27) is used to configure initial navigation parameters and factor graph framework, determine initial factor graph and prior factors, and transmit the initial factor graph and the prior factors to the factor graph fusion module (30). The parameter acquisition module (28) is used to acquire and preprocess GNSS observation data, IMU inertial data and wheeled odometer data, and transmit the GNSS observation data, the IMU inertial data and the wheeled odometer data to the factor graph fusion module (30), which is used to transmit the IMU inertial data and the wheeled odometer data to the multi-constraint fusion module (29). The multi-constraint fusion module (29) is used to receive the IMU inertial data and the wheel odometry data transmitted by the parameter acquisition module (28), perform CoreNav fusion based on the IMU inertial data, the wheel odometry data, zero-speed update constraints and non-holonomic constraints, generate CoreNav multi-constraint edge factors, and transmit the CoreNav multi-constraint edge factors to the factor graph fusion module (30). The factor graph fusion module (30) is used to receive the CoreNav multi-constraint edge factors transmitted by the multi-constraint fusion module (29), receive the GNSS observation data, the IMU inertial data and the wheel odometry data collected by the parameter acquisition module (28), and receive the initial factor graph and the prior factors transmitted by the initial module (27). It is used to construct GNSS measurement factors based on the GNSS observation data, the IMU inertial data and the wheel odometry data, and to add the prior factors, the GNSS measurement factors and the CoreNav multi-constraint edge factors to the initial factor graph to construct a multi-constraint fusion factor graph, and to transmit the multi-constraint fusion factor graph to the optimization solution module (31). The optimization solution module (31) is used to receive the multi-constraint fusion factor graph transmitted by the factor graph fusion module (30), perform optimization solution on the multi-constraint fusion factor graph, and output the optimized navigation state.

Images