A method and system for simultaneous localization and mapping in a microgravity environment

Abstract

The application provides a real-time positioning and mapping method and system in a microgravity environment, and belongs to the field of real-time positioning and mapping of space intelligent flying robots. In order to solve the problems that the traditional RBPF-SLAM algorithm has a large weight, and that multiple sampling of a large number of particles leads to weakening of particle set diversity and sample depletion, and makes filtering estimation poor; or increasing the number of particles leads to a decrease in operation efficiency. The application optimizes the particle sampling process by using a Gaussian particle swarm, uses a particle set diversity measure as a search heuristic factor, and ensures that the group diversity level is optimal, which reduces particle degradation; movement of the particle set to a higher posterior probability area improves the estimation accuracy of the state and improves the convergence of the algorithm; the method provided by the application can obtain higher positioning and map construction accuracy with fewer particles, and achieve better filtering estimation stability.

Description

Technical Field

[0001] This invention relates to the field of real-time localization and mapping technology for flying robots, and more specifically, to a method and system for real-time localization and mapping in a microgravity environment. Background Technology

[0002] With the increasing frequency of various experiments and equipment maintenance tasks in microgravity environments, there is an urgent need for space intelligent flying robots to assist experimental personnel in completing a series of tasks such as tracking, lighting, and photography; equipment monitoring and maintenance; instrument transfer; equipment disassembly and replacement; and scientific experimental care, so as to reduce the workload of astronauts.

[0003] In this process, reliable and accurate environmental perception and safe and efficient path planning are key to ensuring the stable operation of the platform. A prerequisite for achieving this goal is that the robot should possess Simultaneous Localization and Mapping (SLAM) capabilities in microgravity environments. Specifically, in narrow, highly unstructured microgravity environments, the space intelligent flight robot effectively utilizes the coupling characteristics of its wind turbine and flywheel propulsion systems, fully leveraging its high reachability and strong obstacle-crossing capabilities to autonomously construct maps through multi-source perception fusion technology.

[0004] Due to the high requirements for maneuverability of intelligent flying robots, the computing speed of their computational units is relatively limited, thus necessitating the design of a lightweight SLAM method. Because of its fast mapping speed and low computational cost, the Rao-Black-wellized particle filter (RBPF)-based SLAM method has been widely used in recent years. It can build a map in an unknown environment using an onboard camera and simultaneously calculate the motion trajectory of the mobile platform from this map. This method decomposes the SLAM problem into a robot localization problem and an environmental feature position estimation problem based on pose estimation. It uses a particle filter algorithm for pose estimation along the entire path and an extended Kalman filter (EKF) algorithm to estimate the position of environmental features. This method combines the advantages of EKF and probabilistic methods, reducing computational complexity while exhibiting good robustness.

[0005] However, the traditional RBPF-SLAM algorithm suffers from significant duplication in the pose estimation and map building processes due to its sequential importance resampling algorithm. This results in repeated selection of particles with high weights, leading to reduced particle diversity and sample depletion, which in turn worsens filtering estimation performance and can cause filter divergence. The simplest way to overcome sample depletion is to increase the number of particles, but this drastically increases computational complexity and reduces algorithm efficiency. Therefore, this invention uses particle diversity as a heuristic to guide the particle optimization search process, ensuring both global optimality and optimal particle diversity, thus improving the system's filtering estimation performance. Summary of the Invention

[0006] The technical problem to be solved by this invention is:

[0007] To address the problems of traditional RBPF-SLAM algorithms, such as reduced particle set diversity and sample depletion due to multiple sampling of particles with large weights, resulting in poorer filtering estimation; or decreased computational efficiency due to increasing the number of particles.

[0008] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0009] This invention provides a method for real-time localization and mapping in a microgravity environment, comprising the following steps:

[0010] S100. Establish the pose model of the intelligent space flight robot at time t and predict the pose vector value of the intelligent space flight robot; including predicting the pose vector of the intelligent space flight robot at time k based on the control vector of the intelligent space flight robot at time k and the motion model of the intelligent space flight robot, and calculating its covariance matrix.

