A custom follow-layer hierarchical cost map construction method

Abstract

This invention discloses a method for constructing a hierarchical cost map with a custom follow layer. The method first constructs a static layer and an obstacle layer based on point cloud data collected by LiDAR, and then dilates both the static and obstacle layers to construct an expanded layer. Next, it tracks and predicts the velocity of moving obstacles, and constructs a custom follow layer based on the velocity prediction results. Finally, it updates the main cost map for obstacle avoidance using four cost layers. This invention constructs a hierarchical cost map with a custom follow layer using camera and radar data, enabling the robot to choose the optimal travel path in environments with moving obstacles.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, specifically a method for constructing a layered cost map with a custom follower layer. Background Technology

[0002] When a robot plans its path, it needs a map that clearly shows the mobile robot where obstacles are and where passage is possible. Based on this map, the robot designs a route before moving, and subsequent movements follow this designed route. A commonly used navigation map is the cost grid map. Each grid cell in the cost grid map stores the cost value of the current position. Cost values ​​are mainly categorized as fatal cost, inscribed cost, circumscribed cost, free cost, and unknown cost. A fatal cost indicates that an obstacle exists in the cell; if the robot's center reaches this cell, a collision will inevitably occur. An inscribed cost indicates that there may not be an obstacle in the cell, but a collision will definitely occur if the robot's center reaches this cell. A circumscribed cost indicates that there is no obstacle in the cell, but a collision is possible if the robot reaches this cell. A free cost indicates that there is no obstacle in the cell, and a collision will definitely not occur if the robot reaches this cell. An unknown cost indicates that the location is unexplored.

[0003] Based on the aforementioned meaning of cost value, storing all data in a single cost map constitutes a single-source cost map. In practice, the cost value of a single cost map is directly read. However, this method has several drawbacks. First, as a single-layer map, each grid cell can only store a single cost value. If two different sensors simultaneously transmit completely opposite cost values ​​for the same cell, errors will occur. Second, its update strategy is too simplistic; it updates indiscriminately whenever the grid cost value changes, which could be fatal during robot operation.

[0004] The concept of layered cost maps was proposed by DV Lu in 2014. The update method of the layered cost map framework is not to directly update the cost value of the data source onto the cost map. Instead, while having a main cost map, it sets up layered cost maps, mapping the cost values ​​of different data sources or data semantics onto each layered cost map. It then traverses the ordered list of layers, determines the update area of ​​each layered cost map, and updates the main cost map with the cost values ​​of each layer. Depending on the framework of the layered cost map and different task objectives, custom semantic cost layers can be added to improve the safety of the robot during operation. Summary of the Invention

[0005] The purpose of this invention is to provide a method for constructing a hierarchical cost map with a custom follow layer. Based on the constructed cost map, the robot can select the optimal travel path in an environment with moving obstacles.

[0006] The technical solution to achieve the purpose of this invention is: a method for constructing a layered cost map with a custom following layer, comprising the following steps:

[0007] Step 1: Construct a static layer and an obstacle layer based on the point cloud data collected by the LiDAR, calculate their respective cost values, and update the main cost map with the newly calculated cost values.

[0008] Step 2: Based on the pre-set expansion radius, expand the deadly obstacles in the main cost map to build an expansion layer, ensuring that the robot does not approach the area around the deadly obstacles.

[0009] Step 3: Use a monocular camera to detect and track moving obstacles, project the LiDAR point cloud onto the two-dimensional image data, filter out the point cloud belonging to a specific obstacle based on the detection and tracking results, and finally cluster the filtered point cloud.

[0010] Step 4: The clustered point cloud is sent to the velocity prediction module, which is used to predict the velocity and position of obstacles, calculate the cost based on the velocity prediction results, build a custom follow layer, and finally update it to the main cost map.

