Method, System, Device and Computer Readable Storage Medium for AGV Mapping and Positioning

Abstract

The present invention provides a method, a system, a device and a computer-readable storage medium for AGV mapping and positioning. The method for AGV mapping and positioning includes the following steps: S1. Obtain environmental information and robot pose information; S2. Calculate the expected position of a new landmark according to the robot pose information to obtain a control quantity and a control error; S3. Screen features in the environmental information through laser data to obtain the positions of the features; S4. Obtain the optimal new landmark position based on the current position of the robot according to the expected position and the positions of the features; S5. Determine whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, a landmark is established; if the new landmark position has already appeared in the map, the map is updated. The method, system, device and computer-readable storage medium for AGV mapping and positioning of the present invention can improve the positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of AGV positioning, and in particular to methods, systems, devices and computer-readable storage media for AGV map building and positioning. Background Art

[0002] The problem of simultaneous localization and mapping (SLAM) of autonomous mobile robots can be described as follows: in an unknown environment, a mobile robot perceives environmental information through on-board environmental sensing sensors (such as odometers, visual environmental sensing sensors, ultrasonic waves, lasers, etc.), gradually constructs a map of the surrounding environment, and at the same time uses this map to estimate its position and pose. This problem has always been a hot spot and a difficult point in the research field of mobile robots, and is considered to be the key problem for truly realizing autonomous navigation of robots, with broad application prospects.

[0003] Existing map building methods are mainly grid-based maps, which are also called occupancy grid (evidence grid) maps. For grid maps, the entire environment is divided into grids of a certain size, and each grid is assigned a value representing the probability that this cell is occupied. Each cell represents a square block, and a value in the range (0, 1) is used to indicate the probability that this block is occupied, indicating whether there are obstacles at its corresponding physical position. The occupancy grid map clearly shows whether a certain block is an obstacle block or a free space. Occupied grids are assigned values, and free spaces are assigned values. For grid maps, each grid is assigned a value representing the height information of this cell.

[0004] When rasterizing the map, the introduced discrete error problem will affect the positioning accuracy. Summary of the Invention

[0005] In view of this, the technical problem to be solved by the present invention is to provide methods, systems, devices and computer-readable storage media for AGV map building and positioning, which can improve the positioning accuracy.

[0006] The technical solution of the present invention is implemented as follows:

[0007] A method for AGV map building and positioning includes the following steps:

[0008] S1. Obtain environmental information and robot pose information;

[0009] S2. Calculate the expected position of a new landmark according to the robot pose information to obtain a control quantity and a control error;

[0010] S3. Screen features in the environmental information through laser data to obtain the positions of the features;

[0011] S4. Obtain a new landmark position that is optimal based on the robot's current position according to the expected position and the position of the feature;

[0012] S5. Determine whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, establish a landmark; if the new landmark position has already appeared in the map, update the map.

[0013] Preferably, the robot pose information is obtained through an odometer and includes speed and angular velocity information;

[0014] The control quantity u of the system can be obtained according to the odometer information; the motion equation of the robot is expressed as

[0015]

[0016] The control error is expressed as δu, and the state error and covariance caused by it are respectively expressed as

[0017]

[0018] where

[0019]

[0020] is the Jacobian matrix of the control equation with respect to the control quantity, is the transpose of F u ;

[0021] When the information returned by the odometer is the speed v and angular velocity ω of the robot, the control quantity is expressed as

[0022]

[0023] The form of the control equation at this time is

[0024]

[0025] When the odometer returns the pose change within the sampling interval, the control quantity is expressed as

[0026]

[0027] where δ rot1 is the first rotation angle, δ trans is the translation distance, δ rot2 is the second rotation angle, then the control equation can be expressed as:

[0028]

[0029] Substitute the control error δu of the control quantity into Equation (5) to obtain the control error of the state quantity;

[0030] Solve the control error:

[0031] The control quantity of the motion equation adopts Equation (6), uses the differential drive model to transform the odometer data, and then solves the control error;

[0032] The form of Equation (6) is:

[0033]

[0034] where is the reading of the odometer at the previous moment (time t - 1),

[0035] δx, δy, δθ respectively represent the change in the odometer speed, and can be expressed by the differential drive model as

[0036]

[0037] According to the error transfer formula, the covariance matrix of the control quantity is obtained from the covariance matrix of δs r , δs l ; The Jacobian matrices of Equations (8) and (9) are required during the solution, and are respectively:

[0038]

[0039] and

[0040]

[0041] where δs = (δs r + δs l ) / 2, When taking the derivative, first take the right wheel and then the left wheel; the covariance matrix of the moving distances of the left and right wheels is

[0042]

[0043] The covariance matrix of the control quantity can be expressed as

[0044]

[0045] where are respectively transpose, substituting Equation (13) into Equation (2) can obtain the system covariance

[0046] Preferably, the features in the environmental information are screened through laser data, and the positions of the obtained features include:

[0047] Read the data;

[0048] Noise reduction and region segmentation;

[0049] Initial acquisition of line features;

[0050] Line clustering, merging and screening;

[0051] Coordinate transformation;

[0052] Obtain line features.

[0053] Preferably, the region segmentation uses the Euclidean distance, and the distance threshold between laser points is adaptive. The threshold formula is

[0054] d point = 2rsin(α / 2)ε (14)

[0055] where r is the distance measured by the laser beam, α is the angular resolution of the laser, and ε is the magnification factor.

[0056] Preferably, the line clustering is carried out in two steps.

[0057] A Classify according to the direction and find out groups of line segments that are close to parallel

[0058] B Judge whether the line segments in each set can be merged into one line according to the distance between the line segments

[0059] A straight line is fitted from a single set after clustering to obtain the final line segment features.

[0060] The specific steps of S4 include:

[0061] Linearization of the motion equation and the measurement equation, and matrix representation of the state variables; Update calculation of the state variables and covariance by the Kalman filter algorithm;

[0062]

[0063]

[0064]

[0065]

[0066]

[0067] K t is the Kalman gain, H t is the parameter of the measurement system, z t is the landmark position observed by the laser sensor at time t, and Q t is the noise covariance.

[0068] Preferably, the Mahalanobis distance is used to judge whether the new landmark position matches the existing landmarks;

[0069] Set a threshold for the Mahalanobis distance. If the Mahalanobis distance between the currently observed line segment and the existing landmark line segment is greater than the preset threshold, it is determined that a new landmark is discovered.

[0070] The present invention also provides a system for AGV mapping and positioning, including:

[0071] A data acquisition module for acquiring environmental information and robot pose information;

[0072] An odometer prediction information module for calculating the expected position of a new landmark based on the robot pose information to obtain a control quantity and a control error;

[0073] A laser data processing module for screening features in the environmental information through laser data to obtain the positions of the features;

[0074] An EKF algorithm module for obtaining the optimal position of a new landmark based on the current position of the robot according to the expected position and the position of the feature;

[0075] A landmark processing module for determining whether the position of the new landmark matches the existing landmarks. If the position of the new landmark has not appeared in the map, a landmark is established; if the position of the new landmark has already appeared in the map, the map is updated.

[0076] The present invention also provides an AGV mapping and positioning device, including a memory and a processor; the memory is used for storing a computer program; the processor is used for implementing the AGV mapping and positioning method according to any one of claims 1-7 when executing the computer program.

[0077] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the AGV mapping and positioning method according to any one of claims 17 is implemented.

[0078] The AGV mapping and positioning method, system and computer-readable storage medium proposed by the present invention use the Extended Kalman Filter (EKF) algorithm to implement mapping and positioning calculations during the AGV's travel. This algorithm directly uses the position information of environmental features as the observed quantity. As long as the observation accuracy is sufficient, accurate observation information can be obtained, and there is no discrete error problem introduced in the rasterized map. It can reduce the calculation amount while achieving a higher positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is a flowchart of the AGV mapping and positioning method proposed by an embodiment of the present invention;

[0080] Figure 2It is the structural block diagram of the AGV mapping and positioning system of the present invention;

[0081] Figure 3 It is the meaning diagram of δ;

[0082] Figure 4 It is the scatter plot scanned by the laser resolution of 0.5 degrees;

[0083] Figure 5 It is the straight line feature diagram extracted by the 0.5-degree laser;

[0084] Figure 6 It is the straight line feature diagram extracted by the 0.25-degree laser. Specific embodiments

[0085] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0086] As Figure 1 shown, the embodiments of the present invention propose a method for AGV mapping and positioning, including the following steps:

[0087] S101. Obtain environmental information and robot pose information;

[0088] S102. Calculate the expected position of the new landmark according to the robot pose information to obtain the control quantity and control error;

[0089] S103. Screen the features in the environmental information through the laser data to obtain the positions of the features;