[0011] S200, perform data association between the acquired observation information and existing landmarks in the current map; including using the maximization of the observation probability function to sequentially associate the acquired observation information with the landmarks in the estimated map of each particle;

[0012] S300, update road signs and adaptively resample; based on the FastSLAM algorithm, calculate the number of effective particles and judge the degree of particle degradation in real time; including, based on the weight probability, copying particles with larger weights and discarding particles with smaller weights;

[0013] S400, Particle Swarm Optimization: When any diversity measure is determined to be deteriorating according to step S300, the particle swarm optimization algorithm is used to optimize the particles. During the iterative optimization process of the particle swarm, the aggregation and dispersion of particles are guided by the diversity heuristic factor. The optimal diversity of the particle set is ensured by realizing the re-optimization distribution of the particles.

[0014] S500. Update the map by updating the map estimation results. This includes updating the corresponding particle association feature estimates based on the correlation information of each particle data in the particle set, calculating the mean and variance of each feature estimate, and adding observation information that is not associated with existing features in the map as new features to complete the map update.

[0015] Further, in step S100, the following are included:

[0016] Establish the pose model of the space intelligent flying robot at time t:

[0017]

[0018] In the formula, x r (t),y r (t),θ r (t) represents the horizontal and vertical coordinates and azimuth angle of the space intelligent flight robot at time t; v and γ represent the linear velocity and angular velocity of the space intelligent flight robot, respectively; ΔT represents the time step, which is the time interval between two adjacent time points in the discrete-time system.

[0019] The problem of instantaneous localization and map reconstruction for intelligent space-based flying robots is described as follows:

[0020]

[0021] In the formula, p(x) t |z t ,u t ,n t ) represents the posterior distribution of path estimation for a space-based intelligent flying robot; p(θ) i |x t ,z t ,u t ,n t ) represents the posterior distribution of environmental landmarks; x t {x1,x2,…,x t},z t ={z1,z2,…,z t},u t ={u1,u2,…,u t},n t ={n1,n2,…,n t} represent the historical information sets of the state variables, observation variables, control variables, and data association variables of the spatial intelligent flying robot from the initial time to time t;

[0022] Trajectory estimation is performed using a particle filter with m particles, and a single EKF filter is used to estimate the trajectory for an independent environmental landmark; therefore, the moving object has a total of m trajectories, and each trajectory contains N EKF filters; its structure is as follows:

[0023]

[0024] In the formula, x t,i This represents the path estimate for the i-th particle; and When path x t,i Given the mean and variance of the Gaussian function for the o-th landmark.

[0025] Further, in step S200, the following are included:

[0026] Calculate the state estimate of the spatial intelligent flying robot during the interval from sampling time t to t+1. The estimated value p of the covariance matrix t+1 Among them, state estimation Including position estimation for intelligent flying robots in space and landmark status estimation Right now:

[0027]

[0028] In the formula, and p l These are respectively related to position estimation for intelligent flying robots in space. and landmark status The associated error covariance matrix; The covariance matrix between the intelligent flying robot and the landmark; ▽h s 、▽h u These are the attitude update equations for space intelligent flying robots. Regarding posture x s The Jacobian matrix of the input control u; Q and R are the system noise and measurement noise variance matrices, respectively; For the observation model h, the state s t Jacobian matrix; For the observation model h to the input u t Jacobian matrix; U t For control input u t The noise covariance matrix;

[0029] The observed values ​​of a feature point are determined by its distance r and angle θ relative to the intelligent flying robot in space. To indicate, that is The observation equation for the i-th feature point at time t is expressed as:

[0030]

[0031] In the formula, This represents the estimated position of the i-th intelligent flying robot in the global coordinate system; This represents the estimated position of the spatial intelligent flying robot in the global coordinate system at time t; This represents the estimated attitude of the spatial intelligent flying robot in the global coordinate system at time t; This represents the state estimate of a spatial intelligent flying robot at time t; Let represent the expected observation value of the i-th target at time t.

