A PDR / GNSS pedestrian fusion positioning method based on adaptive robust factor graph optimization
By using an adaptive robust factor graph optimization method, a PDR/GNSS fusion positioning model was constructed, which solved the problems of positioning accuracy and stability in complex environments and achieved high-precision and robust pedestrian positioning.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- LIAONING TECHNICAL UNIVERSITY
- Filing Date
- 2026-03-18
- Publication Date
- 2026-07-14
AI Technical Summary
Existing PDR/GNSS fusion positioning methods struggle to balance positioning accuracy and stability in complex environments. In particular, GNSS signals are easily affected in environments such as urban canyons, semi-indoor spaces, or areas with tree obstruction, leading to accumulated positioning errors and unstable positioning.
An adaptive robust factor graph optimization method is adopted. By constructing a PDR state transition factor, a GNSS observation factor, and a trajectory smoothing factor, and combining them with an iterative reweighted least squares method, an adaptive robust fusion of GNSS observations is achieved, which suppresses the influence of abnormal observations and improves trajectory smoothness.
It significantly reduces positioning errors in complex environments, improves positioning accuracy and stability, is suitable for low-cost smart terminals, and has engineering feasibility and application promotion value.
Smart Images
Figure CN122386342A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of integrated navigation and pedestrian positioning technology, and in particular to a PDR / GNSS pedestrian fusion positioning method and apparatus based on adaptive robust factor graph optimization. Background Technology
[0002] With the widespread adoption of smartphones and wearable devices, pedestrian positioning technology based on low-cost inertial sensors and satellite navigation has gained significant attention in applications such as smart cities, public safety, emergency rescue, and location services. Global Navigation Satellite System (GNSS) provides absolute position information and is currently the primary technology for outdoor pedestrian positioning. However, in urban canyons, semi-indoor environments, densely built-up areas, or environments with tree cover, GNSS signals are susceptible to non-line-of-sight propagation, multipath effects, and obstruction, leading to significantly increased measurement errors, and even positioning jumps or short-term loss of lock, making it difficult to guarantee the continuity and stability of positioning results.
[0003] Pedestrian Dead Reckoning (PDR) utilizes acceleration and angular velocity information collected by inertial measurement units (IMUs) to update relative pose through gait detection, step length estimation, and heading angle calculation. It does not rely on external infrastructure and can provide continuous position estimation even when GNSS signals degrade or are interrupted. However, due to issues such as zero bias, noise, and gait differences in inertial sensors, PDR errors accumulate over time, leading to significant trajectory drift during long-term operation.
[0004] To leverage the complementary advantages of GNSS and PDR, existing technologies typically employ Kalman filtering and its extended forms (such as EKF and UKF) to achieve PDR / GNSS fusion positioning. While these methods can suppress PDR error accumulation to some extent, they are generally based on the assumption of fixed noise, making it difficult to accurately characterize the non-Gaussian nature of GNSS observation errors in complex urban environments. When there are anomalous measurements or continuous offsets in GNSS observations, the filtering results are easily and significantly affected, leading to a decrease in the accuracy and stability of fusion positioning.
[0005] In recent years, factor graph optimization methods have been introduced into the field of navigation and positioning. By constructing global constraint relationships between state variables and multi-source observations, the positioning problem is transformed into a nonlinear least squares optimization problem. This method can utilize historical observation information for global consistency optimization and has certain advantages in suppressing random noise and improving trajectory smoothness. However, existing factor graph-based PDR / GNSS fusion methods mostly employ fixed noise models or simple robust strategies, which still have limited adaptability to dynamic changes in GNSS observation quality under complex environments, making it difficult to simultaneously achieve both positioning accuracy and system stability under conditions of strong obstruction and multipath propagation.
[0006] Therefore, there is an urgent need for a PDR / GNSS fusion positioning method that can introduce adaptive and robust mechanisms within the factor graph framework to improve the positioning accuracy, stability, and robustness of pedestrians in complex environments. Summary of the Invention
[0007] The main objective of this invention is to provide a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization.
[0008] Another objective of this invention is to propose a PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization.
[0009] The third objective of this invention is to provide an electronic device.
[0010] The fourth objective of this invention is to provide a non-transitory computer-readable storage medium.
[0011] To achieve the above objectives, a first aspect of the present invention proposes a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization, comprising:
[0012] S1 collects acceleration data, angular velocity data, and absolute position information output by the inertial measurement unit in the pedestrian's mobile terminal, as well as the GNSS model output, and defines a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system. S2, based on the collected acceleration and angular velocity data, through signal processing, gait recognition, step frequency and step length calculation and heading angle integral calculation, the pedestrian state vector is recursively updated to obtain the displacement increment characterizing the relative motion relationship of the pedestrian; S3. The pedestrian state vector is used as the state node. The PDR state transition factor is constructed based on the relative displacement increment. The GNSS observation factor is constructed based on the GNSS absolute position information. At the same time, an adaptive robust mechanism is embedded in the GNSS observation factor and a trajectory smoothing factor is introduced to complete the construction of the factor graph model. S4 establishes a nonlinear least squares optimization problem based on the factor graph model. Iterative reweighted least squares method is used to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, the final pedestrian fusion positioning result is output, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
[0013] Optionally, based on the collected acceleration and angular velocity data, the pedestrian state vector is recursively updated through signal processing, gait recognition, stride frequency and stride length calculation, and heading angle integral calculation to obtain the displacement increment characterizing the relative motion relationship of the pedestrian, including: Vector synthesis is performed on the triaxial acceleration components to obtain a resultant acceleration sequence that eliminates the influence of sensor attitude, and low-pass filtering is applied to the resultant acceleration sequence. The peak detection algorithm identifies gait peaks that meet the amplitude threshold condition, determines the start and end times of each step to complete gait event detection, and calculates the pedestrian's real-time step frequency based on the time interval between adjacent gait peaks. A step size estimation model is constructed based on the maximum and minimum values of the resultant acceleration within the gait cycle, and a step frequency compensation mechanism is introduced to dynamically correct the step size model parameters. The angular velocity of the vertical axis of the gyroscope is integrated to obtain the change in the pedestrian's heading angle. The pedestrian's planar position is then recursively updated based on the step size and heading angle, and the relative displacement increment of the PDR is output.
[0014] Optionally, the step of constructing the PDR state transition factor based on the relative displacement increment further includes: Using the pedestrian state nodes at adjacent time points as constraints, the relative displacement increments obtained from PDR calculations are used as state transition observation inputs; An anisotropic noise model is introduced to characterize the statistical characteristics of the forward and lateral errors of pedestrian motion, respectively. By using a rotation matrix formed by the heading angle, coordinate transformation is performed on the forward and lateral error covariances to construct an error covariance matrix that matches the pedestrian's motion characteristics. PDR state transition constraints are established based on the anisotropic error covariance matrix, forming a relative motion constraint relationship between adjacent state nodes.
[0015] Optionally, the step of constructing GNSS observation factors based on GNSS absolute position information further includes: The absolute position information output by GNSS is used as the observation value, and observation constraints are established with the pedestrian state node at the corresponding time. Construct the residual term between GNSS position observations and state node estimated positions to form the GNSS observation residual; The GNSS observation residuals are incorporated as a constraint term into the factor graph model to suppress the cumulative error divergence of PDR positioning.
[0016] Optionally, embedding the adaptive robust mechanism in the GNSS observation factors further includes: Dynamically calculate observation weights based on the magnitude of GNSS observation residuals; A robust kernel function is used to reweight abnormal GNSS observations with large residuals, thereby achieving automatic weight reduction of abnormal observations; The GNSS observation covariance matrix and information matrix are adaptively adjusted based on the weighting results, so that the high-confidence GNSS observations form a strong constraint on the fusion results, and abnormal GNSS observations are effectively suppressed.
[0017] Optionally, the introduction of the trajectory smoothing factor further includes: Using multiple consecutive adjacent state nodes as constraint objects, construct second-order difference constraint terms for state variables; By imposing constraints on higher-order changes in state nodes, local high-frequency noise in the trajectory is suppressed, and the geometric consistency and smoothness of pedestrian movement trajectories are constrained, thus forming a complete factor graph model.
[0018] Optionally, the iterative optimization solution using the iterative reweighted least squares method further includes: The PDR state transition factor, adaptive robust GNSS observation factor, and trajectory smoothing factor are uniformly constructed into a nonlinear least squares optimization problem. The current state estimate is linearized by a first-order Taylor expansion, an incremental optimization equation is constructed, and the information matrix and coefficient matrix of each factor are updated according to the adaptive robust weight. Iteratively solve for the state increment and update the global state sequence successively; The iteration terminates when the state increment is less than the preset convergence threshold, and the optimized pedestrian plane position and heading angle are output as the final fusion positioning result.
[0019] To achieve the above objectives, a second aspect of the present invention provides a PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization, comprising: The data acquisition module is used to collect acceleration data, angular velocity data, and absolute position information output by the inertial measurement unit in the pedestrian's mobile terminal, and to define a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system. The displacement calculation module is used to recursively update the pedestrian state vector based on the collected acceleration and angular velocity data through signal processing, gait recognition, step frequency and step length calculation, and heading angle integral calculation, so as to obtain the displacement increment that represents the relative motion relationship of the pedestrian. The graph model construction module is used to take the pedestrian state vector as the state node, construct the PDR state transition factor based on the relative displacement increment, construct the GNSS observation factor based on the GNSS absolute position information, and embed an adaptive robust mechanism and introduce a trajectory smoothing factor into the GNSS observation factor to complete the construction of the factor graph model. The optimization solution module is used to establish a nonlinear least squares optimization problem based on the factor graph model. It uses the iterative reweighted least squares method to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, it outputs the final pedestrian fusion positioning result, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
[0020] Regarding the apparatus in the above embodiments, the specific manner in which each module performs its operation has been described in detail in the embodiments related to the method, and will not be elaborated upon here.
[0021] To achieve the above objectives, a third aspect of this application provides an electronic device, including a processor and a memory; wherein the processor reads executable program code stored in the memory to run a program corresponding to the executable program code, for implementing a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization as described in the first aspect embodiment.
[0022] To achieve the above objectives, the fourth aspect of this application proposes a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization as described in the first aspect embodiment.
[0023] The embodiments of the present invention have the following beneficial effects: 1. It can effectively suppress the impact of abnormal GNSS observations on fusion positioning results under complex environments, and significantly reduce the peak positioning error; 2. Compared with the traditional Kalman filter fusion method, it improves the overall accuracy and time stability of pedestrian localization results; 3. In scenarios with GNSS signal degradation or fluctuation, it can still maintain consistent trajectory geometry and reduce continuous offset phenomena; 4. It is suitable for low-cost smart terminals and has strong engineering feasibility and application promotion value. Attached Figure Description
[0024] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein: Figure 1A flowchart of a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization provided for embodiments of the present invention; Figure 2 This is a schematic diagram of accelerometer processing for gait detection provided in an embodiment of the present invention; Figure 3 A schematic diagram illustrating the pedestrian dead reckoning principle provided in this embodiment of the invention; Figure 4 A schematic diagram of ARFGO-PDR / GNSS integrated navigation provided in an embodiment of the present invention; Figure 5 This is a diagram of runway experimental horizontal position error-time provided in an embodiment of the present invention. Figure 6 The experimental horizontal position error-time diagram of the dormitory building provided in this embodiment of the invention; Figure 7 This is a structural diagram of a PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization, provided in an embodiment of the present invention. Detailed Implementation
[0025] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.
[0026] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.
[0027] The following description, with reference to the accompanying drawings, describes a PDR / GNSS pedestrian fusion localization method and apparatus based on adaptive robust factor graph optimization, according to an embodiment of the present invention.
[0028] Example 1 This invention provides a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization. Figure 1 This is a flowchart illustrating a PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization, provided in an embodiment of the present invention. Figure 1 As shown, the method includes the following steps: Step S1: Collect acceleration data, angular velocity data, and absolute position information output by the inertial measurement unit in the pedestrian's mobile terminal, as well as the GNSS model output. Define a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system.
[0029] In this embodiment, acceleration and angular velocity data output by the inertial measurement unit (IMU) of a pedestrian's mobile terminal are first collected and acquired. Simultaneously, the pedestrian's absolute position information calculated by the GNSS module is received. The inertial sensor data and satellite navigation positioning data are used together as the raw input information for this PDR / GNSS integrated navigation system, providing reliable and comprehensive data support for subsequent dead reckoning and multi-source information fusion positioning, ensuring that the entire fusion positioning process can be carried out systematically based on real and valid sensor data.
[0030] Under a unified navigation coordinate system, this application provides a unified model for the motion state of pedestrians at different times, defining the pedestrian's state variables at each time as state vectors containing planar position information and heading angle information. Through standardized state definitions, a unified description of the pedestrian's real-time motion attitude and position information is achieved, effectively avoiding subsequent calculation deviations caused by inconsistent state descriptions. This provides a unified and stable state foundation for subsequent PDR dead reckoning, factor graph construction, and optimization solutions.
[0031] In this embodiment, the navigation coordinate system preferably adopts the Northeast-Sky ENU coordinate system, where the x-axis points due east, the y-axis points due north, and the z-axis is perpendicular to the horizontal plane and pointing upwards. By adopting a standard and universal navigation coordinate system, the consistency, accuracy, and standardization of subsequent calculation processes such as step size calculation, position update, heading integration, and fusion positioning can be effectively guaranteed. It also facilitates compatibility with the coordinate systems of existing navigation systems, improving the practicality and scalability of the technical solution in this application.
[0032] Step S2: Based on the collected acceleration and angular velocity data, the pedestrian state vector is recursively updated through signal processing, gait recognition, step frequency and step length calculation, and heading angle integral calculation to obtain the displacement increment representing the relative motion relationship of the pedestrian.
[0033] First, in this embodiment, a series of signal processing operations are performed on the triaxial acceleration signal output by the inertial measurement unit, including signal synthesis, low-pass filtering, and peak detection, to achieve accurate identification of pedestrian gait events and calculate the pedestrian's walking frequency. In this embodiment, the triaxial acceleration components output by the inertial measurement unit are defined as follows: , , Since the mobile terminal may undergo posture changes during the process of being carried by a pedestrian, the acceleration signal in a single axis may not accurately reflect the pedestrian's walking state. Therefore, this application obtains the combined acceleration value by synthesizing a three-axis acceleration signal, which is specifically calculated according to formula (1):
[0034] In the formula, The resultant acceleration value is the value at the kth sampling time. This resultant acceleration value can effectively eliminate the interference of device posture changes on gait detection and truly reflect the acceleration change characteristics of pedestrians during walking.
[0035] After obtaining the resultant acceleration signal, the embodiments of this application... The signal undergoes low-pass filtering to remove high-frequency interference signals caused by environmental noise, equipment vibration, etc., while retaining low-frequency signals that reflect pedestrian gait characteristics, ensuring the accuracy of subsequent gait detection. After filtering, peak detection is used to identify pedestrian gait events, with the specific criteria being: when the following conditions are met... and When this sampling time is determined, it is considered the starting point of a step. The preset acceleration threshold is obtained through calibration of a large amount of measured data. It can adapt to the walking acceleration characteristics of different pedestrians and avoid missed detections and false detections.
[0036] Step frequency, as a core parameter reflecting the walking rhythm of pedestrians, is also an important input for subsequent step length estimation. In this embodiment, the step frequency f is calculated from the time interval between two adjacent gait events according to equation (2):
[0037] in, , These represent the occurrence times of two adjacent gait events, in seconds. The step frequency f is measured in steps per second, and its value directly reflects the walking speed of a pedestrian. Accurate calculation of the step frequency can provide a reliable basis for the adaptive adjustment of subsequent step length estimation.
[0038] like Figure 2 As shown, Figure 2 This diagram illustrates the accelerometer processing for gait detection. It visually demonstrates the changes in the acceleration signal at each processing stage and the final gait detection result, clearly showing the specific process of gait detection and step frequency calculation in this embodiment. In the diagram, |Acc| represents the composite magnitude of the triaxial acceleration, corresponding to the value calculated in this application. The signal directly reflects the original synthesis effect of the acceleration signal; Lowpass is the result after low-pass filtering the synthesized acceleration signal, and it can be clearly seen that high-frequency noise is effectively suppressed after filtering, the signal becomes smoother, and it is easier to identify gait peaks; Detrend is the acceleration signal after removing the gravity component, further eliminating the influence of gravity on gait detection and focusing on the acceleration changes caused by pedestrian walking; Binary indicates the threshold-based signal. The gait decision binary signal, when the acceleration signal exceeds the threshold When the signal is high, the binary signal is high; otherwise, it is low, clearly marking possible gait events. Detected is the location of the finally detected gait event, corresponding to the peak points of two adjacent high-level signals. Each peak point corresponds to the starting point of a step. By statistically analyzing the time interval between adjacent peak points, the step frequency can be calculated. The attached figure provides an intuitive data change basis for the gait detection and step frequency calculation process, and also verifies the effectiveness of the gait detection method of this application.
[0039] After completing gait detection and gait frequency calculation, this embodiment of the application proceeds to the step length estimation stage. Step length (StepLength, The step length estimation is the core parameter that determines the positioning accuracy of PDR. The accuracy of the step length estimation directly affects the accuracy of the subsequent position recursion update. Therefore, in this embodiment, the step length estimation model is constructed based on the acceleration extreme value within the gait cycle, and the Weinberg formula is used to calculate the step length, as shown in equation (3):
[0040] In the formula, These are the maximum and minimum acceleration values of the resultant acceleration signal within the current gait cycle. The difference between the two values reflects the magnitude of the pedestrian's acceleration change within the current gait cycle and is significantly correlated with stride length. β is the proportionality coefficient, which is determined through calibration using a large amount of measured data. During the calibration process, individual differences such as height, weight, and walking speed of different pedestrians are fully considered to ensure the initial accuracy of stride length estimation.
[0041] Since the cadence of pedestrians varies under different walking conditions (such as brisk walking, slow walking, and normal walking), using a fixed β coefficient would lead to a large error in step length estimation. Therefore, in order to improve the universality and adaptability of the step length estimation model, this application introduces a cadence compensation mechanism to dynamically adjust the β coefficient according to the cadence, specifically according to equation (4):
[0042] in, The initial value for the β coefficient is based on the measured calibration results under normal walking conditions; The reference cadence is typically the average cadence under normal walking conditions; α is a cadence correction factor, obtained by fitting measured data, used to quantify the influence of cadence changes on the β coefficient. By introducing this cadence compensation term, the β coefficient can be adaptively adjusted with changes in cadence, thereby maintaining the stability and accuracy of step length estimation under different walking speeds and carrying postures, effectively reducing step length estimation errors.
[0043] After the step size estimation is completed, the embodiment of this application performs heading angle estimation. The angular velocity (GV) is used to accurately describe the direction of a pedestrian's movement and is a key parameter for recursive position updates; its accuracy directly determines the directional accuracy of the pedestrian's position recursion. In this embodiment, the vertical axis angular velocity output by the gyroscope in the inertial measurement unit is utilized. The change in the pedestrian heading angle is obtained through integration, specifically calculated according to equation (5):
[0044] in, The heading angle at the previous sampling time is used as the initial value for calculating the current heading angle; The angular velocity of the gyroscope along the vertical axis reflects the rate of change in the pedestrian's direction of movement; the integration interval is the previous sampling time. Up to the current sampling time By integrating the angular velocity within this interval, the change in heading angle at the current moment relative to the previous moment can be obtained, and thus the heading angle at the current moment can be obtained. .
[0045] After obtaining the step size With heading angle Subsequently, in this embodiment of the application, the pedestrian's planar position is recursively updated according to equation (6) to achieve continuous calculation of the pedestrian's position:
[0046] in,( , () represents the pedestrian's two-dimensional coordinate position in the Northeast ENU navigation coordinate system at the previous moment, where E represents the eastward coordinate and N represents the northward coordinate; , The formula represents the pedestrian's two-dimensional coordinate position at the current moment. Essentially, this recursive formula converts the step size and heading angle in polar coordinates into position increments in a Cartesian coordinate system. By decomposing the step size in the east and north directions, calculating the position increments in the east and north directions respectively, and then superimposing these increments with the position from the previous moment, the current position coordinates can be obtained. Through the above series of calculations, this embodiment completes the recursive update of the pedestrian state vector, ultimately obtaining the PDR relative displacement increment representing the relative motion relationship of the pedestrian.
[0047] The pedestrian dead reckoning method in this application is an autonomous relative positioning method based on inertial sensors. It does not rely on external signals and can achieve continuous positioning in environments where GNSS signals are unavailable, such as indoors and urban canyons. Its core process is to complete gait detection and step length estimation through acceleration data, complete heading angle calculation through angular velocity data, and then complete position recursion update by combining step length and heading angle. Figure 3 coordinates of the center and the point ( , ), ( , ), ..., ( , The numbers represent the positions of pedestrians at different sampling times, with step sizes of 1 to 2. , … For each gait cycle, the step size estimation results and heading angle are given. , … The calculation results of the heading angle at each time point are connected by arrows, which clearly shows the recursive process of the pedestrian's position and verifies the feasibility and rationality of the PDR dead reckoning method in this application.
[0048] To fully demonstrate the technical advantages of this application, the embodiments of this application use the traditional Kalman filter fusion method as a comparative scheme for illustration.
[0049] PDR (Positioning Direct Response) relies on inertial measurement units (IGUs) to estimate step size and heading, providing continuous position information even in environments without satellite signals; however, its errors accumulate over time. GNSS provides absolute position observations, but is susceptible to obstruction and multipath propagation in complex environments. Therefore, the fusion of PDR and GNSS can achieve complementary advantages, ensuring the continuity and reliability of the overall positioning results throughout the entire process.
[0050] Define the state vector as equation (7):
[0051] in, for Time's up Change in pedestrian position at any given time. and They represent The eastward and northward positional errors at any given time.
[0052] The state transition equation describes the dynamic change of the error between adjacent time steps, and is expressed as equation (8):
[0053] in, Here is the state transition matrix. For process noise, satisfy , Let be the process noise covariance matrix.
[0054] Since the short-term motion of pedestrians has approximately steady characteristics, we can take equation (9):
[0055] GNSS provides absolute position observations, and its measurement equation is Equation (10):
[0056] in, For the observation vector, For the observation matrix, To measure noise, meet , To observe the noise covariance matrix, equation (11) is usually used:
[0057] Kalman filtering consists of two stages: prediction and update.
[0058] The prediction process is shown in equations (12) and (13):
[0059] in, Indicates the predicted state. This represents the error covariance matrix of the prediction.
[0060] The update process is shown in equations (14), (15), and (16):
[0061] in, For Kalman filter gain, It is an identity matrix.
[0062] The error estimate obtained after filtering is used to correct the PDR trajectory, resulting in the fused position result equation (17):
[0063] in, This represents the optimal position after Kalman filtering fusion. This is the original estimated location of PDR. The eastward and northward position errors are estimated by filtering.
[0064] Step S3: Using the pedestrian state vector as the state node, construct the PDR state transition factor based on the relative displacement increment, construct the GNSS observation factor based on the GNSS absolute position information, embed an adaptive robust mechanism and introduce a trajectory smoothing factor into the GNSS observation factor, and complete the construction of the factor graph model.
[0065] This application, within a factor graph optimization framework, constructs a graph optimization model composed of state nodes and factor nodes to achieve unified fusion of multi-source navigation information. This application uses the pedestrian state vector defined in S1 as the state node, and constructs a PDR state transition factor based on the PDR relative displacement increment obtained in S2. It utilizes the displacement increment estimated by PDR at adjacent time points as the state transition constraint, and introduces an anisotropic noise model to describe the forward and lateral error characteristics during pedestrian movement, thereby more accurately reflecting the actual error statistics of PDR positioning.
[0066] Meanwhile, this application uses the absolute position information provided by the GNSS module as an external observation constraint to construct a GNSS observation factor and establish a correlation with the state nodes at corresponding times. This effectively suppresses the drift error that accumulates over time during PDR positioning using the absolute position information. Furthermore, embodiments of this application also construct a trajectory smoothing factor. By imposing constraints on the higher-order differences between multiple adjacent state nodes, it suppresses local high-frequency noise generated during positioning, thereby improving the geometric consistency and overall smoothness of the pedestrian's trajectory.
[0067] To address the issues of non-Gaussian and anomalous jumps in GNSS observations under complex environments, this application introduces an adaptive robust mechanism into the GNSS observation factors. The weights are dynamically calculated based on the magnitude of the observation residuals, and a robust kernel function is used to reweight the residuals. This automatically reduces the weight of anomalous observations and adaptively adjusts the observation covariance, ensuring that high-reliability observations form a strong constraint on the fusion results, and effectively suppressing anomalous observations.
[0068] In this embodiment of the application, the navigation state sequence is represented by equation (18):
[0069] in Let be the position and heading in the ENU coordinate system. The posterior probability density of the navigation state can be decomposed into the product of multiple local factors, as shown in equation (19):
[0070] In the formula, Represents the state transition factor of PDR. Indicates GNSS observation factor, Z represents the trajectory smoothing factor, and Z is the normalization constant.
[0071] According to the Bayesian estimation principle, the maximum a posteriori probability estimate is equivalent to minimizing the weighted sum of squared residuals, i.e., equation (20):
[0072] in This is an information matrix.
[0073] The PDR state transition factor residuals are constructed according to equation (22):
[0074] In the formula, ⊕ represents the local increment being rotated along the heading and then superimposed onto the global pose, and its explicit expression is shown in formula (23):
[0075] To reflect the difference between forward and lateral noise for pedestrians, this application introduces anisotropic covariance, as shown in equation (24):
[0076] in, The rotation matrix is formed by the heading. For the tangential error covariance, Let be the normal error covariance.
[0077] Based on this, the GNSS observation vector at time i+1 is defined as shown in equation (25):
[0078] In the formula, Let be the GNSS position observation vector at time i+1. These are GNSS observations of the eastward position. These are GNSS northward position observations.
[0079] The GNSS observation factor residuals are constructed using the above observation vectors, as shown in equation (26):
[0080] In the formula, H is the observation matrix, used to extract the position components from the system state and match them with GNSS observations. For GNSS position observations. To suppress non-Gaussian noise, this application uses the Tukey kernel function for robust weighting, with the weights calculated as shown in equation (27). The GNSS observation covariance is adaptively adjusted according to the weights, as shown in equation (28), so that abnormal observations are automatically deweighted. The trajectory smoothing factor residual is constructed according to equation (29):
[0081]
[0082]
[0083] Its covariance The value is set to a smaller value to ensure a smooth trajectory.
[0084] Factor graphs transform optimal estimation into a multi-constraint joint minimization problem by connecting PDR state transition factors, GNSS observation factors, trajectory smoothing factors, and state nodes. For example... Figure 4 As shown, Figure 4 This is a schematic diagram of the ARFGO-PDR / GNSS integrated navigation system. The orange area in the diagram... ,blue ,Purple These represent three types of factors. ~ For state nodes, prior represents prior information. ~ The GNSS observations at various times visually demonstrate the multi-factor collaborative fusion mechanism of this application.
[0085] Step S4: Based on the factor graph model, a nonlinear least squares optimization problem is established. The iterative reweighted least squares method is used to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, the final pedestrian fusion positioning result is output, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
[0086] Based on the PDR state transition factor, GNSS observation factor, and trajectory smoothing factor constructed by S3, this application unifies the multi-source constraints into a nonlinear least squares optimization problem, and uses Equation (30) as the overall optimization objective. To achieve robust suppression of anomalous observations, this application introduces a weight adaptive adjustment mechanism during the optimization process, as shown in Equation (31). By updating the confidence of each observation item in real time, the influence of outliers on the overall optimization result is weakened. During the optimization solution process, the nonlinear residual function is expanded in first order Taylor around the current state, as shown in Equation (32):
[0087]
[0088]
[0089] In the formula, J is the Jacobian matrix of the residual function relative to the system state vector, which is used to characterize the degree of influence of state changes on the residual.
[0090] Based on this, the overall cost function is transformed into a standard quadratic optimization form, as shown in equation (33); and the corresponding coefficient matrix is constructed according to the weights of each factor and the Jacobian matrix, as shown in equation (34); thus, the linear incremental equation system is obtained, as shown in equation (35):
[0091]
[0092]
[0093] In the formula, The normalized information coefficient matrix, The gradient vector, The increment of the state to be solved.
[0094] After obtaining the state correction using the above incremental equation, the system state is iteratively updated according to equation (36):
[0095] Repeat the Taylor expansion, equation construction, incremental solution, and state update steps until the state increment is less than the preset convergence threshold. Once the optimization process has reached a stable convergence state, the iteration is terminated and the pedestrian fusion positioning result is output, thereby achieving high-precision and high-robust fusion of PDR relative positioning and GNSS absolute positioning.
[0096] like Figure 5 Runway test horizontal position error-time curve Figure 6 As shown in the horizontal position error-time curve of the dormitory building experiment, compared with traditional GNSS single-point positioning and conventional Kalman filter fusion methods, the ARFGO method proposed in this application can effectively smooth noise and quickly converge positioning errors in open scenes; in complex scenes such as occlusion and multipath, it can significantly suppress abnormal position jumps and gross errors. Experimental results show that the method in this application has significant advantages in positioning accuracy, continuous reliability, and anti-interference capability.
[0097] The adaptive robust factor graph optimization framework proposed in this application has good scalability. Without deviating from the core concept of the invention, the robust kernel function type, trajectory smoothing constraint order, and numerical optimization iteration strategy can all be replaced by equivalent methods. Among them, the adaptive robust weighting mechanism driven by observation residuals and the graph optimization structure with multi-factor joint constraints are the core technical features of this application. They can ensure that the integrated navigation system can still stably output high-precision and highly continuous pedestrian positioning results in complex application scenarios such as urban canyons and indoor-outdoor transitions.
[0098] Example 2 This invention provides a PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization. Figure 7 This is a schematic flowchart illustrating a PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization, provided as an embodiment of the present invention. Figure 7 As shown, the device includes: The data acquisition module 100 is used to acquire acceleration data, angular velocity data and absolute position information output by the inertial measurement unit in the mobile terminal carried by the pedestrian, and to define a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system. The displacement calculation module 200 is used to recursively update the pedestrian state vector based on the collected acceleration and angular velocity data through signal processing, gait recognition, step frequency and step length calculation and heading angle integral calculation, so as to obtain the displacement increment that represents the relative motion relationship of the pedestrian. The graph model construction module 300 is used to take the pedestrian state vector as the state node, construct the PDR state transition factor based on the relative displacement increment, construct the GNSS observation factor based on the GNSS absolute position information, and embed an adaptive robust mechanism and introduce a trajectory smoothing factor into the GNSS observation factor to complete the construction of the factor graph model. The optimization and solution module 400 is used to establish a nonlinear least squares optimization problem based on the factor graph model. It uses the iterative reweighted least squares method to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, it outputs the final pedestrian fusion positioning result, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
[0099] Regarding the apparatus in the above embodiments, the specific manner in which each module performs its operation has been described in detail in the embodiments related to the method, and will not be elaborated upon here.
[0100] Example 3 To implement the methods of the above embodiments, the present invention also provides an electronic device, which includes a memory and a processor; wherein the processor reads executable program code stored in the memory to run a program corresponding to the executable program code, so as to implement the various steps of the methods described above.
[0101] Example 4 To implement the above embodiments, this application also proposes a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the method described in the foregoing embodiments.
[0102] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.
[0103] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.
[0104] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means at least two, such as two, three, etc., unless otherwise explicitly specified.
Claims
1. A PDR / GNSS pedestrian fusion localization method based on adaptive robust factor graph optimization, characterized in that, include: S1 collects acceleration data, angular velocity data, and absolute position information output by the inertial measurement unit in the pedestrian's mobile terminal, as well as the GNSS model output, and defines a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system. S2, based on the collected acceleration and angular velocity data, through signal processing, gait recognition, step frequency and step length calculation and heading angle integral calculation, the pedestrian state vector is recursively updated to obtain the displacement increment characterizing the relative motion relationship of the pedestrian; S3. The pedestrian state vector is used as the state node. The PDR state transition factor is constructed based on the relative displacement increment. The GNSS observation factor is constructed based on the GNSS absolute position information. At the same time, an adaptive robust mechanism is embedded in the GNSS observation factor and a trajectory smoothing factor is introduced to complete the construction of the factor graph model. S4 establishes a nonlinear least squares optimization problem based on the factor graph model. Iterative reweighted least squares method is used to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, the final pedestrian fusion positioning result is output, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
2. The method according to claim 1, characterized in that, Based on the collected acceleration and angular velocity data, the pedestrian state vector is recursively updated through signal processing, gait recognition, stride frequency and stride length calculation, and heading angle integral calculation to obtain the displacement increment characterizing the relative motion relationship of the pedestrian, including: Vector synthesis is performed on the triaxial acceleration components to obtain a resultant acceleration sequence that eliminates the influence of sensor attitude, and low-pass filtering is applied to the resultant acceleration sequence. The peak detection algorithm identifies gait peaks that meet the amplitude threshold condition, determines the start and end times of each step to complete gait event detection, and calculates the pedestrian's real-time step frequency based on the time interval between adjacent gait peaks. A step size estimation model is constructed based on the maximum and minimum values of the resultant acceleration within the gait cycle, and a step frequency compensation mechanism is introduced to dynamically correct the step size model parameters. The angular velocity of the vertical axis of the gyroscope is integrated to obtain the change in the pedestrian's heading angle. The pedestrian's planar position is then recursively updated based on the step size and heading angle, and the relative displacement increment of the PDR is output.
3. The method according to claim 2, characterized in that, The method for constructing the PDR state transition factor based on relative displacement increments also includes: Using the pedestrian state nodes at adjacent time points as constraints, the relative displacement increments obtained from PDR calculations are used as state transition observation inputs; An anisotropic noise model is introduced to characterize the statistical characteristics of the forward and lateral errors of pedestrian motion, respectively. By using a rotation matrix formed by the heading angle, coordinate transformation is performed on the forward and lateral error covariances to construct an error covariance matrix that matches the pedestrian's motion characteristics. PDR state transition constraints are established based on the anisotropic error covariance matrix, forming a relative motion constraint relationship between adjacent state nodes.
4. The method according to claim 3, characterized in that, The construction of GNSS observation factors based on GNSS absolute position information also includes: The absolute position information output by GNSS is used as the observation value, and observation constraints are established with the pedestrian state node at the corresponding time. Construct the residual term between GNSS position observations and state node estimated positions to form the GNSS observation residual; The GNSS observation residuals are incorporated as a constraint term into the factor graph model to suppress the cumulative error divergence of PDR positioning.
5. The method according to claim 4, characterized in that, The method of embedding an adaptive robust mechanism in GNSS observation factors also includes: Dynamically calculate observation weights based on the magnitude of GNSS observation residuals; A robust kernel function is used to reweight abnormal GNSS observations with large residuals, thereby achieving automatic weight reduction of abnormal observations; The GNSS observation covariance matrix and information matrix are adaptively adjusted based on the weighting results, so that the high-confidence GNSS observations form a strong constraint on the fusion results, and abnormal GNSS observations are effectively suppressed.
6. The method according to claim 5, characterized in that, The introduction of the trajectory smoothing factor also includes: Using multiple consecutive adjacent state nodes as constraint objects, construct second-order difference constraint terms for state variables; By imposing constraints on higher-order changes in state nodes, local high-frequency noise in the trajectory is suppressed, and the geometric consistency and smoothness of pedestrian movement trajectories are constrained, thus forming a complete factor graph model.
7. The method according to claim 6, characterized in that, The iterative optimization solution using the iterative reweighted least squares method also includes: The PDR state transition factor, adaptive robust GNSS observation factor, and trajectory smoothing factor are uniformly constructed into a nonlinear least squares optimization problem. The current state estimate is linearized by a first-order Taylor expansion, an incremental optimization equation is constructed, and the information matrix and coefficient matrix of each factor are updated according to the adaptive robust weight. Iteratively solve for the state increment and update the global state sequence successively; The iteration terminates when the state increment is less than the preset convergence threshold, and the optimized pedestrian plane position and heading angle are output as the final fusion positioning result.
8. A PDR / GNSS pedestrian fusion positioning device based on adaptive robust factor graph optimization, characterized in that, include: The data acquisition module is used to collect acceleration data, angular velocity data, and absolute position information output by the inertial measurement unit in the pedestrian's mobile terminal, and to define a pedestrian state vector containing planar position and heading angle in a unified navigation coordinate system. The displacement calculation module is used to recursively update the pedestrian state vector based on the collected acceleration and angular velocity data through signal processing, gait recognition, step frequency and step length calculation, and heading angle integral calculation, so as to obtain the displacement increment that represents the relative motion relationship of the pedestrian. The graph model construction module is used to take the pedestrian state vector as the state node, construct the PDR state transition factor based on the relative displacement increment, construct the GNSS observation factor based on the GNSS absolute position information, and embed an adaptive robust mechanism and introduce a trajectory smoothing factor into the GNSS observation factor to complete the construction of the factor graph model. The optimization solution module is used to establish a nonlinear least squares optimization problem based on the factor graph model. It uses the iterative reweighted least squares method to iteratively optimize and solve the pedestrian state vector that has been recursively updated. After the iteration converges, it outputs the final pedestrian fusion positioning result, realizing the robust fusion of PDR relative positioning and GNSS absolute positioning.
9. An electronic device, characterized in that, Including processor and memory; The processor reads executable program code stored in the memory to run a program corresponding to the executable program code, so as to implement the method as described in any one of claims 1-7.
10. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the method as described in any one of claims 1-7.