A costmap-based memory storage obstacle method

Abstract

The present invention relates to a method for storing obstacles in memory based on a cost map, comprising the following steps: 1) during the driving process of a mobile robot, obstacle data detected by a sensor is processed and added to a two-dimensional grid map in an obstacle layer in the cost map; 2) a time map and a frequency map of the same scale as the grid map are respectively constructed to respectively record the update time of the corresponding position of the grid map and the number of times the obstacle is deleted; 3) the update area of ​​the obstacle layer is determined according to the sensor detection area and the time map; 4) the grid map is updated in the update area, and the corresponding cost value is updated according to a set attenuation function; 5) the obstacles stored in memory are expanded based on the cost value in the expansion layer of the cost map. Compared with the prior art, the present invention has the advantages of dynamically deleting observed obstacles to achieve optimal planning, comprehensive consideration of reality, and wide applicability.

Description

Technical Field

[0001] The present invention relates to the field of robot path planning, and in particular to a method for memorizing obstacles based on a cost map. Background Art

[0002] Path planning is an important part of the robot's motion process, which aims to find a collision-free path between the starting point and the end point. In recent years, the navigation algorithms used for path planning have become more and more complex. Some existing algorithms, such as the Dijkstra algorithm and the A* algorithm, have been well applied to autonomous robots and can plan a collision-free path for the robot in a complex environment. Nowadays, the goal of navigation algorithms has changed from the single goal of pursuing a collision-free path to an optimization problem integrating multiple constraints. For example, on the basis of finding a collision-free path, the path planned by the robot is required to take into account the comfort of pedestrians; when planning the path, attention is paid to the comfort of human passengers when the robot vehicle is moving.

[0003] The above improvements to the path planning algorithms are all optimized from the algorithmic perspective and are implemented on the premise that the map obtained is a real map. However, in actual situations, since the robot detects obstacles through sensors and updates the map, if the robot removes the obstacles when it cannot detect them, the obstacles on the map will not be deleted, which will cause inconsistencies with the actual map. Inaccurate maps will cause path planning to be inconsistent with expectations. The following situations may be encountered in actual planning:

[0004] In case 1, the robot finds an obstacle in front of it and marks it during driving. After driving to a place where the obstacle is no longer visible, the obstacle moves or disappears. If path planning is performed at this time, since the obstacle on the map is not modified as the obstacle is removed in reality, the path planned under the map is likely to be inconsistent with the path planned under the actual map, which means that the planned trajectory is not the optimal trajectory.

[0005] Case 2: When the obstacle is on the only plannable trajectory, path planning cannot be performed and the robot cannot reach the specified location.

[0006] These two situations often occur when the robot is in a dynamic and fast-changing environment. This kind of planning will make people feel confused and unnatural, and increase the cost of reaching the target point. Summary of the invention

[0007] The purpose of the present invention is to provide a method for storing obstacles in a memory based on a cost map in order to overcome the defects of the prior art.

[0008] The purpose of the present invention can be achieved by the following technical solutions:

[0009] A cost map-based memory barrier storage method comprises the following steps:

[0010] 1) During the driving process of the mobile robot, the obstacle data detected by the sensor is processed and added to the two-dimensional grid map in the obstacle layer of the cost map;

[0011] 2) Construct a time map and a frequency map of the same scale as the grid map to record the update time of the corresponding position of the grid map and the number of times the obstacles are deleted;

[0012] 3) Determine the update area of ​​the obstacle layer based on the sensor detection area and time map;

[0013] 4) Update the grid map in the update area and update the corresponding cost value according to the set attenuation function;

[0014] 5) In the expansion layer of the cost map, the obstacles stored in memory are expanded based on the cost value.

[0015] In the step 1), the mobile robot uses a laser radar as a sensor to detect obstacles during driving.

[0016] In the step 2), the time map stores the time when obstacles are added to the corresponding grid map through an array, and its initial value is the start time.

[0017] In the step 2), the number map stores the number of times obstacles in the corresponding grid map are deleted through an array, and its initial value is 0. When the mobile robot detects an obstacle for the first time, the number of deletions stored in the number map array is the initial value. When the number of times an obstacle is deleted reaches the set upper limit, it is identified as a fixed obstacle.

[0018] In the step 3), the grids scanned by the mobile robot laser radar and the grids whose time in the time map is not the initial value and whose existence time exceeds the set maintenance time are added to the update area.

[0019] For new obstacles and timed obstacles scanned by the laser radar in the update area, the grid whose existence time exceeds the set maintenance time is searched according to the time map, and the grid whose number of obstacle deletion times does not reach the set upper limit is searched according to the number map, and the cost value of the corresponding grid is updated. When the cost value c of the updated grid ′ When it is zero, the number of times the obstacle at the corresponding position on the map is deleted increases by one.

