CN-120820141-B - Real-time positioning and mapping method and system in microgravity environment

CN120820141BCN 120820141 BCN120820141 BCN 120820141BCN-120820141-B

Abstract

The invention provides a method and a system for instant positioning and mapping under a microgravity environment, belonging to the field of instant positioning and mapping of space intelligent flying robots. The method aims to solve the problems that the diversity of particle sets is weakened and samples are exhausted due to the fact that particles with larger weight are sampled for multiple times in the traditional RBPF-SLAM algorithm, so that filtering estimation is poor, or the operation efficiency is reduced due to the fact that the number of particles is increased. The method provided by the invention can be used for optimizing the particle sampling process by utilizing the Gaussian particle swarm, ensuring the optimum group diversity level by taking the particle set diversity measure as a search heuristic factor, reducing the particle degradation, improving the state estimation precision and the algorithm convergence by moving the particle set in the area with higher posterior probability, obtaining higher positioning and map construction precision by fewer particles, and achieving better filter estimation stability.

Inventors

ZHANG LIXIAN
ZHANG LINCHEN
DONG YIFEI
WANG YANBIN
CAO XIBIN
DING YIHANG
ZHU YIMIN
LIU YANGGUANG

Assignees

哈尔滨工业大学

Dates

Publication Date: 20260508
Application Date: 20250708

Claims (6)

