Robot positioning method based on AMCL Monte Carlo

Abstract

The invention provides a robot positioning method based on AMCL Monte Carlo, and the method comprises the steps: firstly measuring the surrounding environment of a robot through a laser radar when the position of the robot is positioned, comparing the measured map data with an initially set map, and if the deviation value of a comparison result is large, determining that the robot is located. If yes, current positioning of the robot is judged to be unstable, further positioning is carried out, whether the current displacement of the robot is within the preset displacement range or not is determined by carrying out short-time displacement on the robot, then whether positioning of the robot is accurate or not is determined, and therefore the positioning stability of the robot is improved; and therefore, the adaptability of the robot to the environment is higher.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning, and in particular to a robot positioning method based on AMCL Monte Carlo. Background Art

[0002] In modern times, with the rapid development of robotics technology, robot positioning and mapping solutions based on a variety of sensor technologies have gradually become stable and mature, and have gradually transformed from traditional tracked magnetic navigation to dynamic path navigation based on the environment. The fixed-route robot navigation method has a single task and action. It can ensure that it does not deviate from the navigation track and will suspend operation when encountering obstacles. When there are no obstacles, it will smoothly run to the working point. It is a navigation method that can achieve stable and safe operation. The application of SLAM (simultaneous localization and simultaneous mapping) technology has enabled mobile robots to quickly enter various industries, such as restaurant robots used to deliver meals in restaurants, medical disinfection robots used by the medical industry to disinfect dangerous places, and inspection robots used for patrolling in the streets. So far, we have become accustomed to the services of mobile machines in various occasions. More and more Sensor technology companies have also begun to join the track of robot sensor innovation and research and development, making the market for consumer robot sensors prosperous. LiDARs that generally use the principles of triangulation and light time-of-flight ranging have made them affordable for more robot R&D teams. Robots based on 2D LiDAR SLAM technology are especially favored by many technology companies for their mature and stable navigation solutions and smaller return on investment time difference. However, in widespread use, AMCL (Monte Carlo positioning) based on particle filtering in 2D LiDAR SLAM positioning technology is more or less affected by environmental changes, the influence of inherent manufacturing errors of sensors, and the influence of sensor measurements. When the erroneous data is sent to the laser positioning algorithm for calculation, it will produce large positioning and navigation deviations, resulting in unstable and abnormal robot walking, global positioning jumps, and even positioning failure. Summary of the invention

[0003] The present invention provides a robot positioning method based on AMCL Monte Carlo, which can determine whether the positioning of the robot is accurate, thereby improving the stability of the robot positioning.

[0004] To achieve the above object, the technical solution of the present invention is: a robot positioning method based on AMCL Monte Carlo, the specific steps include: S1 presets the map where the robot is located as the initial map and records the environmental data Q of the initial map.

[0005] S2 establishes the robot lidar observation model, which includes the normal distribution model, the exponential distribution model and the maximum measurement distribution model.

[0006] S3 uses the observation model to process the environmental data obtained by the lidar. The steps are as follows: The S31 observation model classifies and stores the environmental data acquired by the laser radar; the classification method is: the normally measured laser beam is input into the normal distribution model, the truncated laser beam generated by the dynamic object is input into the exponential distribution model, and the laser beam whose return beam is not detected by the laser radar is input into the maximum measurement distribution model.

[0007] S32 The normal distribution model parameterizes the environmental data stored therein to form a result p1, the exponential distribution model parameterizes the environmental data stored therein to form a result p2, and the maximum measurement distribution model parameterizes the environmental data stored therein to form a result p3.

[0008] S33 Add p1, p2, and p3 to form data p.

[0009] S4 obtains environmental data of the initial map.

[0010] S5 compares p and Q; if the deviation value of p and Q is within the preset range, it means that the current positioning of the robot is accurate; if the deviation value of p and Q is not within the preset range; it means that the current positioning of the robot is unstable and position measurement is required, then proceed to step S6.

[0011] S6 Establish a robot position detection model.

[0012] S7 detects the robot's movement through the robot's position sensor and establishes a position detection model.

