Instant positioning and mapping method and system in microgravity environment

Abstract

The invention provides an instant positioning and mapping method and system in a microgravity environment, and belongs to the field of instant positioning and mapping of space intelligent flying robots. In order to solve the problems that in a traditional RBPF-SLAM algorithm, due to the fact that particles with large weights are sampled for multiple times, the diversity of particle sets is weakened, samples are exhausted, and filtering estimation becomes poor; or the calculation efficiency is reduced due to the increase of the number of particles. According to the method, the particle sampling process is optimized by using the Gaussian particle swarm, and the diversity measure of the particle set is used as a search heuristic factor to ensure that the diversity level of the swarm is optimal, so that the particle degeneration is relieved; the particle set moves towards the region with the high posterior probability, so that the estimation precision of the state is improved, and the convergence of the algorithm is also improved; according to the method provided by the invention, higher positioning and map building precision can be obtained through fewer particles, and better filtering estimation stability is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of real-time positioning and mapping of flying robots, and in particular to a real-time positioning and mapping method and system in a microgravity environment. Background Art

[0002] Various experiments and equipment maintenance tasks are being carried out more and more frequently in a microgravity environment. There is an urgent need for space intelligent flying robots to assist experimenters in completing a series of tasks such as tracking, lighting and shooting, equipment monitoring and maintenance, instrument transfer, equipment disassembly and replacement, and scientific experiment care, so as to reduce the workload of astronauts.

[0003] During this process, reliable and accurate environmental perception and safe and efficient path planning are key to ensuring the platform's stable operation. A prerequisite for achieving this goal is the robot's ability to perform simultaneous localization and mapping (SLAM) in a microgravity environment. Specifically, in the narrow, unstructured microgravity environment, the space intelligent flying robot effectively utilizes the coupled characteristics of its fan and flywheel power systems, fully leveraging its advantages of high reach and strong obstacle crossing, and autonomously constructs maps through multi-source perception fusion technology.

[0004] Due to the high demand for the passability of intelligent space flying robots, the computing speed of their computing units is relatively limited, necessitating the design of a lightweight SLAM method. Due to its fast mapping speed and low computational power consumption, the Rao-Blackwellized Particle Filter (RBPF)-based SLAM method has been widely used in recent years. It uses an onboard camera to build a map of an unknown environment and simultaneously calculates the motion trajectory of a mobile platform from this map. This method decomposes the SLAM problem into the robot localization problem and the position estimation of environmental features based on pose estimation. It uses a particle filter algorithm to estimate the pose of the entire path, and an extended Kalman filter (EKF) algorithm to estimate the position of environmental features. This method combines the advantages of the EKF and probabilistic methods, reducing computational complexity while also achieving good robustness.

[0005] However, in the process of estimating pose and building maps, the traditional RBPF-SLAM algorithm uses a sequence importance resampling algorithm, so particles with larger weights are selected multiple times, which results in more repeated parts in the sampling results. This leads to a decrease in particle set diversity and sample exhaustion, which makes the filter estimation performance worse, and in severe cases, filter divergence will occur. The simplest way to overcome sample exhaustion is to increase the number of particles, but the result of doing so will cause a sharp increase in the amount of computation and a decrease in algorithm efficiency. To this end, the present invention uses a particle set diversity measure as an inspiration factor to guide the particle optimization search process, ensuring the global optimal search of the particle swarm and the optimal level of particle set diversity, thereby improving the system filter estimation performance. Summary of the Invention

[0006] The technical problems to be solved by the present invention are:

[0007] In order to solve the problem that the traditional RBPF-SLAM algorithm has multiple sampling of particles with large weights, which leads to weakened diversity of particle sets and sample exhaustion, making the filter estimation worse; or increasing the number of particles leads to decreased computational efficiency.

[0008] The present invention is to solve the above technical problems using the following technical solutions:

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

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

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

[0012] S300, updating the landmarks and adaptively resampling; based on the FastSLAM algorithm, calculating the number of valid particles and judging the degree of particle degradation in real time; including replicating particles with larger weights and discarding particles with smaller weights based on weight probability;

[0013] S400, particle swarm optimization; when it is determined according to step S300 that any diversity measure has deteriorated, a particle swarm algorithm is used to optimize the particles, and during the particle swarm iterative optimization process, a diversity heuristic factor is used to guide the aggregation and dispersion of the particles, thereby achieving a re-optimized distribution of the particles to ensure optimal diversity of the particle set;

