Navigation method based on inelastic boundary constraint integration and multi-dimensional factor combination

Abstract

The present invention discloses a navigation method based on an inelastic boundary constraint integrated multi-dimensional factor combination. First, environmental magnetic fingerprint data is collected in an offline stage to obtain a geomagnetic fingerprint library; and step length, heading angle, GNSS positioning data and magnetic fingerprint measurement values ​​are collected in real time through pedestrian navigation; step length factor, differential heading angle factor, magnetic fingerprint factor, GNSS factor are calculated and a factor graph model is constructed: a sliding window optimization strategy is adopted to only retain the state nodes of the most recent several moments in the preliminary factor graph model for optimization; the optimized factor graph model is applied to obtain the optimal navigation trajectory. This method improves positioning accuracy and robustness in complex environments. It addresses the problems that traditional PDR systems are susceptible to single-step abnormal estimation, magnetic fingerprint positioning is susceptible to local magnetic interference, and GNSS positioning has poor stability in blocked areas.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, in particular to a navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination. Background Art

[0002] With the growing demand for seamless positioning both indoors and outdoors, pedestrian navigation technology has evolved into a multi-source fusion system centered around pedestrian dead reckoning (PDR), global navigation satellite systems (GNSS), and magnetic fingerprint matching. PDR relies on inertial sensors for autonomous positioning, but its heading drift and step-size estimation errors accumulate over time, rapidly degrading positioning accuracy. GNSS provides a stable position reference in open environments, but is susceptible to signal obstruction and multipath effects indoors or in urban canyons. While magnetic fingerprinting can address gaps in indoor positioning by matching geomagnetic field signatures, it faces the dual challenges of electromagnetic interference sensitivity and high fingerprint database maintenance costs. Existing methods fuse these technologies through Kalman filtering or particle filtering. However, due to the heterogeneous data characteristics of PDR / GNSS / magnetic fingerprinting (such as asynchronous sampling and non-Gaussian noise) and the complex dynamics of the environment, traditional filtering frameworks struggle to adaptively adjust the weights of multiple sources. Furthermore, they generally ignore spatial prior constraints such as building structures, resulting in positioning trajectories that may cross physically infeasible areas such as walls. Furthermore, the coupled effect of magnetic fingerprint matching errors and PDR heading drift can easily cause navigation trajectory divergence.

[0003] In recent years, factor graph optimization models have been gradually introduced into the field of integrated navigation due to their support for multi-source asynchronous data fusion and nonlinear constraint expression. However, existing factor graph-based methods often use simple distance or angle constraints, which are insufficient for modeling complex spatial relationships such as building boundaries. The constraint setting relies too heavily on empirical parameters and lacks physical rigidity. Furthermore, in scenarios involving dynamic interference from magnetic fingerprints (such as magnetic field distortion introduced by temporary metal equipment), existing solutions lack error propagation suppression mechanisms, making positioning results susceptible to local mismatches. These shortcomings limit the robustness and accuracy of navigation in complex indoor and outdoor environments, necessitating a new fusion architecture that integrates strong spatial constraints with dynamic robustness mechanisms. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and propose a navigation method based on inelastic boundary constraints and integrated multi-dimensional factor combinations to improve positioning accuracy and robustness in complex environments. This method addresses the problems of traditional PDR systems being susceptible to single-step anomaly estimation, magnetic fingerprint positioning being susceptible to local magnetic interference, and GNSS positioning having poor stability in obscured areas.

[0005] In order to achieve the above object, the technical solutions specifically adopted by the present invention are as follows:

[0006] A navigation method based on inelastic boundary constraints integrating multi-dimensional factors is proposed, the steps are as follows:

[0007] Step 1: In the offline stage, the experimenters use handheld magnetic dynamometers at preset grid points to collect environmental magnetic fingerprint data strictly according to the northeast celestial coordinate system and establish a three-dimensional magnetic vector map.

[0008] Step 2: First, the experimenters used a smartphone to collect consumer-grade sensor data in the local navigation area, including gyroscopes, magnetometers, accelerometers, and GNSS receivers. The system preprocessed the data, using step detection and stride length estimation to obtain step lengths and timestamps. This information was then used to obtain GNSS measurement data and magnetic fingerprint data for the corresponding timestamps. The system then set initial navigation conditions, including starting position information, initial heading angle, and the associated covariance matrix to describe initial uncertainty.