[0013] The S8 robot performs short-term displacement, the position sensor collects the robot's movement trajectory data, and the position detection model calculates the robot's displacement based on the robot's movement trajectory data.

[0014] S9 determines whether the displacement of the robot is within the preset range. If the displacement is within the preset range, it means that the robot is accurately positioned; if the displacement is not within the preset range, the robot needs to be re-positioned globally.

[0015] The beneficial effects of the above method are as follows: when locating the position of the robot, the surrounding environment of the robot is first measured by a laser radar, and then the measured map data is compared with the initially set map. If the deviation value of the comparison result is large, it is determined that the current positioning of the robot is unstable, and further positioning is performed. By performing a short-term displacement on the robot, it is determined whether the current displacement of the robot is within a pre-set displacement range, thereby determining whether the positioning of the robot is accurate, thereby improving the positioning stability of the robot, and making the robot more adaptable to the environment.

[0016] Furthermore, based on the initial map and the current position of the robot, the current environment is measured by laser radar; a normal distribution model of the laser beam based on the distance of the obstacle measured by the laser radar is established; an exponential distribution model based on whether the laser beam of the laser radar is intercepted by a dynamic object between the laser radar and the obstacle is established; and a maximum measurement distribution model of the laser beam that the laser radar does not detect the return beam is established. This method can establish an observation model in a targeted manner, which is convenient for processing the corresponding data.

[0017] Furthermore, the laser beam whose return beam is not detected by the laser radar includes the laser beam emitted by the laser radar to an object outside the laser beam measurement range and the laser beam emitted by the laser radar to an object with high light absorption.

[0018] Furthermore, the data is processed by the normal distribution model to conform to the normal distribution algorithm. The calculation formula of the normal distribution is:

[0019] in, is the noise in the map, x is the measurement value of each laser beam, and μ is the position point on the map corresponding to the laser beam measurement value x.

[0020] In actual application scenarios, the above settings have the existence of noise. Therefore, by adding reasonable noise, it is possible to ensure that the measured map data matches the actual map data better, thereby facilitating the stable positioning of the robot.

[0021] Furthermore, the data processed by the exponential distribution model conforms to the exponential distribution algorithm. The calculation formula of the exponential distribution is: Where λ is the number of laser beam measurements and x is the measured value of each laser beam.

[0022] The above settings can facilitate the calculation of the number of times the laser radar is truncated during measurement.

[0023] Furthermore, the data is processed by the maximum measurement distribution model to meet the maximum measurement distribution algorithm. The calculation formula of the maximum measurement distribution is: Where x max It is the critical value outside the laser beam measurement range.

[0024] The above settings can facilitate the calculation of the maximum measurement range of the lidar.

[0025] Further, step S8 specifically includes: S81 The robot rotates so that its forward direction faces the target direction; S82 The robot moves a certain distance towards the target direction; S83 The robot rotates and readjusts its forward direction toward the target; The S84 position sensor detects the encoder readings generated by the robot when it moves through the rotary encoder and generates the robot's movement trajectory data; The S85 position sensor sends the robot’s movement trajectory data to the position detection model; The S86 position detection model obtains the robot's displacement based on the robot's movement trajectory data.

[0026] The above settings, by setting a position detection model, further determine whether the positioning of the robot is accurate, thereby improving the positioning stability of the robot, thereby making the robot more adaptable to the environment.

[0027] Furthermore, S84 specifically includes that when the rotary encoder rotates in the positive direction, it represents that the robot is performing rotational motion, and when the rotary encoder rotates in the reverse direction, it represents that the robot is performing branch motion. The motion direction and movement trajectory of the robot are determined by the rotation direction of the encoder.

[0028] The above arrangement can facilitate detection of the robot's motion through a rotary encoder, which is practical, convenient and reliable.

[0029] Furthermore, the displacement calculation formula of the robot is:

[0030] Among them, the starting angle of the robot is Angular velocity and time After rotation, the orientation , the machine runs at line speed and angular velocity Move, for a while After the machine moved , and .

[0031] The above arrangement, by decomposing the movement of the robot into three movement stages, enables better detection of the displacement of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] Figure 1 It is the work flow chart of the present invention.

[0033] Figure 2 It is a calculation function curve diagram of the laser radar observation model of the present invention.

