A full coverage navigation method based on nav2 behavior tree logic

Abstract

The present application relates to a full coverage navigation method in navigation technology, and particularly discloses a full coverage navigation method based on a nav2 behavior tree logic, which comprises the following steps: S1, a navigation logic behavior tree based on a nav2 behavior tree is pre-constructed; S2, under the logic of the navigation logic behavior tree, a system navigation planning module receives regional information of a region in need of planning, and then performs regional division to obtain regional division information; S3, under the logic of the navigation logic behavior tree, the system navigation planning module performs regional cost map confirmation and starting point selection on the regional division information; and S4, under the logic of the navigation logic behavior tree, after the regional cost map confirmation and starting point selection in S3 are completed, the system navigation planning module performs adaptive planning on a full coverage navigation path of the region in need of planning. Through the method, real-time calculation and deployment of a navigation path and adaptive planning of a path can be realized.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and in particular to a full-coverage navigation method. 。 Background Technology

[0002] Typically, navigation robots are configured with navigation methods based on their task requirements. One common scenario involves covering a designated area. Existing coverage navigation methods often suffer from poor adaptability to different scenarios, unstable navigation dead zones, cumbersome navigation processes, and high performance demands on the robot. Furthermore, existing coverage navigation methods typically only use one type of navigation path per navigation cycle, such as a zigzag or swivel pattern. This leads to poor adaptability to changing or user-planned environments, potentially resulting in excessively large navigation dead zones. Additionally, most existing coverage navigation methods only support one-time navigation path deployment; adding new areas requires redeployment, making real-time calculation and deployment of navigation paths impossible. Therefore, existing coverage navigation technology for navigation robots needs improvement to better meet the needs of convenient navigation tasks. Summary of the Invention

[0003] The purpose of this invention is to provide a full-coverage navigation method based on NAV2 behavior tree logic that enables real-time calculation and deployment of navigation paths and adaptive path planning.

[0004] To achieve the above objectives, the technical solution of the present invention is: a full-coverage navigation method based on NAV2 behavior tree logic, the method steps of which are as follows:

[0005] S1. Pre-build a navigation logic behavior tree based on the nav2 behavior tree;

[0006] S2. Under the logic of the navigation logic behavior tree, the system navigation planning module receives the area information of the area that needs to be planned and then divides the planning area to obtain the area division information.

[0007] S3. Under the logic of the navigation logic behavior tree, the system navigation planning module confirms the regional cost map and selects the starting point based on the regional division information;

[0008] S4. Under the logic of the navigation logic behavior tree, after completing the regional cost map confirmation and starting point selection in S3, the system navigation planning module adaptively plans a full-coverage navigation path for the area that needs to be planned.

[0009] The navigation logic behavior tree includes nodes named Area_Des_Fallback, Area_Des, and Wait, respectively. The Area_Des_Fallback node is a sequential node whose function is to request the Area_Des node to update the area. The area to be planned includes the navigation planning area customized by the user through a remote device. In step S2, if the Area_Des_Fallback node requests the Area_Des node to update the area, and the Area_Des node returns a failure status, then the Wait node waits for the server to send area information. After receiving the area information, the system navigation planning module adds the area information to the rasterized raster map currently used by the system navigation planning module through the Area_Des_Fallback node, thereby obtaining the area division information.

[0010] Within the Area_Des_Fallback node, a costmap object library is constructed using the Costmap_2d constructor. Assuming the Area_Des_Fallback node receives the vertex coordinates of the polygonal region from the region information, the Area_Des_Fallback node will use the PolygonOutlineCells function in the costmap object library to extract the polygonal region and its boundary from the region information based on these coordinates. Then, the obtained polygonal region is converted from the region in the world coordinate system to the region in the current raster coordinate system. The boundary of the polygonal region occupied by the converted current raster coordinates will be given the cost value of a fatal obstacle by the cost allocation function.