[0009] The third step: construct the step length factor and the differential heading angle factor, wherein the step length factor and the differential heading angle factor constitute the PDR subsystem (pedestrian dead reckoning). During the navigation process, unlike the current robustness improvement scheme, this scheme introduces boundary constraints based on statistical characteristics. The specific approach is: construct the step length factor and the differential heading angle factor to constrain the upper and lower bounds of the step length prediction and the differential heading angle prediction. Through the noise diffusion characteristics of the maximum a posteriori probability model, the reduction of the uncertainty of the step length prediction and the differential heading angle prediction is diffused to the GNSS factor and the magnetic fingerprint factor to achieve a reduction in the uncertainty of the overall model. Construct an inelastic boundary constraint, and use the hyperbolic tangent function as the activation function to map the residuals of the step length and the differential heading angle in the optimization problem with boundary constraints. The nonlinearity and threshold of the hyperbolic tangent function are used to smooth the over-boundary part and make it return to the credible interval. In this embodiment, the boundaries of the step length and the differential heading angle are set to three times the standard deviation of each.

[0010] Step 4: A magnetic fingerprint factor is constructed by combining the magnetic fingerprint library collected in the first step with the magnetic fingerprint of the navigation path collected in the second step. To further improve the system's adaptability to local magnetic anomalies, the present invention introduces a robust kernel function into the magnetic fingerprint factor processing. The magnetic fingerprint factor is adaptively weighted down or up based on the magnitude of the magnetic fingerprint matching residual.

[0011] Step 5: Construct a GNSS factor based on the GNSS positioning data collected in step 2. Due to local navigation requirements, the longitude and latitude in the WGS-84 coordinate system are rotated to the Earth-centered Earth-fixed (ECEF) coordinate system, and then further rotated to the Northeast Sky coordinate system with a known reference point as the origin. Because GNSS signal quality can drop dramatically or even become unavailable in urban canyons, indoor areas, or obscured environments, this invention also incorporates a robust kernel function into the GNSS factor processing. The GNSS factor adaptively decreases or increases its weight based on the magnitude of the GNSS residual.

[0012] Step 6: The step size factor, differential heading angle factor, magnetic fingerprint factor, and GNSS factor constructed in steps 3, 4, and 5 are integrated into a unified factor graph model framework. A sliding window optimization strategy is used to only retain the state nodes at the most recent moments for optimization to obtain the optimal navigation trajectory.

[0013] Ultimately, through multi-source information fusion, robust processing of abnormal observations and dynamic optimization scheduling, the present invention achieves high-precision and high-robustness continuous navigation capabilities in complex environments.

[0014] The present invention has the following characteristics and beneficial effects:

[0015] The method of the present invention can integrate PDR, magnetic fingerprint and GNSS information for robust navigation, enabling high-precision and robust pedestrian navigation in complex environments. The specific performance is as follows:

[0016] The inelastic boundary constraint mechanism is introduced into the PDR subsystem to break through the gradient error problem caused by the traditional elastic boundary constraint optimization in constrained optimization, and significantly reduce the cumulative error caused by sudden heading changes and abnormal step lengths.

[0017] Traditional magnetic fingerprint methods mostly use single-dimensional matching of magnetic intensity, which has limited accuracy and is easily affected by local disturbances. The present invention adopts a strategy that combines magnetic vector interpolation with directionality to make full use of three-dimensional magnetic field information and improve the discrimination ability and robustness of matching.

[0018] In the fusion stage, existing methods mostly use simple weighted or unconstrained optimization models, which cannot effectively suppress the interference of abnormal observations on the system. The present invention introduces a robust kernel function to process magnetic fingerprints and GNSS residuals, which can automatically reduce the weight of abnormal data and effectively improve the stability and reliability of the fusion results.

[0019] The sliding window optimization strategy is adopted in the fusion optimization stage, which can retain historical state constraints and realize an optimization process that emphasizes both temporal continuity and spatial consistency. Compared with traditional filtering methods, it has stronger trajectory constraint capabilities and scalability.

[0020] In summary, the present invention not only improves navigation accuracy, but also greatly enhances the system's robustness to abnormal conditions and environmental interference. It is particularly suitable for pedestrian navigation scenarios in complex environments such as urban canyons, underground spaces, and inside buildings, which require high continuity and accuracy. It has good engineering practicality and promotion prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 Schematic diagram of the system framework of an embodiment of the present invention.

[0022] Figure 2This is a factor graph model integrating PDR / GNSS / magnetic fingerprint in an embodiment of the present invention.

[0023] Figure 3 This is a field map display diagram applied in an embodiment of the present invention.

[0024] Figure 4 Schematic diagram of navigation trajectory in practical application of an embodiment of the present invention.

[0025] Figure 5 Schematic diagram of performance comparison of embodiments of the present invention. DETAILED DESCRIPTION

[0026] The present invention is described in detail below in conjunction with specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.