[0090] S104. Obtain the optimal new landmark position based on the current position of the robot according to the expected position and the position of the feature;

[0091] S105. Determine whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, the landmark is established; if the new landmark position has already appeared in the map, the map is updated.

[0092] It can be seen that the method, system and computer-readable storage medium for AGV mapping and positioning proposed by the present invention use the Extended Kalman Filter (EKF) algorithm to implement the mapping and positioning calculations during the AGV's movement. This algorithm directly uses the position information of environmental features as the observation quantity. As long as the observation accuracy is sufficient, accurate observation information can be obtained, and there is no discrete error problem introduced in the grid map. It can reduce the calculation amount while obtaining higher positioning accuracy.

[0093] The specific implementation of the embodiments of the present invention is as follows:

[0094] Implementation process:

[0095] (1) Data acquisition

[0096] For the data acquisition of both the laser sensor and the odometer, a thread acquisition method is adopted, that is, a thread is allocated to the laser sensor and the odometer respectively to read the data. When the algorithm needs to acquire data for calculation, the data is read from the thread.

[0097] (2) Odometer information processing

[0098] Obtain the speed and angular velocity information of the odometer to get the motion information.

[0099] According to the odometer information, the control quantity u of the system can be obtained. The motion equation of the robot can be expressed as

[0100]

[0101] The control error is expressed as δu, and the state error and covariance caused by it can be expressed as

[0102]

[0103] Where

[0104]

[0105] is the Jacobian matrix of the control equation with respect to the control quantity, is the transpose of F u .

[0106] When the information returned by the odometer is the vehicle body speed v and angular velocity ω, the control quantity can be expressed as

[0107]

[0108] At this time, the form of the control equation is

[0109]

[0110] It can be seen that the pose is three-dimensional at this time, while the control equation is two-dimensional. When calculating the state error, usually

[0111] an additional angle is added to θ.

[0112] When what the odometer returns is the pose change within the sampling interval, the control quantity can be expressed as

[0113]

[0114] The meanings of three of the deltas are as follows Figure 3 shown, where δ rot1 is the first rotation angle, δ trans is the translation distance, and δ rot2 is the second rotation angle. Then the control equation can be expressed as:

[0115]

[0116] According to the error δu of the control quantity, substituting it into Equation (5), the control error of the state quantity can be obtained.

[0117] Solution of the control error:

[0118] The control quantity of the motion equation adopts Equation (6), uses the differential drive model to transform the odometer data, and then solves the control error.

[0119] The specific form of Equation 6 is:

[0120]

[0121] where is the reading of the odometer at the previous moment (time t - 1),

[0122] δx, δy, and δθ respectively represent the changes in the odometer speed, and can be expressed using the differential drive model as

[0123]

[0124] According to the error transfer formula, the covariance matrix of the control quantity can be obtained from the covariance matrix of δs r , δs l . When solving, the Jacobian matrices of Equations (8) and (9) are required, which are respectively:

[0125]

[0126] and

[0127]

[0128] where δs = (δs r + δs l ) / 2, When taking the derivative, first take the right wheel and then the left wheel. The covariance matrix of the moving distances of the left and right wheels is

[0129]

[0130] The covariance matrix of the control quantity can be expressed as

[0131]

[0132] Among them are respectively After substituting equation (13) into equation (2), the system covariance can be obtained

[0133] (3) Laser data processing

[0134] The features in the environment are screened out by laser data, and the positions and error information of these features are obtained. The current landmark features of the 2D laser are mainly linear features, and there are also angular features, arc features, etc. Reflective plates can be added as landmark features when necessary.

[0135] Currently, a frame of laser data is directly processed to extract features. Mean filtering is used for noise reduction.

[0136] Euclidean distance is used for region segmentation, and the distance threshold between laser points is adaptive. The threshold formula is

[0137] d point = 2r sin(α / 2) ε (14)

[0138] where r is the distance measured by the laser beam, α is the angular resolution of the laser, and ε is the magnification factor, which is generally greater than 1 and less than 2.

[0139] The sub-regions obtained by segmentation need to be preliminarily calculated to find out the number of lines contained. The Douglas-

[0140] Peucker algorithm is adopted. This algorithm first finds out the scattered points belonging to the same line, and the least squares algorithm can be used to fit the lines for each group of scattered points. There are overlapping situations in the initially obtained lines, and further clustering is required. The line clustering is carried out in two steps.

