Particle filter-based multi-robot formation positioning method and robot equipmentTechnical Field
The invention relates to the technical field of robots and navigation, in particular to a particle filter-based multi-robot formation positioning method and robot equipment.
Background
With the improvement of embedded computing and communication capabilities and the development of distributed or decentralized ideas, it is increasingly recognized that multi-agent systems can accomplish more complex tasks at a lower cost than single agents, and that multi-robot formation has an increasing demand for applications, especially autonomous navigation functions of robot formation. The automatic navigation system firstly loads a grid map generated by the mapping system and realizes the self-positioning and navigation of the robot on the map. Therefore, in whatever environment, the robot needs to be positioned very accurately to its own position in order for the automatic navigation system to be effective quickly.
Laser-based single-robot self-localization algorithms typically employ a particle filtering algorithm, the common method being AMCL (adaptive monte carlo localization). The pose (usually a point) of the 2D laser includes position and orientation information of the robot, which then self-locates and navigates within the map. The problems with this approach are:
a single robot only depends on the odometer information and the radar information of the robot to carry out particle filtering, and the pose of the robot cannot be effectively corrected;
in the prior art, CN 201811205306.5 (a multi-robot cooperative positioning and control method) provides a positioning method based on multi-robot cooperative control, which optimizes the position of a robot through a gradient descent algorithm according to coordinates of a world coordinate system in a plurality of robots and ranging information obtained by sensors, and the method requires the robot to acquire a lot of data, has strong requirements on conditions, and simultaneously requires a lot of global calculations.
Therefore, those skilled in the art are dedicated to develop a particle filter-based multi-robot formation positioning method and a robot device to solve the problems of inaccurate positioning and insufficient correction caused by single-robot odometer error, inaccurate laser radar scanning and other factors in the prior art.
Disclosure of Invention
In view of the above defects in the prior art, the technical problem to be solved by the present invention is to solve the problems of inaccurate positioning and insufficient correction caused by the odometer error of a single robot, inaccurate scanning of laser radar, etc. in the prior art.
In order to achieve the purpose, the invention provides a particle filter-based multi-robot formation positioning method, which comprises the following steps of:
step 1, establishing a grid map;
step 2, realizing cooperative formation movement by multiple robots;
step 3, iterative updating of the particle filter, including iterative updating of a sampling particle generation process and iterative updating of an importance sampling process;
step 4, the robot acquires the average weight of the particles adjacent to the robot in the formation in real time;
step 5, the robot corrects the average weight of the robot in real time by using the average weight of the particles adjacent to the robot;
and 6, repeating the step 3, and outputting the pose of the robot in real time until the iteration termination condition is reached, and stopping the iteration updating.
Further, the method for establishing the grid map comprises the following steps: based on laser or visual sensors, the robot moves autonomously or is controlled by a user to construct the grid map in a working environment.
Further, the step 2 implementation method comprises: centralized, distributed, or hybrid.
Further, the iterative update of the sampling particle generation process specifically includes: and generating new sampling particles according to the current particle distribution or the particle distribution obtained in the importance sampling process.
Further, the importance sampling process specifically includes: and calculating the weight of the sampling particles according to the observation of the positions of the sampling particles and the observation of the real position of the robot, and performing low-variance resampling to obtain the updated particle distribution.
Further, the implementation method of step 4 includes:
step 4.1, the average weight of the particles obtained by the robot is specifically as follows: the average weight of the particles obtained by fusing the prediction information of the odometer and the measurement data of the laser radar is used for removing the particles with low confidence coefficient and updating the pose of the robot;
and 4.2, receiving data from the centralized processor in real time or directly communicating with the adjacent robots in real time, and if the robots realize communication with the adjacent robots in the form of an adjacency matrix, acquiring the average weight of the particles of the adjacent robots.
Further, step 5 comprises: using weighted fusion or difference correction, such as a method of correcting the average weight using the difference:
wherein, a
ijThe value of (a) indicates whether or not communication is between robots numbered i and j in the robot formation,
is the average weight of the particles at time k for robot j,
corrected particle weight, r, for robot j at time k
ijRepresenting parameters related to the relative positions of robot i and robot j in the formation.
Further, the iteration termination condition at least comprises that the number of iterations is reached or the variance of the particle distribution is smaller than a certain threshold value.
The invention also provides a particle filter-based multi-robot formation positioning robot device, which is characterized by comprising the following components: the device comprises a grid map building module, a formation communication module, a formation control module, a particle filter module and a correction module.
Further, the grid map building module is used for building the grid map; the formation communication module is used for communicating with adjacent robots or communicating with a centralized processor; the formation control module is used for realizing multi-robot cooperation and moving according to a fixed formation; the particle filter module is used for iteratively updating the particle filter, and comprises iterative updating of a sampling particle generation process and an importance sampling process; the correction module is used for correcting the particle average weight of the robot by using the particle average weight of the adjacent robot.
The multi-robot formation can quickly and accurately position the position of the multi-robot formation at any position in navigation, can correct the lost robot, obtains more accurate position estimation through sampling and resampling of the particle filter in a static state, and can realize more accurate formation control based on the positioning method.
The conception, the specific structure and the technical effects of the present invention will be further described with reference to the accompanying drawings to fully understand the objects, the features and the effects of the present invention.
Drawings
FIG. 1 is a flow chart of a multi-robot formation positioning method based on a particle filter according to the present invention;
fig. 2 is a schematic structural diagram of the multi-robot formation positioning robot device based on the particle filter.
Detailed Description
The technical contents of the preferred embodiments of the present invention will be more clearly and easily understood by referring to the drawings attached to the specification. The present invention may be embodied in many different forms of embodiments and the scope of the invention is not limited to the embodiments set forth herein.
Example one
FIG. 1 is a flow chart of the multi-robot formation positioning method based on particle filter of the present invention, which includes:
step 1, establishing a grid map.
The method for establishing the grid map comprises the following steps: based on laser or vision sensors, the robot moves autonomously or is controlled by the user to perform mapping in the work environment.
And 2, realizing the cooperative formation movement by a plurality of robots.
The method for multi-robot collaborative formation movement comprises the following steps: centralized, distributed or hybrid, e.g. multiple robots are moved in a working environment in some fixed team type by a distributed control method of pilots and followers.
And 3, iteratively updating the particle filter, including a sampling particle generation process and an importance sampling process.
The sampling particle generation process specifically comprises the following steps: and generating new sampling particles according to the current particle distribution or the particle distribution obtained in the importance sampling process.
The importance sampling process specifically comprises the following steps: and calculating the weight of the sampling particles according to the observation of the positions of the sampling particles and the observation of the real position of the robot, and performing low variance resampling until the updated particle distribution is reached.
And 4, acquiring the average weight of the particles of the adjacent robots in the formation in real time by the robots.
The method for acquiring the average weight of the particles of the adjacent robots in the formation in real time by the robot comprises the following steps: and receiving data in real time from the centralized processor or directly communicating with the adjacent robot in real time, for example, the robot realizes the communication with the adjacent robot in the form of an adjacency matrix, and acquiring the average weight of particles of the adjacent robot.
The average weight of the particles obtained by the robot is specifically as follows: and the average weight of the particles obtained by fusing the prediction information of the odometer and the measurement data of the laser radar is used for removing the particles with low confidence coefficient and updating the pose of the robot.
And 5, the robot corrects the average weight of the robot in real time by using the average weight of the particles of the adjacent robots, repeats the step 3, and outputs the pose of the robot in real time until an iteration termination condition is reached, and then stops iteration updating.
A method for a robot to correct its own average weight of particles by using the average weights of particles of neighboring robots, comprising: using weighted fusion or difference correction, such as a method of correcting the average weight using the difference:
wherein, a
ijThe value of (a) indicates whether or not communication is between robots numbered i and j in the robot formation,
is the average weight of the particles at time k for robot j,
corrected particle weight, r, for robot j at time k
ijRepresenting parameters related to the relative positions of robot i and robot j in the formation.
And the iteration termination condition at least comprises that the iteration number is reached or the variance of the particle distribution is less than a certain threshold value.
According to the method provided by the embodiment of the invention, the multi-robot formation can quickly and accurately position the position of the multi-robot formation at any position in navigation, and the lost robot can be corrected to obtain more accurate position estimation through sampling and resampling by a particle filter in a static state.
Example two
Fig. 2 is a schematic structural diagram of the multi-robot formation positioning robot device based on particle filter, which comprises:
the grid map building module is used for building a grid map;
a formation communication module for communicating with an adjacent robot or a centralized processor;
the formation control module is used for realizing the cooperation of multiple robots and moving according to a fixed formation;
the particle filter module is used for iteratively updating the particle filter, and comprises a sampling particle generation process and an importance sampling process; stopping iteration updating when an iteration termination condition is reached, and outputting the position information of the robot;
and the correction module is used for correcting the average weight of the particles of the adjacent robots by using the average weight of the particles of the adjacent robots.
The specific detail information of each module refers to the first embodiment.
It should be noted that, in this embodiment, each module (or unit) is in a logical sense, and in particular, when the embodiment is implemented, a plurality of modules (or units) may be combined into one module (or unit), and one module (or unit) may also be split into a plurality of modules (or units).
By the robot equipment of the particle filter-based multi-robot formation positioning method, the multi-robot formation can quickly and accurately position the position of the multi-robot formation at any position in navigation, lost robots can be corrected, and more accurate position estimation can be obtained through particle filter sampling and resampling in a static state.
It will be understood by those skilled in the art that all or part of the processes of the methods of the above embodiments may be implemented by hardware instructions related to a program, and the program may be stored in a computer-readable storage medium, and when executed, may include the processes of the above embodiments of the methods. The storage medium may be a magnetic disk, an optical disk, a Read-only Memory (ROM), a Random Access Memory (RAM), or the like.
The foregoing detailed description of the preferred embodiments of the invention has been presented. It should be understood that numerous modifications and variations could be devised by those skilled in the art in light of the present teachings without departing from the inventive concepts. Therefore, the technical solutions available to those skilled in the art through logic analysis, reasoning and limited experiments based on the prior art according to the concept of the present invention should be within the scope of protection defined by the claims.