[0011] The navigation logic behavior tree also includes nodes named Prepare_Plan, Update_Costmap, Start_Point, Des, Presuppose, and Ergodic. Prepare_Plan is a condition node whose function is to first determine whether the Update_Costmap node has updated the map. In step S3, after the Update_Costmap node returns a success status after determining whether it has updated the map, the Start_Point node is executed to confirm the regional cost map and select the starting point.

[0012] The Update_Costmap node first calls the getcostmap function of the costmap object library to obtain the static cost map in the system navigation planning module. Subsequent updates only update the region division information. During the update, the existing grid map needs to be re-rasterized. The number of grids for re-rasterization is set according to the robot diameter. This is how the region cost map is confirmed.

[0013] The Prepare_Plan node updates the region division information and simultaneously enters the Start_Point node. This node is a sequential node. First, it enters the Presuppose node, which is decorated by the Des node with the inversion function. This node obtains the top-left, top-right, bottom-left, bottom-right, and center of the polygon region as preset initial positions based on the vertex coordinates of the polygon region and stores them in the starting point candidate list. The top-left, top-right, bottom-left, and bottom-right corners of the preset initial positions are determined by moving the coordinates of each corner point one robot diameter position towards the center. It calculates whether each preset initial position is inside the polygon region and whether it is on a runnable grid. If each preset initial position is not inside the polygon region or is on a non-runnable grid, the preset initial position is judged as a non-runnable starting point and is deleted from the starting point candidate list. If all preset initial positions are non-runnable starting points, it enters the Ergodic node to traverse the grid map to find a new starting point that is not covered by obstacle grids and is inside the polygon region and can be started, and stores it in the starting point candidate list. This is how the starting point selection is performed.

[0014] The method for calculating whether each preset initial position is inside the polygonal region is to draw a ray from the current point and determine whether the number of edges of the polygonal region it passes through is odd. If it is odd, then it is not inside the polygonal region. The method for determining whether it is in a runnable grid is implemented using the getcost function of costmap. If the obtained cost value indicates that it is near or on an obstacle, then the preset initial position is an unrunnable starting point.

[0015] The navigation logic behavior tree also includes nodes named Plan and Plan_Des respectively. The Prepare_Plan node is a child node of Plan, and Plan_Des also includes multiple child nodes. The Plan node is a sequential node. When the child node Prepare_Plan returns a success status, it means that step S3 has completed the confirmation of the regional cost map and the selection of the starting point. Then, the Plan_Des node is entered to obtain the final decision of the full coverage planning path calculated by this node.

[0016] The full-coverage planning path is a full-coverage navigation path calculated by multiple child nodes of the Plan_Des node, starting from the selected starting point, based on the grid method. Specifically, according to the updated regional cost map confirmed in step S3, the starting point selected in step S3 is used as the starting point, and the cost re-grid map after re-griding is traversed in the positive x-axis direction of the map coordinate system. When the traversed re-grid contains a grid with a cost value of fatal obstacle, the re-grid is marked as a fatal obstacle, and the traversal direction is adjusted. During the traversal, for each re-grid, the center grid coordinates of the re-grid are converted to map coordinates using the maptoworld function in the costmap object library and stored in a point set P, which represents the execution... The robot navigation process involves traversing a set of points. The calculation method for the pre-selected path varies depending on the type of the starting point in the candidate list for step S3. If the selected starting point is one of the four corner points (top left, top right, bottom left, and bottom right), the path is rotated 180 degrees to form a bow-shaped path and included in point set P when adjusting the traversal direction. If the selected starting point is the center, the path is rotated 90 degrees to form a U-shaped path and included in point set P. After obtaining multiple point sets P from different starting points, the Plan_Des node compares the number of target points in each point set P with the number of times the traversal direction is adjusted when traversing the multi-grid map. The point set P containing the most target points and having the fewest direction adjustments is selected as the final full-coverage planning path.