[0032] Further, in step S300, the following are included:

[0033] The landmarks are updated and adaptively resampled to obtain the actual measurement value z through sensors on the space-based intelligent flying robot. i Then, the posterior augmented matrix state estimate and its covariance matrix are updated respectively:

[0034]

[0035] In the formula, and Let be the posterior Kalman filter mean and variance of the i-th landmark feature point, respectively; and K represents the prior Kalman filter mean and variance of the i-th landmark feature point, respectively; i,t+1 Let G be the filter gain matrix of the i-th landmark feature point at time t+1; θ Let be the Jacobian matrix of the observation equation with respect to the landmark positions; the weights are the ratios of the target distribution to the proposed distribution, i.e.:

[0036]

[0037] Further, in step S400, the following is included:

[0038] Particle swarm optimization, assuming the particle swarm size is n, the longest diagonal length in the search space is l, and the i-th particle in the k-th generation is... j represents the dimension of the search space, and the average center of the population is:

[0039]

[0040] The average point distance of a swarm of particles is defined as the diversity measure F of that particle set. diversity ,Right now:

[0041]

[0042] Subsequently, an upper threshold H for the particle set diversity index was defined. max and lower limit threshold H min When the diversity of the particle set is below the lower limit threshold, i.e., F diversity ≤H min At this time, it is necessary to make the particles move in the opposite direction to disperse the particle swarm; when the diversity of the particle set exceeds the upper limit threshold, i.e., F diversity >H max At this time, it is necessary to make the particles gather towards the global optimal position to reduce the search range; then the velocity evolution equation of the particle swarm is defined as:

[0043] v i (t)=wv i (t)+er1·(p i (t)-x i(t))+er2·(p g (t)-x i (t)) (10)

[0044] In the formula, e is the diversity heuristic factor, which controls the aggregation or dispersion behavior of the particle swarm; when F diversity ≤H min If e is currently 1, then e is set to -1; when F diversity >H max If the current value of e is -1, then e will be set to 1.

[0045] Particle i has two attributes, including position x. i =(x i1 ,x i2 ,…,x im ) T and velocity v i =(v i1 ,v i2 ,…,v in ) T Let particle x i The optimal position of an individual is p i =(p i1 ,p i2 ,…,p in ) T The global optimal position of the population is p. g =(p g1 ,p g2 ,…,p gn ) T In each iteration, the velocity and position of particle i are respectively:

[0046]

[0047] In the formula, i = 1, 2, ..., n; w is the inertia weight; c1 is the cognitive coefficient; c2 is the social coefficient; r1 and r2 are uniformly distributed random numbers;

[0048] The degree of particle degradation is determined by the effective sample N. eff Perform measurement:

[0049]

[0050] In the formula, Let be the weight value of the i-th particle at time t; m is the total number of particles;

[0051] If N eff Below the set threshold N threshold This indicates severe particle degradation, necessitating resampling to improve particle distribution; when F diversity Greater than or less than H minWhen the distribution of the particle set is updated, the particle swarm velocity update equation based on the diversity heuristic factor is performed by equation (4) to make the particle set closer to the true posterior probability density.

[0052] A real-time positioning and mapping system in a microgravity environment, the system having program modules corresponding to the above steps, and executing the steps in the above-described real-time positioning and mapping method in a microgravity environment when running.

[0053] A computer-readable storage medium storing a computer program configured to, when invoked by a processor, implement the steps of a real-time positioning and mapping method in a microgravity environment.

[0054] Compared with the prior art, the beneficial effects of the present invention are:

[0055] ① Based on the design of a simultaneous localization and mapping method using particle swarm optimization, the predicted particles in FastSLAM are updated by using particle swarm optimization to adjust the proposed distribution of particles so that the predicted sampled particles are concentrated near the actual pose of the intelligent flying robot in space.

[0056] ② An improved adaptive resampling design achieves effective resampling by calculating the number of effective particles and evaluating the degree of particle degradation in real time. This improves the sample depletion caused by frequent resampling and enhances the robustness of the FastSLAM algorithm. As a result, the algorithm proposed in this invention not only inherits the advantages of FastSLAM's nonlinearity and non-Gaussian universal applicability, but also improves the estimation accuracy of paths and landmark positions for intelligent flying robots in space.

[0057] ③ RBPF-SLAM Design Based on Particle Swarm Optimization: This method improves upon the basic FastSLAM method by incorporating particle swarm optimization. It employs a particle swarm optimization algorithm for particle redistribution, making the particle set closer to the true posterior probability distribution. More importantly, the algorithm uses particle set diversity as a heuristic to guide the particle optimization search process. By leveraging the global optimal search of the particle swarm, it ensures optimal particle set diversity, thereby improving the system's filtering estimation performance.

[0058] In summary, this invention optimizes the particle sampling process using Gaussian particle swarm optimization and employs particle set diversity as a search heuristic to ensure optimal population diversity, thereby mitigating particle degradation. The movement of the particle set towards regions with higher posterior probabilities improves both the accuracy of state estimation and the convergence of the algorithm. The proposed method achieves higher localization and map-building accuracy with fewer particles and better stability in filtering estimation. Attached Figure Description

[0059] Figure 1This is a flowchart of a real-time positioning and mapping method under microgravity environment according to an embodiment of the present invention;

[0060] Figure 2 The above is a comparison of the simulation results of path trajectory and landmark position before and after the improvement of the real-time localization and mapping method in microgravity environment in an embodiment of the present invention. Among them, (a) is the estimation result of FastSLAM algorithm (100 particles) and (b) is the estimation result of the improved RBPF-FastSLAM algorithm (10 particles) of the present invention.

[0061] Figure 3 This is a comparison of particle diversity before and after an improvement to a real-time localization and mapping method under microgravity conditions, as described in an embodiment of the present invention.

[0062] Figure 4 This is a map updated using a real-time positioning and mapping method under microgravity conditions, as described in an embodiment of the present invention. Detailed Implementation

[0063] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0064] Specific Implementation Plan 1: Combining Figure 1 As shown, this invention provides a method for real-time positioning and mapping in a microgravity environment, comprising the following steps:

[0065] S100. Establish the pose model of the space intelligent flight robot at time t.

[0066] Predicting the pose vector value of a space intelligent flight robot includes predicting the pose vector of the space intelligent flight robot at time k based on the control vector of the space intelligent flight robot at time t and the motion model of the space intelligent flight robot, and calculating its covariance matrix;

[0067] Specifically, it includes:

[0068] Establish the pose model of the space intelligent flying robot at time t:

[0069]

[0070] In the formula, x r (t),y r (t),θ r (t) represents the horizontal and vertical coordinates and azimuth angle of the space intelligent flight robot at time t; v and γ represent the linear velocity and angular velocity of the space intelligent flight robot, respectively; ΔT represents the time step, which is the time interval between two adjacent time points in the discrete-time system.

[0071] Therefore, the SLAM problem for intelligent flying robots in space can be described as:

[0072]

[0073] In the formula, p(x) t |z t ,u t ,n t ) represents the posterior distribution of path estimation for a space-based intelligent flying robot; p(θ) i |x t ,z t ,u t ,n t ) represents the posterior distribution of environmental landmarks; x t {x1,x2,…,x t},z t ={z1,z2,…,z t},u t ={u1,u2,…,u t},n t ={n1,n2,…,n t} represent the historical information sets of the state variables, observation variables, control variables, and data association variables of the spatial intelligent flying robot from the initial time to time t;

[0074] Θ = {θ1, θ2, ..., θ N} represents an environmental map consisting of N landmarks, where each landmark is independent of the others;