[0027] A navigation method based on inelastic boundary constraints integrating multi-dimensional factors, such as Figure 1 As shown, the following steps are included:

[0028] Step 1: Collect environmental magnetic fingerprint data in the offline phase and build a three-dimensional magnetic vector map; and collect step length, heading angle, GNSS positioning data and magnetic fingerprint measurement values ​​in real time through pedestrian navigation;

[0029] Specifically, in this embodiment, in the offline stage, the experimenter collects environmental magnetic fingerprint data in strict accordance with the northeast celestial coordinate system using a handheld magnetic dynamometer at preset grid points to establish a three-dimensional magnetic vector map.

[0030] During the real-time acquisition phase, experimenters used a smartphone to collect consumer-grade sensor data from various sensors in the local navigation area, including gyroscopes, magnetometers, accelerometers, and GNSS receivers. The system preprocessed the data, using step detection and stride length estimation to obtain step lengths and timestamps. This information was then used to obtain GNSS measurement data and magnetic fingerprint data for the corresponding timestamps. The system also set initial navigation conditions, including starting position information, initial heading angle, and the associated covariance matrix to describe initial uncertainty.

[0031] Furthermore, the data preprocessing method is:

[0032] Construct the state of the combined navigation system, and set any time as At this moment, this embodiment uses this setting, and the system state can be expressed as:

[0033] (1)

[0034] in Indicates the system status, Respectively represent the north and east coordinates of the pedestrian in the navigation coordinate system. The update of the system state in the two-dimensional plane can be expressed as the following formula:

[0035] (2)

[0036] in express The pace of time, express The heading angle at the moment. The measured values ​​of step length and heading angle can be calculated from the collected sensor data and are expressed as follows:

[0037] (3)

[0038] (4)

[0039] in represents the initial heading angle, Indicates gyroscope Axis The measured value at the moment, express The range of acceleration during the sub-step, represents the step size coefficient, which is set to 1.131 in this embodiment. represents the bias coefficient, which is set to 0.159 in this embodiment.

[0040] Step 2: Build a factor graph model:

[0041] Construction of step length factor and differential heading angle factor: Based on the collected step length and heading angle and combined with the estimated step length and heading angle, the step length factor and heading angle factor are obtained respectively through boundary constraints.

[0042] Specifically, at the initial moment, the assumed step length estimation value is initially set as the step length measurement value in this embodiment, and the residual between it and the real-time measurement is expressed as:

[0043] (5)

[0044] in Represents the step size residual, that is, the initial moment , Represents the estimated value of the step size at the same time. The differential heading angle residual has the same definition and can be expressed as follows:

[0045] (6)

[0046] (7)

[0047] in Represents the differential heading angle, which is obtained from Time has come The change in heading angle at each moment is determined by represents the differential heading angle residual, Represents the predicted value of the differential heading angle. Then, the inelastic boundary is introduced into the step size factor and the differential heading angle factor in the form of an activation function, which can be expressed as:

[0048] (8)

[0049] (9)

[0050] in Represent the step size factor and differential heading angle factor respectively, In this real-time example, the Euler number is set to 2.718281828459045. They represent the constraint boundaries of the step size factor and the differential heading angle factor, respectively. In this embodiment, they are set to three times the standard deviation of the step size and differential heading angle noise. are the noise covariance matrices of the step size and differential heading angle, respectively.

[0051] Construction of magnetic fingerprint factor: The magnetic fingerprint factor is constructed by combining the magnetic fingerprint data collected in the offline stage and the magnetic fingerprint measurement values ​​collected in real time;

[0052] Specifically, Kriging interpolation is used to predict the magnetic fingerprint information of any position in the magnetic fingerprint database. The Gaussian semivariogram function is selected. , which can be expressed as:

[0053] (10)

[0054] in represents the Euclidean distance between the prediction point and the grid collection point, represents the spatial autocorrelation range of the magnetic field, which is set to 2 in this embodiment. Indicates the fluctuation amplitude of the magnetic field in the area. Select the distance target point from the fingerprint library closest The Kriging equations can be constructed using the following expression:

[0055] (11)

[0056] in Indicates that it is in the magnetic fingerprint library area Dot and The Gaussian semivariogram value corresponding to the Euclidean distance between points, represents the Lagrange multiplier, represents the weight coefficient, and the magnetic measurement function can be expressed as:

[0057] (12)

[0058] in represents the magnetic measurement function, Represents a valid point in the magnetic fingerprint database The magnetic intensity of , thus the magnetic factor after adding the robust kernel function can be expressed as:

[0059] (13)

[0060] in,

[0061] (14)

[0062] (15)

[0063] (16)