[0017] This full-coverage navigation method replaces the straight-line path planning method of the right-hand tree in the Nav2 Follow_Path native behavior tree. The robot's controller accesses the motion portion of the left-hand tree in the Follow_Path native behavior tree. This motion package includes the control of motion frequency and the publication and updating of motion target points.

[0018] By adopting the above technical solution, the beneficial effects of the present invention are as follows: The above-mentioned full-coverage navigation method is a full-coverage navigation method based on NAV2 behavior tree logic. By utilizing the process control mechanism introduced by NAV2 behavior tree, it realizes modular control of full-coverage planning and navigation through behavior tree to optimize the full-coverage navigation process. This makes the process control more flexible, allows for easier access to the mobile control module, and enables real-time calculation and deployment of navigation paths based on adaptive path planning with grid cost and NAV2 behavior tree-based modes. At the same time, adaptive navigation path selection enables automatic selection of suitable global navigation paths according to different usage scenarios, reducing navigation dead zones and solving the problem of unstable urban area size, thereby achieving the above-mentioned objectives of the present invention. Attached Figure Description

[0019] Figure 1 This invention relates to a full-coverage navigation method based on nav2 behavior tree logic, which displays a behavior tree construction graph in Groot.

[0020] Figure 2 This invention relates to a flowchart of a full-coverage navigation method based on NAV2 behavior tree logic. Detailed Implementation

[0021] To further explain the technical solution of the present invention, the present invention will be described in detail below through specific embodiments.

[0022] This embodiment discloses a full-coverage navigation method based on NAV2 behavior tree logic, which replaces the right-side tree straight-line path planning method in the native Follow_Path behavior tree of NAV2. The method is described in detail below. Figure 1 The behavior tree construction graph shown in Groot has a motion component on the left in the Follow_Path native behavior tree of nav2. This motion component includes the control of motion frequency and the publication and updating of motion target points. The robot's controller connects to this motion component and performs motion control. The full-coverage navigation method disclosed in this embodiment is described in detail below.

[0023] A comprehensive navigation method based on NAV2 behavior tree logic, the method flow is as follows: Figure 2 As shown, the method steps are as follows:

[0024] Step S1: Pre-build the navigation logic behavior tree based on the nav2 behavior tree.

[0025] The navigation logic behavior tree includes nodes that are custom-named Area_Des_Fallback, Area_Des, and Wait, respectively; it also includes nodes that are custom-named Prepare_Plan, Update_Costmap, Start_Point, Des, Presuppose, and Ergodic, respectively; and it also includes nodes that are custom-named Plan and Plan_Des, where the Prepare_Plan node is a child node of Plan, and Plan_Des also includes multiple child nodes, including three child nodes that are custom-named Other, Center, and Corner, as shown in the figure.

[0026] Step S2: Under the logic of the navigation logic behavior tree, the system navigation planning module receives the area information of the area that needs to be planned and then divides the planning area to obtain the area division information. The area that needs to be planned includes the navigation planning area that is customized by the user through remote devices (such as host computer, mobile terminal device, etc.).

[0027] The Area_Des_Fallback node is a sequential node whose function is to request the Area_Des node to update the area. In this step, if the Area_Des_Fallback node requests the Area_Des node to update the area, and the Area_Des node returns a failure status (here, the failure status is defined as FAILURE in the existing system), then it enters the Wait node to wait for the server to send the area information. After receiving the area information, the system navigation planning module adds the area information to the rasterized raster map currently used by the system navigation planning module through the Area_Des_Fallback node, thereby obtaining the area division information. Specifically, within the Area_Des_Fallback node, a costmap object library is constructed using the Costmap_2d constructor. Assuming the Area_Des_Fallback node receives the vertex coordinates of the polygonal region from the area information, the Area_Des_Fallback node will use the PolygonOutlineCells function in the costmap object library to extract the polygonal region and its boundary from the area information based on these coordinates. Then, the obtained polygonal region is converted from the region in the world coordinate system to the region in the current grid coordinate system. The boundary of the polygonal region occupied by the converted region in the current grid coordinate system will be assigned the cost value of fatal obstacles by the cost assignment function, so as to divide the area to be planned in the subsequent navigation planning module.

[0028] Step S3: Under the logic of the navigation logic behavior tree, the system navigation planning module confirms the regional cost map and selects the starting point based on the regional division information.

[0029] The Prepare_Plan node is a conditional node whose function is to first determine whether the Update_Costmap node has updated the map. In this step, after the Update_Costmap node returns a success status (the success status is defined as SUCCESS in the current system), the Start_Point node is executed to confirm the regional cost map and select the starting point. Specifically, the Update_Costmap node first calls the getcostmap function of the costmap object library to obtain the static cost map in the system navigation planning module, i.e., the global_costmap in the current system. This acquisition of the global_costmap is only required during the initial map update. Subsequent updates only update the regional division information to reduce computational pressure. During the update, the existing grid map needs to be re-rasterized, that is, multiple grids are merged into a larger grid to adapt to the navigation function. The number of grids for this re-rasterization is set according to the robot diameter. This is how the regional cost map is confirmed.

[0030] The Prepare_Plan node updates the region partitioning information and simultaneously enters the Start_Point node. This node is a sequential node, first entering the Presuppose node decorated by the Des node (which uses the inversion function). This node obtains the top-left, top-right, bottom-left, bottom-right, and center positions of the polygon region based on the vertex coordinates of the polygon region and stores them in a candidate list of starting points. The top-left, top-right, bottom-left, and bottom-right corners are the four corner points of the polygon region. The positions of these four corner points can be determined directly by judging the extreme values ​​of the grid coordinates within the polygon on the X and Y axes. The center is the center point of the polygon region, which can be determined by directly taking the average of all fixed-point map coordinates. Note that... The determination of the four corner points (top left, top right, bottom left, and bottom right) in the preset initial position requires moving the coordinates of each corner point one robot diameter position towards the center to ensure that the robot will not get stuck in fatal obstacles when it goes to the starting point. In addition, it is also necessary to calculate whether each preset initial position is inside the polygonal area and whether it is on a runnable grid. If each preset initial position is not inside the polygonal area or is on a non-runnable grid, the preset initial position is judged as a non-runnable starting point and is deleted from the starting point candidate list. If all preset initial positions are non-runnable starting points, the Ergodic node will be entered to traverse the grid map to find a new starting point that is not covered by obstacle grids and is inside the polygonal area and can be started, and store it in the starting point candidate list. This is how the starting point selection is performed. The above calculation of whether each preset initial position is inside the polygonal region is implemented by the following method in this embodiment: drawing out the ray from the current point to determine whether the number of edges of the polygonal region it passes through is odd. If it is odd, then it is not inside the polygonal region. It is then determined whether it is in a runnable grid and implemented using the getcost function of costmap. If the obtained cost value indicates that it is near or on an obstacle, then the starting point of the preset initial position is an unrunnable starting point.

[0031] Step S4: Under the logic of the navigation logic behavior tree, after completing the regional cost map confirmation and starting point selection in S3, the system navigation planning module adaptively plans a full-coverage navigation path for the area that needs to be planned.

[0032] The Plan node is a sequential node. When the child node Prepare_Plan returns a success status, it means that step S3 has completed the confirmation of the regional cost map and the selection of the starting point. Then, it enters the Plan_Des node to obtain the final decision of the full coverage planning path calculated by this node.

