Robot Integrated Navigation Method Based on Dual-Weight Optimized Unscented Particle Filter

Abstract

This invention discloses a robot integrated navigation method based on dual-weighted optimized unscented particle filtering. The method first defines the state vector of the mobile robot and establishes a nonlinear state-space model; then, it performs unscented particle filtering (UPF) initialization to obtain an initial particle set and initial weights, and constructs the importance distribution of the particles; next, it uses unscented Kalman filtering to construct the importance distribution for each particle and samples new particles; then, it combines satellite navigation observation information and updates the particle weights through a maximum entropy-Tukey dual-weighting mechanism to obtain normalized particle weights; finally, it performs particle degradation discrimination and determines whether resampling is needed, and finally outputs the robot navigation state estimation result through weighted fusion. This invention can effectively cope with non-Gaussian noise and outlier interference caused by satellite signal obstruction and multipath effects, significantly improving the state estimation accuracy and stability of the integrated navigation system in complex scenarios.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot navigation and positioning technology, specifically relating to a robot integrated navigation method based on dual-weighted optimized unscented particle filtering. Background Technology

[0002] With the widespread application of mobile robot technology in logistics, outdoor inspection, and autonomous driving, higher demands are being placed on its navigation accuracy and robustness in complex dynamic environments. Inertial Navigation Systems (INS) are characterized by strong autonomy and high short-term accuracy, but their errors accumulate over time. Global Navigation Satellite Systems (GNSS) can provide absolute position information, but in scenarios such as urban canyons, under overpasses, and forests, they are susceptible to interference from signal blockage and multipath effects, leading to non-Gaussian noise and even outliers. Therefore, how to achieve efficient INS / GNSS fusion and improve the system's positioning stability and accuracy in interference environments has become one of the key issues in the field of robot navigation.

[0003] In recent years, particle filtering has been increasingly applied to integrated navigation state estimation due to its advantages in handling nonlinear and non-Gaussian systems. However, traditional particle filtering does not fully utilize current observation information during importance sampling, which can easily lead to particle degradation and estimation bias. Unscented particle filtering (UPF) constructs an importance distribution through unscented Kalman filtering (UKF), which improves sampling efficiency to some extent, but it still relies on the Gaussian likelihood assumption, and its filtering performance degrades significantly when GNSS observations are subjected to abnormal interference. In addition, observation noise in real-world environments is time-varying and uncertain, and a fixed noise covariance is difficult to accurately describe its statistical characteristics, further affecting the filter's adaptability and estimation accuracy. Summary of the Invention

[0004] To address the aforementioned problems, this invention proposes a robot integrated navigation method based on dual-weighted optimization unscented particle filtering. First, to improve robustness to anomaly observations, a Tukey dual-weight function based on the maximum entropy criterion is introduced to construct a red-decreasing robust likelihood model, replacing the traditional Gaussian likelihood to suppress the influence of outliers on particle weight updates. Second, to adapt to dynamically changing environmental noise, an online adaptive observation noise covariance estimation mechanism based on gradient iteration is designed to achieve real-time updates and feedback adjustments of noise statistical characteristics. Finally, the dual-weighted optimization and adaptive estimation strategies are integrated into the UPF framework to form an INS / GNSS integrated navigation filtering method that combines robustness, adaptability, and high accuracy, providing effective technical support for reliable positioning of mobile vehicles in complex outdoor environments.

[0005] The above objectives are achieved through the following technical solutions:

[0006] The robot integrated navigation method based on dual-weighted optimized unscented particle filtering of the present invention includes the following steps:

[0007] S1. Define the state vector of the mobile robot and establish a nonlinear state-space model;

[0008] S2. Based on the nonlinear state-space model established in step S1, perform unscented particle filter (UPF) initialization operation to obtain the initial particle set and initial weights, and construct the importance distribution of the particles;

[0009] S3. Based on the initial particle set, initial weights, and importance distribution obtained in step S2, construct the importance distribution for each particle and sample new particles using unscented Kalman filtering;

[0010] S4. Based on the new particles at time k obtained from sampling in step S3, and combined with satellite navigation observation information, the particle weights are updated through the maximum entropy-Tukey dual-weight mechanism to obtain the normalized particle weights;

[0011] S5. Based on the normalized particle weights obtained in step S4, particle degradation is detected, and it is determined whether to resample. Finally, the robot navigation state estimation result is output through weighted fusion.

[0012] Furthermore, step S1 specifically includes the following steps:

[0013] S1.1. Define the state vector:

[0014] The combined navigation state vector of the mobile robot at time k is defined as follows:

[0015] ,

[0016] in, This refers to the position of the mobile robot in the navigation coordinate system, including latitude, longitude, and altitude. This refers to the velocity of the mobile robot in the navigation coordinate system. These are the attitude angles of the mobile robot, including heading, pitch, and roll. and These are the zero-bias errors of the accelerometer and gyroscope mounted on the mobile robot, respectively.

[0017] S1.2. Establish a nonlinear state-space model based on the state vector defined in step S1.1:

[0018] Based on satellite navigation observation information and the output information of the inertial measurement unit on the mobile robot, the discrete nonlinear state-space model of the robot's integrated navigation system is established as follows:

[0019] ,

[0020] in: For a moment The system state vector; The control inputs obtained from the inertial measurement unit include specific force and angular velocity measurements; This is the nonlinear state transition function of the system, used to describe the inertial navigation mechanics equations and the sensor error propagation process; For a moment The measurement vector is composed of position or velocity information output by the satellite navigation system; It is a nonlinear measurement function used to establish the mapping relationship between the system state and the observed quantities; Let be the process noise vector, which follows a zero-mean Gaussian distribution, i.e.:

[0021] ,

[0022] in The process noise covariance matrix;

[0023] The measurement noise vector follows a zero-mean Gaussian distribution:

[0024] ,

[0025] in This is the measurement noise covariance matrix.

[0026] Furthermore, step S2 specifically includes the following steps:

[0027] S2.1 Based on the nonlinear state-space model established in step S1.2, perform the initialization operation of the unscented particle filter UPF: at the initial time of filtering Given the initial state probability distribution based on the system's prior information. Randomly select from them The initial particle set consists of 10 particles:

[0028] ,

[0029] All particles are assigned the same initial weights. :

[0030] ;

[0031] S2.2 Based on the nonlinear state-space model in step S1.2 and the initial particle set obtained in step S2.1, an importance distribution is constructed: an unscented Kalman filter mechanism is introduced for each particle, and the current measurement information is fused using the unscented transformation without linearization, thereby constructing a Gaussian importance distribution for the particles. ;

[0032] in, (·) indicates that the unscented Kalman filter is the first... The optimal importance distribution probability density function for each particle construction. The first one is obtained by updating based on unscented Kalman filtering. The mean of the importance distribution of each particle; For the corresponding importance distribution covariance matrix, for Time of the first The system state vector of each particle for Time of the first The system state vector of each particle;

[0033] but Get the particle set at any time ,in for Time of the first The normalized weights of each particle.

[0034] Furthermore, step S3 in The set of particles obtained at each time step Based on this, a corresponding unscented Kalman filter is constructed for each particle to generate a particle importance distribution that integrates the current measurement information, and new particles are sampled accordingly. The specific steps include the following:

[0035] S3.1. Based on the initial particle set from step S2.1 and the importance distribution construction approach from step S2.2, generate the unscented transformation Sigma point: Let the dimension of the system state vector be... For the first A particle, at time... The state mean and covariance are denoted as follows: Based on the rules of unscented transformation, construct... Sigma points:

[0036] ,

[0037] in, For the first Each particle at time The One Sigma point; The scaling parameter for the unscented transformation; Represents the Cholesky decomposition of a matrix; The first element of the decomposition matrix represents the first element of the decomposition matrix. Column vector;

[0038] against To achieve the weighted calculation of mean and covariance in the unscented transformation, the corresponding mean weights and covariance weights are defined as follows:

[0039] ,

[0040] in These are unscented transformation parameters used to adjust higher-order statistical properties; For the first Each particle at time The mean weight of the 0th Sigma point, For the first Each particle at time The mean weight of the j-th Sigma point, For the first Each particle at time The covariance weights of the 0th Sigma point, For the first Each particle at time The covariance weight of the j-th Sigma point;

[0041] S3.2. Based on the Sigma point generated in step S3.1 and the system state transition function in step S1.2, complete the Sigma point time update.

[0042] Substitute the generated Sigma points into the system's nonlinear state transition function to complete the state prediction:

[0043] ,

[0044] in For inertial sensor input, For the first Each particle at time The j-th Sigma point, For the first Each particle at time The j-th Sigma point is obtained after prediction by the system state transition function. The Sigma point of each moment prior;

[0045] Calculate the i-th particle based on the predicted Sigma point. Time-prior predicted state mean With the i-th particle Time-prior prediction state covariance :

[0046] ,

[0047] in Let T be the process noise covariance matrix, with the superscript T indicating the transpose of the matrix;

[0048] S3.3. Based on the predicted Sigma points from step S3.2 and the measurement function from step S1.2, complete the measurement update and construct the optimized importance distribution:

[0049] Map the predicted Sigma points to the measurement space:

[0050] ,

[0051] in For the system's nonlinear measurement function, For the first Each particle at time The measurement space Sigma point obtained by mapping the j-th Sigma point through the measurement function;

[0052] Calculate the mean of the predicted measurement :

[0053] ,

[0054] And further calculate the predicted measurement covariance. Cross-covariance with state measurement :

[0055] ,

[0056] in The measurement noise covariance matrix;

[0057] Calculate the first Kalman gain of individual particles :

[0058] ,

[0059] And update the importance distribution parameters corresponding to the particle:

[0060] ,

[0061] in and The first Each particle at time The importance distribution means and covariance;

[0062] S3.4. Based on the importance distribution construction approach determined in step S2.2 and the Gaussian importance distribution finally updated in step S3.3, sample and generate time points from them. new particles :

[0063] ,

[0064] This completes the construction and sampling process of the importance distribution for each particle based on unscented Kalman filtering, resulting in the particle set. ,in Indicates the first Each particle at time State samples.