1. The method for real-time positioning and mapping under the microgravity environment is characterized by comprising the following steps: s100, establishing space intelligent flying robot in the air Predicting the pose vector value of the space intelligent flying robot by using the moment pose model comprises the steps of according to the space intelligent flying robot Time control vector and motion model of space intelligent flying robot, prediction The position and attitude vectors of the space intelligent flying robot at the moment are calculated, and a covariance matrix of the position and attitude vectors is calculated; Comprising the steps of (a) a step of, Establishing space intelligent flying robot in And (5) a moment pose model: in the formula, Respectively, space intelligent flying robots are in The position of the abscissa and the azimuth angle of the moment; And The linear speed and the angular speed of the movement of the intelligent space flying robot are respectively; Representing a time step, i.e. the time interval between two adjacent time points in a discrete-time system; the problem of real-time positioning and map reconstruction of a space intelligent flying robot is described as follows: in the formula, Representing posterior distribution of the space intelligent flying robot path estimation; representing posterior distribution of environmental roadmap; respectively from the initial time to the initial time A historical information set of state quantity, observed quantity, control quantity and data association quantity of the time space intelligent flying robot; By using Particle filters of individual particles perform trajectory estimation using a single EKF filter for an independent environmental landmark, thus, moving objects share Tracks, each track comprising The EKF filters have the following structure: in the formula, Represent the first Estimating paths of the individual particles; And Respectively the current paths Knowing the mean and variance of the gaussian function of the o-th landmark; s200, carrying out data association on the obtained observation information and the existing signposts in the current map, wherein the method comprises the steps of adopting a maximized observation probability function to sequentially carry out data association on the obtained observation information and the signposts in the estimated map of each particle; S300, updating road signs and adaptively resampling, calculating the number of effective particles and judging the degradation degree of the particles in real time on the basis of a FastSLAM algorithm, wherein the method comprises the steps of copying particles with larger weight according to weight probability and discarding particles with smaller weight; S400, optimizing a particle swarm, namely optimizing particles by adopting a particle swarm algorithm when judging that any diversity measure is worse according to the step S300, guiding aggregation and dispersion of the particles by using diversity heuristic factors in the iterative optimization process of the particle swarm, and ensuring the optimal diversity of the particle set by realizing re-optimized distribution of the particles; And S500, updating the map to update the map estimation result, wherein the method comprises the steps of updating corresponding particle associated feature estimation according to the particle data associated information in the particle set, simultaneously calculating the mean value and the variance of each feature estimation, adding the observation information which is not associated with the existing features in the map as new features into the map, and completing the map updating.
2. The method for real-time positioning and mapping in a microgravity environment of claim 1, wherein in step S200, comprising, From the sampling moment To the point of Calculating state estimation of a space intelligent flying robot within an interval of And an estimation value of covariance matrix Wherein the state estimation Position estimation including a space intelligent flying robot And landmark state estimation The method comprises the following steps: in the formula, And Position estimation for intelligent flying robots in space And landmark states An associated error covariance matrix; The covariance matrix between the space intelligent flying robot and the landmark is obtained; 、 respectively updating equations for attitude of intelligent space flying robot With respect to gestures And input control Jacobian matrix of (a); And The system noise variance matrix and the measurement noise variance matrix are respectively; For observing the model To the state Jacobian matrix of (a); For observing the model Input to Jacobian matrix of (a); to control input Is a noise covariance matrix of (1); the observed value of the feature point is determined by the distance of the feature point relative to the space intelligent flying robot And angle of Expressed, i.e Then Time of day The observation equation for each feature point is expressed as: (5) in the formula, Intelligent flying robot representing space An estimated position in a global coordinate system; Representation of The estimated position of the time space intelligent flying robot in the global coordinate system; Representation of Estimating the gesture of the intelligent flying robot in the moment space in the global coordinate system; Representation of Estimating the state of the intelligent flying robot in the moment space; Representation of Time of day pair Expected observations of individual targets.
3. The method for real-time positioning and mapping in a microgravity environment of claim 2, wherein in step S300, comprising, Updating road signs and adaptively resampling, obtaining actual measurement values by sensors on a space intelligent flying robot And then, updating the state estimation of the posterior augmentation matrix and the covariance matrix of the posterior augmentation matrix respectively: in the formula, ; And Respectively the first Posterior Kalman filtering mean and variance of each road sign characteristic point; And Respectively the first The prior Kalman filtering mean and variance of each road sign characteristic point; Is the first Road sign feature point is at A filtering gain matrix of time; The weight is the ratio of the target distribution to the suggested distribution, namely: 。
4. the method for real-time positioning and mapping in a microgravity environment of claim 3, wherein in step S400, comprising, Particle swarm optimization, assuming particle swarm size of The longest diagonal length in the search space is First, the Substituted particles Is that , Representing the dimensions of the search space, the average center of the population is: the average point distance of the particle concentrated group is defined as the diversity measure factor of the particle group The method comprises the following steps: subsequently, an upper threshold of the particle set diversity index is defined And a lower threshold When the diversity of particle sets is below the lower threshold, i.e When the particle diversity is greater than the upper threshold, i.e. when the particle population is dispersed by moving the particles in opposite directions When the particles are required to be gathered towards the global optimal position, the searching range is reduced, and then the speed evolution equation of the particle swarm is defined as: in the formula, Controlling aggregation or dispersion behavior of particle swarm as diversity heuristic factor when At this time if it is current 1, Then Set to-1 when At this time if it is current Is-1, then Set to 1; Particles Having two properties, including position Sum speed of Particles are made The optimal position of the individual is The global optimal position of the population is In each iteration, the particles The speed and position of (a) are respectively: in the formula, ; Is an inertial weight; Is a cognitive coefficient; Is a social coefficient; Is a uniformly distributed random number; The degree of particle degradation employs an effective sample And (3) measuring: in the formula, Is the first Individual particles are at A weight value of the moment; Is the total particle number; If it is Below a set threshold Indicating that the particle degradation phenomenon is serious, the particle distribution needs to be improved by resampling when Greater or less than And (3) in the process, the particle group velocity updating equation based on the diversity heuristic factors is carried out through the formula (4) to update the distribution of the particle groups, so that the particle groups are closer to the true posterior probability density.
5. A system for real-time positioning and mapping in a microgravity environment is characterized in that the system is provided with a program module corresponding to the steps of any one of claims 1-4, and the steps in the method for real-time positioning and mapping in the microgravity environment are executed in running.
6. A computer readable storage medium, wherein said computer readable storage medium stores a computer program configured to implement the steps of a method for real-time localization and mapping in a microgravity environment of any one of claims 1-4 when called by a processor.