[0064] (17)

[0065] in represents the magnetic fingerprint factor, represents the magnetic fingerprint residual, represents the noise covariance matrix of the magnetic fingerprint, represents the magnetic fingerprint vector, They respectively represent the measurement values ​​of the magnetic fingerprint at the corresponding moment in the carrier coordinate system. It should be noted that the carrier coordinate system refers to the coordinate system centered on the pedestrian and is a specific name. represents the magnetic measurement function vector, They represent the magnetic measurement functions of the offline magnetic fingerprint library in each direction under the Northeast Sky navigation coordinate system. The specific logic can be seen Definition, Represents the rotation matrix from the northeast celestial coordinate system to the carrier coordinate system, The predicted value of the heading angle is obtained by accumulating the predicted values ​​of the differential heading angle. The scale parameter representing the control robustness is set to 4 in this embodiment.

[0066] This embodiment

[0067] Construction of GNSS factors: GNSS factors are constructed using GNSS positioning data collected in real time;

[0068] First, the longitude and latitude data of the GNSS positioning data are converted to the ECEF coordinate system, which is represented as follows:

[0069] (18)

[0070] (19)

[0071] in Represents the coordinates in the ECEF coordinate system, Indicates latitude, Indicates longitude, Indicates height, represents the radius of curvature of the Maoyou circle, represents the first eccentricity, which is set to 0.0818191908426 in this embodiment. Indicates the length of the Earth's semi-major axis, which is set to 6378137 in this embodiment. The ECEF coordinates of the reference point are then set to , latitude and longitude are , coordinates in the Northeast Celestial Coordinate System It can be expressed as:

[0072] (20)

[0073] In this embodiment, only two-dimensional positioning is considered. The GNSS factor after adding the robust kernel function is expressed as follows:

[0074] (twenty one)

[0075] (twenty two)

[0076] (twenty three)

[0077] in Indicates the measured coordinates of GNSS in the northeast celestial coordinate system. represents the GNSS factor, represents the noise covariance matrix of GNSS, represents the GNSS residual, The scale parameter representing the control robustness is set to 2 in this embodiment.

[0078] Finally, a preliminary factor graph model is derived based on the maximum a posteriori probability estimation of the factor graph model by combining the step size factor, differential heading angle factor, magnetic fingerprint factor, and GNSS factor.

[0079] Specifically, such as Figure 2 As shown, the transfer equation of the optimal system state can be expressed by the optimal estimated step size and differential heading angle:

[0080] (twenty four)

[0081] (25)

[0082] in represents the optimal system state estimate, represents the optimal heading angle estimate, Indicates the initial heading angle measurement value, which is set to 0 in this embodiment.

[0083] Therefore, the system state can be expressed using the optimal estimated step size residual and differential heading angle residual. The time series length is The navigation sequence factor graph model is represented as follows:

[0084] (26)

[0085] Step 3: Use the sliding window optimization strategy to only retain the state nodes of the most recent moments in the preliminary factor graph model for optimization.

[0086] Specifically, the factor graph model is derived and batch optimization is performed using a sliding window.

[0087] Step 4: Apply the optimized factor graph model to obtain the optimal navigation trajectory.

[0088] Applying the optimized factor graph model, we obtain two complete sequences: and , and then use about The optimal navigation trajectory is obtained by solving the state transition equation.

[0089] like Figure 3 As shown in FIG, this embodiment constructs a geomagnetic fingerprint library in an outdoor square and an indoor hall, and designs a test scenario that includes both outdoor and indoor environments. Figure 4 As shown in the figure, the five latest comparison algorithms are selected for comparison. From the trajectory, it can be seen that the present invention is closest to the real trajectory in outdoor scenes, indoor scenes and indoor and outdoor seamless scenes, and can still maintain stable positioning in areas where other comparison algorithms have deviations. Figure 5 As shown, the test results are quantitatively analyzed using the cumulative error probability distribution diagram. 93% of the positioning errors of the present invention are less than 1 meter, which is better than other current comparison algorithms.

[0090] The above shows and describes the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions are merely preferred examples of the present invention and are not intended to limit the present invention. Various changes and improvements may be made to the present invention without departing from the spirit and scope of the present invention. Such changes and improvements fall within the scope of the present invention. The scope of protection claimed in the present invention is defined by the appended claims and their equivalents.

Claims