[0065] Furthermore, step S4 obtains the particle set based on step S3. A Tukey dual-weight robust mechanism based on the maximum entropy criterion is introduced to jointly adjust the particle weights, specifically including the following steps:

[0066] S4.1. Based on the new particle at time k obtained from sampling in step S3.4 and the measurement function from step S1.2, calculate the measurement residual and the normalized residual. For the k-th time... Each particle is defined with a corresponding measurement residual. for:

[0067] ,

[0068] in, For a moment The actual measurement vector; For the system's nonlinear measurement function;

[0069] Further define the normalized residual scalar :

[0070] ,

[0071] in The measurement noise covariance matrix;

[0072] S4.2. Based on the normalized residuals from step S4.1, construct the Tukey double-weighted robust cost function and weight function:

[0073] Set Tukey cutoff threshold Define the Tukey dual-weight robust cost function. for:

[0074] ,

[0075] in For normalized residual scalar , Set the Tukey truncation threshold;

[0076] Therefore, the first... Robust observation terms for individual particles :

[0077] ,

[0078] Simultaneously define the Tukey double weight function. :

[0079] ;

[0080] S4.3. Based on the robust observation term and weight function from step S4.2, the initial particle weights from step S2.1, and the importance distribution from step S2.2, recursively update the unnormalized particle weights:

[0081] Within the framework of unscented particle filtering, the first Each particle at time The unnormalized weights are updated to :

[0082] ,

[0083] in: For the first Each particle at time Normalized weights; The prior probability density of the system state transition is determined by the process model and the statistical characteristics of process noise. The importance distribution is constructed using the unscented Kalman filter. and These are the robust observation term and the reweighting factor constructed based on Tukey's dual-weighting mechanism, respectively.

[0084] S4.4. Based on the unnormalized weights from step S4.3, maximum entropy weight adjustment and normalization are performed. To prevent excessive concentration of particle weights from causing sample degradation, a maximum entropy adjustment strategy is introduced to smooth the unnormalized weights:

[0085] ,

[0086] in The maximum entropy adjustment coefficient is when At that time, it can effectively increase the particle weight distribution entropy value and delay the particle degradation process;

[0087] After maximum entropy adjustment, the weights of all particles are normalized:

[0088] ,

[0089] in For the first Individual particles Normalized weights at time points, For the first Individual particles The unnormalized weights after maximum entropy adjustment at each time step;

[0090] This completes the particle weight update steps for integrating the maximum entropy criterion and the Tukey dual-weight robust mechanism.

[0091] Furthermore, step S5 specifically includes the following steps:

[0092] S5.1. Based on the normalized particle weights from step S4.4, perform particle degeneration discrimination and calculate the number of effective particles according to the particle weight distribution:

[0093] ,

[0094] in, For a moment The Normalized weights for each particle; Used to measure the number of valid samples in the current particle set;

[0095] Set resampling threshold When satisfied If the particle set is determined to have degenerated, the resampling operation in step S5.2 needs to be performed; otherwise, proceed directly to step S5.3.

[0096] S5.2. If step S5.1 determines that the particles are degenerate, resampling is performed based on the normalized particle weights. When the resampling condition is triggered, the particle set is resampled according to the current particle weight distribution to obtain a new particle set:

[0097] ,

[0098] After resampling, reset all particle weights to:

[0099] ;

[0100] S5.3. Based on the particle set and weights corresponding to the discrimination result in step S5.1, output the robot navigation state estimation result. Based on the particle set and corresponding weights at the current moment, calculate the weighted estimate of the system state as the state output of the integrated navigation system:

[0101] ,

[0102] Simultaneously, the state estimation covariance is calculated in one step to characterize the estimation uncertainty:

[0103] ,

[0104] in, For a moment The system state estimate; The corresponding state estimate covariance matrix;

[0105] S5.4. Based on the state estimation output from step S5.3, perform time recursion and loop.

[0106] After completing the state estimation output, update the time index as follows:

[0107] ,

[0108] Then return to steps S5.2 to S5.4 and repeat the above process until the observation sequence ends or the preset termination time is reached.

[0109] The advantages of this invention compared to the prior art are:

[0110] 1. This invention addresses the problem that traditional unscented particle filtering is susceptible to abnormal measurements, non-Gaussian noise, and sudden interference during robot integrated navigation. It introduces the Tukey dual-weight robust estimation mechanism into the unscented particle filtering framework. By performing red-down shaping on the measurement residuals, the influence of abnormal measurements on particle weight updates is effectively suppressed, thereby significantly improving the robustness and positioning reliability of the integrated navigation system in complex environments.

[0111] 2. To address the problems of excessive concentration of particle weights, rapid decline in the number of effective particles, and sample depletion caused by frequent resampling in existing particle filtering methods, this invention further introduces a maximum entropy weight optimization strategy. During the weight update stage, the particle weight distribution is adjusted by entropy constraints, which effectively improves the uniformity of particle weight distribution, delays the particle degradation process, and thus enhances the stability and long-term performance of the filtering algorithm.