[0075] This invention employs a particle filter with m particles for trajectory estimation and a single EKF filter for estimating an independent environmental landmark; therefore, the space intelligent flight robot has a total of m trajectories, each containing N EKF filters; its structure can be represented as:

[0076]

[0077] In the formula, x t,i This represents the path estimate for the i-th particle; and When path x t,i Given the mean and variance of the Gaussian function for the o-th landmark;

[0078] S200. Establish an observation model and perform data correlation.

[0079] The acquired observation information is correlated with existing landmarks in the current map, including by using the maximum observation probability function to sequentially correlate the acquired observation information with the landmarks in the estimated map of each particle.

[0080] Specifically, it includes:

[0081] Calculate the state estimate of the spatial intelligent flying robot during the interval from sampling time t to t+1. The estimated value p of the covariance matrix t+1 Among them, state estimation Including position estimation for intelligent flying robots in space and landmark status estimation Right now:

[0082]

[0083] In the formula, and p l These are respectively related to position estimation for intelligent flying robots in space. and landmark status The associated error covariance matrix; The covariance matrix between the intelligent flying robot and the landmark; ▽h s 、▽h u These are the attitude update equations for space intelligent flying robots. Regarding posture x s The Jacobian matrix of the input control u; Q and R are the system noise and measurement noise variance matrices, respectively; For the observation model h, the state s t The Jacobian matrix represents how the observation changes with the state; For the observation model h to the input u t The Jacobian matrix represents how the observation changes with the control input; U t For control input u t The noise covariance matrix represents the control error;

[0084] The observed values ​​of a feature point are determined by its distance r and angle θ relative to the intelligent flying robot in space. To indicate, that is The observation equation for the i-th feature point at time t can be expressed as:

[0085]

[0086] In the formula, This represents the estimated position of the i-th intelligent flying robot in the global coordinate system; This represents the estimated position of the spatial intelligent flying robot in the global coordinate system at time t; This represents the estimated attitude of the spatial intelligent flying robot in the global coordinate system at time t; This represents the state estimate of a spatial intelligent flying robot at time t; This represents the expected observation value of the i-th target at time t;

[0087] S300, update road signs and adaptively resample

[0088] Based on the FastSLAM algorithm, this method calculates the number of effective particles and evaluates the degree of particle degradation in real time. This includes replicating particles with larger weights and discarding particles with smaller weights based on their weight probabilities, thereby achieving effective resampling and mitigating the sample depletion caused by frequent resampling. Furthermore, it uses multiple weighted random samples to represent the posterior probability distribution of the problem under consideration, further mitigating the sample depletion caused by frequent resampling and improving the robustness of the FastSLAM algorithm.

[0089] Specifically, it includes:

[0090] The landmarks are updated and adaptively resampled to obtain the actual measurement value z through sensors on the space-based intelligent flying robot. i Then, the posterior augmented matrix state estimate and its covariance matrix are updated respectively:

[0091]

[0092] In the formula, and Let be the posterior Kalman filter mean and variance of the i-th landmark feature point, respectively; and K represents the prior Kalman filter mean and variance of the i-th landmark feature point, respectively; i,t+1 Let G be the filter gain matrix of the i-th landmark feature point at time t+1; θ Let be the Jacobian matrix of the observation equation with respect to the landmark positions; the weights are the ratios of the target distribution to the proposed distribution, i.e.:

[0093]

[0094] S400, Particle Swarm Optimization

[0095] To address the particle degradation and sample depletion issues in FastSLAM, a method for real-time monitoring of particle set diversity measures, specifically the "population average point distance," is proposed. This method involves optimizing particles using a particle swarm optimization algorithm when any diversity measure is identified as deteriorating according to step S300. During the iterative optimization process, a diversity heuristic factor guides particle aggregation and dispersion, ensuring optimal particle set diversity through re-optimization. The "population average point distance" measure is used as the primary indicator of particle set diversity, guiding appropriate dispersion and aggregation of particles to maintain optimal diversity. The effectiveness of the proposed method in suppressing particle degradation, avoiding sample depletion, and improving positioning accuracy is verified through particle diversity comparison charts.