[0033] The full-coverage planning path is a full-coverage navigation path calculated by the three child nodes of the Plan_Des node, starting from the selected starting point, based on the grid method. Specifically, according to the updated regional cost map confirmed in step S3, the starting point selected in step S3 is used as the starting point, and the cost re-grid map after re-griding is traversed in the positive x-axis direction of the map coordinate system. When the traversed re-grid contains a grid with a cost value of fatal obstacle, the re-grid is marked as a fatal obstacle, and the traversal direction is adjusted. During the traversal, for each re-grid, the center grid coordinates of the re-grid are converted to map coordinates using the maptoworld function in the costmap object library and stored in a point set P, which represents the execution... The robot navigation process involves traversing a set of points. The calculation method for the pre-selected path varies depending on the type of the starting point in the candidate list for starting point in step S3. If the selected starting point is one of the four corner points (top left, top right, bottom left, and bottom right), the path is rotated 180 degrees to form a bow-shaped path and included in point set P when adjusting the traversal direction. If the selected starting point is the center point, the path is rotated 90 degrees to form a square-shaped path and included in point set P. After obtaining multiple point sets P from different starting points, the Plan_Des node compares the number of target points in each point set P with the number of times the traversal direction is adjusted when traversing the multi-grid map. The point set P containing the most target points and having the fewest direction adjustments is selected as the final full-coverage planning path.

[0034] The terms "costmap object library," "getcostmap function," "global_costmap," "SUCCESS," and "FAILURE" mentioned above are names defined in existing systems. The scope of protection claimed by this invention is limited to the object itself, not to the name. If the system defines the object with other names, it does not substantially depart from the scope of protection claimed by this invention.

[0035] The above embodiments and figures are not intended to limit the product form and style of the present invention. Any appropriate changes or modifications made by those skilled in the art should be considered as not departing from the patent scope of the present invention.

Claims

1. A full-coverage navigation method based on NAV2 behavior tree logic, characterized in that, The steps are as follows: S1. Pre-build a navigation logic behavior tree based on the nav2 behavior tree; The navigation logic behavior tree includes nodes that are custom-named Area_Des_Fallback, Area_Des, and Wait, respectively. The Area_Des_Fallback node is a sequential node whose function is to request the Area_Des node to update the area. The navigation logic behavior tree also includes nodes that are custom-named Prepare_Plan, Update_Costmap, Start_Point, Des, Presuppose, and Ergodic, respectively. Prepare_Plan is a condition node whose function is to first determine whether the Update_Costmap node has updated the map. The navigation logic behavior tree also includes nodes that are custom named Plan and Plan_Des respectively. The Prepare_Plan node is a child node of Plan, and Plan_Des also includes multiple child nodes. The Plan node is a sequential node. S2. Under the logic of the navigation logic behavior tree, the system navigation planning module receives the area information of the area that needs to be planned and then divides the planning area to obtain the area division information. The area requiring planning includes the area where users can customize navigation planning via remote devices; In this step, if the Area_Des_Fallback node requests the Area_Des node to update the area, and the Area_Des node returns a failure status, then the Wait node will wait for the server to send the area information. After receiving the area information, the system navigation planning module will add the area information to the raster map currently used by the system navigation planning module through the Area_Des_Fallback node, thereby obtaining the area division information. S3. Under the logic of the navigation logic behavior tree, the system navigation planning module confirms the regional cost map and selects the starting point based on the regional division information; In this step, after first determining whether the Update_Costmap node has updated the map and returned a successful status, the Start_Point node is executed to confirm the regional cost map and select the starting point; When the child node Prepare_Plan returns a success status, it means that this step has completed the confirmation of the regional cost map and the selection of the starting point. Then, it enters the Plan_Des node to obtain the final decision of the full coverage planning path calculated by this node. S4. Under the logic of the navigation logic behavior tree, after completing the regional cost map confirmation and starting point selection in S3, the system navigation planning module adaptively plans a full-coverage navigation path for the area that needs to be planned. This full-coverage navigation method replaces the straight-line path planning method of the right tree in the Nav2 Follow_Path native behavior tree, and the robot's controller accesses the motion part of the left tree in the Follow_Path native behavior tree.

2. The full-coverage navigation method based on NAV2 behavior tree logic as described in claim 1, characterized in that, The Area_Des_Fallback node constructs a costmap object library using the Costmap_2d constructor. Assuming the Area_Des_Fallback node receives the vertex coordinates of the polygon region from the region information, it will use the PolygonOutlineCells function in the costmap object library to extract the polygon region and its boundary from the region information based on these coordinates. Then, the obtained polygon region is converted from the region in the world coordinate system to the region in the current raster coordinate system. The boundary of the polygon region occupied by the converted current raster coordinate system will be given the cost value of a fatal obstacle by the cost allocation function.