[0020] For newly detected obstacles, after the first detection, the cost reduction formula of the grid corresponding to the newly detected obstacles is:

[0021]

[0022] Δt=t now -t re -T

[0023] Where c is the initial cost value of the grid, c′ is the cost value of the updated grid, and the cost value is an integer, t now is the current time, t re is the update time of the stored obstacle, t s is the initial value, and T is the set maintenance time.

[0024] When the number of obstacle deletions increases by one, the decreasing speed of the cost value is reduced by the attenuation formula. When the number of deletions reaches the set upper limit, the cost value no longer changes. The attenuation formula is:

[0025]

[0026] Among them, n is the number of times the obstacles saved at the corresponding position are deleted, and N is the set upper limit of the number of deletions.

[0027] For the grid corresponding to the timeout obstacle that exists for longer than the set maintenance time, the descent speed of the cost value is reduced by using the attenuation formula, and the attenuation formula is:

[0028]

[0029] Δt=t now -t re -T.

[0030] The step 5) is specifically as follows:

[0031] The cost map expands the obstacles in the update area. The cost value of the expansion grid of the memory obstacle is different from that of the fixed obstacle. When expanding, the fixed obstacle is first expanded around it, and then expanded together with the memory obstacle. The cost value of the grid around the fixed obstacle is determined by the distance and the cost value of the memory obstacle, so:

[0032] cost′=cost*scost / obstacle

[0033] Among them, cost is the cost value calculated according to the distance, scost is the cost value of the nearest obstacle, obstacle is the cost value of the fixed obstacle, and cost′ is the cost value of the final grid.

[0034] Compared with the prior art, the present invention has the following advantages:

[0035] 1. In an environment with large dynamic changes, such as a busy museum, the present invention can dynamically delete obstacles observed by sensors to prevent the planned path from being inconsistent with the actual optimal plan due to too many dynamic obstacles in the environment.

[0036] Second, the method of removing obstacles adopts a method similar to the human memory curve. The rate of decrease of the cost value decreases with time, and the more observations are made, the slower the rate of decrease is in the same time.

[0037] 3. When the sensor observes the same obstacle a certain number of times, it will be considered as a fixed obstacle, and the cost value will no longer change. This can take into account the situation that there are indeed some obstacles in the environment that will not change their positions.

[0038] Fourth, this method only improves the cost map and is not targeted at specific robots, path planning methods, sensors, etc.

[0039] 5. The maintenance time and the upper limit of the number of deletions in this method can be modified in the configuration file as needed, so that the parameters can be modified according to environmental changes. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 This is the updating process of the overall map of the present invention.

[0041] Figure 2 This is the update process of the memory obstacle layer map.

[0042] Figure 3 The map is when the sensor detects a moving obstacle.

[0043] Figure 4 This is the cost value change curve when an obstacle is observed three times when the upper limit of the number of deletions is 3.

[0044] Figure 5 A map of the attenuation process of moving obstacles.

[0045] Figure 6 This is the map after the moving obstacles are removed from the map. DETAILED DESCRIPTION

[0046] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0047] The present invention provides a memory storage method for obstacles based on a cost map to achieve the storage of obstacles. Similar to the Ebbinghaus memory curve, the cost value of the obstacle is maintained within a certain time range, but becomes smaller over time after exceeding the time range. However, when the same obstacle is observed multiple times, its decrease becomes slower. When the number of observations reaches a certain number, the obstacle is considered to be a fixed obstacle, and its cost value no longer changes with time. The present invention implements the storage process of obstacles as follows:

[0048] (1) During the driving process of the mobile robot, the obstacle data detected by the sensor is processed and added to the two-dimensional grid map.

[0049] The cost map manages obstacles with different semantic information by using different layers, including the static layer that manages the original map, the obstacle layer that manages obstacles detected by sensors such as lidar, and the expansion layer that manages obstacle expansion. The obstacles detected by the sensors during the driving of the mobile robot will be processed in the obstacle layer in the cost map and added to the map of the obstacle layer. When the obstacles are integrated into a map, the expansion layer will inflate the obstacles.

[0050] (2) Use an array to store the time when an obstacle is added to the corresponding grid map; use an array to store the number of times the obstacle is deleted. When the number of deletions reaches an upper limit, it is identified as a fixed obstacle.

[0051] A time map of the same size as the two-dimensional grid map is established to store the update time of the corresponding position of the grid map. The initial value of each grid is the initial time of the program running. When the obstacle layer map is updated, the time of the corresponding position is updated to the current time. Then a frequency map of the same size as the two-dimensional grid map is established to store the number of times the obstacle is deleted at the corresponding position of the two-dimensional grid map. When the obstacle is deleted, the number is increased by one. When the number of deletions reaches the upper limit, it is considered to be a fixed obstacle and its cost value is no longer updated according to time.