[0096] Specifically, it includes:

[0097] Particle swarm optimization, assuming the particle swarm size is n, the longest diagonal length in the search space is l, and the i-th particle in the k-th generation is... If j represents the dimension of the search space, then the average center of the population is:

[0098]

[0099] The average point distance of the particle group is then defined as the diversity measure factor F of the particle set. diversity ,Right now:

[0100]

[0101] Subsequently, an upper threshold H for the particle set diversity index was defined. max and lower limit threshold H min When the diversity of the particle set is below the lower limit threshold, i.e., F diversity ≤H min When the particle diversity exceeds a set threshold, it indicates that the particles in the swarm are too dense, exceeding the set value. The particles need to be moved in the opposite direction to appropriately disperse the swarm, expand the search range, and improve the swarm's search capability. When the particle diversity exceeds the upper limit threshold, i.e., F... diversity >H max When this occurs, it indicates that the particles in the swarm are too scattered, and it is necessary to gather the particles towards the optimal position to reduce the search range; therefore, the velocity evolution equation of the particle swarm is defined as:

[0102] v i (t)=wv i (t)+er1·(p i (t)-x i (t))+er2·(p g (t)-x i (t)) (22)

[0103] In the formula, e is a diversity heuristic factor that controls the aggregation (convergence) or dispersion (exploration) behavior of the particle swarm; when F diversity ≤H min If the particle swarm is too concentrated and needs to be dispersed, then if the current e is 1, set e to -1; when F diversity >H max If the particle swarm is too scattered and needs to be gathered, then if the current e is -1, then set e to 1.

[0104] To address particle degradation, a resampled particle set is used as the initial population, and adaptive resampling is performed to determine the effective number of particles in the population to improve algorithm efficiency. Particle i has two attributes: position and velocity v. i =(vi1 ,v i2 ,…,v in ) T And let particle x i The optimal position of an individual is p i =(p i1 ,p i2 ,…,p in ) T The global optimal position of the population is p. g =(p g1 ,p g2 ,…,p gn ) T In each iteration, the velocity and position of particle i are respectively:

[0105]

[0106] In the formula, i = 1, 2, ..., n; w is the inertia weight; c1 is the cognitive coefficient; c2 is the social coefficient; r1 and r2 are uniformly distributed random numbers;

[0107] The degree of particle degradation is determined by the effective sample N. eff Perform the measurement and set a threshold N. threshold :

[0108]

[0109] In the formula, Let be the weight value of the i-th particle at time t; m is the total number of particles;

[0110] If N eff Below the set threshold N threshold This indicates severe particle degradation, requiring resampling to improve particle distribution; if F diversity Greater than or less than H min The distribution of the particle set is updated by using the particle swarm velocity update equation based on the diversity heuristic factor through equation (16), so that the particle set is closer to the true posterior probability density.

[0111] S500, Update Map

[0112] The map estimation results are updated; this includes updating the corresponding particle association feature estimates based on the correlation information of each particle in the particle set, calculating the mean and variance of each feature estimate, and adding observations that are not correlated with existing features in the map as new features. The updated map is as follows: Figure 4 As shown.

[0113] Specific Implementation Scheme 2: The present invention provides a real-time positioning and mapping system in a microgravity environment. The system has a program module corresponding to the above steps, and executes the steps in the above-mentioned real-time positioning and mapping method in a microgravity environment when running.

[0114] The other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme 1.

[0115] Specific Implementation Scheme 3: The present invention provides a computer-readable storage medium storing a computer program configured to implement, when called by a processor, the steps of a real-time positioning and mapping method in a microgravity environment.

[0116] The other combinations and connections in this implementation scheme are the same as in Specific Implementation Scheme 1.