[0014] S500, update the map, update the map estimation results; including updating the corresponding particle association feature estimation based on the data association information of each particle in the particle set, and calculating the mean and variance of each feature estimation. Observation information that is not associated with existing features in the map is added as a new feature to the map to complete the map update.

[0015] Furthermore, in step S100, it includes:

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

[0017]

[0018] Where x r (t),y r (t),θ r (t) are the horizontal and vertical coordinate positions and azimuth angles of the space intelligent flying robot at time t; v and γ are the linear velocity and angular velocity of the space intelligent flying robot respectively; ΔT represents the time step, that is, the time interval between two adjacent time points in a discrete time system;

[0019] The real-time positioning and map reconstruction problem of space intelligent flying robots can be described as follows:

[0020]

[0021] In the formula, p(x t |z t ,u t ,n t ) represents the posterior distribution of the path estimation of the space 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} respectively represent the historical information set of the state quantity, observation quantity, control quantity and data association quantity of the spatial intelligent flying robot from the initial moment to moment t;

[0022] A particle filter with m particles is used for trajectory estimation, and a separate EKF filter is used to estimate an independent environmental landmark. Therefore, the moving object has a total of m trajectories, and each trajectory contains N EKF filters. Its structure is:

[0023]

[0024] Where x t,i represents the path estimate of the i-th particle; and When the path x t,i The mean and variance of the Gaussian function of the o-th landmark when known.

[0025] Furthermore, in step S200, including:

[0026] Calculate the state estimation of the space intelligent flying robot from the sampling time t to t+1 and the estimated value of the covariance matrix p t+1 , where the state estimate Including position estimation of space intelligent flying robots and landmark state estimation Right now:

[0027]

[0028] Where, and p l Position estimation of space intelligent flying robots and landmark status the associated error covariance matrix; is the covariance matrix between the space intelligent flying robot and the landmark; ▽h s 、▽h u They are the attitude update equations of the space intelligent flying robot About Posturex s and the Jacobian matrix of the input control u; Q and R are the variance matrices of the system noise and measurement noise respectively; For the observation model h to state s t The Jacobian matrix of ; For the observation model h to input u t The Jacobian matrix of U t is the control input u t The noise covariance matrix of ;

[0029] The observation value of the feature point is composed of its distance r and angle θ relative to the space intelligent flying robot. To express, that is Then the observation equation of the i-th feature point at time t is expressed as:

[0030]

[0031] Where, represents the estimated position of the i-th space intelligent flying robot in the global coordinate system; represents the estimated position of the spatial intelligent flying robot in the global coordinate system at time t; represents the estimated posture of the spatial intelligent flying robot in the global coordinate system at time t; represents the state estimation of the space intelligent flying robot at time t; represents the expected observation value of the i-th target at time t.

[0032] Furthermore, in step S300, it includes:

[0033] Update the landmarks and adaptively resample to obtain the actual measurement value z through the sensors on the space intelligent flying robot i After that, the posterior augmented matrix state estimate and its covariance matrix are updated respectively:

[0034]

[0035] Where, and are the mean and variance of the posterior Kalman filter of the i-th landmark feature point; and are the prior Kalman filter mean and variance of the i-th landmark feature point; K i,t+1 is the filter gain matrix of the i-th landmark feature point at time t+1; G θ is the Jacobian matrix of the observation equation with respect to the landmark position; the weight is the ratio of the target distribution to the proposed distribution, that is:

[0036]

[0037] Furthermore, in step S400, it includes:

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

[0039]

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

[0041]

[0042] Then, define the upper limit threshold H of the particle set diversity index max and the lower threshold H min , when the particle set diversity is lower than the lower threshold, that is, F diversity ≤H min When the particle set diversity is greater than the upper threshold, that is, F diversity >H max When , it is necessary to make the particles gather toward the global optimal position and 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] Where, e is the diversity heuristic factor, which controls the aggregation or dispersion behavior of the particle swarm; when F diversity ≤H min , if the current e is 1, then set e to -1; when F diversity >H max , at this time, if the current e is -1, then set e to 1;

[0045] Particle i has two properties, including position x i =(x i1 ,x i2 ,…,x im ) T and speed 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:

[0046]

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

[0048] The particle degradation degree is calculated using the effective sample N eff To measure:

[0049]