[0011] Compared with the prior art, the present invention has significant advantages: the present invention adds a custom follow layer on the basis of the static layer, obstacle layer and dilation layer of the general layered cost map. It can not only calculate the cost of general obstacles in the environment, but also identify fine-grained moving obstacles through the deep network module and obtain the speed prediction value of moving obstacles, so that the robot can better avoid obstacles and select the optimal driving path in subsequent tasks. Attached Figure Description

[0012] Figure 1 A simulation environment for constructing a hierarchical cost map.

[0013] Figure 2 For constructing a static cost layer.

[0014] Figure 3 The obstacle layer that is constructed.

[0015] Figure 4 This is the expanded main cost map.

[0016] Figure 5 This is a fusion diagram of point cloud data and image data.

[0017] Figure 6 This is the main cost map containing follower layers in a simulation environment.

[0018] Figure 7 Overall flowchart of the present invention. Detailed Implementation

[0019] The present invention will now be further described with reference to the accompanying drawings.

[0020] The present invention provides a method for constructing a layered cost map with a custom following layer, the method comprising the following steps:

[0021] Step 1: Construct a static layer and an obstacle layer based on the point cloud data collected by the LiDAR, calculate the cost value of each layer, and update the main cost map with the calculated and updated cost values. This includes the following steps:

[0022] 1) Cartographer receives point cloud data collected by LiDAR, builds a 2D raster map, and publishes the map data in ROS format. The static layer initialization module subscribes to the map data sent by Cartographer and, upon receiving the map data, converts the values ​​of the 2D raster map into the cost values ​​of the static layer. The cost conversion algorithm is as follows:

[0023] ①Pre-set the fatal cost value. If the current grid's cost value is greater than the fatal cost value, then mark the current grid as fatal.

[0024] ② If the cost value of the current grid is -1, then mark the current grid as a free region.

[0025] ③ If the current grid value is less than the fatal value and the current value is greater than or equal to 0, then the current value is proportionally converted and updated to the grid. The conversion algorithm is 255 × (current value / fatal value).

[0026] 2) Determine the update range of the static layer, update the value of each raster within the update range, compare the cost value of the raster calculated by the static layer with the cost value of the main cost map, and take the maximum value to update the cost value of the new main cost map.

[0027] 3) The point cloud data collected by the lidar is sent to the obstacle detection module. The obstacle detection module identifies obstacles by analyzing the vertical and horizontal information of the point cloud and converts the detected obstacles into cost values ​​in the cost map. The cost value calculation method is as follows:

[0028] Determine the distance between the obstacle point cloud and the sensor origin. If the distance between the two does not exceed the farthest distance of the marked obstacle, then set the cost of the raster map corresponding to the point cloud as the fatal cost.

[0029] 4) Determine the update range of the obstacle layer, traverse the grids within the update range, and directly overwrite the cost of the obstacle layer onto the main cost map.

[0030] Step 2: Based on the pre-set expansion radius, expand the deadly obstacles in the main cost map to construct an expansion layer, ensuring that the robot does not approach the area around the deadly obstacles. This includes the following steps:

[0031] 1) Read the pre-set expansion radius and calculate the number of grid cells in the raster map that should be expanded according to the scale ratio of pixels to the real world.

[0032] 2) For grids in the main cost map whose grid cost value is the fatal cost value, take each grid as the origin and the number of expanded grids as the radius, calculate the straight-line distance between the grid to be expanded and the origin. If the distance is 0, set the cost value to the fatal cost value. If the distance is less than the robot's inscribed circle radius, set the cost value to the inscribed obstacle cost value. If the distance is greater than the robot's inscribed circle radius but less than the expansion radius, set the cost value proportional to the distance.

[0033] 3) Obtain the main cost map and the expansion layer, determine the map range that needs to be updated, traverse the grids within the update range, and if the cost value of the main cost map grid is a fatal cost value, skip it; if the cost value of the main cost map grid is not a fatal cost value, then use the cost value of the corresponding grid of the expansion layer to overwrite the original cost value.

[0034] Step 3: Use a monocular camera to detect and track moving obstacles, project the LiDAR point cloud onto the two-dimensional image data, filter out the point cloud belonging to a specific obstacle based on the detection and tracking results, and finally cluster the filtered point cloud.

[0035] 1) Send the image data acquired by the camera to the detection and following module to obtain the confidence score of the detected object, the position of the detection box in the 2D image where the object is located, the following ID of the object, and the category to which the object belongs.

[0036] 2) Traverse the entire point cloud and project it onto the 2D image data according to the camera's transformation matrix. Based on the detection and following results obtained in the previous step, determine whether the point cloud falls within the target bounding box of the detected object after projection. If it does, save the point cloud along with the following ID.

[0037] 3) Cluster all point clouds under each following ID, obtain the point cloud cluster center point, and save the center point as the location of the object.

[0038] Step 4: The clustered point cloud is fed into the velocity prediction module. This module predicts the velocity and position of obstacles, calculates the cost based on the velocity prediction results, constructs a custom follow layer, and finally updates it to the main cost map. Specifically, this includes the following steps:

[0039] 1) Based on the clustering results, the clustered point cloud is sent to the velocity prediction module. Assuming T-1 is the previous time and T is the current time, the point cloud belonging to obstacles is sent to the velocity prediction module, using state X. T-1 To represent the state of the object at time T-1:

[0040]

[0041] Where x T-1 y T-1 This indicates the position of the obstacle at time T-1. This represents the velocity of the obstacle at time T-1; for the first obstacle to appear, its velocity at time T-1 is assigned to 0; at this time, X... T-1 Substitute into the prediction formula to predict the state value of the obstacle at time T. The prediction formula is as follows:

[0042]

[0043] Where A is the system's state transition matrix, and Q is the covariance matrix of the prediction process noise. It is the covariance matrix of the prediction error at time T;

[0044] At time T, the position information of the object observed by the lidar is Z. T Because lidar can only detect the position of an object but not its velocity, Z... T It only contains the object's location information.

[0045]

[0046] Due to the measurement error of the lidar, the observed value Z at time T... T It may be inaccurate and needs to be combined with the predicted value at time T. Update the object's true state X at time T together. T The updated equation is as follows:

[0047]

[0048] in, K is an estimate of the obstacle's motion state at time T. T H is the Kalman gain matrix, H is the system observation matrix, and x is the Kalman gain matrix. TIt is a comprehensive posterior estimate of the obstacle's motion state, P T It is the updated estimate of the prediction error covariance matrix; X T In That is, the best predicted value of the obstacle's velocity at the current moment, × T x in T y T This is the optimal predicted value of the obstacle's position at the current moment.

[0049] 2) After obtaining the velocity information of the clustered point cloud based on the results of the velocity prediction module, the position and velocity information of the objects need to be sent to the follow layer update module via ROS messages for cost value calculation. The definition of ROS messages is shown in the table below:

[0050] Data item format Data item name Meaning of data items string name Object tracking ID geometry_msgs / Point position The position of the object geometry_msgs / Point velocity The speed of an object float64 reliability Confidence level of an object

[0051] The object's position is represented by coordinates x and y, the object's velocity is represented by two values ​​vx and vy, and the object's confidence level is between 0 and 1, with the closer to 1 indicating a higher confidence level.

[0052] 3) Determine the calculation range of the cost map, traverse the entire map grid, and calculate the cost of each grid using a Gaussian function based on the position and speed of the moving obstacles. The specific calculation method is as follows:

[0053] Traverse the grid map and perform the following calculations for each grid cell: Obtain the x and y coordinates of the current grid cell; based on the obstacle location coordinates cx and cy obtained from the velocity prediction module, calculate the distance between the current grid cell and the obstacle grid cell, denoted as dx and dy; and calculate the distance between the two grid cells, denoted as h. The specific formula is as follows:

[0054] dx = x - cx, dy = y - cy

[0055]

[0056] Based on the obstacle's velocity information vx, vy obtained from the velocity prediction module, and the distances dx, dy calculated in the previous step, the angle between two grids and the angle skew in the direction of the detected object's velocity are calculated. The specific formulas are as follows:

[0057] angle = aretan(dy′, dx)

[0058] skew = arctan(vy, ux)

[0059] The specific formulas for calculating the component mx of the spacing between two grid cells in the direction parallel to the object's velocity and the component my in the direction perpendicular to the object's velocity are as follows:

[0060] mx = cos(angle - skew) × h

[0061] my = sin(angle - skew) × h

[0062] Based on the preset peak adjustment amplitude and mx, my calculates the final generation value a of the current raster, where e represents the natural logarithm. The specific formula is as follows:

[0063] α = amplitude × e - (mx) 2 +my 2 ).

[0064] 4) Obtain the cost value of the main cost map. Set the update range of the main cost map to a size known to the following layer. Traverse the grids within the update range. If the calculated cost value of the current grid is greater than the minimum cost threshold for updating, select the maximum value between the currently calculated cost value and the cost value of the main cost map to update the main cost map cost value. If the calculated cost value of the current grid is less than the minimum cost threshold for updating, do not update. See the table below for details:

[0065]

[0066] The present invention will be further described below with reference to the embodiments.

[0067] This embodiment is based on the following hardware platform: CPU configured as Intel Core. TM The system features an i9-12900H processor, 16GB DDR5 4800MHz RAM, a 512GB Samsung PCIe 4.0 storage, an Nvidia 3060 6GB Laptop graphics card, a Hikvision DS-2CS54U0B-S USB webcam, and a Velodyne 16-line LiDAR. The webcam and LiDAR are connected to the AGX, which in turn is connected to the mobile robot's control chassis via USB. The device is powered by a lithium battery carried by the mobile robot itself.

[0068] This embodiment is based on the following software environment: computer operating system: Ubuntu 20.04, ROS Noetic robot operating system, and Anaconda virtual environment with PyTorch 1.7.0 installed.

[0069] This embodiment requires the following data packets: point cloud data packets acquired by the Velodyne 16-line LiDAR and image data packets acquired by the Hikvision camera.

[0070] Combination Figure 7 This invention is a layered cost map with a custom following layer, and the specific implementation steps are as follows:

[0071] Step 1: Set the fatal cost value to 254, load the point cloud data package, and send it to Cartographer for SLAM mapping. The raster values ​​of the SLAM map are converted into static layer cost values ​​using a cost calculation algorithm, and the main cost map is updated using the static layer. Figure 2 This is a static layer that has been constructed.

[0072] Step 2: Load the point cloud data package, send it to the obstacle detection module, construct an obstacle layer, and update the main cost map using the obstacle layer. Figure 3 The obstacle layer that is constructed.

[0073] Step 3: Set the expansion radius to 0.55, expand the main cost map according to the expansion radius, and update the main cost map with the expanded layer. Figure 4 This is the expanded main cost map.

[0074] Step 4: The image data is fed into the detection and following module to obtain the detection and following results. Then, the point cloud is projected onto the image data, and the point cloud is filtered and distances are extracted. Figure 5 The left side shows moving obstacles detected by the camera, and the right side shows point clouds after filtering and distance extraction.

[0075] Step 5: Input the extracted point cloud into the velocity prediction module to obtain the object's position and velocity prediction results. Based on the velocity prediction results and position, construct a custom follow layer and update the main cost map using this custom follow layer. Figure 6 Add a custom follow layer to the main cost map for the simulation environment.

[0076] Step 6: Set the update frequency to 5 seconds. Based on the set update frequency, repeat steps 1 to 5 to update the layered cost map.

Claims

1. A method for constructing a layered cost map with a custom following layer, characterized in that, Includes the following steps: Step 1: Construct a static layer and an obstacle layer based on the point cloud data collected by the LiDAR, calculate their respective cost values, and update the main cost map with the newly calculated cost values. Step 2: Based on the pre-set expansion radius, expand the deadly obstacles in the main cost map to construct an expansion layer; Step 3: Use a monocular camera to detect and track moving obstacles, project the LiDAR point cloud onto the two-dimensional image data, filter out the point cloud belonging to a specific obstacle based on the detection and tracking results, and finally cluster the filtered point cloud. Step 4: The clustered point cloud is sent to the velocity prediction module to predict the velocity and position of obstacles. The cost is calculated based on the velocity prediction results and a custom follow layer is constructed. Finally, it is updated to the main cost map.

2. The method for constructing a layered cost map with a custom following layer according to claim 1, characterized in that, In the fourth step, the point cloud containing the obstacle is sent to the velocity prediction module to obtain the obstacle's velocity and position information. The prediction result needs to be sent to the cost value calculation module via ROS messages for cost value calculation. The corresponding ROS message definitions are as follows: The specific methods for speed prediction are as follows: Assuming T-1 is the previous time and T is the current time, the point cloud containing the obstacle is fed into the velocity prediction module, using state X. T-1 To represent the state of the object at time T-1: Where x T-1 y T-1 This indicates the position of the obstacle at time T-1. This represents the velocity of the obstacle at time T-1; for the first obstacle to appear, its velocity at time T-1 is assigned to 0; at this time, X... T-1 Substitute into the prediction formula to predict the state value of the obstacle at time T. The prediction formula is as follows: Where A is the system's state transition matrix, and Q is the covariance matrix of the prediction process noise. It is the covariance matrix of the prediction error at time T; At time T, the position information of the object observed by the lidar is Z. T Z T Only contains the object's position information Since the observed value at time T may contain errors, it is necessary to combine it with the predicted value at time T. Update the object's true state X at time T together. T The updated equation is as follows: in, K is an estimate of the obstacle's motion state at time T. T H is the Kalman gain matrix, H is the system observation matrix, and X is the Kalman gain matrix. T It is a comprehensive posterior estimate of the obstacle's motion state, P T It is the updated estimate of the prediction error covariance matrix; X T In This is the best predicted value of the obstacle's velocity at the current moment.

3. The method for constructing a layered cost map with a custom following layer according to claim 1, characterized in that, In the fourth step, the method for calculating the cost of the custom follow layer is as follows: 1) Traverse the grid, and perform the following calculations for each grid: Obtain the x and y coordinates of the current grid; based on the obstacle location coordinates cx and cy obtained from the velocity prediction module, calculate the distance between the current grid and the grid where the object is located, denoted as dx and dy; and calculate the distance between the two grids, denoted as h; the specific formula is as follows: dx = x - cx, dy = y - cy 2) Based on the obstacle velocity information vx, vy obtained from the velocity prediction module and the distances dx, dy calculated in the previous step, calculate the angle between the two grids and the angle skew in the direction of the detected object's velocity. The specific formulas are as follows: angle = arctan(dy, dx) shew = arctan(vy, vx) 3) Calculate the component mx of the spacing between two grid cells in the direction parallel to the object's velocity, and the component my in the direction perpendicular to the object's velocity. The specific formulas are as follows: mx = cos(anlge - skew) × h my = sin(angle - skew) × h 4) Calculate the final cost 'a' based on the preset peak adjustment amplitude and mx,my, where e represents the natural logarithm. The specific formula is as follows:

4. The method for constructing a layered cost map with a custom following layer according to claim 1, characterized in that, In the fourth step, after calculating the cost of the custom tracking layer TrackingMap, the main cost map is updated. The specific update strategy is as follows: Update the range to match the maximum range of TrackingMap; Minimum update threshold, the minimum threshold for cost map updates; The update strategy involves iterating through the TrackingMap and determining whether the cost value is greater than the minimum update threshold. If it is less than the update threshold, the grid is skipped. If it is greater than the update threshold, the maximum value between the current cost value and the main cost map is selected as the new cost value of the main cost map.

Images