3. The full-coverage navigation method based on NAV2 behavior tree logic as described in claim 2, characterized in that, The Update_Costmap node first calls the getcostmap function of the costmap object library to obtain the static cost map in the system navigation planning module. Subsequently, it only updates the area division information. When updating, the existing grid map needs to be re-rasterized. The number of grids for re-rasterization is set according to the robot diameter. This is how the area cost map is confirmed. The Prepare_Plan node updates the region division information and simultaneously enters the Start_Point node. This node is a sequential node. First, it enters the Presuppose node, which is decorated by the Des node with the inversion function. This node obtains the top-left, top-right, bottom-left, bottom-right, and center of the polygon region as preset initial positions based on the vertex coordinates of the polygon region and stores them in the starting point candidate list. The top-left, top-right, bottom-left, and bottom-right corners of the preset initial positions are determined by moving the coordinates of each corner point one robot diameter position towards the center. It calculates whether each preset initial position is inside the polygon region and whether it is on a runnable grid. If each preset initial position is not inside the polygon region or is on a non-runnable grid, the preset initial position is judged as a non-runnable starting point and is deleted from the starting point candidate list. If all preset initial positions are non-runnable starting points, it enters the Ergodic node to traverse the grid map to find a new starting point that is not covered by obstacle grids and is inside the polygon region and can be started, and stores it in the starting point candidate list. This is how the starting point selection is performed.

4. The full-coverage navigation method based on NAV2 behavior tree logic as described in claim 3, characterized in that, The method for calculating whether each preset initial position is inside the polygonal region is to draw a ray from the current point and determine whether the number of edges of the polygonal region it passes through is odd. If it is odd, then it is not inside the polygonal region. The method for determining whether it is in a runnable grid is implemented using the getcost function of costmap. If the obtained cost value indicates that it is near or on an obstacle, then the preset initial position is an unrunnable starting point.

5. A full-coverage navigation method based on NAV2 behavior tree logic as described in any one of claims 1-4, characterized in that, The full-coverage planning path is a full-coverage navigation path calculated by multiple child nodes of the Plan_Des node, starting from the starting point selected by the starting point, based on the grid method. Specifically, according to the updated regional cost map confirmed in step S3, the starting point selected in step S3 is used as the starting point, and the cost grid map after re-griding is traversed in the positive x-axis direction of the map coordinate system. When the traversed grid contains a grid with a cost value of fatal obstacle, the grid is marked as a fatal obstacle, and the traversal direction is adjusted. During the traversal, for each grid, the center grid coordinates of the grid are converted into map coordinates using the maptoworld function in the costmap object library and stored in a point set P. This point set P represents the set of points that the robot needs to traverse when performing the full-coverage navigation process. Depending on the type of the starting point in the candidate starting point list in step S3, the calculation method for the pre-selected path varies. If the starting point is selected as one of the four corner points (top left, top right, bottom left, and bottom right), then when adjusting the traversal direction, the path is rotated 180 degrees based on the relative position of the corner point and the center point to form a bow-shaped path, which is then included in the point set P. If the starting point is selected as the center point, the path is rotated 90 degrees to form a square-shaped path, which is also included in the point set P. After obtaining multiple point sets P from different starting points, the Plan_Des node needs to compare the number of target points contained in each point set P with the number of times the traversal direction is adjusted when traversing the multi-grid map, and select the point set P that contains the most target points and has the fewest times the direction is adjusted as the final selected full-coverage planning path.

6. A full-coverage navigation method based on NAV2 behavior tree logic as described in any one of claims 1-4, characterized in that, The motion component includes the control of motion frequency and the release and updating of motion target points.

Images