[0050] Where, is 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 , indicating that the particle degradation phenomenon is serious and resampling is needed to improve the particle distribution; when F diversity Greater or less than H minWhen , the particle swarm velocity update equation based on the diversity heuristic factor is performed through formula (4) to update the distribution of the particle set, making the particle set closer to the true posterior probability density.

[0052] A system for real-time positioning and mapping in a microgravity environment has a program module corresponding to the above steps, and executes the steps of the above-mentioned method for real-time positioning and mapping in a microgravity environment when running.

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

[0054] Compared with the prior art, the present invention has the following beneficial effects:

[0055] ① Design of a simultaneous localization and mapping method based on particle swarm optimization. The particle swarm optimization method is used to update the estimated particles in FastSLAM and adjust the proposed distribution of particles so that the predicted sampling particles are concentrated near the actual pose of the space intelligent flying robot.

[0056] ② The improved design of adaptive resampling realizes effective resampling operation by calculating the number of effective particles and judging the degree of particle degradation in real time, thereby improving the sample depletion effect caused by frequent resampling and improving the robustness of the FastSLAM algorithm. This makes the algorithm proposed in this invention not only inherit the nonlinear and non-Gaussian universal applicability advantages of FastSLAM, but also improves the estimation accuracy of the path and landmark position of the intelligent flying robot in space.

[0057] ③ RBPF-SLAM design based on particle swarm evolutionary computation. This method improves the basic FastSLAM method with a particle swarm algorithm, using a particle swarm algorithm to search and optimize for particle redistribution, making the particle set closer to the true posterior probability distribution. More importantly, the algorithm uses the particle set diversity measure as a heuristic factor to guide the particle optimization search process. The particle swarm's global optimal search ensures the optimal level of particle set diversity, improving the system's filter estimation performance.

[0058] In summary, this invention utilizes Gaussian particle swarms to optimize the particle sampling process, using a particle set diversity measure as a search heuristic to ensure optimal swarm diversity, thereby mitigating particle degradation. The movement of particle sets toward regions with higher posterior probabilities improves both state estimation accuracy and algorithm convergence. The proposed method achieves higher positioning and mapping accuracy with fewer particles, while also achieving improved filter estimation stability. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0060] Figure 2 Figure 1 compares the simulation results of path trajectories and landmark positions before and after the improvement of a real-time positioning and mapping method in a microgravity environment according to an embodiment of the present invention. (a) shows the estimation result of the FastSLAM algorithm (100 particles), and (b) shows the estimation result of the improved RBPF-FastSLAM algorithm (10 particles) according to the present invention.

[0061] Figure 3 A comparison diagram of particle diversity before and after improvement of a real-time positioning and mapping method in a microgravity environment according to an embodiment of the present invention;

[0062] Figure 4 This is a map updated using a real-time positioning and mapping method in a microgravity environment in an embodiment of the present invention. DETAILED DESCRIPTION

[0063] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0064] Specific implementation plan 1: Combined Figure 1 As shown, the present invention provides a method for real-time positioning and mapping in a microgravity environment, comprising the following steps:

[0065] S100, establish the position model of the space intelligent flying robot at time t

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

[0067] Specifically include:

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

[0069]

[0070] Where x r (t),y r (t),θ r (t) are the horizontal and vertical coordinate positions and azimuth angles of the space intelligent flying robot at time t; v and γ are the linear velocity and angular velocity of the space intelligent flying robot respectively; ΔT represents the time step, that is, the time interval between two adjacent time points in a discrete time system;

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

[0072]

[0073] In the formula, p(x t |z t ,u t ,n t ) represents the posterior distribution of the path estimation of the space 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} respectively represent the historical information set of the state quantity, observation quantity, control quantity and data association quantity of the spatial intelligent flying robot from the initial moment to moment t;

[0074] Θ={θ1,θ2…,θ N}, represents an environment map composed of N landmarks, and each landmark is independent of each other;

[0075] The present invention uses a particle filter with m particles for trajectory estimation and a separate EKF filter to estimate an independent environmental landmark. Therefore, the space intelligent flying robot has a total of m trajectories, each of which contains N EKF filters. Its structure can be expressed as:

[0076]

[0077] Where x t,i represents the path estimate of the i-th particle; and When the path x t,i When known, the mean and variance of the Gaussian function of the oth landmark;

[0078] S200, establish observation model and make data association

[0079] Associating the acquired observation information with existing landmarks in the current map, including using a maximization observation probability function to sequentially associate the acquired observation information with landmarks in the estimated map of each particle;

[0080] Specifically include:

[0081] Calculate the state estimation of the space intelligent flying robot from the sampling time t to t+1 and the estimated value of the covariance matrix p t+1 , where the state estimate Including position estimation of space intelligent flying robots and landmark state estimation Right now:

[0082]

[0083] Where, and p l Position estimation of space intelligent flying robots and landmark status the associated error covariance matrix; is the covariance matrix between the space intelligent flying robot and the landmark; ▽h s 、▽h u They are the attitude update equations of the space intelligent flying robot About Posturex s and the Jacobian matrix of the input control u; Q and R are the variance matrices of the system noise and measurement noise respectively; For the observation model h to state s t The Jacobian matrix of , which represents how the observation changes with the state; For the observation model h to input u t The Jacobian matrix of U represents how the observation changes with the control input; t is the control input u t The noise covariance matrix of represents the control error;

[0084] The observation value of the feature point is composed of its distance r and angle θ relative to the space intelligent flying robot. To express, that is Then the observation equation of the i-th feature point at time t can be expressed as:

[0085]

[0086] Where, represents the estimated position of the i-th space intelligent flying robot in the global coordinate system; represents the estimated position of the spatial intelligent flying robot in the global coordinate system at time t; represents the estimated posture of the spatial intelligent flying robot in the global coordinate system at time t; represents the state estimation of the space intelligent flying robot at time t; 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, the number of valid particles and the degree of particle degradation are calculated in real time. This includes duplicating particles with larger weights and discarding particles with smaller weights based on weighted probabilities to achieve effective resampling operations, thereby effectively improving the sample depletion effect caused by frequent resampling. In addition, multiple random samples with weights are used to represent the posterior probability distribution of the problem under consideration, effectively improving the sample depletion effect caused by frequent resampling and enhancing the robustness of the FastSLAM algorithm.

[0089] Specifically include:

[0090] Update the landmarks and adaptively resample to obtain the actual measurement value z through the sensors on the space intelligent flying robot i After that, the posterior augmented matrix state estimate and its covariance matrix are updated respectively:

[0091]

[0092] Where, and are the mean and variance of the posterior Kalman filter of the i-th landmark feature point; and are the prior Kalman filter mean and variance of the i-th landmark feature point; K i,t+1 is the filter gain matrix of the i-th landmark feature point at time t+1; G θ is the Jacobian matrix of the observation equation with respect to the landmark position; the weight is the ratio of the target distribution to the proposed distribution, that is:

[0093]

[0094] S400, Particle Swarm Optimization

[0095] To address the particle degradation and sample exhaustion issues of FastSLAM, a method for real-time monitoring of the "group average point distance" particle set diversity measure is proposed. This method includes: when any diversity measure is judged to have deteriorated according to step S300, a particle swarm algorithm is used to optimize the particles, and a diversity heuristic factor is used to guide the aggregation and dispersion of the particles during the iterative optimization process of the particle swarm, thereby ensuring the optimal diversity of the particle set by re-optimizing the distribution of the particles; the "group average point distance" measure is used as the main indicator to measure the quality of the particle set diversity, and this measure is used as an heuristic factor to guide the appropriate dispersion and aggregation of each particle in the particle set to maintain the optimal diversity; and the effectiveness of the proposed method in suppressing particle degradation, avoiding sample exhaustion, and improving positioning accuracy is verified by comparing the particle diversity graph.

[0096] Specifically include:

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

[0098]

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

[0100]

[0101] Then, define the upper limit threshold H of the particle set diversity index max and the lower threshold H min , when the particle set diversity is lower than the lower threshold, that is, F diversity ≤H min When , it indicates that the particles in the particle swarm are too dense, exceeding the set value, and it is necessary to make the particles move in the opposite direction to disperse the particle swarm appropriately, expand the search range, and improve the search ability of the particle swarm; when the diversity of the particle set is greater than the upper limit threshold, that is, F diversity >H max When , it indicates that the particles in the particle swarm are too dispersed, and it is necessary to make the particles gather toward the optimal position and reduce the search range; then 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] Where e is the diversity heuristic factor, which controls the aggregation (convergence) or dispersion (exploration) behavior of the particle swarm; when F diversity ≤H min , the particle group is too concentrated and needs to be dispersed. If the current e is 1, then e is set to -1; when F diversity >H max , the particle group is too dispersed and needs to be aggregated. At this time, if the current e is -1, then e is set to 1;