[0141] (1) Classify according to the direction to find out the groups of line segments that are close to parallel

[0142] (2) Judge whether the line segments in each set can be merged into one line according to the distance between the line segments

[0143] A single set after clustering can fit out a line, which is the final line segment feature.

[0144] The position vector of the midpoint of the obtained line segment feature in the global coordinate system can be obtained, and this vector is the observation value of the line segment.

[0145] The experiment and results of the linear feature are as Figures 4 - 6 .

[0146] The scanning data of the laser head is read and processed. Figure 4 The scattered points scanned with a laser resolution of 0.5 degrees are shown. Figure 5The straight line features extracted by the 0.5-degree laser. Figure 6 The straight line features extracted by the 0.25-degree laser. The goodness of fit of each straight line is marked in the figure, showing the processing results at two scanning resolutions. It can be seen that under the same environment and scanning position, denser scanning can more effectively extract the features of the environment.

[0147] (4) EKF SLAM (Simultaneous Localization and Mapping based on Extended Kalman Filter) algorithm

[0148] This module mainly consists of two parts: the first part is the linearization of the motion equation and the measurement equation, as well as the matrix representation of the state variables; the second part is the update calculation of the state variables and covariance by the Kalman filter algorithm.

[0149]

[0150]

[0151]

[0152]

[0153]

[0154] K t is the Kalman gain, and H t is the parameter of the measurement system, z t is the landmark position observed by the laser sensor at time t, and Q t is the noise covariance.

[0155] (5) New landmark recognition module

[0156] This module will determine whether the currently estimated optimal landmark position matches the existing landmarks. Currently, the Mahalanobis distance is used as the criterion for new landmark recognition. Given a threshold of the Mahalanobis distance, if the Mahalanobis distance between the currently observed line segment and the existing landmark line segment is greater than the given threshold, it can be determined that a new landmark has been discovered.

[0157] As Figure 2 shown, an embodiment of the present invention also proposes a system for AGV mapping and positioning, including:

[0158] A data acquisition module 21 for acquiring environmental information and robot pose information;

[0159] An odometer prediction information module 22 for calculating the expected position of new landmarks based on the robot pose information to obtain the control quantity and control error;

[0160] The laser data processing module 23 is used to screen the features in the environmental information through laser data to obtain the positions of the features;

[0161] The EKF algorithm module 24 is used to obtain the optimal new landmark position based on the current position of the robot according to the desired position and the positions of the features;

[0162] The landmark processing module 25 is used to determine whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, a landmark is established; if the new landmark position has already appeared in the map, the map is updated.

[0163] The present invention also provides an AGV mapping and positioning device, including a memory and a processor; the memory is used to store a computer program; the processor is used to implement the AGV mapping and positioning method as described above when executing the computer program.

[0164] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the AGV mapping and positioning method as described above is implemented.

[0165] The method, system and computer-readable storage medium for AGV mapping and positioning proposed by the present invention use the Extended Kalman Filter (EKF) algorithm to implement mapping and positioning calculations during the AGV's movement. This algorithm directly uses the position information of environmental features as the observation quantity. As long as the observation accuracy is sufficient, accurate observation information can be obtained, and there is no problem of discrete errors introduced in the rasterized map. It can reduce the calculation amount while obtaining a higher positioning accuracy.

[0166] Through the description of the above embodiments, those skilled in the art can clearly understand that the present application can be implemented by means of software plus necessary general hardware, and of course, it can also be implemented by dedicated hardware including application-specific integrated circuits, dedicated CPUs, dedicated memories, dedicated components, etc. Generally, functions completed by computer programs can be easily implemented by corresponding hardware, and the specific hardware structures for implementing the same function can also be various, such as analog circuits, digital circuits or dedicated circuits, etc. However, for the present application, in more cases, software program implementation is a better implementation method. Based on such an understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product is stored in a readable storage medium, such as a floppy disk, USB flash drive, mobile hard disk, ROM, RAM, magnetic disk or optical disc of a computer, etc., and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods of the various embodiments of the present application.