[0034] Figure 3It is the moving trajectory diagram of the robot of the present invention. DETAILED DESCRIPTION

[0035] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0036] like Figures 1 to 3 As shown, a robot positioning method based on AMCL Monte Carlo, the specific steps include: S1 presets the map where the robot is located as the initial map and records the environmental data Q of the initial map.

[0037] S2: Establishing a robot laser radar observation model, which is used to receive environmental data acquired by the laser radar, and classify and process the environmental data acquired by the laser radar. The observation model includes a normal distribution model, an exponential distribution model, and a maximum measurement distribution model. In this step, the laser radar is used to measure the current environment based on the initial map and the current position of the robot, and a normal distribution model of the laser beam according to the obstacle distance measured by the laser radar is established, an exponential distribution model is established according to whether the laser beam of the laser radar is intercepted by a dynamic object between the laser radar and the obstacle, and a maximum measurement distribution model of the laser beam that the laser radar does not detect the return beam is established. The laser beam that the laser radar does not detect the return beam includes the laser beam emitted by the laser radar to an object outside the laser beam measurement range and the laser beam emitted by the laser radar to an object with high light absorption, that is, in a large area, if the object exceeds the range of the laser radar, the laser radar cannot receive the reflected signal. In addition, for objects with high light absorption, when the laser beam emitted by the laser radar reaches the object, it is basically absorbed, and the laser radar cannot receive the reflected signal.

[0038] S3 uses the observation model to process the environmental data obtained by the lidar. The steps are as follows: The S31 observation model classifies and stores the environmental data acquired by the laser radar; the classification method is: the normally measured laser beam is input into the normal distribution model, the truncated laser beam generated by the dynamic object is input into the exponential distribution model, and the laser beam whose return beam is not detected by the laser radar is input into the maximum measurement distribution model.

[0039] S32 The normal distribution model parameterizes the environmental data stored therein to form a result p1, the exponential distribution model parameterizes the environmental data stored therein to form a result p2, and the maximum measurement distribution model parameterizes the environmental data stored therein to form a result p3.

[0040] Among them, the algorithm for processing data through the normal distribution model conforms to the normal distribution. The calculation formula of the normal distribution is:

[0041] in, is the noise in the map, x is the measurement value of each laser beam, and μ is the position point on the initial map corresponding to the laser beam measurement value x.

[0042] Processing data through the exponential distribution model conforms to the exponential distribution algorithm. The calculation formula of the exponential distribution is: Where λ is the number of laser beam measurements and x is the measured value of each laser beam.

[0043] The data processed by the maximum measurement distribution model conforms to the algorithm of the maximum measurement distribution. The calculation formula of the maximum measurement distribution is:

[0044] Where x max It is the critical value outside the laser beam measurement range. For example, when the laser beam measurement range is 50m, the critical value outside the laser beam measurement range is 50m.

[0045] S33 Add p1, p2, and p3 to form data p.

[0046] S4 obtains environmental data of the initial map.

[0047] S5 compares p and Q; if the deviation value of p and Q is within the preset range, it means that the current positioning of the robot is accurate; if the deviation value of p and Q is not within the preset range; it means that the current positioning of the robot is unstable and position measurement is required, then proceed to step S6.

[0048] S6 Establish a robot position detection model.

[0049] S7 detects the robot's movement through the robot's position sensor and establishes a position detection model.

[0050] The S8 robot performs short-term displacement, and the position sensor collects the robot's movement trajectory data. The position detection model calculates the robot's displacement based on the robot's movement trajectory data; specifically: S81 The robot rotates so that its forward direction faces the target direction.

[0051] S82 The robot moves a certain distance in the direction of the target.

[0052] S83 The robot rotates and readjusts its direction toward the target.

[0053] The S84 position sensor detects the encoder readings generated by the robot when it moves through a rotary encoder and generates the robot's movement trajectory data; specifically, the positive rotation of the rotary encoder represents that the robot is performing rotational motion, and the reverse rotation of the rotary encoder represents that the robot is performing linear motion. The robot's movement direction and movement trajectory are determined by the rotation direction of the encoder.

[0054] The S85 position sensor sends the robot's movement trajectory data to the position detection model.

[0055] The S86 position detection model obtains the displacement of the robot based on the robot's moving trajectory data. The displacement calculation formula of the robot is: ; Among them, the starting angle of the robot is Angular velocity and time After rotation, the orientation , the machine runs at line speed and angular velocity Move, for a while After the machine moved , and .

[0056] S9 determines whether the displacement of the robot is within the preset range. If the displacement is within the preset range, it means that the robot is accurately positioned; if the displacement is not within the preset range, the robot needs to be re-positioned globally.

[0057] In this embodiment, in actual application scenarios, due to the presence of noise, by adding reasonable noise, it is possible to ensure that the measured map data matches the actual map data better, thereby facilitating stable positioning of the robot.

[0058] like Figure 2 As shown in the figure, by establishing the robot's lidar observation model, since the lidar measurement itself has noise, the measurement of the lidar laser beam is approximated to a normal distribution, corresponding to Figure 2 The curve at number 1 is a normal distribution curve. The robot's environment is usually dynamic, while the robot's initial map is static. Objects that are not originally included in the initial map intercept the laser beam, causing the laser radar to accidentally measure a shorter distance. This unexpected phenomenon is treated as an exponential distribution, which corresponds to Figure 2The curve at number 2 is an exponential distribution curve. Sometimes the laser radar emits a beam that encounters an object with high light absorption or exceeds the range of the laser radar. Since the radar cannot receive the return beam, the measurement will fail completely. This happens frequently, so it is set to obey a uniform measurement distribution, corresponding to Figure 2 The curve at number 3 is a uniform value distribution curve. By combining such distributions, we can get Figure 2 The overall curve is used to establish the lidar observation model of the surrounding obstacles to process the robot's positioning information.

[0059] Step (31) specifically includes subjecting each laser beam of the laser radar measurement laser frame to normal distribution, exponential distribution and uniform distribution respectively and then inputting them into different observation models.

[0060] In this embodiment, the data is processed by a normal distribution model, an exponential distribution model, and a maximum measurement distribution model, which can facilitate the stable measurement of the robot's surrounding environment.

[0061] By setting the position detection model, it is possible to further determine whether the robot's positioning is accurate, thereby improving the robot's positioning stability and making the robot more adaptable to the environment.

[0062] The rotary encoder can be used to detect the movement of the robot, which is practical, convenient and reliable.

[0063] By decomposing the robot's motion into three motion stages, the robot's displacement can be better detected.

[0064] like Figure 3 As shown, in this embodiment, the position sensor decomposes the robot's motion steps into three motion stages, the first motion is a rotational motion, such as Figure 3 As shown in the middle label A, the robot receives a rotation command. In this movement, the robot moves from the original direction to the target direction. After rotating to an appropriate angle, the second movement is a linear movement, as shown in Figure 3 As shown by the label B, this movement makes the robot move toward the target point, and the third movement is a rotational movement, such as Figure 3 As shown in the middle mark C, this movement allows the robot to finally rotate to the correct orientation, so that the robot can accurately move to the target point, and then obtain the trajectory data of the robot's movement through the position sensor to generate the robot's displacement.

[0065] The beneficial effects of the present invention are as follows: when locating the position of the robot, the surrounding environment of the robot is first measured by a laser radar, and then the measured map data is compared with the initially set map. If the deviation value of the comparison result is large, it is determined that the current positioning of the robot is unstable, and further positioning is performed. By performing a short-term displacement on the robot, it is determined whether the current displacement of the robot is within a pre-set displacement range, thereby determining whether the positioning of the robot is accurate, thereby improving the positioning stability of the robot, and making the robot more adaptable to the environment.

Claims