[0104] In order to solve the particle degradation phenomenon, the particle set obtained by resampling is used as the initial population, and the effective number of particles in the population is determined to perform adaptive resampling to improve the efficiency of the algorithm; 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:

[0105]

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

[0107] The particle degradation degree is calculated using the effective sample N eff Measure and set the threshold N threshold :

[0108]

[0109] Where, is 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 , indicating that the particle degradation phenomenon is serious and resampling is needed to improve the particle distribution; if F diversity Greater or less than H min , the particle swarm velocity update equation based on the diversity heuristic factor is performed through formula (16) to update the distribution of the particle set, making the particle set closer to the true posterior probability density;

[0111] S500, updated map

[0112] Update the map estimation results; including updating the corresponding particle correlation feature estimation based on the data correlation information of each particle in the particle set, and calculating the mean and variance of each feature estimation, adding those observation information that are not associated with the existing features in the map as new features to the map. The updated map is as follows Figure 4 shown.

[0113] Specific implementation scheme 2: The present invention provides a system for real-time positioning and mapping in a microgravity environment. The system has a program module corresponding to the above steps, and executes the steps of the above-mentioned method for real-time positioning and mapping in a microgravity environment during operation.

[0114] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0115] Specific implementation scheme three: The present invention provides a computer-readable storage medium, which stores a computer program. The computer program is configured to implement the steps of a method for instant positioning and mapping in a microgravity environment when called by a processor.

[0116] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0117] Simulation experiment

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

[0119] In order to verify the effectiveness of the algorithm proposed in this invention, Matlab operating environment was selected to carry out simulations using the basic FastSLAM algorithm and the RBPF-SLAM optimized by Gaussian particle swarm optimization inspired by diversity. The displacement error of the space intelligent flying robot in the motion of the improved RBPF-SLAM algorithm proposed in this invention using 10 particles was compared with the result of the basic FastSLAM algorithm using 100 particles. The result was more accurate, as shown in Figure 2. Figure 2 The RBPF-FastSLAM algorithm proposed in this invention uses 10 particles to estimate the trajectory and landmarks of a space intelligent flying robot more accurately than the basic FastSLAM algorithm using 100 particles, indicating that the algorithm proposed in this invention can achieve higher-precision filter estimation using fewer particles.

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

[0121] Although the present invention is disclosed as above, the scope of protection disclosed by the present invention is not limited thereto. Those skilled in the art of the present invention may make various changes and modifications without departing from the spirit and scope of the present invention, and these 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: The following steps are involved: S100, establishing a pose model of the space intelligent flying robot at time t and predicting the pose vector value of the space intelligent flying robot; including predicting the pose vector of the space intelligent flying robot at time k based on the control vector of the space intelligent flying robot and the motion model of the space intelligent flying robot, and calculating its covariance matrix; S200, data-associate the acquired observation information with existing landmarks in the current map; including using a maximization observation probability function to sequentially associate the acquired observation information with landmarks in the estimated map of each particle; S300, updating the landmarks and adaptively resampling; based on the FastSLAM algorithm, calculating the number of valid particles and judging the degree of particle degradation in real time; including replicating particles with larger weights and discarding particles with smaller weights based on weight probability; S400, particle swarm optimization; when it is determined according to step S300 that any diversity measure has deteriorated, a particle swarm algorithm is used to optimize the particles, and during the particle swarm iterative optimization process, a diversity heuristic factor is used to guide the aggregation and dispersion of the particles, thereby achieving a re-optimized distribution of the particles to ensure optimal diversity of the particle set; S500, update the map, update the map estimation results; including updating the corresponding particle association feature estimation based on the data association information of each particle in the particle set, and calculating the mean and variance of each feature estimation. Observation information that is not associated with existing features in the map is added as a new feature to the map 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: In step S100, it includes: Establish the posture model of the space intelligent flying robot at time t: Where x r (t),y r (t),θ r (t) are the horizontal and vertical coordinate positions and azimuth angles of the space intelligent flying robot at time t; v and γ are the linear velocity and angular velocity of the space intelligent flying robot respectively; ΔT represents the time step, that is, the time interval between two adjacent time points in a discrete time system; The real-time positioning and map reconstruction problem of space intelligent flying robots can be described as follows: In the formula, p(x t |z t ,u t ,n t ) represents the posterior distribution of the path estimation of the space 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 } respectively represent the historical information set of the state quantity, observation quantity, control quantity and data association quantity of the spatial intelligent flying robot from the initial moment to moment t; A particle filter with m particles is used for trajectory estimation, and a separate EKF filter is used to estimate an independent environmental landmark; therefore, the moving object has a total of m trajectories, and each trajectory contains N EKF filters; Its structure is: Where x t,i represents the path estimate of the i-th particle; and When the path x t,i The mean and variance of the Gaussian function of the o-th landmark when known.

3. The method for real-time positioning and mapping in a microgravity environment according to claim 2, characterized in that: In step S200, it includes: Calculate the state estimation of the space intelligent flying robot from the sampling time t to t+1 and the estimated value of the covariance matrix p t+1 , where the state estimate Including position estimation of space intelligent flying robots and landmark state estimation Right now: Where p st and p l Position estimation of space intelligent flying robots and landmark status the associated error covariance matrix; is the covariance matrix between the space intelligent flying robot and the landmark; ▽h s 、▽h u They are the attitude update equations of the space intelligent flying robot About Posturex s and the Jacobian matrix of the input control u; Q and R are the variance matrices of the system noise and measurement noise respectively; For the observation model h to state s t The Jacobian matrix of ; For the observation model h to input u t The Jacobian matrix of U t is the control input u t The noise covariance matrix of ; The observation value of the feature point is composed of its distance r and angle θ relative to the space intelligent flying robot. To express, that is Then the observation equation of the i-th feature point at time t is expressed as: Where, represents the estimated position of the i-th space intelligent flying robot in the global coordinate system; represents the estimated position of the spatial intelligent flying robot in the global coordinate system at time t; represents the estimated posture of the spatial intelligent flying robot in the global coordinate system at time t; represents the state estimation of the space intelligent flying robot at time t; represents the expected observation value of the i-th target at time t.

4. The method for real-time positioning and mapping in a microgravity environment according to claim 3, characterized in that: In step S300, it includes: Update the landmarks and adaptively resample to obtain the actual measurement value z through the sensors on the space intelligent flying robot i After that, the posterior augmented matrix state estimate and its covariance matrix are updated respectively: Where, and are the mean and variance of the posterior Kalman filter of the i-th landmark feature point; and are the prior Kalman filter mean and variance of the i-th landmark feature point; K i,t+1 is the filter gain matrix of the i-th landmark feature point at time t+1; G θ is the Jacobian matrix of the observation equation with respect to the landmark position; the weight is the ratio of the target distribution to the proposed distribution, that is:

5. The method for real-time positioning and mapping in a microgravity environment according to claim 4, characterized in that: In step S400, it includes: Particle swarm optimization, assuming that the particle swarm size is n, the longest diagonal length in the search space is l, and the k-th generation particle i is j represents the dimension of the search space, and the mean center of the population is: The average point distance of a particle set is defined as the diversity measure factor F of the particle set. diversity ,Right now: Then, define the upper limit threshold H of the particle set diversity index max and the lower threshold H min , when the particle set diversity is lower than the lower threshold, that is, F diversity ≤H min When the particle set diversity is greater than the upper threshold, that is, F diversity >H max When , it is necessary to make the particles gather toward the global optimal position and reduce the search range; then the velocity evolution equation of the particle swarm is defined as: v i (t)=wv i (t)+er1·(p i (t)-x i (t))+er2·(p g (t)-x i (t)) (10) Where, e is the diversity heuristic factor, which controls the aggregation or dispersion behavior of the particle swarm; when F diversity ≤H min , if the current e is 1, then set e to -1; when F diversity >H max , at this time, if the current e is -1, then set e to 1; Particle i has two properties, including position x i =(x i1 ,x i2 ,…,x im ) T and speed 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: Where i = 1, 2, ..., n; w is the inertia weight; c1 is the cognitive coefficient; c2 is the social coefficient; r1, r2 are uniformly distributed random numbers; The particle degradation degree is calculated using the effective sample N eff To measure: N threshold =0.3m Where, is the weight value of the i-th particle at time t; m is the total number of particles; If N eff Below the set threshold N threshold , indicating that the particle degradation phenomenon is serious and resampling is needed to improve the particle distribution; when F diversity Greater or less than H min When , the particle swarm velocity update equation based on the diversity heuristic factor is performed through formula (4) to update the distribution of the particle set, making the particle set closer to the true posterior probability density.

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

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a method for real-time positioning and mapping in a microgravity environment according to any one of claims 1 to 5 when called by a processor.

Images