[0167] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions according to the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center in a wired manner (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or wirelessly (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that a computer can store or a data storage device such as a server or data center that includes one or more integrated available media. The available medium can be a magnetic medium (for example, a floppy disk, a hard disk, a magnetic tape), an optical medium (for example, a DVD), or a semiconductor medium (for example, a solid state disk (SSD)), etc. Finally, it should be noted that the above are only the preferred embodiments of the present invention, which are only used to illustrate the technical solutions of the present invention and are not used to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention are included in the protection scope of the present invention.

Claims

1. A method for AGV mapping and positioning, characterized in that, it includes the following steps: S1. Obtain environmental information and robot pose information; S2. According to the robot pose information, calculate the expected position of the new landmark, and obtain the control quantity and control error; S3. Screen the features in the environmental information through laser data to obtain the positions of the features; S4. According to the expected position and the position of the features, obtain the optimal new landmark position based on the current position of the robot; S5. Judge whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, establish the landmark; if the new landmark position has already appeared in the map, update the map; wherein, screening the features in the environmental information through laser data to obtain the positions of the features includes: region segmentation and line clustering; the region segmentation adopts the Euclidean distance, and the distance threshold between laser points is adaptive; the threshold formula is: d point = 2rsin(α / 2)ε where r is the distance measured by the laser beam, α is the angular resolution of the laser, and ε is the magnification factor; Perform preliminary calculations on the sub-regions obtained by region segmentation to find out the number of included lines; for the situation where the obtained lines overlap, perform line clustering, and the line clustering is carried out in two steps: A. Classify according to the direction to find out the groups of line segments that are close to parallel; B. Judge whether the line segments in each set can be merged into one line according to the distance between the line segments; Fit a straight line for a single set after clustering to obtain the final line segment features.

2. The AGV mapping and positioning method according to claim 1, characterized in that, the robot pose information is obtained through an odometer and includes speed and angular velocity information.

3. The AGV mapping and positioning method according to claim 1, characterized in that, screening the features in the environmental information through laser data to obtain the positions of the features includes: Reading data; Noise reduction and region segmentation; Preliminarily obtain line features; Line clustering, merging and screening; Coordinate transformation; Obtain line features.

4. The AGV mapping and positioning method according to claim 1, characterized in that, the specific content of S4 includes: Linearization of the motion equation and the measurement equation, and matrix representation of the state quantity; Update calculation of the state quantity and covariance by the Kalman filter algorithm; K t is the Kalman Gain, H t is the parameter of the measurement system, z t is the landmark position observed by the laser sensor at time t, Q t is the noise covariance.

5. The AGV mapping and positioning method according to claim 1, characterized in that, Use the Mahalanobis distance to judge whether the new landmark position matches the existing landmarks; Preset the threshold of the Mahalanobis distance. If the Mahalanobis distance between the currently observed line segment and the existing landmark line segment is greater than the preset threshold, it is determined that a new landmark is found.

6. An AGV mapping and positioning system, characterized in that, it includes: A data acquisition module for acquiring environmental information and robot pose information; An odometer prediction information module for calculating the expected position of the new landmark according to the robot pose information to obtain the control quantity and control error; A laser data processing module for screening the features in the environmental information through laser data to obtain the positions of the features; An EKF algorithm module; for obtaining the optimal new landmark position based on the current position of the robot according to the expected position and the position of the features; Landmark processing module; used to determine whether the new landmark position matches the existing landmarks. If the new landmark position has not appeared in the map, a landmark is established; if the new landmark position has already appeared in the map, the map is updated; Among them, screening the features in the environmental information through laser data to obtain the positions of the features includes: region segmentation and line clustering; The region segmentation uses the Euclidean distance, and the distance threshold between laser points is adaptive; The threshold formula is: d point = 2rsin(α / 2)ε where r is the distance measured by the laser beam, α is the angular resolution of the laser, and ε is the magnification factor; Perform preliminary calculations on the sub-regions obtained by region segmentation to find out the number of lines contained; for the case where the obtained lines overlap, perform line clustering, and the line clustering is carried out in two steps: The line clustering is carried out in two steps: A Classify according to the direction to find out the groups of line segments that are close to parallel; B Judge whether the line segments in each set can be merged into one line according to the distance between the line segments; A single set after clustering fits out a line to obtain the final line segment feature.

7. An AGV mapping and positioning device, Characterized in that, It includes a memory and a processor; the memory is used to store a computer program; the processor is used to implement the AGV mapping and positioning method according to any one of claims 1-5 when executing the computer program.

8. A computer-readable storage medium, Characterized in that, A computer program is stored on the storage medium, and when the computer program is executed by a processor, the AGV mapping and positioning method according to any one of claims 1-5 is implemented.

Images