1. A robot positioning method based on AMCL Monte Carlo, characterized by: The specific steps include: S1 presets the map where the robot is located as the initial map and records the environmental data Q of the initial map; S2 establishes a robot laser radar observation model, which includes a normal distribution model, an exponential distribution model, and a maximum measurement distribution model; S3 uses the observation model to process the environmental data obtained by the lidar. The steps are as follows: The S31 observation model classifies and stores the environmental data acquired by the laser radar; the classification method is: the laser beam for normal measurement is input into the normal distribution model, the truncated laser beam generated by the dynamic object is input into the exponential distribution model, and the laser beam for which the laser radar does not detect the return beam is input into the maximum measurement distribution model; S32 The normal distribution model parameters the stored environmental data to form a result p1, the exponential distribution model parameters the stored environmental data to form a result p2, and the maximum measurement distribution model parameters the stored environmental data to form a result p3; S33 adds p1, p2, and p3 to form data p; S4 obtains environmental data of the initial map; S5: Compare p with Q; if the deviation between p and Q is within the preset range, it means that the current positioning of the robot is accurate; if the deviation between p and Q is not within the preset range, it means that the current positioning of the robot is unstable and position measurement is required, then proceed to step S6; S6 establishes a robot position detection model; S7 detects the robot's motion through the robot's position sensor and establishes a position detection model; The S8 robot performs a short-term displacement, the position sensor collects the robot's movement trajectory data, and the position detection model calculates the robot's displacement based on the robot's movement trajectory data; S9 determines whether the displacement of the robot is within a preset range. If the displacement is within the preset range, it means that the positioning of the robot is accurate; if the displacement is not within the preset range, the robot needs to be re-positioned globally.

2. The robot positioning method based on AMCL Monte Carlo according to claim 1, characterized in that: Based on the initial map and the current position of the robot, the current environment is measured by laser radar; a normal distribution model of the laser beam based on the obstacle distance measured by the laser radar is established; an exponential distribution model is established based on whether the laser beam of the laser radar is intercepted by a dynamic object between the laser radar and the obstacle; and a maximum measurement distribution model of the laser beam based on the return beam not detected by the laser radar is established.

3. The robot positioning method based on AMCL Monte Carlo according to claim 2, characterized in that: The laser beams for which the laser radar does not detect a return beam include the laser beams emitted by the laser radar to an object outside the measurement range of the laser beam and the laser beams emitted by the laser radar to an object with high light absorption.

4. The robot positioning method based on AMCL Monte Carlo according to claim 1, characterized in that: The algorithm for processing data through the normal distribution model conforms to the normal distribution. The calculation formula of the normal distribution is: in, is the noise in the map, x is the measurement value of each laser beam, and μ is the position point on the initial map corresponding to the laser beam measurement value x.

5. The robot positioning method based on AMCL Monte Carlo according to claim 1, characterized in that: Processing data through the exponential distribution model conforms to the exponential distribution algorithm. The calculation formula of the exponential distribution is: Where λ is the number of laser beam measurements and x is the measured value of each laser beam.

6. The robot positioning method based on AMCL Monte Carlo according to claim 1, characterized in that: The data processed by the maximum measurement distribution model conforms to the algorithm of the maximum measurement distribution. The calculation formula of the maximum measurement distribution is: Where x max It is the critical value outside the laser beam measurement range.

7. The robot positioning method based on AMCL Monte Carlo according to claim 1, characterized in that: S8 specifically includes: S81 The robot rotates so that the robot moves in the direction of the target; S82 The robot moves a certain distance towards the target direction; S83 The robot rotates and readjusts its forward direction toward the target; The S84 position sensor detects the encoder readings generated by the robot when it moves through the rotary encoder and generates the robot's movement trajectory data; The S85 position sensor sends the robot’s movement trajectory data to the position detection model; The S86 position detection model obtains the displacement of the robot based on the robot's movement trajectory data.

8. The robot positioning method based on AMCL Monte Carlo according to claim 7, characterized in that: S84 specifically includes that when the rotary encoder rotates in the positive direction, it means that the robot is performing rotational motion, and when the rotary encoder rotates in the reverse direction, it means that the robot is performing linear motion. The motion direction and moving trajectory of the robot are determined by the rotation direction of the encoder.

9. The robot positioning method based on AMCL Monte Carlo according to claim 7, characterized in that: The calculation formula of the robot's displacement is: Among them, the starting angle of the robot is Angular velocity and time After rotation, the orientation During intermediate operation, the machine runs at line speed and angular velocity Move, for a while After the machine moved and .

Images