[0117] Simulation Experiment

[0118] The space-based intelligent flying robot is 266mm long, 235mm wide, and 170mm high. The measured prototype weighs 4.5kg. It utilizes NVIDIA's Jetson Xavier... TM NX, run the algorithm.

[0119] To verify the effectiveness of the proposed algorithm, a Matlab environment was selected, and simulations were performed using both the basic FastSLAM algorithm and the diversity-inspired Gaussian particle swarm optimization RBPF-SLAM. The displacement error of the proposed improved RBPF-SLAM algorithm using 10 particles in the motion of a spatial intelligent flying robot was compared with the result of the basic FastSLAM algorithm using 100 particles, showing greater accuracy. Figure 2 As shown, the RBPF-FastSLAM algorithm proposed in this invention provides more accurate estimation results for the trajectory and landmarks of a spatial intelligent flying robot using 10 particles than the estimation results of the basic FastSLAM algorithm using 100 particles. This demonstrates that the algorithm proposed in this invention can achieve high-precision filtering estimation using fewer particles.

[0120] Combination Figure 3 As shown, by comparing particle diversity, it was found that the diversity measure of the improved RBPF-SLAM algorithm proposed in this invention has been relatively stable and has always been within the appropriate range of diversity (upper and lower threshold range), while the diversity measure of the basic FastSLAM algorithm fluctuates greatly, is extremely unstable, and deviates far from the appropriate range of diversity.

[0121] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A method for real-time positioning and mapping in a microgravity environment, characterized in that, Includes the following steps: S100, establishing intelligent flight robots in space A time-space pose model predicts the pose vector values ​​of a space-based intelligent flight robot; including based on the space-based intelligent flight robot's... Time-based control vectors and motion models of space-based intelligent flying robots for prediction The spatial pose vector of the intelligent flying robot at any given time is calculated, and its covariance matrix is ​​also calculated. include ， Establishing space intelligent flying robots in Time-of-motion pose model: In the formula, They are respectively space intelligent flying robots in The horizontal and vertical coordinates and azimuth at any given time; and These are the linear velocity and angular velocity of the space intelligent flying robot, respectively. It represents the time step, which is the time interval between two adjacent time points in a discrete-time system; The problem of instantaneous localization and map reconstruction for intelligent space-based flying robots is described as follows: In the formula, This represents the posterior distribution of the path estimation for a spatial intelligent flying robot; Represents the posterior distribution of environmental landmarks; They represent the time from the initial time to... A historical information set of state variables, observation variables, control variables, and data correlation variables of a time-space intelligent flying robot; use Trajectory estimation is performed using a particle filter for each particle, and a single EKF filter is used to estimate the trajectory of an independent environmental landmark; therefore, the moving object has a total of 3 tracks, each track contains One EKF filter; Its structure is as follows: In the formula, Indicates the first Path estimation for individual particles; and Each is the path Given the mean and variance of the Gaussian function for the o-th landmark; S200, perform data association between the acquired observation information and existing landmarks in the current map; including using the maximization of the observation probability function to sequentially associate the acquired observation information with the landmarks in the estimated map of each particle; S300, update road signs and adaptively resample; based on the FastSLAM algorithm, calculate the number of effective particles and judge the degree of particle degradation in real time; including, based on the weight probability, copying particles with larger weights and discarding particles with smaller weights; S400, Particle Swarm Optimization: When any diversity measure is determined to be deteriorating according to step S300, the particle swarm optimization algorithm is used to optimize the particles. During the iterative optimization process of the particle swarm, the aggregation and dispersion of particles are guided by the diversity heuristic factor. The optimal diversity of the particle set is ensured by realizing the re-optimization distribution of the particles. S500. Update the map by updating the map estimation results. This includes updating the corresponding particle association feature estimates based on the correlation information of each particle data in the particle set, calculating the mean and variance of each feature estimate, and adding observation information that is not associated with existing features in the map as new features to complete the map update.