[0112] 3. This invention combines the maximum entropy criterion with Tukey's dual-weight robust estimation to propose a dual-weight optimization-based unscented particle filter combined navigation method. While making full use of unscented transformation to improve particle sampling efficiency, it achieves high-precision and robust fusion of multi-source navigation information of the robot. Compared with traditional combined navigation technology based on Gaussian assumption or single robust filtering method, it has higher positioning accuracy and stronger environmental adaptability. Attached Figure Description

[0113] Figure 1 This is a flowchart of the robot integrated navigation method based on dual-weighted optimized unscented particle filtering described in this invention;

[0114] Figure 2 This is a simulated dead reckoning trajectory diagram of the method of the present invention;

[0115] Figure 3 The curves show the eastward, northward, and celestial position errors calculated using the simulated dead reckoning method of this invention. Detailed Implementation

[0116] The method of the present invention will be further described below with reference to the accompanying drawings and specific experimental data.

[0117] like Figure 1 As shown, the robot integrated navigation method based on dual-weighted optimized unscented particle filtering of the present invention includes the following steps:

[0118] S1. Define the state vector of the mobile robot and establish a nonlinear state-space model.

[0119] S1.1. Definition of State Vector:

[0120] The combined navigation state vector of the mobile vehicle at time k is defined as follows:

[0121] ,

[0122] in, This refers to the position (latitude, longitude, altitude) in the navigation coordinate system. Velocity in the navigation coordinate system; For attitude angles (heading, pitch, roll); and These are the zero bias errors of the accelerometer and gyroscope, respectively.

[0123] S1.2. Nonlinear state-space model:

[0124] Based on the output of the inertial measurement unit and satellite navigation observation information, the discrete nonlinear state-space model of the robot integrated navigation system is established as follows:

[0125] ,

[0126] in: For a moment The system state vector; The control inputs obtained from the inertial measurement unit include specific force and angular velocity measurements; This is the nonlinear state transition function of the system, used to describe the inertial navigation mechanics equations and the sensor error propagation process; For a moment The measurement vector is composed of position or velocity information output by the satellite navigation system; It is a nonlinear measurement function used to establish the mapping relationship between the system state and the observed quantities; The process noise vector follows a zero-mean Gaussian distribution:

[0127] ,

[0128] in The process noise covariance matrix; The measurement noise vector follows a zero-mean Gaussian distribution:

[0129] ,

[0130] in This is the measurement noise covariance matrix.

[0131] The above model can simultaneously characterize the nonlinear dynamics of the system and the observation uncertainty, providing a unified state-space description for the design of subsequent unscented particle filter algorithms.

[0132] S2. Based on the nonlinear state-space model established in step S1, perform unscented particle filter (UPF) initialization operation to obtain the initial particle set and initial weights, and construct the importance distribution of the particles.

[0133] At the initial moment of filtering Given the initial state probability distribution based on the system's prior information. Randomly select from them The initial particle set consists of 10 particles:

[0134] ,

[0135] All particles are assigned the same initial weights:

[0136] ;

[0137] In the particle filter framework, the selection of the importance distribution directly affects sampling efficiency and estimation accuracy. Traditional particle filters typically employ prior state transition probability density. As an importance distribution, this method does not incorporate current measurement information, which can easily cause particles to stray from the high likelihood region. Extended particle filtering uses extended Kalman filtering to construct the importance distribution, but linearization errors and the Gaussian assumption limit its accuracy.

[0138] To improve the importance distribution's ability to approximate nonlinear systems, an unscented Kalman filter mechanism is introduced for each particle. This unscented transform fuses current measurement information without linearization, thereby constructing a Gaussian importance distribution for the particle.

[0139] ,

[0140] in: The ws-th digit obtained by updating based on unscented Kalman filtering The mean of the importance distribution of each particle; This represents the covariance matrix of the corresponding importance distribution. By introducing an unscented transformation mechanism into the particle filter framework, the importance distribution can simultaneously reflect the system's nonlinear dynamics and current observation information, laying the foundation for subsequent particle sampling and weight updates.

[0141] S3. Based on the initial particle set, initial weights, and importance distribution obtained in step S2, construct the importance distribution for each particle and sample new particles using unscented Kalman filtering.

[0142] At any moment Particle set obtained Based on this, a corresponding unscented Kalman filter is constructed for each particle to generate a particle importance distribution that integrates the current measurement information, and the sampling of new particles is completed accordingly.

[0143] S3.1. Unscented Transformation Sigma Point Generation:

[0144] Let the dimension of the system state vector be... For the first A particle, at time... The state mean and covariance are denoted as follows:

[0145] ,

[0146] Based on the rules of unscented transformation, construct Sigma points:

[0147] ,

[0148] in, For the first Each particle at time The Sigma points; The scaling parameter for the unscented transformation; Represents the Cholesky decomposition of a matrix; The first element of the decomposition matrix represents the first element of the decomposition matrix. Column vector.

[0149] The corresponding mean weights and covariance weights are defined as follows:

[0150] ,

[0151] in These are unscented transformation parameters used to adjust higher-order statistical properties.

[0152] S3.2. Time Update: Sigma Point State Prediction:

[0153] Substitute the generated Sigma points into the system's nonlinear state transition function to complete the state prediction:

[0154] ,

[0155] in The system state transition function, This is the input quantity for the inertial sensor.

[0156] Calculate the predicted state mean and covariance based on the predicted Sigma points:

[0157] ,

[0158] in Let be the process noise covariance matrix.

[0159] S3.3. Measurement update, constructing particle importance distribution:

[0160] Map the predicted Sigma points to the measurement space:

[0161] ,

[0162] in This is the nonlinear measurement function of the system.

[0163] Calculate the mean of the predicted measurements:

[0164] ,

[0165] Furthermore, the covariance of the predicted measurements and the cross-covariance of the state-one measurements are calculated:

[0166] ,

[0167] in This is the measurement noise covariance matrix.

[0168] Calculate the first Kalman gain for each particle:

[0169] ,

[0170] And update the importance distribution parameters corresponding to the particle:

[0171] ,

[0172] in and The first Each particle at time The importance distribution means and covariance.

[0173] S3.4. New Particle Sampling:

[0174] Based on the Gaussian importance distribution constructed above, time points are sampled and generated from it. New particles:

[0175] ,

[0176] This completes the construction and sampling process of the importance distribution for each particle based on unscented Kalman filtering, providing a particle basis for the subsequent introduction of robust observation weights and weight updates.

[0177] S4. Based on the new particle at time k obtained from step S3, and combined with satellite navigation observation information, the particle weights are updated through the maximum entropy-Tukey dual-weight mechanism to obtain the normalized particle weights.

[0178] At any moment The importance distribution constructed through unscented Kalman filtering has completed the sampling of new particles, resulting in a particle set.

[0179] ,

[0180] in Indicates the first Each particle at time State samples.

[0181] To suppress the impact of anomalous observations on the particle weight update process and improve the robustness of the filter in non-Gaussian noise environments, a Tukey dual-weight robust mechanism based on the maximum entropy criterion is introduced to jointly adjust the particle weights.

[0182] S4.1. Definitions of Measurement Residual and Normalized Residual:

[0183] For the For each particle, the corresponding measurement residual is defined as:

[0184] ,

[0185] in, For a moment The actual measurement vector; This is the nonlinear measurement function of the system.

[0186] Further define the normalized residual scalar:

[0187] ,

[0188] in This is the measurement noise covariance matrix.

[0189] Normalized residuals are used to characterize the degree of consistency between the predicted state of a particle and the actual observation, and are the core criterion for robust weight adjustment.

[0190] S4.2. Construction of Tukey's dual-weight robust cost function:

[0191] Set Tukey cutoff threshold The Tukey dual-weight robust cost function is defined as follows:

[0192] ,

[0193] Therefore, the first... Robust observation terms for individual particles:

[0194] ,

[0195] This robust observation term maintains weight variation characteristics similar to the Gaussian likelihood when the residuals are small, but when the residuals exceed a threshold... After that, the cost no longer increases, thus effectively weakening the impact of abnormal measurements on weight updates.

[0196] Simultaneously define the Tukey double weight function:

[0197] ,

[0198] The weighting function is used to further weight the particle contributions, so that the particle weights that are highly inconsistent with the anomalous observations are significantly suppressed.

[0199] S4.3. Particle weight recursion for fusion robust observation terms:

[0200] Within the framework of unscented particle filtering, the first Each particle at time The unnormalized weights are updated as follows:

[0201] ,

[0202] in: For the first Each particle at time Normalized weights; The prior probability density of the system state transition is determined by the process model and the statistical characteristics of process noise. The importance distribution is constructed using unscented Kalman filtering; and These are the robust observation term and the reweighting factor constructed based on Tukey's dual-weighting mechanism, respectively.

[0203] By incorporating robust statistical concepts into the particle weight recursion process in the above manner, adaptive suppression of abnormal measurements can be achieved.

[0204] S4.4. Maximum Entropy Weight Adjustment:

[0205] To prevent sample degradation caused by excessive concentration of particle weights, a maximum entropy adjustment strategy is introduced to smooth the unnormalized weights:

[0206] ,

[0207] in The maximum entropy adjustment coefficient is when At this time, it can effectively increase the particle weight distribution entropy value and delay the particle degradation process.

[0208] After maximum entropy adjustment, the weights of all particles are normalized:

[0209] ,

[0210] This completes the particle weight update steps that integrate the maximum entropy criterion and the Tukey dual-weight robust mechanism.

[0211] S5. Based on the normalized particle weights obtained in step S4, particle degradation is detected, and it is determined whether to resample. Finally, the robot navigation state estimation result is output through weighted fusion.

[0212] After completing the particle weight update and normalization process in step S3, in order to avoid insufficient number of effective samples due to particle degradation, the current particle set is degraded and resampling is performed if necessary. Then the system state estimation results are output.

[0213] S5.1. Particle Degradation Detection:

[0214] Calculate the effective number of particles based on the particle weight distribution:

[0215] ,

[0216] in, For a moment The Normalized weights for each particle; Used to measure the number of valid samples in the current particle set.

[0217] Set resampling threshold When satisfied If the particle set is deemed to have degenerated, a resampling operation is required; otherwise, the resampling step is skipped.

[0218] S5.2. Particle resampling:

[0219] When the resampling condition is triggered, the particle set is resampled according to the current particle weight distribution to obtain a new particle set:

[0220] ,

[0221] During the resampling process, high-weight particles are copied and retained, while low-weight particles are eliminated, thereby improving the ability of the particle set to approximate the true posterior probability distribution.

[0222] The resampling method can be any of the following: systematic resampling, residual resampling, or polynomial resampling.

[0223] After resampling, reset all particle weights to:

[0224] ,

[0225] It should be noted that resampling only updates the particle set and weights, and does not change the recursive logic at the filtering time.

[0226] S5.3. System State Estimation Output:

[0227] Based on the current particle set and corresponding weights, a weighted estimate of the system state is calculated as the state output of the integrated navigation system:

[0228] ,

[0229] Furthermore, the state estimation covariance can be calculated to characterize the estimation uncertainty.

[0230] ,

[0231] in, For a moment The system state estimate; This is the corresponding state estimation covariance matrix.

[0232] S5.4. Time Recursion:

[0233] After completing the state estimation output, update the time index as follows:

[0234] ,

[0235] Then return to steps S5.2 to S5.4 and repeat the above process until the observation sequence ends or the preset termination time is reached.

[0236] Simulation experiment:

[0237] To verify the effectiveness of the present invention, the following simulation experiment was designed:

[0238] (I) Simulation Platform and Experimental Equipment:

[0239] This simulation was conducted using the MATLAB / Simulink simulation platform to build a simulation environment for the robot's integrated navigation system. High-precision modeling of the navigation sensors carried by the actual mobile robot was performed to simulate the real sensor data output characteristics. The sensor model includes: inertial measurement unit (IMU), global navigation satellite system (GNSS), and integrated navigation solution module.

[0240] (II) Experimental Scenario Design:

[0241] A complex outdoor path navigation scenario for a mobile robot was simulated. The robot's trajectory was designed as a smooth, continuous two-dimensional trajectory, with a total simulation time of 100 seconds. 20% of the observation points were randomly selected to introduce GNSS outliers, simulating satellite signal interference in complex environments. The remaining time was under normal GNSS observation conditions. The focus was on verifying the anti-interference capability and navigation stability of the proposed method under outlier interference.

[0242] (III) Experimental Comparison Scheme:

[0243] Two sets of comparative experiments were set up, using the same initial state, sensor error parameters, motion trajectory, and interference conditions, with only the filtering algorithm being changed:

[0244] Control group: Traditional unscented particle filtering (UPF) method;

[0245] Experimental group: The dual-weighted optimization unscented particle filtering method proposed in this invention.

[0246] (iv) Evaluation indicators:

[0247] We selected core evaluation metrics in the field of navigation and positioning to quantitatively compare the performance of the two algorithms:

[0248] 1. Trajectory tracking consistency: Intuitively compare the degree of fit between the algorithm-estimated trajectory and the robot's actual motion trajectory;

[0249] 2. Root Mean Square Error (RMSE): The root mean square error of the eastward, northward, and total positions is calculated separately. The smaller the root mean square error, the higher the positioning accuracy.

[0250] 3. Error stability: Analyze the position error fluctuation at the time of outlier disturbance to verify the robustness of the algorithm against outliers.

[0251] (V) Experimental Procedure:

[0252] 1. Set the robot's initial state in the simulation platform (initial position, velocity, and attitude angles are consistent with the real trajectory), and initialize the filter parameters: number of particles. Unscented Transform Parameters Tukey dual-weight cutoff threshold Maximum entropy adjustment coefficient resampling threshold (for the total number of particles) Simultaneously initialize the unscented Kalman filter as the first... The probability density function of the importance distribution of each particle is optimized to complete the initial allocation of the particle set and weights.

[0253] 2. Simulation measurement data of IMU and GNSS were collected synchronously at a simulation step size of 0.1s. The control group used the traditional UPF for integrated navigation calculation, while the experimental group used the dual-weighted optimized UPF method of this invention for calculation.

[0254] 3. At 20% of random time points, introduce outliers into the GNSS measurement data to simulate complex environmental interference.

[0255] 4. Record the position estimation results of the two algorithms in real time. After the simulation, calculate the RMSE of the eastward, northward and total positions, draw the trajectory comparison chart and the position error time series chart, and analyze the performance differences between the two algorithms.

[0256] (VI) Experimental Results and Analysis:

[0257] Figure 2 As can be clearly seen from the figure, outlier GNSS points deviate significantly from the true trajectory. The standard UPF-estimated trajectory is heavily influenced by outliers, exhibiting significant positional deviations and abnormal fluctuations in the trajectory curve near outlier points. In contrast, the robust UPF-estimated trajectory is smoother, closer to the true trajectory, and maintains good tracking accuracy even near outlier points. Near normal GNSS observation points, both algorithms provide relatively accurate trajectory estimates.

