AGV laser navigation positioning method based on feature matching and particle swarm optimization

Abstract

The invention discloses an AGV laser navigation positioning method based on feature matching and particle swarm optimization, and the method employs a real-time data collection module, a reflector feature extraction module, a global positioning module, and an AMCL local positioning module. The first real-time data acquisition module is used for monitoring and acquiring laser and IMU and odometer data in real time according to respective frequencies when the AGV operates. According to the method, the global positioning algorithm is deployed in the AMCL positioning program, the problem of non-fixed point online can be effectively solved, the flexibility of AGV operation is improved, when the vehicle body pose is lost and needs to be restarted, manual intervention is not needed to move the vehicle body to the nearest online point, global positioning can be achieved on the original working path section, and the working efficiency is improved. And the AGV is restored to be positioned from a'complete loss' state, so that subsequent path planning, obstacle avoidance and task execution are supported, and the robustness of the system is improved.

Description

Technical Field

[0001] The present invention relates to the field of AGV positioning and navigation technology, and in particular to an AGV laser navigation and positioning method based on feature matching and particle swarm optimization. Background Art

[0002] When AGVs participate in production practices, they must solve three problems: "Where am I?", "Where am I going?", and "How do I get there?" The first and foremost question is to answer the AGV's question "Where am I?", that is, to solve its positioning problem and inform the AGV of its specific location. If the AGV does not have a positioning function, it will be like a headless fly, unable to carry out handling work and may even pose a safety hazard. The AGV's global positioning technology is designed to solve the problem of determining the vehicle's posture (position and attitude) in an unknown environment. In a completely unknown environment or when its own posture information is lost, it uses sensor data and map information to re-determine its absolute position and orientation in the map. Its core is to achieve autonomous positioning through sensor information fusion and environmental feature matching.

[0003] If the AGV's starting position is fixed, such as at a charging station or the starting point of a warehouse, default pose parameters can be set directly in the positioning program's configuration file to shorten particle convergence time. Meanwhile, QR codes or AprilTags can be deployed in key areas. The robot uses its visual sensor to identify the beacon's absolute pose and thus obtain its initial pose. This method can achieve good results in some structured areas. For large-scale scenarios, multiple positioning markers are required throughout the site to improve the flexibility and usability of global positioning, but this also increases operational and maintenance costs. Even so, initialization at any arbitrary position is not possible. Another global positioning method, when the initial position is unknown, randomly distributes particles throughout the free space of the entire map, gradually converging to the correct pose as the vehicle moves. This method is suitable for scenarios without prior information, but it is time-consuming. The generated particle swarm density in large scenarios is sparse and prone to pose kidnapping. Localization efficiency and success rate decrease as the map area increases.

[0004] Aiming at the existing AGV's demand for global positioning and the problems encountered by actual positioning algorithms, in order to obtain the AGV's initial positioning efficiently and quickly, give the positioning algorithm a good initial state estimation, and improve the accuracy and reliability of positioning, an AGV laser navigation positioning method based on feature matching and particle swarm optimization is proposed. The possible starting poses are searched through feature point extraction and feature matching methods, in order to show better results in the positioning problem without initial pose and the problem of using a small number of particles to kidnap the robot. Summary of the Invention

[0005] The purpose of the present invention is to provide an AGV laser navigation and positioning method based on feature matching and particle swarm optimization to solve the problems in the above technology.

[0006] To achieve the above objectives, the present invention provides the following technical solutions: an AGV laser navigation and positioning method based on feature matching and particle swarm optimization, the positioning method comprising a real-time data acquisition module, a reflector feature extraction module, a global positioning module, and an AMCL local positioning module. The real-time data acquisition module is used to monitor and collect laser, IMU, and odometer data in real time at their respective frequencies when the AGV is running. The reflector feature extraction module is used to perform spatial clustering on the reflector point set and calculate the weighted centroid to obtain a reflector polar coordinate combination. The global positioning module is used to screen out the path point with the highest probability and form multiple particle clusters around it. The AMCL local positioning module is used to iteratively update the particle swarm to make it converge to the position of the vehicle body.

[0007] The specific steps of the positioning method are as follows:

[0008] Step 1: Get the global coordinates of the feature points and the body posture of the AGV's travel path as prior information. The global coordinates of the feature points can be obtained with the help of surveying instruments. Before the positioning program is started, the feature matrix can be calculated in advance through the prior information. Retrieve data during actual operation to reduce the amount of computation and improve efficiency.

[0009] Step 2: The AGV starts around the work path and switches to manual mode. The positioning program obtains the laser scanning frame of the surrounding environment. The laser data is transmitted in the form of a topic in the RO step. When the laser topic is received, the global positioning related program segment is executed in the topic callback function.

[0010] Step 3: Identify and extract the feature points of the first frame of laser scanning data obtained in step 2, obtain the coordinates of several feature points, calculate their Euclidean distance, and further obtain the observed feature vector

[0011] Step 4: The observed feature vector obtained in step 3 With the characteristic matrix Compare the feature vectors in each column and evaluate the similarity between them, which is used to calculate the weight W of the path point. k The basis for

[0012] Step 5: For the weight set {W}=[W1…W ii], the weight value represents the possibility that the posture of the path point is the true posture of the vehicle body. The larger the weight, the higher the possibility. According to the difference in the weight of the path points, the path points with larger weight values ​​are proportionally screened out, and the path points with larger differences from the map features are filtered out;

[0013] Step 6: With the path point selected in step 5 as the center, a normally distributed particle cluster is formed around it. The total number of particles in each particle cluster is the same and the particle density is comparable. The final particle swarm is a mixture of multiple normally distributed particles. This particle swarm is used as the initial distribution particle swarm of AMCL. After the AGV drives a certain distance, the AMCL algorithm initializes the particle filter with the initial particle swarm. It then receives the latest observation data and odometer integration results and performs motion update, observation update, and resampling to make the particle swarm converge to the actual vehicle body posture.

[0014] Step 7: Update the covariance value of the particle swarm in real time. The uncertainty of the pose is represented by the size of the covariance value. If the covariance values ​​are all less than the set threshold, the particle swarm is considered to have converged. The mileage is calculated from the start of the vehicle. Only when the vehicle moves within the distance D1, the particle swarm converges successfully. If the particle swarm remains converged after continuing to move forward for the distance D2, the global positioning is considered to be successful. Otherwise, it is considered a failure.

[0015] Step 8: When global positioning is successful, the algorithm will exit and the AMCL algorithm will use the converged particle swarm to update the vehicle posture. When global positioning fails, the vehicle position needs to be moved for a second global positioning.

[0016] Preferably, the feature matrix in step 1 The calculation steps are as follows:

[0017] Assume the number is feature_sum, the center path of the vehicle body is composed of a set of path points Assuming the total number of path points is path_length, the pose of a single path point can be expressed as

[0018] Then, the external parameter l=(l x l y l θ ) T , calculate the position of the laser radar under the vehicle position That is, the position of the lidar path point;

[0019] The relationship between and l can be expressed using the standard composition operator on the special Euclidean Lie algebra se(2) To express:

[0020]

[0021] Following this approach, find the radar path point corresponding to each vehicle center path point to obtain a complete set of lidar path points Then, calculate the feature vector of each path point Calculate the distance d from the feature point in the map to a single radar path point j , then the eigenvector is expressed as Combine the feature vectors of each path point into a feature matrix

[0022] Preferably, the observed feature vector in step 3 The calculation steps are as follows:

[0023] Since the feature points are reflective cylinders placed in the actual working scene, the presence of feature points can be determined based on the intensity value of the lidar point. The polar coordinates of the cylinder center in the radar coordinate system are calculated by combining the polar coordinate information of the radar point and the diameter value R of the reflective cylinder.

[0024] Traversing the laser data and filtering by intensity threshold, we can get multiple sets of continuous laser radar point sets. For a single point set {scan i}={[d 1 ,θ 1 ,intensity 1 ] T …[d k ,θ k ,intensity k ] T}, perform weighted average of the polar coordinate values ​​of each set of data according to the intensity value, and calculate the closest distance dist′ from the outer wall of the cylinder to the laser radar i and angle value angle′ i ;

[0025]

[0026] The polar coordinates of the final cylinder center are [dist i angle i ]=[dist′ i +R / 2angle′ i ], then the Euclidean distance from the feature point to the radar

[0027]

[0028] Then we can get the observed eigenvector ii is the total number of observed feature points.

[0029] Preferably, the path point weight W in step 4 is k The calculation steps are as follows:

[0030] Each observation feature point can generate a weight Waypoint weight W k The weight corresponding to the observed feature point For a single observation feature point, the distance d′ from the observation feature point to the radar is i and Compare each element of to get the minimum absolute distance deviation value

[0031]

[0032] The weight generated by this feature point

[0033]

[0034] Where e is a natural constant, σ is the standard deviation of the residual normal distribution, and the weight of a single path point can be expressed as

[0035]

[0036] Finally, the weight W k and the kth vehicle center path point associated.

[0037] Preferably, the path points selected in step 5 are the possible positions of the AGV body. Each path point represents a possibility of the body position. In order to ensure the high efficiency and reliability of global positioning, it is advisable to retain 30-50 path points, which can ensure the diversity of particles and avoid the robot kidnapping problem caused by the symmetrical environment.

[0038] Preferably, the covariance value of the particle swarm in step 7 can be calculated by the following formula:

[0039]

[0040] Where N is the total number of particles, μ is the mean particle position, and x i is the position of the ith particle.

[0041] Preferably, the positioning method also includes a storage module, an execution module and a display module, the storage module is used to store relevant data, the execution module is used to read the global posture control AGV real-time positioning, and the display module is used to display the posture information and other contents of the AGV in real time.

[0042] In the above technical solution, the technical effects and advantages provided by the present invention are:

[0043] 1. Deploying the global positioning algorithm into the AMCL positioning program effectively solves the problem of non-fixed point placement, improving the flexibility of AGV operations. When the vehicle's posture is lost and needs to be restarted, the vehicle can be moved to the nearest placement point without manual intervention. Global positioning can be achieved on the original working path segment, allowing the AGV to recover its positioning from a "complete loss" state, thereby supporting subsequent path planning, obstacle avoidance, and task execution, and improving system robustness.

[0044] 2. AGVs have fixed paths when performing tasks. This global positioning method restricts the vehicle's posture to the path segment rather than the entire free space of the map. This results in low computational cost and high efficiency in actual operation. Furthermore, the algorithm's feature matching process uses the distance from feature points to the radar and the feature map for matching, thus avoiding the influence of the vehicle's posture angle on global positioning.

[0045] 3. In order to adapt to the dynamically changing environment, the global positioning algorithm retains multiple vehicle body postures, improving the stability and reliability of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is the algorithm flow chart of the present invention;

[0047] Figure 2 This is a layout diagram of the reflector of the present invention;

[0048] Figure 3 This is the rendering of the global positioning algorithm of the present invention. DETAILED DESCRIPTION

[0049] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below in conjunction with embodiments and drawings.

[0050] Example

[0051] It should be noted that, in this embodiment, the reflective plates are arranged as follows:

[0052] In a 30m*30m industrial warehouse scenario, 50 reflective columns with a diameter of 75mm are dynamically arranged at intervals of 1.5-4.0m. Figure 2 shown.

[0053] like Figure 1 、 Figure 3 As shown, the AGV laser navigation and positioning method based on feature matching and particle swarm optimization of the present invention includes the following steps:

[0054] 1. Obtain the global coordinates of the feature points and the body posture of the AGV's travel path as prior information.

[0055] 1.1. The global coordinates of the feature points can be obtained with the help of surveying instruments. Assuming the number is feature_sum, the center path of the vehicle body is composed of a set of path points Assuming the total number of path points is path_length, the pose of a single path point can be expressed as

[0056] 1.2. Then, the external parameter l=(l x l y l θ ) T , calculate the position of the laser radar under the vehicle position That is, the LiDAR path point pose, The relationship between and l can be expressed using the standard composition operator on the special Euclidean Lie algebra se(2) To express.

[0057]

[0058] 1.3. Following this approach, find the radar path point corresponding to each vehicle center path point to obtain a complete set of lidar path points Then, calculate the feature vector of each path point Calculate the distance d from the feature point in the map to a single radar path point j , then the eigenvector is expressed as Combine the feature vectors of each path point into a feature matrix

[0059] The eigenvector represents the characteristics observed at different locations on the map, which can intuitively reflect the complexity of the environment at that location. Before the positioning program is started, the feature matrix can be calculated in advance through prior information. The data is retrieved during the actual operation process to reduce the amount of calculation during operation and improve efficiency.

[0060] 2. The AGV starts around the work path and switches to manual mode. The positioning program obtains the laser scanning frame of the surrounding environment. The laser data is transmitted in the form of topics in ROS. When the laser topic is received, the global positioning related program segment is executed in the topic callback function.

[0061] 3. Identify and extract feature points from the first frame of laser scanning data, obtain the coordinates of several feature points through coordinate quantities, calculate their Euclidean distances, and further obtain the distance feature vector.

[0062] 3.1. The feature points are reflective tubes arranged in the actual working scene. Therefore, the presence of feature points can be judged by the intensity value of the laser radar point. The polar coordinates of the tube center in the radar coordinate system are calculated by combining the polar coordinate information of the radar point and the diameter value R of the reflective tube. The specific method is to traverse the laser data and filter it by the intensity threshold to obtain multiple sets of continuous laser radar point sets. For a single point set {scan i}={[d 1 ,θ 1 ,intensity 1 ] T …[d k ,θ k ,intensity k ] T}.

[0063] 3.2. Take the weighted average of the polar coordinate values ​​of each set of data according to the intensity value and calculate the closest distance dist′ from the outer wall of the cylinder to the lidar i and angle value angle′ o .

[0064]

[0065] 3.3. The polar coordinates of the final cylinder center are [dist i angle i ]=[dist i ′ +R / 2angle i ′ ], then the Euclidean distance from the feature point to the radar

[0066]

[0067] Then we can get the observed eigenvector ii is the total number of observed feature points.

[0068] 4. The observed feature vector obtained in the previous step With the characteristic matrix Compare the feature vectors in each column and evaluate the similarity between them, which is used to calculate the weight W of the path point. k The basis of.

[0069] 4.1. Each observation feature point can generate a weight Waypoint weight W k The weight corresponding to the observed feature point For a single observation feature point, the distance d′ from the observation feature point to the radar is o and Compare each element of to get the minimum absolute distance deviation value

[0070]

[0071] 4.2. Weight generated by the feature point

[0072]

[0073] Where e is a natural constant, σ is the standard deviation of the residual normal distribution, and the weight of a single path point can be expressed as

[0074]

[0075] Finally, the weight W k and the kth vehicle center path point associated.

[0076] 5. For the weight set {W}=[W1…W ii ], the weight value represents the possibility that the posture of the path point is the true posture of the vehicle body. The larger the weight, the higher the possibility. According to the difference in the weights of the path points, the path points with larger weight values ​​are proportionally screened out, and the path points with larger differences from the map features are filtered out.

[0077] These path points are the possible positions of the AGV body. Each path point represents a possible position of the body. In order to ensure the high efficiency and reliability of global positioning, experiments have found that it is appropriate to retain 30-50 path points in the end. This can ensure the diversity of particles and avoid the robot kidnapping problem caused by the symmetrical environment.

[0078] 6. Then, with the selected path points as the center, normally distributed particle clusters are formed around them. Considering the problem that the dynamic interference of the environment will cause the path points near the actual vehicle posture to have a small weight, the total number of particles in each particle cluster is the same and the particle density is comparable. The final particle group is a mixture of multiple normally distributed particles.

[0079] 7. The particle swarm is used as the initial distribution particle swarm of AMCL. After the AGV moves forward for a certain distance, the AMCL algorithm initializes the particle filter through the initial particle swarm. Then, it receives the latest observation data and odometer integration results and performs motion update, observation update and resampling to make the particle swarm converge to the real vehicle posture.

[0080] In order to improve the security of global positioning and prevent unnecessary losses caused by incorrect positioning, the reliability of the positioning results is determined, and the covariance value of the particle swarm is updated in real time. The uncertainty of the posture is characterized by the size of the covariance value. If the covariance values ​​are all less than the set threshold, the particle swarm is considered to have converged. The mileage is calculated from the start of the vehicle. Only when the vehicle moves within a distance of D1, the particle swarm converges successfully. If the particle swarm remains converged after continuing to move forward for a distance of D2, the global positioning is judged to be successful. Otherwise, it is considered a failure.

[0081] The covariance value of the particle swarm can be calculated by the following formula:

[0082]

[0083] Where N is the total number of particles, μ is the mean particle position, and x i is the position of the ith particle.

[0084] 8. When global positioning is successful, the algorithm will exit and the AMCL algorithm will use the converged particle swarm to update the vehicle posture. When global positioning fails, the vehicle position needs to be moved for a second global positioning.

[0085] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. AGV laser navigation and positioning method based on feature matching and particle swarm optimization, characterized by: The positioning method includes a real-time data acquisition module, a reflector feature extraction module, a global positioning module, and an AMCL local positioning module. The real-time data acquisition module is used to monitor and collect laser, IMU, and odometer data in real time at their respective frequencies when the AGV is running. The reflector feature extraction module is used to perform spatial clustering on the reflector point set and calculate the weighted centroid to obtain a reflector polar coordinate combination. The global positioning module is used to screen out the path point with the highest probability and form multiple particle clusters around it. The AMCL local positioning module is used to iteratively update the particle swarm to make it converge to the position of the vehicle body. The specific steps of the positioning method are as follows: Step 1: Get the global coordinates of the feature points and the body posture of the AGV's travel path as prior information. The global coordinates of the feature points can be obtained with the help of surveying instruments. Before the positioning program is started, the feature matrix can be calculated in advance through the prior information. Retrieve data during actual operation to reduce the amount of computation and improve efficiency. Step 2: The AGV starts around the work path and switches to manual mode. The positioning program obtains the laser scanning frame of the surrounding environment. The laser data is transmitted in the form of a topic in the RO step. When the laser topic is received, the global positioning related program segment is executed in the topic callback function. Step 3: Identify and extract the feature points of the first frame of laser scanning data obtained in step 2, obtain the coordinates of several feature points, calculate their Euclidean distance, and further obtain the observed feature vector Step 4: The observed feature vector obtained in step 3 With the characteristic matrix Compare the feature vectors in each column and evaluate the similarity between them, which is used to calculate the weight W of the path point. k The basis for Step 5: For the weight set {W}=[W1…W ii ], the weight value represents the possibility that the posture of the path point is the true posture of the vehicle body. The larger the weight, the higher the possibility. According to the difference in the weight of the path points, the path points with larger weight values ​​are proportionally screened out, and the path points with larger differences from the map features are filtered out; Step 6: With the path point selected in step 5 as the center, a normally distributed particle cluster is formed around it. The total number of particles in each particle cluster is the same and the particle density is comparable. The final particle swarm is a mixture of multiple normally distributed particles. This particle swarm is used as the initial distribution particle swarm of AMCL. After the AGV drives a certain distance, the AMCL algorithm initializes the particle filter with the initial particle swarm. It then receives the latest observation data and odometer integration results and performs motion update, observation update, and resampling to make the particle swarm converge to the actual vehicle body posture. Step 7: Update the covariance value of the particle swarm in real time. The uncertainty of the pose is represented by the size of the covariance value. If the covariance values ​​are all less than the set threshold, the particle swarm is considered to have converged. The mileage is calculated from the start of the vehicle. Only when the vehicle moves within the distance D1, the particle swarm converges successfully. If the particle swarm remains converged after continuing to move forward for the distance D2, the global positioning is considered to be successful. Otherwise, it is considered a failure. Step 8: When global positioning is successful, the algorithm will exit and the AMCL algorithm will use the converged particle swarm to update the vehicle posture. When global positioning fails, the vehicle position needs to be moved for a second global positioning.

2. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 1 is characterized in that: The feature matrix in step 1 The calculation steps are as follows: Assume the number is feature_sum, the center path of the vehicle body is composed of a set of path points Assuming the total number of path points is path_length, the pose of a single path point can be expressed as Then, the external parameter l=(l x l y l θ ) T , calculate the position of the laser radar under the vehicle position That is, the position of the lidar path point; The relationship between and l can be expressed using the standard composition operator on the special Euclidean Lie algebra se(2) To express: Following this approach, find the radar path point corresponding to each vehicle center path point to obtain a complete set of lidar path points Then, calculate the feature vector of each path point Calculate the distance d from the feature point in the map to a single radar path point j , then the eigenvector is expressed as Combine the feature vectors of each path point into a feature matrix 3. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 1 is characterized in that: The observed feature vector in step 3 The calculation steps are as follows: Since the feature points are reflective cylinders placed in the actual working scene, the presence of feature points can be determined based on the intensity value of the lidar point. The polar coordinates of the cylinder center in the radar coordinate system are calculated by combining the polar coordinate information of the radar point and the diameter value R of the reflective cylinder. Traversing the laser data and filtering by intensity threshold, we can get multiple sets of continuous laser radar point sets. For a single point set {scan i }={[d 1 ,θ 1 ,intensity 1 ] T …[d k ,θ k ,intensity k ] T }, perform weighted average of the polar coordinate values ​​of each set of data according to the intensity value, and calculate the closest distance dist′ from the outer wall of the cylinder to the laser radar i and angle value angle′ i ; The polar coordinates of the final cylinder center are [dist i angle i ]=[dist′ i +R / 2 angle′ i ], then the Euclidean distance from the feature point to the radar Then we can get the observed eigenvector ii is the total number of observed feature points.

4. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 2 is characterized in that: The path point weight W in step 4 k The calculation steps are as follows: Each observation feature point can generate a weight Waypoint weight W k The weight corresponding to the observed feature point For a single observation feature point, the distance d′ from the observation feature point to the radar is i and Compare each element of to get the minimum absolute distance deviation value The weight generated by this feature point Where e is a natural constant, σ is the standard deviation of the residual normal distribution, and the weight of a single path point can be expressed as Finally, the weight W k and the kth vehicle center path point associated.

5. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 1 is characterized in that: The path points selected in step 5 are the possible positions of the AGV body. Each path point represents a possible position of the body. In order to ensure the high efficiency and reliability of global positioning, it is advisable to retain 30-50 path points. This can ensure the diversity of particles and avoid the robot kidnapping problem caused by the symmetrical environment.

6. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 1 is characterized in that: The covariance value of the particle swarm in step 7 can be calculated by the following formula: Where N is the total number of particles, μ is the mean particle position, and x i is the position of the ith particle.

7. The AGV laser navigation and positioning method based on feature matching and particle swarm optimization according to claim 1 is characterized in that: The positioning method also includes a storage module, an execution module and a display module. The storage module is used to store relevant data, the execution module is used to read the global posture control AGV real-time positioning, and the display module is used to display the posture information of the AGV and other contents in real time.

Images