2. The method for real-time positioning and mapping in a microgravity environment according to claim 1, characterized in that: Step S200 includes, From sampling time arrive State estimation of a computational space intelligent flying robot within an interval The estimated value of the covariance matrix Among them, state estimation Including position estimation for intelligent flying robots in space and landmark status estimation ,Right now: In the formula, and These are respectively related to position estimation for intelligent flying robots in space. and landmark status The associated error covariance matrix; The covariance matrix between the intelligent flying robot and the landmark; , These are the attitude update equations for space intelligent flying robots. Regarding posture and input control The Jacobian matrix; and These are the system noise and measurement noise variance matrices, respectively. For observation model State The Jacobian matrix; For observation model For input The Jacobian matrix; To control input The noise covariance matrix; The observed value of a feature point is determined by its distance relative to the space-based intelligent flying robot. and angle To indicate, that is ,but The first moment The observation equation for each feature point is expressed as: （5） In the formula, The first space intelligent flying robot Estimated position in the global coordinate system; express Estimated position of the time-space intelligent flying robot in the global coordinate system; express Estimated attitude of a time-space intelligent flying robot in the global coordinate system; express State estimation of time-space intelligent flying robots; express Time for the first The expected observations of each target.

3. The method for real-time positioning and mapping in a microgravity environment according to claim 2, characterized in that: Step S300 includes, The navigation markers are updated and adaptively resampled to obtain actual measurements via sensors on the intelligent space-flying robot. Then, the posterior augmented matrix state estimate and its covariance matrix are updated respectively: In the formula, ; and The first The posterior Kalman filter mean and variance of each landmark feature point; and The first The prior Kalman filter mean and variance of each landmark feature point; For the first Road sign feature points The filter gain matrix at time step; Let be the Jacobian matrix of the observation equation with respect to the landmark positions; the weights are the ratios of the target distribution to the proposed distribution, i.e.: 。 4. The method for real-time positioning and mapping in a microgravity environment according to claim 3, characterized in that: Step S400 includes, Particle swarm optimization, assuming the particle swarm size is... The longest diagonal in the search space is , No. Sub-particles for , The dimension of the search space is represented by: The average center of the population is: The average point distance of a swarm of particles is defined as the diversity measure of that particle set. ,Right now: Subsequently, an upper threshold for the particle set diversity index was defined. and lower threshold When the diversity of the particle set is below the lower threshold, i.e. At this time, it is necessary to make the particles move in the opposite direction to disperse the particle swarm; when the diversity of the particle set exceeds the upper limit threshold, that is... At this time, it is necessary to make the particles gather towards the global optimal position to reduce the search range; then the velocity evolution equation of the particle swarm is defined as: In the formula, As a diversity heuristic, it controls the aggregation or dispersion behavior of particle swarms; when If at this time If it is 1, then... Set to -1; when If at this time If it is -1, then... Set to 1; particle It has two attributes, including position. and speed , causing particles The optimal position of an individual is The global optimal position of the population is In each iteration, the particles The speed and position are as follows: In the formula, ; Inertial weight; Cognitive coefficient; For social coefficients; These are uniformly distributed random numbers; The degree of particle degradation is determined using valid samples. Perform measurement: In the formula, For the first Individual particles The weight value at each moment; This represents the total number of particles; like Below the set threshold This indicates severe particle degradation, necessitating resampling to improve particle distribution; when Greater than or less than When the distribution of the particle set is updated, the particle swarm velocity update equation based on the diversity heuristic factor is performed by equation (4) to make the particle set closer to the true posterior probability density.

5. A real-time positioning and mapping system in a microgravity environment, characterized in that: The system has a program module corresponding to the steps described in any one of claims 1-4 above, and executes the steps in the above-described method for real-time positioning and mapping in a microgravity environment when it is run.

6. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to, when invoked by a processor, implement the steps of the real-time positioning and mapping method in a microgravity environment as described in any one of claims 1-4.

Images