[0052] (3) Determine the update area based on the sensor detection area and storage time.

[0053] The cost map is to process maps of different layers into an overall map. First, the area that needs to be updated is determined, and then each layer of obstacles is added according to the rules. When determining the update area of ​​the obstacle layer, the area detected by the current sensor is first included. It is also necessary to find the grid time in the time map that is not the initial value and exists for more than the set maintenance time, and add it to the update area.

[0054] (4) Update the grid map in the update area and update its cost value according to the given decay function.

[0055] When actually updating the cost value, the existence time of the obstacle is determined, and the cost value of the timed-out obstacle is updated according to its expiration time and the number of times it has been deleted.

[0056] (5) Expand the memory storage obstacles.

[0057] The cost map will dilate the obstacles in the update area using the obstacle maps obtained from other layers. The cost value of the memory obstacle layer is different from that of the fixed obstacles, and the cost value of its dilated grid is also different from that of the fixed obstacles. When dilating, the fixed obstacles are first dilated once and then dilated together with the memory obstacles.

[0058] Example

[0059] This embodiment is a method for storing obstacles in a memory based on a cost map. Obstacles detected by sensors are stored in a memory manner, such as Figure 1 As shown, the overall map update process of this method includes the following five steps:

[0060] (1) During the driving process of the mobile robot, the obstacle data detected by the sensor is processed and added to the two-dimensional grid map. This part is completed by the costmap2D package in ROS.

[0061] In this example, a laser radar is used as a sensor to detect obstacles in the driving process of the mobile robot. The obstacle data is processed and added to the two-dimensional grid map through the obstacle layer of the cost map. Figure 3 As shown, a moving obstacle is detected and added to the obstacle layer map, and the cost value of the corresponding position grid of the obstacle layer is recorded as occupancy.

[0062] (2) An array is used to store the time when an obstacle is added to the corresponding grid map, and an array is used to store the number of times the obstacle is deleted. When the number of deletions reaches an upper limit, it is identified as a fixed obstacle.

[0063] In this example, a time map of the same size as the grid map is created, and the initial value is the running time at the beginning of the program. When a moving obstacle is added to the map, the current adding time is put into the corresponding position of the time map, and a number map of the same size as the grid map is created, and the initial value is zero. In this example, this is the first time the obstacle is detected, and the number of deletions is the initial value.

[0064] (3) Determine the update area. When determining the update area in the obstacle layer, not only the area scanned by the laser radar is considered, but also the grids whose time in the time map is not the initial value and whose existence time exceeds the set maintenance time need to be found and added to the update area.

[0065] (4) When updating the grid map in the update area, search for outdated obstacles and update their cost values ​​according to the given function.

[0066] In this example, the specific process of updating the grid in the update area is as follows: Figure 2As shown, since there are new obstacles and timed obstacles scanned by the lidar in the update area, we first look for grids whose existence time exceeds the maintenance time, and then look for grids whose deletion times have not reached the upper limit. The cost value of the grid is updated according to the time and the number of deletions according to the formula. If the updated cost value is zero, the number of deletions is increased by one.

[0067] After the obstacle is observed for the first time, the cost value decreases as follows:

[0068]

[0069] Δt=t now -t re -T

[0070] Where c is the initial cost value of the grid, c′ is the cost value of the updated grid, and the cost value is an integer, t now is the current time, t re is the time to store the obstacle, t s is the initial value, and T is the set obstacle maintenance time.

[0071] When the cost value c′ of the updated grid becomes zero, the value of the corresponding position in the frequency map is increased by one, and the cost value decreases more slowly in the future. The greater the number of deletions, the slower the decrease. When the number exceeds the set upper limit, the cost value no longer changes. The attenuation formula is as follows:

[0072]

[0073] Where n is the number of obstacle deletions saved at the corresponding position, and N is the upper limit of the set number of deletions.

[0074] According to the above analysis, when determining the update area in the obstacle layer, not only the area detected by the current laser radar is considered, but also the time map is needed to determine whether there is an obstacle that has existed for longer than the set maintenance time. If it exceeds the maintenance time, check the number of times the current obstacle has been deleted and modify its cost according to the following formula:

[0075]

[0076] Δt=t now -t re -T

[0077] In this example, N is set to 3. Figure 4 This is the cost effect of the obstacle after three observations and deletions.

[0078] (5) Expand the memory storage obstacles.

[0079] When the obstacles in the update area are expanded, the possible modified area includes the circle of expansion radius outside the update area. The expansion method of the original expansion layer is to find obstacles in the update area and the part that expands outward, that is, the grid with the cost value of the fixed obstacle (255), and then expand from the grids from near to far from the obstacle. The visited grids are marked, and the cost value of the grid is determined by the distance to the nearest obstacle.

[0080] However, the cost value of the memory obstacle is not necessarily 255. According to the original method, the memory obstacle will not be expanded. Therefore, the expansion method needs to be changed: since there are only obstacles in the update area and the obstacles have not been expanded, all the cost values ​​that are not the free area (0) are obstacles. Since the cost value of the grid with the same expansion distance as the fixed obstacle will be greater than that of the memory obstacle, it is unreasonable to find the nearest obstacle; if the obstacle is allowed to expand arbitrarily, the method of selecting a higher cost value when a conflict occurs can obtain the most accurate cost value, but it is found through experiments that it will take a lot of time and cannot achieve the required map update frequency; therefore, in this example, the importance of the memory obstacle is made equal to the grid at a certain distance from the fixed obstacle, that is, its expansion starts when the fixed obstacle expands to a certain distance, and the cost value of the surrounding grids is determined by the distance and the cost value of the memory obstacle, and the formula is as follows:

[0081] cost′=cost*scost / obstacle

[0082] Where cost is the cost value calculated according to the distance in the original method, scost is the cost value of the nearest obstacle, obstacle is the cost value of the fixed obstacle, and cost′ is the cost value of the final grid.

[0083] In this example, Figure 3 The map is expanded after obstacles are detected and added to the map; Figure 5 The map after the obstacle is expanded when the mobile robot is judged as a timed-out obstacle after leaving it and the cost value gradually decreases; Figure 6 This is the map after deleting the obstacle.

Claims

1. A method for storing obstacles in memory based on a cost map, characterized in that: The following steps are involved: 1) During the driving process of the mobile robot, the obstacle data detected by the sensor is processed and added to the two-dimensional grid map in the obstacle layer of the cost map; 2) Construct a time map and a frequency map of the same scale as the grid map to record the update time of the corresponding position of the grid map and the number of times the obstacles are deleted; 3) Determine the update area of ​​the obstacle layer based on the sensor detection area and time map; 4) Update the grid map in the update area and update the corresponding cost value according to the set attenuation function; 5) Inflate the memory-stored obstacles based on the cost value in the expansion layer of the cost map; In the step 2), the number map stores the number of times obstacles in the corresponding grid map are deleted through an array, and its initial value is 0. When the mobile robot detects an obstacle for the first time, the number of deletions stored in the number map array is the initial value. When the number of times an obstacle is deleted reaches a set upper limit, it is identified as a fixed obstacle. In the step 3), the grids scanned by the mobile robot laser radar and the grids whose time in the time map is not the initial value and whose existence time exceeds the set maintenance time are added to the update area; For new obstacles and timed obstacles scanned by the laser radar in the update area, search for grids whose existence time exceeds the set maintenance time according to the time map, and search for grids whose number of obstacle deletions does not reach the set upper limit according to the number map, and update the cost value of the corresponding grid. When the cost value c′ of the updated grid is zero, the number of times the obstacle is deleted at the corresponding position in the number map is increased by one; For the newly detected obstacles, after the first detection, the attenuation function of the grid corresponding to the newly detected obstacles is: Δt=t now -t re -T Where c is the initial cost value of the grid, c′ is the cost value of the updated grid, and the cost value is an integer, t now is the current time, t re is the update time of the stored obstacle, t s is the initial value, T is the set maintenance time; When the number of obstacle deletions increases by one, the decreasing speed of the cost value is reduced by the attenuation formula. When the number of deletions reaches the set upper limit, the cost value no longer changes. The attenuation function is: Where n is the number of times the obstacles saved at the corresponding position are deleted, and N is the upper limit of the number of deletions set; For the grid corresponding to the timeout obstacle that exists for longer than the set maintenance time, the decay formula is used to reduce the descent speed of the cost value. The decay function is: Δt=t now -t re -t。 2. A method for storing obstacles in memory based on a cost map according to claim 1, characterized in that: In the step 1), the mobile robot uses a laser radar as a sensor to detect obstacles during driving.

3. The method for storing obstacles based on a cost map according to claim 1, characterized in that: In the step 2), the time map stores the time when obstacles are added to the corresponding grid map through an array, and its initial value is the start time.

4. The method for storing obstacles based on a cost map according to claim 1, characterized in that: The step 5) is specifically as follows: The cost map expands the obstacles in the update area. The cost value of the expansion grid of the memory obstacle is different from that of the fixed obstacle. When expanding, the fixed obstacle is first expanded around it, and then expanded together with the memory obstacle. The cost value of the grid around the fixed obstacle is determined by the distance and the cost value of the memory obstacle, so: cost′=cost*scost / obstacle Among them, cost is the cost value calculated according to the distance, scost is the cost value of the nearest obstacle, obstacle is the cost value of the fixed obstacle, and cost′ is the cost value of the final grid.

Images