[0258] Figure 3Eastward error analysis shows that the RMSE of the standard UPF is 5.92 meters, while that of the robust UPF is 1.44 meters, representing an improvement rate of 75.7%. Northward error analysis shows that the RMSE of the standard UPF is 2.60 meters, while that of the robust UPF is 0.88 meters, representing an improvement rate of 66.1%. Total position error analysis shows that the RMSE of the standard UPF is 6.46 meters, while that of the robust UPF is 1.69 meters, representing an overall improvement rate of 73.9%. The time series data shows that near the outlier, the error of the standard UPF exhibits a significant peak, while the error curve of the robust UPF remains relatively stable.

Claims

1. A robot integrated navigation method based on double weight optimization unscented particle filter, characterized in that, The method includes the following steps: S1. Define the state vector of the mobile robot and establish a nonlinear state-space model; specifically, this includes the following steps: S1.

1. Define the state vector: The combined navigation state vector of the mobile robot at time k is defined as follows: wherein, is the position of the mobile robot in the navigation coordinate system, including latitude, longitude, and altitude; is the velocity of the mobile robot in the navigation coordinate system; is the attitude angle of the mobile robot, including heading, pitch angle, and roll angle; and are the zero offset errors of the accelerometer and the gyroscope, respectively, carried by the mobile robot; S1.

2. Establish a nonlinear state-space model based on the state vector defined in step S1.1: Based on satellite navigation observation information and the output information of the inertial measurement unit on the mobile robot, the discrete nonlinear state-space model of the robot's integrated navigation system is established as follows: in: For a moment The system state vector; The control inputs obtained from the inertial measurement unit include specific force and angular velocity measurements; This is the nonlinear state transition function of the system, used to describe the inertial navigation mechanics equations and the sensor error propagation process; For a moment The measurement vector is composed of position or velocity information output by the satellite navigation system; It is a nonlinear measurement function used to establish the mapping relationship between the system state and the observed quantities; Let be the process noise vector, which follows a zero-mean Gaussian distribution, i.e.: in The process noise covariance matrix; The measurement noise vector follows a zero-mean Gaussian distribution, i.e.: in The measurement noise covariance matrix; S2. Based on the nonlinear state-space model established in step S1, perform unscented particle filter (UPF) initialization operation to obtain the initial particle set and initial weights, and construct the importance distribution of the particles; S3. Based on the initial particle set, initial weights, and importance distribution obtained in step S2, construct the importance distribution for each particle and sample new particles using unscented Kalman filtering; S4. Based on the new particles at time k obtained from sampling in step S3, and combined with satellite navigation observation information, the particle weights are updated through the maximum entropy-Tukey dual-weight mechanism to obtain the normalized particle weights; S5. Based on the normalized particle weights obtained in step S4, particle degradation is detected, and it is determined whether to resample. Finally, the robot navigation state estimation result is output through weighted fusion.

2. The robot integrated navigation method based on dual-weighted optimized unscented particle filtering according to claim 1, characterized in that, Step S2 specifically includes the following steps: S2.1 Based on the nonlinear state-space model established in step S1.2, perform the initialization operation of the unscented particle filter UPF: at the initial time of filtering Given the initial state probability distribution based on the system's prior information. Randomly select from them The initial particle set consists of 10 particles: All particles are assigned the same initial weights. : S2.2 Based on the nonlinear state-space model in step S1.2 and the initial particle set obtained in step S2.1, an importance distribution is constructed: an unscented Kalman filter mechanism is introduced for each particle, and the current measurement information is fused using the unscented transformation without linearization, thereby constructing a Gaussian importance distribution for the particles. ; in, (·) indicates that the unscented Kalman filter is the first... The optimal importance distribution probability density function for each particle construction. The first one is obtained by updating based on unscented Kalman filtering. The mean of the importance distribution of each particle; For the corresponding importance distribution covariance matrix, for Time of the first The system state vector of each particle for Time of the first The system state vector of each particle; but Get the particle set at any time ,in for Time of the first The normalized weights of each particle.

3. The robot integrated navigation method based on dual-weighted optimized unscented particle filtering according to claim 2, characterized in that, Step S3 in The set of particles obtained at each time step Based on this, a corresponding unscented Kalman filter is constructed for each particle to generate a particle importance distribution that integrates the current measurement information, and new particles are sampled accordingly. The specific steps include the following: S3.

1. Based on the initial particle set from step S2.1 and the importance distribution construction approach from step S2.2, generate the unscented transformation Sigma point: Let the dimension of the system state vector be... For the first A particle, at time... The state mean and covariance are denoted as follows: Based on the rules of unscented transformation, construct... Sigma points: in, For the first Each particle at time The One Sigma point; The scaling parameter for the unscented transformation; Represents the Cholesky decomposition of a matrix; Describes the first decomposition matrix. Column vector; against To achieve the weighted calculation of mean and covariance in the unscented transformation, the corresponding mean weights and covariance weights are defined as follows: in These are unscented transformation parameters used to adjust higher-order statistical properties; For the first Each particle at time The mean weight of the 0th Sigma point, For the first Each particle at time The mean weight of the j-th Sigma point, For the first Each particle at time The covariance weights of the 0th Sigma point, For the first Each particle at time The covariance weight of the j-th Sigma point; S3.