Description

Real-time positioning and mapping method and system in microgravity environment Technical Field The invention relates to the technical field of real-time positioning and mapping of flying robots, in particular to a real-time positioning and mapping method and a real-time positioning and mapping system under a microgravity environment. Background Various experiments and equipment maintenance tasks are carried out in a microgravity environment more and more frequently, and a space intelligent flying robot is needed to assist an experimenter to complete a series of working tasks such as tracking illumination shooting, equipment monitoring maintenance, instrument transfer, equipment disassembly and replacement, scientific experiment care and the like so as to reduce the workload of spacecrafts. In the process, reliable and accurate environment sensing and safe and efficient path planning are key points for ensuring stable operation of the platform. The precondition for achieving the object is that the robot should have the functions of real-time positioning and map reconstruction (SLAM) of the microgravity environment. Specifically, in a narrow and unstructured microgravity environment, the intelligent space flying robot effectively utilizes the coupling characteristic of a fan power system and a flywheel power system, fully exerts the advantages of high accessibility and strong obstacle crossing, and autonomously realizes map construction through a multi-source perception fusion technology. In view of the high demand for the availability of the space intelligent flying robot, the operation speed of the computing unit is limited, so that a lightweight SLAM method needs to be designed. Because of the fast mapping speed and low computing power consumption, the SLAM method based on Rao-Black-wellized particle filtering (RBPF) has been widely used in recent years, and a map can be built in an unknown environment by using an onboard camera, and meanwhile, the motion trail of a mobile platform can be calculated by the map. The method decomposes the SLAM problem into a robot positioning problem and an environmental characteristic position estimation problem based on pose estimation, adopts a particle filter algorithm to make pose estimation of the whole path, and adopts an Extended Kalman Filter (EKF) algorithm to estimate the position of the environmental characteristic. The method combines the advantages of the EKF and probability methods, reduces the complexity of calculation and has good robustness. However, in the process of estimating the pose and establishing the map, the conventional RBPF-SLAM algorithm resamples the importance of the sequence by the resampling algorithm, and particles with larger weights are selected multiple times, so that more repeated parts appear in the sampling result. This results in reduced diversity of particle sets and exhaustion of samples, so that the filter estimation performance is deteriorated, and filter divergence occurs when it is severe. The simplest way to overcome sample depletion is to increase a sufficient number of particles, and the result of this would be a dramatic expansion of the computation and a decrease in the efficiency of the algorithm. Therefore, the invention takes the particle set diversity measure as a heuristic factor to guide the particle optimization searching process, ensures the global optimal searching of the particle group and the optimal level of the particle set diversity, and improves the system filtering estimation performance. Disclosure of Invention The invention aims to solve the technical problems that: The method aims to solve the problems that the diversity of particle sets is weakened and samples are exhausted due to the fact that particles with larger weight are sampled for multiple times in the traditional RBPF-SLAM algorithm, so that filtering estimation is poor, or the operation efficiency is reduced due to the fact that the number of particles is increased. The invention adopts the technical scheme for solving the technical problems: the invention provides a method for immediately positioning and constructing a map in a microgravity environment, which comprises the following steps: S100, establishing a pose model of the space intelligent flying robot at the moment t, and predicting a pose vector value of the space intelligent flying robot, wherein the method comprises the steps of predicting the pose vector of the space intelligent flying robot at the moment k according to a control vector of the space intelligent flying robot at the moment k and a motion model of the space intelligent flying robot, and calculating a covariance matrix of the pose vector; s200, carrying out data association on the obtained observation information and the existing signposts in the current map, wherein the method comprises the steps of adopting a maximized observation probability function to sequentially carry out data association on the obtained observ