AGV position and pose determination method and apparatuses, and storage medium
By combining the matching rates of the first and second types of obstacles, particle filtering technology is used to update the AGV position, which solves the problem of large positioning errors in AGV in dynamic environments, and improves positioning accuracy and stability.
Patent Information
- Application Number
- PCT/CN2024/087374
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2023-05-08
- Filing Date
- 2024-04-12
- Publication Date
- 2024-11-14
AI Technical Summary
The existing AGV positioning technology is difficult to provide stable positioning in dynamic environments, especially in factories and other scenarios, resulting in accumulating positioning errors and reducing accuracy.
Using the AGV posture determination method combining the first and second types of obstacles, the obstacles scanned by the sensor are calculated to map the map under the posture of each particle through particle filtering technology, and the matching of the two types of obstacles is comprehensively considered. to calculate the weight of the particles, thereby updating the estimated position of the AGV.
It improves the positioning stability and accuracy of AGV in dynamic environments, reduces positioning errors, and obtains more stable and robust positioning results.
Smart Images

Figure CN2024087374_14112024_PF_FP_ABST
Abstract
Description
AGV posture determination method and device, and storage medium
[0001] This disclosure claims priority to Chinese patent application No. 202310515390.5, filed on May 8, 2023, the entire contents of which are incorporated herein by reference. Technical Field
[0002] The present disclosure relates to the field of positioning technology, and in particular to an AGV posture determination method and device, and a storage medium. Background Art
[0003] Automated guided vehicles (AGVs) are commonly used in warehousing and logistics environments. Equipped with electromagnetic or optical automatic guidance sensors, they can navigate along a prescribed path, providing safety protection and load transfer capabilities. The most commonly used positioning technology for AGV autonomous navigation is simultaneous localization and mapping (SLAM). This technology requires the presence of a large number of fixed obstacles with distinct texture features to accurately estimate the AGV's position. However, in real-world scenarios (such as factories), most objects are dynamic, and the environment is open and cluttered, making it difficult to provide an AGV with a stable positioning environment.
[0004] Summary of the Invention
[0005] On the one hand, a method for determining an AGV posture is provided. The AGV posture determination method includes: obtaining an estimated posture of the AGV and a posture of at least one particle, where the particle is a random sample used to characterize the AGV; obtaining measurement data from a sensor, where the measurement data includes: a distance and an angle between the sensor and an obstacle scanned by the sensor, where the obstacle includes a first type of obstacle and a second type of obstacle, where the second type of obstacle is an object made of reflective material; calculating coordinates of the obstacle scanned by the sensor mapped to a map based on the measurement data under the posture of the particle, where the coordinates of the obstacle scanned by the sensor mapped to the map include a first coordinate of the first type of obstacle mapped to the map and a second coordinate of the second type of obstacle mapped to the map; matching the first coordinate and the second coordinate with coordinates of obstacles preset on the map, respectively, to obtain a first matching rate and a second matching rate corresponding to the particle, where the first matching rate is a matching rate obtained by matching the first coordinate with the coordinates of the first type of obstacle preset on the map, and the second matching rate is a matching rate obtained by matching the second coordinate with the coordinates of the second type of obstacle preset on the map; calculating a weight of the particle based on the first matching rate and the second matching rate corresponding to the particle; and updating the estimated posture of the AGV based on the weight and posture of the at least one particle.
[0006] On the other hand, an AGV posture determination device is provided. The AGV posture determination device includes: an acquisition unit for acquiring the estimated posture of the AGV and the posture of at least one particle, the posture of the particle is a random sample for characterizing the posture of the AGV; acquiring measurement data of the sensor, the measurement data including: the distance and angle from the sensor to the obstacle scanned by the sensor; the obstacles include first-class obstacles and second-class obstacles, the second-class obstacles are objects with reflective materials; a calculation unit for calculating the coordinates of the obstacles scanned by the sensor mapped to the map based on the measurement data under the posture of the particle, wherein the coordinates of the obstacles scanned by the sensor mapped to the map include the first-class obstacles mapped to the map and the second-class obstacles mapped to the map. The first coordinate and the second coordinate are mapped to the second coordinate of the map; the matching unit matches the first coordinate and the second coordinate with the coordinates of the obstacle preset on the map respectively to obtain a first matching rate and a second matching rate corresponding to the particle, wherein the first matching rate is a matching rate obtained by matching the first coordinate with the coordinates of the first type of obstacle preset on the map, and the second matching rate is a matching rate obtained by matching the second coordinate with the coordinates of the second type of obstacle preset on the map; the calculation unit is further used to calculate the weight of the particle based on the first matching rate and the second matching rate corresponding to the particle; the updating unit is used to update the estimated posture based on the weight and posture of at least one particle.
[0007] In another aspect, an AGV posture determination device is provided. The AGV posture determination device includes: a memory and a processor; the memory and the processor are coupled; the memory is used to store a computer program; and the processor implements the AGV posture determination method described in the above aspect when executing the computer program.
[0008] In another aspect, a computer-readable storage medium is provided, wherein computer program instructions are stored on the computer-readable storage medium, and when the computer program instructions are executed by a processor, the method for determining the AGV posture described in the above aspect is implemented.
[0009] In another aspect, a computer program product is provided, which includes computer program instructions, and when the computer program instructions are executed by a processor, the method for determining the AGV posture described in the above aspect is implemented. BRIEF DESCRIPTION OF THE DRAWINGS
[0010] To more clearly illustrate the technical solutions of the present disclosure, the following briefly introduces the drawings required for use in some embodiments of the present disclosure. Obviously, the drawings described below are only drawings of some embodiments of the present disclosure, and those skilled in the art can also derive other drawings based on these drawings.
[0011] FIG1 is a schematic diagram of an AGV posture determination device involved in an AGV posture determination method according to some embodiments of the present disclosure.
[0012] FIG2 is a flowchart of a method for determining an AGV posture according to some embodiments of the present disclosure.
[0013] FIG3 is a schematic diagram of a driving scenario of an AGV according to some embodiments of the present disclosure.
[0014] FIG4 is a schematic diagram of another driving scenario of an AGV according to some embodiments of the present disclosure.
[0015] FIG5 is a schematic diagram of a scanned map according to some embodiments of the present disclosure.
[0016] FIG6 is another schematic diagram of a scanned map according to some embodiments of the present disclosure.
[0017] FIG7 is a schematic diagram of a grid map according to some embodiments of the present disclosure.
[0018] FIG8 is a flowchart of determining an obstacle according to some embodiments of the present disclosure.
[0019] FIG9 is a schematic structural diagram of an AGV posture determination device according to some embodiments of the present disclosure. DETAILED DESCRIPTION
[0020] To help those skilled in the art better understand the technical solutions of the embodiments of the present disclosure, the technical solutions of the present disclosure will be clearly and completely described below in conjunction with the drawings in the present disclosure. Obviously, the embodiments described are only some of the embodiments of the present disclosure, not all of them. Based on the embodiments of the present disclosure, all other embodiments obtained by those of ordinary skill in the art without making any creative efforts shall fall within the scope of protection of the present disclosure.
[0021] It should be noted that in this disclosure, words such as "exemplarily" or "for example" are used to indicate examples, illustrations, or descriptions. Any embodiment or design described in this disclosure as "exemplary" or "for example" should not be construed as being preferred or advantageous over other embodiments or designs. Rather, the use of words such as "exemplarily" or "for example" is intended to present the relevant concepts in a concrete manner.
[0022] In the following, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the quantity of the technical features being referred to. Therefore, a feature defined with the terms "first," "second," etc., may explicitly or implicitly include one or more of such features.
[0023] In the description of this disclosure, unless otherwise specified, " / " means "or." For example, A / B can mean A or B. "And / or" herein is merely a description of an association relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can mean: only A, only B, and both A and B. Furthermore, "at least one" means one or more, and "a plurality" means two or more.
[0024] In some technologies, a commonly used AGV positioning method is a positioning method based on particle filtering. The principle of this method is to use the postures of multiple particles as candidate postures for the AGV, map the contours of multiple obstacles on the map based on the AGV's lidar data at each particle's posture, match the contour coordinates of the multiple obstacles with the contour coordinates of the obstacles preset in the map, select the contour coordinates with the higher matching value, and perform a comprehensive calculation on the postures of the particles corresponding to the contour coordinates with the higher matching value to obtain the AGV's posture. This method only considers the matching rate between the mapped contour of each particle in the map and the obstacle. When there are fewer obstacles in the scene, positioning drift is prone to occur, resulting in increasing cumulative positioning errors and reduced positioning accuracy.
[0025] Based on this, the present disclosure proposes a method for determining the posture of an AGV. In this method, in addition to the first type of obstacles (e.g., walls) in the scene where the AGV is traveling, a second type of obstacles (e.g., reflectors) are also deployed. At the posture of each particle, the first type of obstacles and the second type of obstacles scanned by the sensor are mapped to the map, and the matching rate between the first type of obstacles and the second type of obstacles after being mapped to the map and the first type of obstacles and the second type of obstacles preset in the map is calculated to calculate the weight of each particle and then determine the posture of the AGV. It can be understood that in this method, when calculating the weight of each particle, the matching rate of the first type of obstacles and the second type of obstacles is comprehensively considered. The second type of obstacles are objects with highly reflective materials, which can greatly enhance the stability and accuracy of positioning in scenes with few first type obstacles. Therefore, the present disclosure combines the two types of obstacles to reduce positioning errors and obtain more stable and robust positioning results.
[0026] The implementation of the embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings.
[0027] 1 , there is shown an AGV posture determination device 100 involved in an AGV posture determination method provided by an embodiment of the present disclosure.
[0028] The AGV posture determination device 100 includes: a processor 101 , a bus 102 , a memory 103 and a communication interface 104 .
[0029] The processor 101 may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with the embodiments of the present disclosure. The processor 101 may be a central processing unit, a general-purpose processor, a digital signal processor, an application-specific integrated circuit, a field programmable gate array, or other programmable logic device, a transistor logic device, a hardware component, or any combination thereof. The processor 101 may implement or execute the various exemplary logic blocks, modules, and circuits described in conjunction with the embodiments of the present disclosure. The processor 101 may also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a DSP and a microprocessor, etc.
[0030] Bus 102 can be an Extended Industry Standard Architecture (EISA) bus, etc. Bus 102 can be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, FIG1 shows only one thick solid line, but this does not mean that there is only one bus or only one type of bus.
[0031] The memory 103 may be a read-only memory (ROM) or other type of static storage device that can store static information and instructions, a random access memory (RAM) or other type of dynamic storage device that can store dynamic information and instructions, an electrically erasable programmable read-only memory (EEPROM), a disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited thereto.
[0032] In some embodiments, the memory 103 may exist independently of the processor 101 and may be connected to the processor 101 via the bus 102 for storing instructions or program codes. When the processor 101 calls and executes the instructions or program codes stored in the memory 103, the AGV posture determination method provided in the embodiments of the present disclosure can be implemented.
[0033] In some embodiments, the memory 103 may also be integrated with the processor 101 .
[0034] The communication interface 104 is used to connect to other devices via a communication network, such as Ethernet, wireless access network, or wireless local area network (WLAN).
[0035] The AGV position determination device 100 can be the AGV itself or another device that communicates with the AGV. For example, the other device that communicates with the AGV can be a terminal such as a mobile phone, tablet computer, desktop computer, laptop computer, notebook computer, netbook computer, etc., or a server. The disclosed embodiments do not impose any particular limitations on the form of the AGV position determination device 100.
[0036] When the AGV posture determination device 100 is the AGV itself, the AGV posture determination device 100 further includes: a sensor 105 .
[0037] The sensor 105 is an electromagnetic or optical sensor used by the AGV to obtain the location of obstacles scanned in the environment and the light intensity information of the scanned obstacles, etc. The sensor 105 can also be used to estimate the AGV posture.
[0038] For example, the sensor 105 may include one or more of a laser radar, a wheel encoder, an inertial measurement unit (IMU), a camera, a magnetic medium, and a global positioning system (GPS).
[0039] Generally, the combined use of the above sensors can improve the accuracy of AGV posture estimation.
[0040] The following describes the AGV posture determination method provided by the embodiment of the present disclosure:
[0041] Refer to Figure 2, which is a flowchart of a method for determining an AGV's posture provided by an embodiment of the present disclosure. This method is applied to the AGV posture determination device shown in Figure 1 . The following description uses an AGV as an example. As shown in Figure 2 , the method may include steps S101-S112.
[0042] S101: AGV performs map initialization.
[0043] AGV map initialization includes S101a-S101c.
[0044] S101a: The AGV scans the AGV driving scene and creates a scan map.
[0045] The AGV driving scene may include indoor or outdoor locations, such as a factory or on the road. The driving scene includes the first type of obstacles and the second type of obstacles.
[0046] The first type of obstacles are generally fixed obstacles in the driving scene, including walls, support columns, and large cabinets.
[0047] The second type of obstacles are objects with reflective materials placed in the AGV driving scene, including but not limited to: reflective boards or reflective columns.
[0048] In some embodiments of the present disclosure, the second type of obstacles are arranged in an area where the space occupied by the first type of obstacles in the AGV driving scene is less than a preset percentage, for example, an area where the space occupied by the first type of obstacles is less than 30%.
[0049] In one example, in an AGV driving scenario as shown in Figure 3, where the space occupied by the first-type obstacle (wall) is less than 30%, such as next to an open route, the second-type obstacle is deployed on one side. Alternatively, in an AGV driving scenario as shown in Figure 4, the second-type obstacle is deployed on both sides of an open route. In Figures 3 and 4, if the second-type obstacle is a reflective column, which is generally around 10 cm in diameter, the deployment interval is generally around 5-10 meters. Figures 3 and 4 use 5 meters as an example for illustration.
[0050] In related methods that use only second-class obstacles to determine the AGV's position, the deployment requirements for second-class obstacles are relatively high. The AGV must be able to identify three or more second-class obstacles at all locations and viewing angles in the operating area, and the distance between each second-class obstacle must be no less than 1 meter. When second-class obstacles are deployed on both sides of a straight path, the positions of the second-class obstacles cannot be symmetrical, the number cannot be the same, and the spacing between them cannot be equal. However, in the embodiments of the present disclosure, due to the combined use of first-class and second-class obstacles, the deployment of second-class obstacles in the embodiments of the present disclosure has no special requirements for the deployment location, number, etc., compared to traditional methods that rely solely on second-class obstacles, and the deployment conditions are more flexible and simple.
[0051] The scanned map is a map generated by the AGV after it scans the AGV driving scene through sensors, such as a SLAM map.
[0052] In one example, the method for AGV to establish a scanning map includes: the AGV moves in the AGV driving scene under manual control or automatically according to a preset route, and at the same time uses Google's open source algorithm Cartographer to scan the first type of obstacles and the second type of obstacles (such as reflective columns) in the driving scene into the map through sensors to generate a scanning map.
[0053] As shown in FIG5 , FIG5 shows a schematic diagram of a scan map. FIG5 is a scan map obtained after scanning the AGV driving scene in FIG3 . The scan map is generally a grayscale image, in which obstacles are black, feasible areas are white, and unknown content (for example, noise) is gray. It should be noted that, in general, obstacles are marked black by default, but during calculation, it is necessary to distinguish between the first type of obstacles and the second type of obstacles (for example, by different colors). To distinguish the second type of obstacles, the second type of obstacles can be marked with a different mark from the first type of obstacles. For example, the second type of obstacles can be marked orange, corresponding to the white circular mark in FIG5 or the oblique line mark in FIG6 . As shown in FIG5 or FIG6 , after the second type of obstacles in the scan map are marked, black represents the first type of obstacles, orange represents the second type of obstacles, gray represents unknown content, and white represents the feasible area.
[0054] S101b: The AGV converts the scanned map into a raster map.
[0055] A raster map is a map created by dividing a scanned map into cells. The x- and y-axis resolutions define the size of a single grid cell. When the x-axis resolution equals the y-axis resolution, each grid cell is a square. Smaller resolutions indicate higher raster map accuracy. Raster maps maintain consistency with scanned maps in content, geometric accuracy, and color.
[0056] FIG7 shows a schematic diagram of the scanned map shown in FIG6 after conversion to a raster map. Different obstacle categories are displayed with different markers on the raster map. For example, black on the raster map indicates the first category of obstacles, while orange (corresponding to the diagonal lines in FIG7 ) indicates the second category of obstacles.
[0057] The grids with the first-class obstacle mark are defined as first-class grids, and the grids with the second-class obstacle mark are defined as second-class grids. For example, the black grids in the grid map are first-class grids, and the orange grids are second-class grids.
[0058] Since raster maps are easy to construct, represent and save, the position of each grid is unique, and it is convenient to plan short paths, the scanned map is converted into a raster map and then processed in the embodiment of the present disclosure, which can improve the calculation speed of AGV posture determination.
[0059] The above-mentioned conversion of the scanned map into the raster map is only a specific implementation method, and the embodiment of the present disclosure does not limit which specific map is used for processing.
[0060] S101c: The AGV calculates and stores the distance between each grid in the grid map and the first type grid closest to the grid, as well as the distance between the grid and the second type grid closest to the grid.
[0061] In some embodiments, a method for calculating the distance between each grid and the nearest grid of the first type, and the distance between each grid and the nearest grid of the second type is proposed below.
[0062] Let the grid map be m, and the coordinates of the i-th grid on the grid map be m i (x i ,y i ) characterization, for example, m i (1,2) indicates that the grid is located in the first column on the x-axis and the second row on the y-axis of the grid map. The distance between each grid and the nearest grid of the first type is calculated using formula (1), and the distance between each grid and the nearest grid of the second type is calculated using formula (2).
[0063] (x jocc ,y jocc ) represents the coordinates of the jth first-category grid, and dcell,occ_max is the maximum distance between the preset grid and the first-category obstacle.
[0064] (x jref ,y jref ) represents the coordinates of the j-th second-category grid, and dcell,ref_max is the maximum distance between the preset grid and the second-category obstacle.
[0065] In this operation, the distance between each grid in the grid map and the nearest first-category obstacle and second-category obstacle is calculated in advance. When matching is performed subsequently, this distance is directly used to increase the matching calculation speed and improve the efficiency of AGV posture determination.
[0066] S102: AGV positioning initialization, presetting at least one particle.
[0067] Positioning initialization means the AGV obtains its initial position. In this operation, you can manually set the initial position of the AGV relative to the map to complete positioning initialization.
[0068] Particles are random samples that represent AGVs. Generally, when AGV selects particles, it selects the poses of multiple particles as candidate poses to improve the accuracy of AGV pose determination.
[0069] Multiple particles are preset, that is, multiple pose points are randomly selected as particles near the initialization position of the AGV, and the pose of each particle represents the candidate pose of the AGV.
[0070] S103: After the AGV travels a certain distance in the above driving scene, an estimated position and posture of the AGV is obtained.
[0071] A distance can be a distance that the AGV travels along a preset path in the above driving scenario, or a distance that the AGV travels freely.
[0072] The estimated pose is generally based on the sensor measurement data on the AGV, such as lidar, wheel encoders and / or IMU measurement data, which estimates the pose of the AGV after traveling a certain distance.
[0073] S104: The AGV determines whether the change between the estimated posture and the AGV posture at the previous moment is greater than a posture threshold.
[0074] When the estimated posture change of the AGV is greater than the posture threshold, execute S105;
[0075] When the estimated posture change of the AGV is less than or equal to the posture threshold, S112 is executed.
[0076] The last moment AGV pose is the pose output by the AGV last time, such as the pose initialized by positioning, or the pose output after the last estimated pose is obtained, or the pose output after the last estimated pose is updated.
[0077] You can set the AGV to output its position every preset time period or every preset distance it travels. For example, the AGV can output its position every 5 seconds or every meter it travels.
[0078] In one example, an AGV outputs its position every 5 seconds, and the position threshold is set at 1 meter of travel distance and a ±10° change in heading angle. If the estimated AGV position changes by more than 1 meter and more than ±10° in heading angle compared to the position output by the AGV 5 seconds ago, the change in the estimated AGV position is considered to be greater than the position threshold.
[0079] In this operation, after the AGV has traveled a certain distance, it determines whether the difference between the estimated pose and the previous pose is greater than a threshold. If so, it is considered that the estimated pose may have a significant deviation and needs to be corrected. The corrected AGV pose is then used as the current AGV pose. If the difference is less than or equal to the threshold, the estimated pose deviation is considered small and the estimated AGV pose can be used directly as the current AGV pose.
[0080] S105: The AGV obtains the position and posture of at least one particle.
[0081] In some embodiments, the AGV performs at least one particle motion update and obtains at least one updated particle motion posture.
[0082] Particle motion update can be understood as updating the particle pose. The updated particle pose is determined based on the estimated pose of the AGV. The particle moves the same distance as the AGV moves, that is, the particle moves as the AGV moves.
[0083] In one example, when the AGV moves from a first position to a second position, particles around the first position also move to the second position. The AGV obtains the positions of multiple particles around the second position.
[0084] Since at least one particle is preset in S102 and its position is a candidate position for the AGV, after the AGV has traveled a certain distance in S103, the position of the at least one particle needs to be updated based on the change in the AGV's position. Specifically, the AGV performs motion updates on the at least one particle and obtains the updated position of the at least one particle.
[0085] S106: The AGV obtains sensor measurement data.
[0086] The sensor measurement data includes the distance and angle between the sensor and the obstacles scanned by the sensor. The obstacles include the first type of obstacles and the second type of obstacles.
[0087] The distance and angle between the sensor and the obstacle scanned by the sensor include the distance and angle between each scanning point of the sensor and the obstacle scanned by the sensor. The scanning point is the point on the obstacle when the sensor scans the obstacle.
[0088] S107: The AGV calculates the coordinates of the obstacles scanned by the sensor and mapped to the map based on the measurement data under the particle's position.
[0089] The coordinates of the obstacles scanned by the sensor mapped to the map include first coordinates of the first type of obstacles mapped to the map, and second coordinates of the second type of obstacles mapped to the map.
[0090] The above-mentioned particle is any one of the at least one particle.
[0091] Calculating coordinates of the obstacle scanned by the sensor mapped onto the map includes: calculating coordinates of the obstacle scanned by each scanning point of the sensor mapped onto the map.
[0092] In some embodiments, let the position of a particle at time t be x t =(xy θ) T , the coordinates of the sensor relative to the AGV are (x k,sens ,y k,sens ) T , using θ k,sens Represents the angle of the sensor relative to the AGV driving direction. Based on the above data, the kth scanning point z of the sensor isk Mapped to the map m under the particle posture, the k-th scanning point z is calculated by formula (3) k Mapped to the coordinates of map m:
[0093] above Includes the distance and angle from the kth scanning point to the obstacle scanned by the sensor.
[0094] Based on the coordinates of the obstacle scanned by each scanning point of the sensor mapped to the map, the coordinates of the obstacle scanned by the sensor mapped to the map can be obtained.
[0095] S108: The AGV classifies obstacles scanned by the sensor into first-category obstacles and second-category obstacles.
[0096] The distinguishing method includes: the AGV determines whether the scanning point falls on the second type of obstacle, thereby distinguishing the obstacles scanned by the sensor.
[0097] In the case where the scanning point meets the preset conditions, it is determined that the sensor scanning point falls on the second type of obstacle, otherwise it falls on the first type of obstacle.
[0098] If the sensor includes a laser radar, the sensor measurement data also includes light intensity at the scanning point. In this case, the preset condition includes at least one of the following:
[0099] The light intensity at the scanning point is greater than the light intensity threshold;
[0100] The distance from the scanning point to the sensor is less than the distance threshold;
[0101] The scanning point is mapped to the grid in the grid map, and the distance between the second type grid closest to the grid mapped to the grid map is less than the grid distance threshold; the second type grid is the grid containing the identification of the second type obstacle.
[0102] Generally, when the above-mentioned preset conditions are met at the same time, the result of distinguishing the first type of obstacles from the second type of obstacles is most accurate.
[0103] Based on the above preset conditions, the AGV scans the kth scanning point z of the sensor in turn. k Perform judgment, as shown in FIG8 , the judgment operation includes S108 a - S108 d.
[0104] S108a: The AGV determines whether the grid map includes the second type of grid.
[0105] If yes, execute S108b;
[0106] If not, it is determined that the k-th scanning point falls on the first type of obstacle, and subsequent S108b-S108d are not executed.
[0107] Since the grid containing the mark of the second type of obstacle is the second type of grid, if the mark of the second type of obstacle is orange, it is verified whether the grid map contains orange grids. If it contains orange grids, it indicates that the grid map includes the second type of grids, and S108b can be continued at this time. Otherwise, it indicates that the grid map does not include the second type of grids, and there are no second type of obstacles deployed in the AGV driving environment. At this time, there is no need to distinguish the obstacles, and the subsequent S108b-S108d are not executed.
[0108] S108b: The AGV determines whether the light intensity at the scanning point is greater than a light intensity threshold.
[0109] Assume the light intensity at the scanning point is The light intensity threshold is I min ,like The obstacle scanned by the scanning point is considered to be a second-category obstacle; otherwise, it is determined that the k-th scanning point falls on a first-category obstacle, and subsequent S108c-S108d are not executed.
[0110] When the sensor is a laser radar, the reflection effect of the laser radar on reflective materials of different materials is different. The light intensity reflected by the laser beam emitted by the laser radar of different brands and models when scanning the scanning point of the same reflective material is different. The sizes are also different, so we can determine whether the scanning point falls on the second type of obstacle by judging the light intensity.
[0111] The embodiment of the present disclosure can effectively distinguish whether the scanning point falls on the second type of obstacle or the first type of obstacle by judging the light intensity of the scanning point.
[0112] S108c: The AGV determines whether the distance between the scanning point and the laser radar is less than the light intensity point distance threshold.
[0113] The light intensity point distance threshold is the maximum effective distance that the lidar can scan to reflective materials.
[0114] Assume the distance between the scanning point and the laser radar is d I , the light intensity point distance threshold is d Imax , if d I <d Imax , then the obstacle scanned by the scanning point is considered to be a second-category obstacle; otherwise, it is determined that the k-th scanning point falls on a first-category obstacle, and the subsequent S108d is not executed.
[0115] The maximum effective distance that different brands and models of laser radar can scan for different reflective materials is generally 6 to 10 meters. If it exceeds this value, it indicates that the distance between the scanning point and the laser radar may be abnormal. ImaxThe value of is generally set based on different lidars.
[0116] S108d: The AGV determines whether the distance between the grid where the scanned point is mapped in the grid map and the nearest second - type grid is less than the grid distance threshold.
[0117] The above - mentioned grid distance threshold is the preset maximum distance from the second - type obstacle.
[0118] Given the coordinates of the scanned point mapped in the grid map, since the distance between each grid and the second - type grid has been calculated in S101c, the distance between the grid where the scanned point is mapped in the grid map and the second - type grid is d. cell,ref , if d cell,ref < dcell,ref_max, it is considered that the obstacle scanned by this scanned point is a second - type obstacle; otherwise, it is determined that the k - th scanned point falls on a first - type obstacle.
[0119] Since the first - type obstacle may include objects with reflective materials (e.g., aluminum plate of metal material), to avoid interference from the scanned points verified in S108b - S108c that are objects with reflective materials included in the first - type obstacle, after the scanned points are mapped on the grid map, this operation further excludes the interference of reflective materials in the first - type obstacle by determining whether the distance from the second - type grid is less than the preset maximum distance from the second - type obstacle.
[0120] In the above S108a - S108d, each parameter can be set in the AGV in advance before S102. In one example, taking the sensor as a SICK lidar, let the light intensity threshold be I min be 240, and the light intensity point distance threshold be d Imax be 8 meters.
[0121] When the scanned point satisfies all conditions after completing the above S108a - S108d verification process, it is considered that the scanned point falls on a second - type obstacle. The present disclosure embodiment does not limit the order of the above verification process S108a - S108d. Generally, after the scanned point does not meet any one item in the above - mentioned order of verification, there is no need to execute the remaining processes, saving computing resources.
[0122] S109: The AGV matches the first coordinate and the second coordinate with the coordinates of the preset obstacles on the map respectively, and obtains the first matching rate and the second matching rate corresponding to the particle.
[0123] The first matching rate is the matching rate obtained by matching the first coordinate with the coordinates of the first - type obstacles preset on the map; the second matching rate is the matching rate obtained by matching the second coordinate with the coordinates of the second - type obstacles preset on the map.
[0124] In some embodiments, when the sensor scanning point falls on the first type of obstacle, the AGV maps each scanning point scanned by the sensor to the first type of obstacle to a grid of the grid map and matches it with the first type of grid preset on the grid map to obtain a matching rate for each scanning point scanned by the sensor to the first type of obstacle.
[0125] The first matching rate is equal to the product of the matching rates of each scanning point scanned by the sensor to the first type of obstacle.
[0126] The following is for the kth scanning point z k The calculation method of the matching rate when falling on the first type of obstacles is explained below:
[0127] Based on the idea of likelihood domain model, the k-th scanning point z is obtained through the Gaussian distribution model and random noise model in formula (4): k Map to raster map matching rate:
[0128] The above formula (4) is a measurement model for the scanning point falling on the first type of obstacle. Based on this model, the matching rate of the scanning point can be obtained, which can also be called the probability that the scanning point is mapped on the first type of obstacle.
[0129] When the sensor scanning point falls on the second type of obstacle, the AGV maps each scanning point scanned by the sensor to the second type of obstacle to the grid of the grid map and matches it with the second type of grid preset on the grid map to obtain the matching rate of each scanning point scanned by the sensor to the second type of obstacle.
[0130] The second matching rate is equal to the product of the matching rates of each scanning point scanned by the sensor to the second type of obstacle.
[0131] The following is for the kth scanning point z k The calculation method of the matching rate when falling on the second type of obstacles is explained below:
[0132] In the case of the second type of obstacle being a reflective column, since the sensor scans the surface of the reflective column, in order to improve the accuracy of the matching rate, it is necessary to scan the z k The distance to the sensor is compensated by the method of measuring the scanning point z k The distance to the sensor plus the radius of the reflector.
[0133] Let scanning point z k Distance to sensor The diameter of the reflective column is d ref , then the scan point z after compensation k The distance to the sensor is
[0134] Based on the idea of likelihood domain model, the k-th scanning point z is obtained through the Gaussian distribution model and random noise model in formula (5): k Map to raster map matching rate:
[0135] The above formula (5) is a measurement model for the scanning point falling on the second type of obstacle. Based on this model, the matching rate of the scanning point can be obtained, which can also be called the probability that the scanning point is mapped on the second type of obstacle.
[0136] S110: The AGV calculates the weight of the particle based on the first matching rate and the second matching rate corresponding to the particle.
[0137] In some embodiments, the first matching rate, the weighting coefficient of the first matching rate, the second matching rate, and the weighting coefficient of the second matching rate are multiplied together to obtain the weight of the particle. The weighting coefficient of the first matching rate is smaller than the weighting coefficient of the second matching rate.
[0138] The weighted coefficient of the first matching rate may be set to a fixed value, such as 1.
[0139] The weighting coefficient of the second matching rate is related to the target distance. The smaller the target distance, the higher the weighting coefficient of the second matching rate. The target distance is the distance between the scan point falling on the second type of obstacle and the target coordinates on the map and the closest second type of obstacle.
[0140] For the kth scanning point z k , when the kth scanning point falls on the first type of obstacle, the kth scanning point z k The confidence level is
[0141] For the kth scanning point z k , when the kth scanning point falls on the second type of obstacle, the weighted coefficient of the kth scanning point is calculated by formula (6)
[0142] The above w ref It is the confidence coefficient set for the second type of obstacles. The setting of this coefficient value is determined by experience and is generally greater than or equal to 1.
[0143] The k-th scanning point z k The matching rate and weighted coefficient of the k-th scanning point z are calculated by formula (7): k Confidence level:
[0144] From the above formula, we can see that the closer the scanning point is to the second type of obstacle, the higher the confidence coefficient and the higher the confidence level. This shows that the closer the scanning point is to the second type of obstacle, the more valuable the location of the scanning point is.
[0145] From the above process, we can see that if the weight of each particle is w, the weight of each particle can be obtained by formula (8):
[0146] It can be seen from the above formula (8) that the weight of each particle is obtained by multiplying the confidence of each scanning point of the sensor under the position of the particle.
[0147] In an example, assuming there are 100 scanning points, 1 to 30 are scanning points that fall on the first type of obstacles, and 31 to 100 are scanning points that fall on the second type of obstacles, then the weight of a particle is:
[0148] It can be understood that the weight of each particle is related to each scanning point. When a weighting coefficient is set for the scanning point falling on the second obstacle to determine the confidence of the scanning point, the weight of the particle can be determined more accurately, providing more valuable particles for the subsequent determination of the AGV posture.
[0149] S111: The AGV updates the estimated pose of the AGV based on the weight and pose of at least one particle.
[0150] In some embodiments, the AGV performs a clustering algorithm on the particles based on the poses of multiple particles, selects a portion of particles with higher weights, takes an average of the poses of these particles, and uses the average to update the estimated pose of the AGV.
[0151] S112: The AGV outputs the AGV posture.
[0152] In this operation, the AGV outputs the pose as the estimated pose in S103 or the updated estimated pose in S111.
[0153] In a method for determining the posture of an AGV proposed in the present disclosure, since the estimated posture of the AGV may drift after it has traveled a certain distance, in order to update the estimated posture and make its output posture more accurate, multiple postures are selected around the estimated posture of the AGV as candidate postures of the AGV, that is, the postures of multiple particles are selected as candidate postures of the AGV. Under the posture of each particle, the matching rate of the first and second obstacles scanned by the sensor after being mapped to the map with the first and second obstacles preset in the map is calculated, and the weight of each particle is determined based on the matching rate, thereby determining the posture of the AGV. It can be understood that the higher the weight of the particle, the closer the posture of the particle is to the true posture of the AGV. In the present disclosure, when calculating the particle weight, the matching rate of the first and second obstacles is comprehensively considered, which can reduce the positioning error and obtain a more stable and robust positioning result.
[0154] It is understood that, in order to achieve the above functions, the AGV posture determination device includes hardware structures and / or software modules corresponding to the execution of each function. Those skilled in the art should readily appreciate that, in conjunction with the algorithmic steps of the various examples described in the embodiments of this disclosure, the present disclosure can be implemented in the form of hardware or a combination of hardware and computer software. Whether a function is executed in hardware or in a manner where computer software drives hardware depends on the specific application and design constraints of the technical solution. Professional and technical personnel may use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this disclosure.
[0155] The embodiment of the present disclosure can divide the functional modules of the AGV posture determination device according to the above-mentioned method embodiment. For example, each functional module can be divided corresponding to each function, or two or more functions can be integrated into one functional module. The above-mentioned integrated module can be implemented in the form of hardware or software. It should be noted that the division of modules in the embodiment of the present disclosure is schematic and is only a logical function division. There may be other division methods in actual implementation. The following is an example of dividing each functional module corresponding to each function.
[0156] FIG9 is a schematic diagram of the structure of an AGV posture determination device provided by an embodiment of the present disclosure. The AGV posture determination device can execute the AGV posture determination method provided by the above method embodiment. As shown in FIG9 , the AGV posture determination device 200 includes:
[0157] The acquisition unit 201 is used to obtain the estimated posture of the AGV and the posture of at least one particle; the particle is a random sample used to characterize the AGV; obtain the measurement data of the sensor; the measurement data includes: the distance and angle from the sensor to the obstacle scanned by the sensor; the obstacles include the first type of obstacles and the second type of obstacles; the second type of obstacles are objects with reflective materials; the calculation unit 202 calculates the coordinates of the obstacles scanned by the sensor mapped to the map based on the measurement data under the posture of the particle; wherein the coordinates of the obstacles scanned by the sensor mapped to the map include the first coordinates of the first type of obstacles mapped to the map, and the second type of obstacles mapped to the map. a second coordinate of the map; a matching unit 203, configured to match the first coordinate and the second coordinate with the coordinates of an obstacle preset on the map, respectively, to obtain a first matching rate and a second matching rate corresponding to the particle; wherein the first matching rate is a matching rate obtained by matching the first coordinate with the coordinates of a first type of obstacle preset on the map; and the second matching rate is a matching rate obtained by matching the second coordinate with the coordinates of a second type of obstacle preset on the map; the calculation unit 202 is further configured to calculate the weight of the particle based on the first matching rate and the second matching rate corresponding to the particle; and the updating unit 204 is configured to update the estimated posture of the AGV based on the weight and posture of at least one particle.
[0158] In some embodiments, the acquisition unit 201 is configured to acquire the estimated posture of the AGV when a change between the estimated posture of the AGV and the posture of the AGV at a previous moment is greater than a posture threshold; and acquire the postures of multiple particles.
[0159] In some embodiments, the calculation unit 202 is configured to multiply the first matching rate, the weighting coefficient of the first matching rate, the second matching rate, and the weighting coefficient of the second matching rate to obtain the weight of the particle; wherein the weighting coefficient of the first matching rate is smaller than the weighting coefficient of the second matching rate.
[0160] In some embodiments, the weighting coefficient of the second matching rate is related to the target distance; the smaller the target distance, the higher the weighting coefficient of the second matching rate; the target distance is the distance between the scanning point falling on the second type of obstacle and the target coordinates on the map and the second type of obstacle closest to the target coordinates.
[0161] In some embodiments, the sensor includes a lidar, and the measurement data also includes the light intensity of the scanning point; the scanning point is the point where the sensor falls on the obstacle when scanning the obstacle; the map includes a grid map; the AGV posture determination device 200 also includes a determination unit 205, which is used to calculate the coordinates of the obstacle scanned by the sensor mapped to the map according to the measurement data under the posture of each particle, and then determine that the scanning point falls on the second type of obstacle when the scanning point meets the preset conditions; wherein the preset conditions include at least one of the following: the light intensity of the scanning point is greater than the light intensity threshold; the distance from the scanning point to the sensor is less than the distance threshold; the scanning point is mapped to the grid in the grid map, and the distance of the second type of grid closest to the grid mapped to the grid map by the scanning point is less than the grid distance threshold; the second type of grid is a grid containing the identification of the second type of obstacle.
[0162] In some embodiments, the AGV posture determination device 200 further includes a scanning unit 206, which scans the AGV's driving scene and establishes a scan map before obtaining the estimated posture of the AGV and the postures of multiple particles; converts the scan map into a raster map; and displays different labels for different types of obstacles in the raster map.
[0163] In some embodiments, the calculation unit 202 is further used to calculate and store the distance between each grid in the grid map and the first type grid closest to each grid, and the distance to the second type grid closest to each grid; wherein the first type grid is the grid containing the identification of the first type obstacle.
[0164] In some embodiments, the second type of obstacles are arranged in an area of the AGV's driving scene where the space occupied by the first type of obstacles is less than a preset percentage.
[0165] In some embodiments, the second type of obstacles include reflective poles or reflective panels.
[0166] Some embodiments of the present disclosure provide a computer-readable storage medium (e.g., a non-transitory computer-readable storage medium), which stores computer program instructions. When the computer program instructions are executed on a computer, the computer executes the AGV posture determination method as described in any of the above embodiments.
[0167] For example, the computer-readable storage media may include, but are not limited to, magnetic storage devices (e.g., hard disks, floppy disks, or magnetic tapes), optical disks (e.g., compact disks (CDs), digital versatile disks (DVDs), etc.), smart cards, and flash memory devices (e.g., erasable programmable read-only memories (EPROMs), cards, sticks, or key drives, etc.). The various computer-readable storage media described in the present disclosure may represent one or more devices and / or other machine-readable storage media for storing information. The term "machine-readable storage medium" may include, but is not limited to, wireless channels and various other media capable of storing, containing, and / or carrying instructions and / or data.
[0168] An embodiment of the present disclosure provides a computer program product comprising instructions. When the computer program product is run on a computer, the computer is enabled to execute the AGV posture determination method described in any one of the above embodiments.
[0169] The above is only a specific embodiment of the present disclosure, but the scope of protection of the present disclosure is not limited thereto. Any changes or replacements within the technical scope disclosed in the present disclosure should be included in the scope of protection of the present disclosure. Therefore, the scope of protection of the present disclosure should be based on the scope of protection of the claims.
Claims
1. A method for determining the position and posture of an unmanned guided vehicle (AGV), wherein: The AGV includes a sensor, and the method includes: Obtaining an estimated pose of the AGV and a pose of at least one particle, wherein the particle is a random sample used to characterize the AGV; Acquire measurement data of the sensor, the measurement data comprising: a distance and an angle from the sensor to an obstacle scanned by the sensor; the obstacle comprising a first type of obstacle and a second type of obstacle, the second type of obstacle being an object having a reflective material; Under the position and posture of the particle, the coordinates of the obstacles scanned by the sensor mapped to the map are calculated according to the measurement data; wherein the coordinates of the obstacles scanned by the sensor mapped to the map include the first coordinates of the first type of obstacles mapped to the map, and the second coordinates of the second type of obstacles mapped to the map; Matching the first coordinate and the second coordinate with the coordinates of obstacles preset on the map respectively, to obtain a first matching rate and a second matching rate corresponding to the particle; wherein the first matching rate is a matching rate obtained by matching the first coordinate with the coordinates of a first type of obstacle preset on the map; and the second matching rate is a matching rate obtained by matching the second coordinate with the coordinates of a second type of obstacle preset on the map; Calculating a weight of the particle based on the first matching rate and the second matching rate corresponding to the particle; and Based on the weight and the pose of the at least one particle, the estimated pose of the AGV is updated.
2. The method according to claim 1, wherein: When a change between the estimated posture of the AGV and the posture of the AGV at the last moment is greater than a posture threshold, obtaining the estimated posture of the AGV and the posture of at least one particle includes: Obtaining an estimated position and posture of the AGV; The position and posture of the at least one particle are obtained.
3. The method according to claim 1, wherein: Calculating the weight of the particle based on the first matching rate and the second matching rate corresponding to the particle includes: The first matching rate, the weighting coefficient of the first matching rate, the second matching rate and the weighting coefficient of the second matching rate are multiplied to obtain the weight of the particle; wherein the weighting coefficient of the first matching rate is smaller than the weighting coefficient of the second matching rate.
4. The method according to claim 3, wherein: The weighting coefficient of the second matching rate is related to the target distance; the smaller the target distance, the higher the weighting coefficient of the second matching rate; the target distance is the distance between the scan point falling on the second type of obstacle and the target coordinates on the map and the second type of obstacle closest to the target coordinates.
5. The method according to claim 4, wherein: The sensor includes a laser radar, the measurement data also includes light intensity at a scanning point; the scanning point is a point on the obstacle where the sensor falls when scanning the obstacle; the map includes a grid map; in the position of the particle, after calculating the coordinates of the obstacle scanned by the sensor mapped to the map according to the measurement data, the method further includes: When the scanning point meets a preset condition, determining that the scanning point falls on the second type of obstacle; wherein the preset condition includes at least one of the following: The light intensity at the scanning point is greater than the light intensity threshold; The distance from the scanning point to the sensor is less than a distance threshold; The scanning point is mapped to a grid in the grid map, and the distance between the second type of grid closest to the grid mapped to the scanning point in the grid map is less than the grid distance threshold; the second type of grid is a grid containing the identification of the second type of obstacle.
6. The method according to claim 5, wherein: Before obtaining the estimated posture of the AGV and the postures of the plurality of particles, the method further includes: Scanning the driving scene of the AGV and establishing a scanning map; The scanned map is converted into the grid map; different types of obstacles in the grid map are displayed with different labels.
7. The method according to claim 5, further comprising: The distance between each grid in the grid map and the first-type grid closest to each grid and the distance between each grid and the second-type grid closest to each grid are calculated and stored; wherein the first-type grid is a grid containing an identification of the first-type obstacle.
8. The method according to claim 1, wherein: The second type of obstacles are arranged in an area of the driving scene of the AGV where the space occupied by the first type of obstacles is lower than a preset percentage.
9. An AGV posture determination device, comprising a memory and a processor; the memory and the processor are coupled; the memory is used to store computer program codes, the computer program codes comprising computer instructions; wherein, When the processor executes the computer instructions, the AGV posture determination device executes the AGV posture determination method according to any one of claims 1-8.
10. A computer-readable storage medium, wherein: The computer-readable storage medium stores computer instructions; wherein, when the computer instructions are executed on an unmanned guided vehicle (AGV) posture determination device, the AGV posture determination device executes the AGV posture determination method according to any one of claims 1-8.
Citation Information
Patent Citations
Mobile robot real-time positioning method based on laser radar and map matching
CN107390681A
Pose repositioning method, device and equipment of laser sweeper and medium
CN112819891A
Robot positioning method and device, computer storage medium and electronic equipment
CN113448326A
Robot positioning method and device based on reflector and robot
CN113625296A
Pose determination method and device of mobile robot
CN114911232A
Cited By
Positioning method based on reflective column, storage medium and electronic equipment
CN119779267A
A positioning method based on a retroreflective column, a storage medium and an electronic device
CN119779267B
Semantic grid map optimization method and system based on multi-task road surface information
CN121033084A
A semantic grid map optimization method and system based on multi-task road surface information
CN121033084B
Method for correcting steering wheel zero position of single-steering-wheel AGV
CN121680224A