2. Based on the Sigma points generated in step S3.1 and the system state transition function in step S1.2, complete the Sigma point time update. First, substitute the generated Sigma points into the system nonlinear state transition function to complete the state prediction: in For the first Each particle at time The j-th Sigma point, For the first Each particle at time The j-th Sigma point is obtained after prediction by the system state transition function. The Sigma point of each moment prior; Calculate the i-th particle based on the predicted Sigma point. Time-prior predicted state mean With the i-th particle Time-prior prediction state covariance : in Let T be the process noise covariance matrix, with the superscript T indicating the transpose of the matrix; S3.

3. Based on the predicted Sigma points from step S3.2 and the measurement function from step S1.2, complete the measurement update and construct the optimized importance distribution: Map the predicted Sigma points to the measurement space: in For the system's nonlinear measurement function, For the first Each particle at time The measurement space Sigma point obtained by mapping the j-th Sigma point through the measurement function; Calculate the mean of the predicted measurement : And further calculate the predicted measurement covariance. Cross-covariance with state measurement : in The measurement noise covariance matrix; Calculate the first Kalman gain of individual particles : And update the importance distribution parameters corresponding to the particle: in and The first Each particle at time The importance distribution means and covariance; S3.

4. Based on the importance distribution construction approach determined in step S2.2 and the Gaussian importance distribution finally updated in step S3.3, sample and generate time points from them. new particles : This completes the construction and sampling process of the importance distribution for each particle based on unscented Kalman filtering, resulting in the particle set. ,in Indicates the first Each particle at time State samples.

4. The robot integrated navigation method based on dual-weighted optimized unscented particle filtering according to claim 3, characterized in that, Step S4 obtains the particle set based on step S3. A Tukey dual-weight robust mechanism based on the maximum entropy criterion is introduced to jointly adjust the particle weights, specifically including the following steps: S4.

1. Based on the new particle at time k obtained from sampling in step S3.4 and the measurement function from step S1.2, calculate the measurement residual and the normalized residual. For the k-th time... Each particle is defined as its corresponding measurement residual. for: in, For a moment The actual measurement vector; For the system's nonlinear measurement function; Further define the normalized residual scalar : in The measurement noise covariance matrix; S4.

2. Based on the normalized residuals from step S4.1, construct the Tukey double-weighted robust cost function and weight function: Set Tukey cutoff threshold Define the Tukey dual-weight robust cost function. for: in For normalized residual scalar , Set the Tukey truncation threshold; Therefore, the first... Robust observation terms for individual particles : Simultaneously define the Tukey double weight function. : S4.

3. Based on the robust observation term and weight function from step S4.2, the initial particle weights from step S2.1, and the importance distribution from step S2.2, recursively update the unnormalized particle weights: Within the framework of unscented particle filtering, the first Each particle at time The unnormalized weights are updated to : in: For the first Each particle at time Normalized weights; The prior probability density of the system state transition is determined by the process model and the statistical characteristics of process noise. The importance distribution is constructed using the unscented Kalman filter. and These are the robust observation term and the reweighting factor constructed based on Tukey's dual-weighting mechanism, respectively. S4.

4. Based on the unnormalized weights from step S4.3, maximum entropy weight adjustment and normalization are performed. To prevent excessive concentration of particle weights from causing sample degradation, a maximum entropy adjustment strategy is introduced to smooth the unnormalized weights: in The maximum entropy adjustment coefficient is when At that time, it can effectively increase the particle weight distribution entropy value and delay the particle degradation process; After maximum entropy adjustment, the weights of all particles are normalized: in For the first Individual particles Normalized weights at time points, For the first Individual particles The unnormalized weights after maximum entropy adjustment at each time step; This completes the particle weight update steps for integrating the maximum entropy criterion and the Tukey dual-weight robust mechanism.

5. The robot integrated navigation method based on dual-weighted optimized unscented particle filtering according to claim 4, characterized in that, Step S5 specifically includes the following steps: S5.

1. Based on the normalized particle weights from step S4.4, perform particle degeneration discrimination and calculate the number of effective particles according to the particle weight distribution: in, For a moment The Normalized weights for each particle; Used to measure the number of valid samples in the current particle set; Set resampling threshold When satisfied If the particle set is determined to have degenerated, the resampling operation in step S5.2 needs to be performed; otherwise, proceed directly to step S5.

3. S5.

2. If step S5.1 determines that the particles are degenerate, resampling is performed based on the normalized particle weights. When the resampling condition is triggered, the particle set is resampled according to the current particle weight distribution to obtain a new particle set: After resampling, reset all particle weights to: S5.

3. Based on the particle set and weights corresponding to the discrimination results in step S5.1, output the robot navigation state estimation result. Based on the particle set and corresponding weights at the current moment, calculate the weighted estimate of the system state as the state output of the integrated navigation system: Simultaneously, the state estimation covariance is calculated in one step to characterize the estimation uncertainty: in, For a moment The system state estimate; The corresponding state estimate covariance matrix; S5.

4. Based on the state estimation output from step S5.3, perform time recursion and loop. After completing the state estimation output, update the time index as follows: Then return to steps S5.2 to S5.4 and repeat the above process until the observation sequence ends or the preset termination time is reached.

Images