1. A navigation method based on inelastic boundary constraints integrating multi-dimensional factors, characterized in that: The steps include: Step 1: Collect environmental magnetic fingerprint data in the offline stage, obtain the geomagnetic fingerprint library, and build a three-dimensional magnetic vector map; and collect step length, heading angle, GNSS positioning data and magnetic fingerprint measurement values ​​in real time through pedestrian navigation; Step 2: Build a factor graph model: According to the collected step length and heading angle and combined with the step length estimation value and the heading angle estimation value, the step length factor and the heading angle factor are obtained respectively through boundary constraints; The magnetic fingerprint factor is constructed by combining the magnetic fingerprint data collected in the offline stage and the magnetic fingerprint measurement values ​​collected in real time; Construct GNSS factors through real-time collected GNSS positioning data; According to the step size factor, differential heading angle factor, magnetic fingerprint factor, and GNSS factor, a preliminary factor graph model is derived based on the maximum a posteriori probability estimation of the factor graph model; Step 3: Use the sliding window optimization strategy to only retain the state nodes of the most recent moments in the preliminary factor graph model for optimization; Step 4: Apply the optimized factor graph model to obtain the optimal navigation trajectory.

2. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 1 is characterized in that: The step length, heading angle, GNSS positioning data and magnetic fingerprint measurement values ​​are collected and obtained by sensors on the mobile terminal.

3. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 1, characterized in that: In step 2, initial conditions are set for the navigation system of the mobile terminal, and the initial conditions include the covariance matrix of the starting position information, the initial heading angle and the step size factor, the differential heading angle factor, the GNSS factor, and the magnetic fingerprint factor.

4. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 1, characterized in that: In step 2, the step size factor is obtained as follows: Set the initial step length estimate and the step length measurement value to be the same, and subtract the step length estimate from the step length measurement value to obtain the step length residual; Construct an inelastic boundary constraint and use the hyperbolic tangent function as the activation function to map the step size residual to obtain the step size factor, which is expressed as follows: ； in, represents the step size factor, represents the step size residual, Represents the estimated value of the step size at the same time; represents the Euler number; represents the constraint bound of the step factor; The noise covariance matrix representing the step size.

5. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 4 is characterized in that: In step 2, the method for obtaining the differential heading angle factor is: According to the measured value of the heading angle, by calculating Time has come The change of the heading angle at each moment is used to obtain the differential heading angle; The initial heading angle estimate and the heading angle measurement are set to be the same, and the differential heading angle prediction value is obtained by applying the same method as the differential heading angle acquisition; Subtract the differential heading angle estimate from the differential heading angle to obtain the differential heading angle residual; An inelastic boundary constraint is constructed and the hyperbolic tangent function is used as the activation function to map the differential heading angle residual to obtain the differential heading angle factor.

6. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 5, characterized in that: In step 2, the method for obtaining the magnetic fingerprint factor is: Select Gaussian semivariogram , expressed as: ； in It represents the Euclidean distance between the predicted point derived from the heading angle estimate and the step length estimate and any grid point of the geomagnetic fingerprint library collected offline. represents the spatial autocorrelation range of the magnetic field, Indicates the fluctuation amplitude of the magnetic field in the region; Select the distance target point from the geomagnetic fingerprint library collected offline closest Effective points are combined with Gaussian semivariogram to construct the Kriging equations, which are expressed as follows: ； in Indicates that it is in the offline collected magnetic fingerprint library area Point and The Gaussian semivariogram value corresponding to the Euclidean distance between points, represents the Lagrange multiplier, represents the weight coefficient, where ; The magnetic measurement function is obtained according to the Kriging equations; The magnetic fingerprint factor is obtained by adding a robust kernel function to the magnetic measurement function. The expression is as follows: ； in, represents the magnetic factor, represents the magnetic fingerprint residual, where represents the scale parameter that controls robustness, Represents the noise covariance matrix of the magnetic fingerprint.

7. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 6, characterized in that: In step 2, the method for obtaining the GNSS factor is: The longitude and latitude data in the GNSS positioning data are converted into the ECEF coordinate system. By setting a reference point on the ECEF coordinate system, the coordinates of the target point in the GNSS positioning data in the Northeast Sky coordinate system are obtained through conversion, i.e., the GNSS measurement coordinates. The GNSS factor is obtained by adding the robust kernel function based on the system state and GNSS measurement coordinates of the target point. The expression is as follows: ； in represents the GNSS residual, represents the noise covariance matrix of GNSS, represents the GNSS factor, represents the scale parameter that controls the robustness.

8. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 7, characterized in that: In step 3, the factor graph model expression is as follows: 。 9. The navigation method based on inelastic boundary constraint integrated multi-dimensional factor combination according to claim 8, characterized in that: In step 3, using the sliding window for batch optimization can obtain two complete sequences: and , according to the sequence and Reuse About The state transition equation is used to obtain the navigation trajectory with the optimal system state estimate.

Images