CN108931245B - Local self-positioning method and equipment for mobile robot - Google Patents
Local self-positioning method and equipment for mobile robot Download PDFInfo
- Publication number
- CN108931245B CN108931245B CN201810872628.9A CN201810872628A CN108931245B CN 108931245 B CN108931245 B CN 108931245B CN 201810872628 A CN201810872628 A CN 201810872628A CN 108931245 B CN108931245 B CN 108931245B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- pose
- particle
- information
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application provides a local self-positioning method and equipment of a mobile robot, which can determine the motion state of the mobile robot according to mileage information and acceleration information, acquire particle distribution information for predicting the pose of the mobile robot when the motion state is an abnormal state, cluster particles, determine the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a predicted pose, match a predicted pose map with laser observation data at the current moment, determine the observation matching degree and the pose of the mobile robot, and re-determine the observation matching degree and the pose of the mobile robot when the observation matching degree is smaller than a first threshold value, thereby realizing accurate self-positioning of the mobile robot and avoiding the problem of positioning loss of the mobile robot when the current environment is larger than the map or the mobile robot has abnormal conditions such as wheel slip, idle running and the like, the scheme is simple and easy to realize, and the self-positioning accuracy of the mobile robot is improved.
Description
Technical Field
The application relates to the technical field of mobile robots, in particular to a local self-positioning method and equipment of a mobile robot.
Background
As mobile robots have been developed and further landed, research on mobile robot related technologies has been increasingly conducted, and autonomous intelligence research of mobile robots has become extremely important due to unstructured and uncertain working environments of mobile robots. The mobile robot self-positioning technology has attracted extensive attention as one of three major hot techniques for mobile robot research.
Currently, common self-positioning algorithms of mobile robots include various self-positioning methods such as odometer calculation based, visual-based landmark identification, global positioning based on map matching, gyro navigation, GPS and the like, and each technology has respective advantages and limitations. The odometer-based estimation has high short-term accuracy and low cost, but cannot avoid infinite accumulation of errors. The gyro navigation does not need external reference, but drifts along with time, and is not suitable for long-time accurate positioning. The GPS positioning is easily limited by factors such as precision, safety and the like, and has the problems of indoor incapability and the like. The visual positioning is easily affected by illumination, deformation, high-speed motion and the like, and the calculation complexity is high.
The chinese patent CN201397390Y positioning and navigation system for an automatic container handling vehicle combines inertial navigation, GPS and laser positioning to realize self-positioning of a mobile robot, although positioning is real-time and accurate, it still depends on GPS to realize self-positioning, and the method cannot be used in indoor environment. In addition, the conventional mobile robot self-positioning method realized by combining the inertial navigation system and the laser positioning and based on the self-adaptive Monte Carlo self-positioning algorithm cannot realize accurate self-positioning of the mobile robot when the difference between the current environment and a map is large or the mobile robot has abnormal conditions such as wheel slipping and idling, and the like, so that the problems of positioning loss and the like are easy to occur.
Content of application
An object of the present application is to provide a local self-positioning method and apparatus for a mobile robot, so as to solve the problem of inaccurate local positioning of the mobile robot caused by slipping, idling, large difference between the observation environment and the existing map, and the like in the prior art.
To achieve the above object, the present application provides a local self-positioning method of a mobile robot, wherein the mobile robot includes an inertial navigation system including an odometer, an accelerometer, and a gyroscope, and a laser radar, the method including:
determining the motion state of the mobile robot according to the mileage information and the acceleration information acquired from the inertial navigation system;
when the motion state is an abnormal state, acquiring particle distribution information for predicting the pose of the mobile robot according to the mileage information, the acceleration information and the angle change information acquired from the inertial navigation system;
clustering the particles according to the particle distribution information, and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a prediction pose;
matching the map corresponding to the predicted pose with the laser observation data at the current moment, and determining the observation matching degree and the pose of the mobile robot;
and if the observation matching degree is smaller than the first threshold value, re-determining the observation matching degree and the pose of the mobile robot according to the motion range of the mobile robot corresponding to the matched observation window, wherein the matched observation window comprises multi-frame laser observation data which are obtained through a laser radar and have a time sequence relation.
Further, determining the motion state of the mobile robot according to the mileage information and the acceleration information acquired from the inertial navigation system includes:
and if the mileage information acquired from the odometer continuously increases and the change of the acceleration information acquired from the accelerometer is less than a second threshold value, determining the motion state of the mobile robot as an abnormal state.
Further, acquiring particle distribution information for predicting the pose of the mobile robot based on the mileage information, the acceleration information, and the angle change information acquired from the inertial navigation system, includes:
and acquiring particle distribution information for predicting the pose of the mobile robot through a self-adaptive Monte Carlo self-positioning algorithm according to the mileage information acquired from the odometer, the acceleration information acquired from the accelerometer and the angle change information acquired from the gyroscope.
Further, acquiring particle distribution information for predicting the pose of the mobile robot through an adaptive Monte Carlo self-positioning algorithm, wherein the method comprises the following steps:
initializing particle distribution information for predicting the pose of the mobile robot according to the mileage information, the acceleration information and the angle change information;
updating the particle position according to the mileage information;
matching the current-time laser observation data acquired from the laser radar with a map of the position of the particle, and determining the weight of the particle according to the matching result;
and performing particle resampling according to the particle weight to obtain the resampled particle distribution information.
Further, updating the particle position according to the mileage information includes:
and acquiring the particle distribution information of the current moment through a preset motion model according to the particle distribution information of the previous moment.
Further, performing particle resampling according to the particle weights, comprising:
and regenerating the sampling particles through a random sampling strategy according to the weight of the particles at the last moment.
Further, clustering the particles according to the particle distribution information, and determining the pose corresponding to the particle weight mean value in the classification with the largest particle weight mean value as a predicted pose, including:
clustering the particles according to the particle distribution information by an unsupervised learning clustering algorithm to obtain particle classification;
obtaining the classification with the largest particle weight mean value according to the particle classification;
and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a predicted pose.
Further, matching the map corresponding to the predicted pose with the laser observation data at the current moment, and determining the observation matching degree and the pose of the mobile robot, wherein the method comprises the following steps:
and performing point cloud matching on the map corresponding to the predicted pose and the laser observation data at the current moment through an iterative closest point algorithm to obtain an observation matching degree and the pose of the mobile robot.
Further, according to the motion range of the mobile robot corresponding to the matched observation window, the pose of the mobile robot is determined again, and the method comprises the following steps:
determining the motion range of the mobile robot according to the pose of the mobile robot matched with the multi-frame laser observation data in the observation window;
according to the motion range of the mobile robot, adjusting parameters of a preset motion model of particle distribution and acquiring particle distribution information for predicting the pose of the mobile robot;
clustering the particles according to the particle distribution information, and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a prediction pose;
and matching the map corresponding to the predicted pose with the laser observation data in the matched observation window, and determining the observation matching degree and the pose of the mobile robot.
Further, the laser observation data is laser scanning information containing angle and distance data.
Further, the method further comprises:
and when the motion state is an abnormal state, reducing the number of the laser observation data in the matched observation window.
Further, the method further comprises:
and when the matching degree of the current laser observation data and the map is greater than a third threshold value, increasing the number of the laser observation data in the matched observation window.
The present application also provides a local self-localization apparatus of a mobile robot, the apparatus comprising a memory for storing computer program instructions and a processor for executing the program instructions, wherein the computer program instructions, when executed by the processor, cause the apparatus to perform the aforementioned local self-localization method of a mobile robot.
The present application also provides a computer readable medium having stored thereon computer readable instructions executable by a processor to implement the aforementioned local self-positioning method of a mobile robot.
Compared with the prior art, the scheme provided by the application can determine the motion state of the mobile robot according to the mileage information and the acceleration information, acquire the particle distribution information for predicting the pose of the mobile robot when the motion state is abnormal, cluster the particles, determine the pose corresponding to the particle weight mean value in the classification with the largest particle weight mean value as the predicted pose, match the map corresponding to the predicted pose with the laser observation data at the current moment, determine the observation matching degree and the pose of the mobile robot, and re-determine the observation matching degree and the pose of the mobile robot when the observation matching degree is smaller than a first threshold value, so that the accurate self-positioning of the mobile robot can be realized when the difference between the current environment and the map is larger or the mobile robot has abnormal conditions such as wheel slipping, idle running and the like, and the problem of the loss of the positioning of the mobile robot can be avoided, the scheme is simple and easy to realize, and the self-positioning accuracy of the mobile robot is improved.
Drawings
Other features, objects and advantages of the present application will become more apparent upon reading of the following detailed description of non-limiting embodiments thereof, made with reference to the accompanying drawings in which:
fig. 1 is a flowchart of a local self-positioning method for a mobile robot according to an embodiment of the present disclosure.
Fig. 2 is a schematic view of an observation matching degree between current laser observation data of a preferred mobile robot and a map according to an embodiment of the present application.
Fig. 3 is a schematic diagram of a local self-positioning system of a preferred mobile robot according to an embodiment of the present application.
Detailed Description
The present application is described in further detail below with reference to the attached figures.
In a typical configuration of the present application, the terminal, the device serving the network, and the trusted party each include one or more processors (CPUs), input/output interfaces, network interfaces, and memory.
The memory may include forms of volatile memory in a computer readable medium, Random Access Memory (RAM) and/or non-volatile memory, such as Read Only Memory (ROM) or flash memory (flash RAM). Memory is an example of a computer-readable medium.
Computer-readable media, including both non-transitory and non-transitory, removable and non-removable media, may implement information storage by any method or technology. The information may be computer readable instructions, data structures, modules of a program, or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), Static Random Access Memory (SRAM), Dynamic Random Access Memory (DRAM), other types of Random Access Memory (RAM), Read Only Memory (ROM), Electrically Erasable Programmable Read Only Memory (EEPROM), flash memory or other memory technology, compact disc read only memory (CD-ROM), Digital Versatile Discs (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other non-transmission medium that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include non-transitory computer readable media (transient media), such as modulated data signals and carrier waves.
Some embodiments of the present application provide a method of local self-positioning of a mobile robot. The mobile robot may include an inertial navigation system and a lidar. Inertial navigation systems may include, but are not limited to, odometers, accelerometers, and gyroscopes. Lidar may include, but is not limited to, single line lidar and multiline lidar. As shown in fig. 1, the method specifically includes the following steps:
step S101, determining the motion state of the mobile robot according to the mileage information and the acceleration information acquired from the inertial navigation system;
step S102, when the motion state is an abnormal state, acquiring particle distribution information for predicting the pose of the mobile robot according to the mileage information, the acceleration information and the angle change information acquired from the inertial navigation system;
step S103, clustering the particles according to the particle distribution information, and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a prediction pose;
step S104, matching the map corresponding to the predicted pose with the laser observation data at the current moment, and determining the observation matching degree and the pose of the mobile robot;
and S105, if the observation matching degree is smaller than a first threshold value, re-determining the observation matching degree and the pose of the mobile robot according to the motion range of the mobile robot corresponding to a matched observation window, wherein the matched observation window comprises a plurality of frames of laser observation data which are obtained through a laser radar and have a time sequence relation.
The scheme is particularly suitable for local self-positioning scenes of the mobile robot, whether the mobile robot is in an abnormal motion state or not can be judged according to the mileage information and the acceleration information, if the mobile robot is in the abnormal motion state, local self-positioning is carried out on the mobile robot according to the information provided by the inertial navigation system and the laser radar, and the current pose of the mobile robot is determined again. Here, the local self-positioning method of this scheme is a method of determining the pose of the mobile robot by an offset from a certain starting point.
In step S101, the motion state of the mobile robot is determined based on the mileage information and the acceleration information acquired from the inertial navigation system. Here, the mileage information is acquired from an odometer in the inertial navigation system, and the acceleration information is acquired from an accelerometer in the inertial navigation system. The mileage information is statistical information of a code disc (wheel revolution number), corresponding accumulation can be carried out as long as the wheel rotates, and the mileage information reflects the change of x and y coordinates on a two-dimensional plane and the change of the orientation of the mobile robot relative to the last moment. The accelerometer provides primarily directional acceleration information in three dimensions (x, y, z). The gyroscope mainly provides relative orientation change, namely the gyroscope obtains an angle through integration, and finally obtains a final angle through Kalman filtering according to the gravity direction detected by the accelerometer.
In the actual work of the mobile robot, various abnormal situations occur, for example, the mobile robot has a situation of idle running and slipping, that is, the wheel rotates but the position of the robot does not move or the wheel does not rotate but the position of the robot moves, and the mileage is accumulated by mistake when the wheel slips and slips, so that the robot is positioned by mistake. In some embodiments of the present application, the auxiliary information is provided by the acceleration information to determine whether the robot is in place, for example, if the accelerometer has not changed in the x and y directions of the two-dimensional plane, the robot may be determined to be in place. For another example, if it is monitored that the odometer changes all the time but the accelerometer does not change significantly, it can be used as the basis for the robot to slip and spin,
specifically, if the mileage information acquired from the odometer continuously increases and the acceleration information acquired from the accelerometer changes by less than the second threshold value, the motion state of the mobile robot is determined to be an abnormal state. Here, the continuous increase of the mileage information means that the wheels of the mobile robot are rotating all the time, but the information detected by the accelerometer is not changed significantly, the second threshold value is a small preset value for indicating the degree of change of the acceleration information, and if the change of the acceleration information is smaller than the second threshold value, it is considered that the acceleration information of the mobile robot is not changed much. Therefore, when the wheels of the mobile robot are turned and the acceleration is not changed, the mobile robot is considered to be in an abnormal state, and the motion state is determined to be in the abnormal state.
In addition, the mobile robot may also have a situation that the mileage information changes insignificantly or slightly, but the acceleration information continues for a while, which indicates that the wheel rotation amplitude of the mobile robot is small, but the acceleration change is large, the mobile robot may slip, and the mobile robot is also in an abnormal state, and the motion state of the mobile robot may also be determined to be in an abnormal state.
In step S102, if the robot motion state is an abnormal state, particle distribution information for predicting the pose of the mobile robot is acquired based on the mileage information, the acceleration information, and the angle change information acquired from the inertial navigation system. When the abnormal motion state of the mobile robot is detected, the pose of the mobile robot may be inaccurate, and local self-positioning needs to be performed on the current pose state of the mobile robot to determine the accurate pose again. Here, the pose of the mobile robot includes the position and posture of the mobile robot for specifying the position and orientation of the mobile robot.
Here, the mileage information is acquired from a speedometer in the inertial navigation system, the acceleration information is acquired from an accelerometer in the inertial navigation system, and the angle change information is acquired from a gyroscope in the inertial navigation system. Specifically, according to the mileage information, the acceleration information and the angle change information, the particle distribution information for predicting the pose of the mobile robot is obtained through a self-adaptive Monte Carlo self-positioning algorithm.
An Adaptive Monte Carlo Localization Algorithm (AMCL) is a probability statistical method for a mobile robot, and the method tracks the pose of the robot by using a particle filter algorithm on the basis of a known map.
In some embodiments of the present application, obtaining particle distribution information for predicting the pose of a mobile robot through an adaptive monte carlo self-localization algorithm may include:
initializing particle distribution information for predicting the pose of the mobile robot according to the mileage information, the acceleration information and the angle change information;
updating the particle position according to the mileage information;
matching the current-time laser observation data acquired from the laser radar with a map of the position of the particle, and determining the weight of the particle according to the matching result;
and performing particle resampling according to the particle weight to obtain the resampled particle distribution information.
The acquired mileage information, acceleration information and angle change information are used as prior information of the self-adaptive Monte Carlo self-positioning algorithm, and the self-adaptive Monte Carlo self-positioning algorithm initializes the particle distribution information according to the prior information.
And after the initial particle distribution information exists, updating the particle position according to the mileage information. Specifically, the particle distribution information at the current moment is obtained through a preset motion model according to the particle distribution information at the previous moment. In some embodiments of the present application, the preset motion model is a gaussian model, and the mileage information can be input into the motion model to obtain the covariance and mean of the model. The particle updating is to obtain the predicted distribution of each particle at the current moment by combining the distribution of each particle at the previous moment with the motion model.
And updating the positions of the particles to obtain updated particle distribution information, matching the laser observation data at the current moment acquired from the laser radar with a map of the positions of the particles, and determining the weight of the particles according to the matching result. Here, the laser observation data is laser scanning information including angle and distance data, and the laser observation data is acquired from a laser radar. Meanwhile, the mobile robot also carries a global map of the mobile environment, and the global map can be established through scanning data of the laser radar and can also be used for matching with laser observation data. If the matching of the laser observation data with the map of a certain particle position is high, the weight of the particle may be determined to be high, and if the matching of the laser observation data with the map of a certain particle position is low, the weight of the particle may be determined to be low.
And after determining the corresponding weight for each particle, resampling the particles according to the weight of the particles, and acquiring the resampled particle distribution information. Specifically, for each particle, the sampled particle is regenerated by a random sampling strategy based on the particle weight w (t-1) at the previous time, and the resampled particle distribution information is obtained.
In step S103, the particles are clustered according to the particle distribution information, and the pose corresponding to the particle weight mean in the classification with the largest particle weight mean is determined as the predicted pose. Here, the particles for predicting the pose of the mobile robot are distributed in a wide range, each particle has a respective weight, the sum of the weights of all the particles is 1, and the weight of the particle is the probability that the mobile robot appears at the corresponding position of the particle. Because the self-adaptive Monte Carlo self-positioning algorithm does not determine the most probable position of the mobile robot, in some embodiments of the application, the most probable current pose, namely the predicted pose, of the mobile robot is predicted by clustering particles, and whether the predicted pose is correct or not is determined according to the perception of the current pose.
Specifically, in some embodiments of the present application, the particles are clustered and the predicted pose is determined, and the particles are clustered by an unsupervised learning clustering algorithm according to the particle distribution information to obtain the particle classification. The essence of clustering is to map data into different attribute sets according to a certain principle. Preferably, in some embodiments of the present application, a DBSCAN clustering algorithm may be used to cluster the particles. DBSCAN (Density-Based Spatial Clustering of Applications with Noise) is a relatively representative Density-Based Clustering algorithm that, unlike other partitioning and hierarchical Clustering methods, defines clusters as the largest set of Density-connected points, can partition regions with sufficiently high Density into clusters, and can find clusters of arbitrary shape in noisy Spatial databases.
And after the clustered particles are classified, obtaining the classification with the largest particle weight mean value according to the particle classification. In some embodiments of the present application, the particle weight mean of each particle classification is calculated according to weights corresponding to all particles in the particle classification, and then the particle classification with the largest particle weight mean is obtained.
And after the classification of the particles with the largest particle weight average value is determined, determining the pose corresponding to the particle weight average value in the classification with the largest particle weight average value as the predicted pose. Here, the weight average of the particles is already obtained, and the weight average may be the same as or similar to the weight of some particles, in some embodiments of the present application, the pose corresponding to the weight average may be determined as the pose corresponding to the particle with the same or similar weight average, and the pose corresponding to the weight average is the predicted pose, that is, the pose of the mobile robot predicted according to the mileage information, the acceleration information, and the angle change information.
In step S104, the map corresponding to the predicted pose is matched with the laser observation data at the current time, and the observation matching degree and the pose of the mobile robot are determined. Because the predicted pose of the mobile robot obtained by particle clustering is a relatively rough pose, in some embodiments of the application, point cloud matching is performed on the laser observation data at the current moment and a map corresponding to the predicted pose through an iterative closest point algorithm to obtain an observation matching degree and the pose of the mobile robot, and the matching is relatively fine matching, so that a more accurate pose can be obtained.
An Iterative Closest Point (ICP) algorithm is an Iterative calculation method, and Point cloud data under different coordinates can be merged into the same coordinate system. The purpose of the ICP algorithm is to find a rotation parameter R and a translation parameter T between point cloud data to be registered and reference cloud data, so that the optimal matching between the two points of data meets a certain measurement criterion. The ICP algorithm is essentially an optimal registration method based on a least square method, and the algorithm repeatedly selects corresponding relation point pairs and calculates optimal rigid body transformation until the convergence precision requirement of correct registration is met.
And matching by an ICP (inductively coupled plasma) algorithm to obtain corresponding observation matching degree and the pose of the mobile robot, wherein the pose is the most accurate matched pose, namely the final predicted pose of the current mobile robot. As shown in fig. 2, a part formed by white rectangles in the drawing is an indoor environment where the mobile robot is located, a plurality of obstacles exist in the indoor environment, a shadow part in the indoor environment is a scanning range where the laser radar is not obstructed, points on the edge of the indoor environment or the indoor obstacles represent laser observation data, and according to related data obtained from the drawing, the observation matching degree of the laser observation data and a map reaches more than 95%, almost all the data can be matched, and the final predicted pose of the mobile robot is very close to the real pose.
In step S105, if the observation matching degree is smaller than the first threshold, the observation matching degree and the pose of the mobile robot are determined again according to the motion range of the mobile robot corresponding to the matched observation window. The matched observation window comprises a plurality of frames of laser observation data, the laser observation data are obtained through a laser radar and have a time sequence relation, namely the laser observation data are obtained at different moments and are arranged according to the sequence of the moments. The first threshold is a preset observation matching degree threshold used for determining whether the mobile robot needs to be repositioned, and the first threshold can be set according to actual conditions or adjusted according to positioning conditions of the mobile robot in actual scenes. In some embodiments of the present application, if the observed matching degree is smaller than the first threshold, it indicates that the self-positioning effect on the mobile robot is not good, and therefore, the mobile robot needs to be locally repositioned. And if the observation matching degrees are all larger than the first threshold value, the self-positioning of the mobile robot is successful, and the final pose of the mobile robot is the final predicted pose of the mobile robot obtained after matching through the ICP algorithm.
Specifically, firstly, the motion range of the mobile robot is determined according to the pose of the mobile robot matched with multi-frame laser observation data in an observation window. The motion range of the mobile robot refers to the range of the robot pose change corresponding to the matched observation window, the robot pose in the first frame of laser observation data in the matched observation window can be recorded first when the motion range of the mobile robot is determined, the robot pose in the subsequent frame of laser observation data in the matched observation window is recorded, and the motion range of the mobile robot is determined according to the robot poses.
And then, according to the obtained motion range of the mobile robot, adjusting parameters of a preset motion model of particle distribution and acquiring particle distribution information for predicting the pose of the mobile robot. In some embodiments of the present application, the preset motion model is a gaussian model, and the particle distribution can be adjusted by adjusting parameters of the preset motion model, so as to uniformly distribute particles in the motion range, thereby obtaining new particle distribution information.
Similar to the method for determining the pose of the mobile robot according to the particle distribution information, the particles are clustered according to the particle distribution information, the pose corresponding to the particle weight mean value in the classification with the largest particle weight mean value is determined as a predicted pose, and then a map corresponding to the predicted pose is matched with laser observation data in a matched observation window, so that the observation matching degree and the pose of the mobile robot are determined. The pose of the mobile robot obtained at this time is the finally determined pose of the robot, and the pose is probably close to the real pose of the mobile robot.
In some embodiments of the application, the number of laser observation data in the matched observation window can be reduced when the motion state of the mobile robot is an abnormal state, the historical accumulated observation data participating in the self-positioning of the robot can be dynamically adjusted by reducing the number of the laser observation data, the matching efficiency is improved, and the self-positioning time of the robot is shortened.
In some embodiments of the present application, the number of the laser observation data in the matching observation window may also be increased when the matching degree of the current laser observation data and the map is greater than the third threshold. The third threshold is an upper threshold of the preset observation matching degree and is used for determining whether to adjust the size of the matching observation window, and if the matching degree of the current laser observation data and the map is larger and exceeds the third threshold, the matching observation window is enlarged, namely the number of the laser observation data in the matching observation window is increased, so that false triggering is avoided.
In some embodiments of the present application, there is also provided a local self-positioning system of a preferred mobile robot, as shown in fig. 3, the system mainly comprises three parts: the device comprises a dynamic window generation module, a self-positioning module and a data acquisition module.
The dynamic window generation module is used for judging whether the robot is in an idle state or not by combining mileage information and acceleration information provided by the inertial navigation system, and if the robot is in an abnormal state, the size of a dynamic window (namely a matched observation window) is reduced so as to rapidly recalculate particle distribution in a local part and eliminate the problem of abnormal positioning.
The data acquisition module is mainly used for acquiring laser observation data, odometer information and acceleration information and providing data support for the operation of other modules, and mainly comprises a sensor data acquisition filtering module, an odometer information acquisition module and an acceleration information acquisition module. The sensor data acquisition filtering module is used for acquiring laser sensor data and removing redundant laser measurement data and redundant noise points by adopting a related filtering algorithm, namely filtering the laser observation data and removing the noise points. The filtering algorithm can adopt single-point filtering, median filtering, houghline extraction and the like to avoid the problem of plane-like non-correspondence caused by inaccurate measurement. The odometer information acquisition module is used for acquiring mileage data and providing priori knowledge for the self-positioning module. The acceleration information acquisition module is used for acquiring acceleration information and providing judgment basis for judging whether wheels of the robot slip or not and idle running.
The self-positioning module is mainly used for realizing the self-positioning function of the robot by adopting an improved particle filter algorithm according to an environment map, odometer information, laser observation information and a dynamic window. The particle distribution calculation system mainly comprises an improved particle filtering module, a particle clustering module, an ICP (inductively coupled plasma) matching algorithm module and a window-based particle distribution calculation module.
The improved particle filtering module is used for regenerating sampling particles based on the weight w (t-1) of the last moment by using a random sampling strategy for each particle, updating the positions of the particles by using robot motion information obtained by a milemeter, and calculating the weight of the particles by using the similarity of laser observation data and a map of the positions of the particles to obtain the particle distribution under current observation. And the particle clustering module is used for obtaining the maximum clustering according to the particle distribution by adopting a clustering algorithm. And the ICP matching algorithm module is used for carrying out point cloud matching by combining the maximum clustering mean pose, laser observation and a global map for positioning to obtain the matching degree and the pose after accurate matching. The window-based particle distribution calculation module is used for recalculating particle distribution and particle weight according to the size of the dynamic window of the robot when the current positioning matching degree is less than a certain set threshold value and the matching times reach the number of laser observation data in the window (namely the matching degree is greater than the size of the window) according to the dynamic window generated by the dynamic window generation module.
Some embodiments of the present application also provide a local self-localization apparatus of a mobile robot, the apparatus comprising a memory for storing computer program instructions and a processor for executing the program instructions, wherein the computer program instructions, when executed by the processor, cause the apparatus to perform the aforementioned local self-localization method of a mobile robot.
Some embodiments of the present application also provide a computer readable medium having stored thereon computer readable instructions executable by a processor to implement the aforementioned local self-positioning method of a mobile robot.
In summary, the solution provided by some embodiments of the present application can determine the motion state of the mobile robot according to the mileage information and the acceleration information, obtain the particle distribution information for predicting the pose of the mobile robot when the motion state is abnormal, then cluster the particles, determine the pose corresponding to the particle weight mean value in the classification with the largest particle weight mean value as the predicted pose, match the map corresponding to the predicted pose with the laser observation data at the current time, determine the observation matching degree and the pose of the mobile robot, and re-determine the observation matching degree and the pose of the mobile robot when the observation matching degree is smaller than the first threshold, so that the accurate self-positioning of the mobile robot can be realized when the current environment is different from the map greatly, or the mobile robot has abnormal conditions such as wheel slipping and idle running, and the problem of the mobile robot positioning loss can be avoided, the scheme is simple and easy to realize, and the self-positioning accuracy of the mobile robot is improved.
It should be noted that the present application may be implemented in software and/or a combination of software and hardware, for example, implemented using Application Specific Integrated Circuits (ASICs), general purpose computers or any other similar hardware devices. In one embodiment, the software programs of the present application may be executed by a processor to implement the steps or functions described above. Likewise, the software programs (including associated data structures) of the present application may be stored in a computer readable recording medium, such as RAM memory, magnetic or optical drive or diskette and the like. Additionally, some of the steps or functions of the present application may be implemented in hardware, for example, as circuitry that cooperates with the processor to perform various steps or functions.
In addition, some of the present application may be implemented as a computer program product, such as computer program instructions, which when executed by a computer, may invoke or provide methods and/or techniques in accordance with the present application through the operation of the computer. Program instructions which invoke the methods of the present application may be stored on a fixed or removable recording medium and/or transmitted via a data stream on a broadcast or other signal-bearing medium and/or stored within a working memory of a computer device operating in accordance with the program instructions. An embodiment according to the present application comprises a device comprising a memory for storing computer program instructions and a processor for executing the program instructions, wherein the computer program instructions, when executed by the processor, trigger the device to perform a method and/or a solution according to the aforementioned embodiments of the present application.
It will be evident to those skilled in the art that the present application is not limited to the details of the foregoing illustrative embodiments, and that the present application may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the application being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned. Furthermore, it is obvious that the word "comprising" does not exclude other elements or steps, and the singular does not exclude the plural. A plurality of units or means recited in the apparatus claims may also be implemented by one unit or means in software or hardware.
Claims (10)
1. A method of local self-positioning of a mobile robot, wherein the mobile robot comprises an inertial navigation system comprising an odometer, an accelerometer and a gyroscope, and a lidar, the method comprising:
determining the motion state of the mobile robot according to the mileage information and the acceleration information acquired from the inertial navigation system, wherein if the acquired mileage information is continuously increased and the change of the acceleration information is less than a second threshold value, the motion state of the mobile robot is determined to be an abnormal state;
when the motion state is an abnormal state, acquiring particle distribution information for predicting the pose of the mobile robot through a self-adaptive Monte Carlo self-positioning algorithm according to the mileage information, the acceleration information and the angle change information acquired from the inertial navigation system;
clustering the particles according to the particle distribution information, and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a prediction pose;
matching the map corresponding to the predicted pose with the laser observation data at the current moment through an iterative closest point algorithm, and determining the observation matching degree and the pose of the mobile robot;
if the observation matching degree is smaller than the first threshold value, according to the motion range of the mobile robot corresponding to the matched observation window, acquiring particle distribution information used for predicting the pose of the mobile robot again, determining a new predicted pose of the mobile robot according to the particle distribution information, matching a map corresponding to the new predicted pose with laser observation data in the matched observation window, and determining the observation matching degree and the pose of the mobile robot again, wherein the matched observation window comprises multi-frame laser observation data which are acquired through a laser radar and have a time sequence relation.
2. The method of claim 1, wherein acquiring particle distribution information for predicting mobile robot pose by an adaptive Monte Carlo self-localization algorithm comprises:
initializing particle distribution information for predicting the pose of the mobile robot according to the mileage information, the acceleration information and the angle change information;
updating the particle position according to the mileage information;
matching the current-time laser observation data acquired from the laser radar with a map of the position of the particle, and determining the weight of the particle according to the matching result;
and performing particle resampling according to the particle weight to obtain the resampled particle distribution information.
3. The method of claim 2, wherein updating particle locations according to mileage information comprises:
and acquiring the particle distribution information of the current moment through a preset motion model according to the particle distribution information of the previous moment.
4. The method of claim 2, wherein resampling particles according to particle weights comprises:
and regenerating the sampling particles through a random sampling strategy according to the weight of the particles at the last moment.
5. The method of claim 1, wherein clustering particles according to the particle distribution information and determining a pose corresponding to a particle weight mean in a classification with the largest particle weight mean as a predicted pose comprises:
clustering the particles according to the particle distribution information by an unsupervised learning clustering algorithm to obtain particle classification;
obtaining the classification with the largest particle weight mean value according to the particle classification;
and determining the pose corresponding to the particle weight mean value in the classification with the maximum particle weight mean value as a predicted pose.
6. The method of claim 1, wherein the laser observation data is laser scan information comprising angle and distance data.
7. The method of claim 1, wherein the method further comprises:
and when the motion state is an abnormal state, reducing the number of the laser observation data in the matched observation window.
8. The method of claim 1, wherein the method further comprises:
and when the matching degree of the current laser observation data and the map is greater than a third threshold value, increasing the number of the laser observation data in the matched observation window.
9. A local self-positioning apparatus of a mobile robot, the apparatus comprising a memory for storing computer program instructions and a processor for executing the program instructions, wherein the computer program instructions, when executed by the processor, cause the apparatus to perform the method of any of claims 1 to 8.
10. A computer readable medium having computer readable instructions stored thereon which are executable by a processor to implement the method of any one of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810872628.9A CN108931245B (en) | 2018-08-02 | 2018-08-02 | Local self-positioning method and equipment for mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810872628.9A CN108931245B (en) | 2018-08-02 | 2018-08-02 | Local self-positioning method and equipment for mobile robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108931245A CN108931245A (en) | 2018-12-04 |
| CN108931245B true CN108931245B (en) | 2021-09-07 |
Family
ID=64445544
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810872628.9A Active CN108931245B (en) | 2018-08-02 | 2018-08-02 | Local self-positioning method and equipment for mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108931245B (en) |
Families Citing this family (27)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109633666B (en) * | 2019-01-18 | 2021-02-02 | 广州高新兴机器人有限公司 | Positioning method based on laser radar in indoor dynamic environment and computer storage medium |
| CN111609853A (en) * | 2019-02-25 | 2020-09-01 | 北京奇虎科技有限公司 | Three-dimensional map construction method, sweeping robot and electronic equipment |
| CN109947103B (en) * | 2019-03-18 | 2022-06-28 | 深圳一清创新科技有限公司 | Unmanned control method, device and system and bearing equipment |
| CN111765888A (en) * | 2019-04-01 | 2020-10-13 | 阿里巴巴集团控股有限公司 | Device positioning method and device, electronic device and readable storage medium |
| CN110260855B (en) * | 2019-06-24 | 2021-06-29 | 北京壹氢科技有限公司 | Indoor pedestrian navigation positioning method integrating pedestrian dead reckoning, geomagnetic information and indoor map information |
| CN110361744B (en) * | 2019-07-09 | 2022-11-01 | 哈尔滨工程大学 | RBMCDA underwater multi-target tracking method based on density clustering |
| CN110488818B (en) * | 2019-08-08 | 2020-07-17 | 深圳市银星智能科技股份有限公司 | Laser radar-based robot positioning method and device and robot |
| CN110779528B (en) * | 2019-11-07 | 2022-05-31 | 四川长虹电器股份有限公司 | Particle filter-based positioning recovery method and robot equipment |
| CN111076724B (en) * | 2019-12-06 | 2023-12-22 | 苏州艾吉威机器人有限公司 | Three-dimensional laser positioning method and system |
| CN111077495B (en) * | 2019-12-10 | 2022-02-22 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| CN111421548B (en) * | 2020-04-21 | 2021-10-19 | 武汉理工大学 | Mobile robot positioning method and system |
| CN111578958A (en) * | 2020-05-19 | 2020-08-25 | 山东金惠新达智能制造科技有限公司 | Mobile robot navigation real-time positioning method, system, medium and electronic device |
| CN111707252B (en) * | 2020-06-11 | 2022-03-08 | 上海有个机器人有限公司 | Positioning judgment method and device |
| CN112068547A (en) * | 2020-08-05 | 2020-12-11 | 歌尔股份有限公司 | Robot positioning method and device based on AMCL and robot |
| CN112344933B (en) * | 2020-08-21 | 2023-04-07 | 北京京东乾石科技有限公司 | Information generation method and device and storage medium |
| CN114088093A (en) * | 2020-11-09 | 2022-02-25 | 北京京东乾石科技有限公司 | Point cloud map construction method, device and system and storage medium |
| CN112880664A (en) * | 2021-01-21 | 2021-06-01 | 深圳市镭神智能系统有限公司 | Positioning method, device and equipment of driving equipment and storage medium |
| CN112965076A (en) * | 2021-01-28 | 2021-06-15 | 上海思岚科技有限公司 | Multi-radar positioning system and method for robot |
| CN113296503A (en) * | 2021-05-08 | 2021-08-24 | 西安达升科技股份有限公司 | AGV trolley path optimization method and device and storage medium |
| CN113310484B (en) * | 2021-05-28 | 2022-06-24 | 杭州艾米机器人有限公司 | Mobile robot positioning method and system |
| CN113514843A (en) * | 2021-07-09 | 2021-10-19 | 深圳华芯信息技术股份有限公司 | Multi-subgraph laser radar positioning method and system and terminal |
| CN113503876B (en) * | 2021-07-09 | 2023-11-21 | 深圳华芯信息技术股份有限公司 | Multi-sensor fusion laser radar positioning method, system and terminal |
| CN114543819B (en) * | 2021-09-16 | 2024-03-26 | 北京小米移动软件有限公司 | Vehicle positioning method, device, electronic equipment and storage medium |
| CN114199251B (en) * | 2021-12-03 | 2023-09-15 | 江苏集萃智能制造技术研究所有限公司 | Anti-collision positioning method for robot |
| CN114643579B (en) * | 2022-03-29 | 2024-01-16 | 深圳优地科技有限公司 | Robot positioning method and device, robot and storage medium |
| CN115127559A (en) * | 2022-06-28 | 2022-09-30 | 广东利元亨智能装备股份有限公司 | Positioning method, device, equipment and storage medium |
| CN117589154A (en) * | 2024-01-19 | 2024-02-23 | 深圳竹芒科技有限公司 | Relocation method of self-mobile device, self-mobile device and readable storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105652871A (en) * | 2016-02-19 | 2016-06-08 | 深圳杉川科技有限公司 | Repositioning method for mobile robot |
| CN106123890A (en) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | A kind of robot localization method of Fusion |
| CN107356252A (en) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | A kind of Position Method for Indoor Robot for merging visual odometry and physics odometer |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100877071B1 (en) * | 2007-07-18 | 2009-01-07 | 삼성전자주식회사 | Method and apparatus of pose estimation in a mobile robot based on particle filter |
-
2018
- 2018-08-02 CN CN201810872628.9A patent/CN108931245B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105652871A (en) * | 2016-02-19 | 2016-06-08 | 深圳杉川科技有限公司 | Repositioning method for mobile robot |
| CN106123890A (en) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | A kind of robot localization method of Fusion |
| CN107356252A (en) * | 2017-06-02 | 2017-11-17 | 青岛克路德机器人有限公司 | A kind of Position Method for Indoor Robot for merging visual odometry and physics odometer |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108931245A (en) | 2018-12-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108931245B (en) | Local self-positioning method and equipment for mobile robot | |
| CN109144056B (en) | Global self-positioning method and device for mobile robot | |
| CN108152831B (en) | Laser radar obstacle identification method and system | |
| EP3361278B1 (en) | Autonomous vehicle localization based on walsh kernel projection technique | |
| EP3639241B1 (en) | Voxel based ground plane estimation and object segmentation | |
| CN110675307B (en) | Implementation method from 3D sparse point cloud to 2D grid graph based on VSLAM | |
| US10496104B1 (en) | Positional awareness with quadocular sensor in autonomous platforms | |
| CN112639502A (en) | Robot pose estimation | |
| Rummelhard et al. | Conditional monte carlo dense occupancy tracker | |
| JP2022538927A (en) | 3D target detection and intelligent driving | |
| KR101572851B1 (en) | Method for building map of mobile platform in dynamic environment | |
| KR101711964B1 (en) | Free space map construction method, free space map construction system, foreground/background extraction method using the free space map, and foreground/background extraction system using the free space map | |
| JP2020134517A (en) | Method of position identification of autonomous vehicle including multiple sensors | |
| JP2015148601A (en) | System and method for mapping, localization and pose correction | |
| WO2023050638A1 (en) | Curb recognition based on laser point cloud | |
| US20220291012A1 (en) | Vehicle and method for generating map corresponding to three-dimensional space | |
| WO2021011185A1 (en) | Robust localization | |
| CN109885046B (en) | Mobile robot positioning and accelerating method based on particle filtering | |
| Baig et al. | A robust motion detection technique for dynamic environment monitoring: A framework for grid-based monitoring of the dynamic environment | |
| KR102125538B1 (en) | Efficient Map Matching Method for Autonomous Driving and Apparatus Thereof | |
| CN111045433B (en) | Obstacle avoidance method for robot, robot and computer readable storage medium | |
| CN112036274A (en) | Driving region detection method and device, electronic equipment and storage medium | |
| CN111768489B (en) | Indoor navigation map construction method and system | |
| US20200334900A1 (en) | Landmark location reconstruction in autonomous machine applications | |
| Yan et al. | RH-Map: Online Map Construction Framework of Dynamic Object Removal Based on 3D Region-wise Hash Map Structure |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |