A robot navigation system and method in a dynamic environment
By using the combination of map processing and navigation modules in a dynamic environment, guiding paths are generated and obstacle avoidance in real time, the problem of inefficient navigation in the existing technology is solved, and fast and accurate navigation route generation and dynamic obstacle avoidance are achieved.
Patent Information
- Application Number
- CN202210963947.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-11
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2042-08-11
AI Technical Summary
The prior art is difficult to quickly generate navigation routes in complex dynamic environments, and cannot effectively avoid dynamic obstacles, resulting in insufficiency of navigation.
Using a system that includes a map processing module, a guide path generation module and a navigation module, the heuristic module, obstacle detection module and collision risk detection module are used to load map data, generate guidance paths, and detect and avoid movement in real time during the navigation process. Obstacles, choose the path with the least collision risk.
It realizes the rapid generation of navigation routes in complex dynamic environments, improves navigation efficiency and accuracy, reduces calculation time, and effectively avoids dynamic obstacles.
Smart Images

Figure CN115451963B_ABST
Abstract
Description
Technical Field
[0001] The method belongs to the field of robot navigation technology, and specifically relates to a robot navigation system and method in a dynamic environment. Background Art
[0002] Today, the field of robot navigation is becoming more and more popular, and various new navigation technologies are emerging one after another. In the whole process, due to the time delay characteristics of data processing, the speed of path planning is much faster than the speed of navigation. Many excellent methods have been born in this field, including RRT-Connect, MT-RRT, Risk-RRT and other methods. The emergence of these new methods and technologies has greatly promoted the progress of autonomous driving technology.
[0003] Document CN202011145680.8 discloses a method using A * The method of integrating the algorithm and the RRT algorithm improves the performance of path planning. However, it requires that the map data is completely known. * Only then can an optimal path be planned. It is highly dependent on the complete map data and does not take into account the situation in the dynamic environment, so it is impossible to achieve navigation in the dynamic environment. Document CN201911086247.9 discloses an improved dynamic RRT * The mobile robot motion planning method takes into account the influence of dynamic obstacles, but RRT * The convergence time of the algorithm is a more prominent problem. Under complex road conditions, the calculation time is too long and the effect of dynamic navigation to avoid obstacles is not good. Summary of the invention
[0004] The technical problem to be solved by the present invention is to provide a robot navigation system and method in a dynamic environment, so as to quickly generate a navigation route in a complex dynamic environment, effectively avoid dynamic obstacles, and realize fast, accurate and high-quality dynamic navigation.
[0005] In order to solve the above technical problems, the technical solution adopted by the present invention is:
[0006] A robot navigation system in a dynamic environment comprises: a map processing module, a guidance path generation module and a navigation module, wherein the navigation module comprises: a heuristic module, an obstacle detection module and a collision risk detection module.
[0007] A robot navigation method in a dynamic environment, the steps are as follows:
[0008] Step 1: Load the data in the map library to obtain the static obstacle space dataset C of the map sobj and the free space dataset C free ;
[0009] Step 2: Given the end point and the insertion point of the insertion-assisted guidance path L, determine the guidance path subset l n and nodes that need RRT exploration;
[0010] Step 3: Generate a complete guidance path based on the planning algorithm;
[0011] Step 4: After the guidance path is generated, it is used as a heuristic guidance path to complete the navigation path L nav generate;
[0012] Step 5: When encountering a moving obstacle C during navigation dobj When the collision risk detection module plans a feasible route, it selects a path with the lowest collision risk as the navigation path for obstacle avoidance.
[0013] Step 6: When the moving obstacle disappears, the navigation process grows rapidly toward the end point under the action of the heuristic module, obstacle detection module, and the heuristic domain of the nodes in the guidance path. When encountering a moving obstacle, it returns to step 5 until it reaches the end point domain, completing the navigation process.
[0014] A further improvement of the technical solution of the present invention is that: in step 1, the data in the map library is loaded for binarization processing, or the binarized map data is directly loaded to obtain the static obstacle space data set C of the map. sobj and the free space dataset C free .
[0015] A further improvement of the technical solution of the present invention is that: in step 2, after the end point is given in the map, the coordinates of the starting point and the end point in the map are both known, and the starting point coordinate X init (x init ,y init ), end point coordinate X end (x end ,y end ), insert n nodes on the line connecting the starting point and the end point on the map as auxiliary nodes for guiding path generation, so that the guiding path L can be generated faster. The coordinate calculation formula of the inserted node is:
[0016]
[0017]
[0018] It means dividing the line segment into N equal parts. n ,y n ) is in free space, that is, (x n ,y n )∈C free , if it exists, keep it, otherwise delete it.
[0019] The further improvement of the technical solution of the present invention is that: Step 2 connects the remaining points from the starting point to the end point in sequence without considering the dynamic obstacles, and then determines the set l of each line segment. n (n=1,2,3…) Is it in the free space C? free inside, if Then delete l n Set, if l n ∈C free Then keep this line segment as a spare subset l to construct the complete guidance path L n .
[0020] A further improvement of the technical solution of the present invention is that: in step 2, each point where the connection fails is subjected to random tree exploration in the direction of the connection failure.
[0021] A further improvement of the technical solution of the present invention is that: in step 3, when the Euclidean distance between the nodes of the two trees is less than a time step distance d t Then connect and merge them into one tree and continue to explore the random tree. Repeat this step until the starting point and the end point are in the same tree, that is, (X init ∪X end )∈L; time step distance d t is the distance explored by the random tree per unit time, here we set d t =1; when detection (X init ∪X end )∈L, which means that a complete guidance path is constructed; the guidance path L is not used as the navigation path L nav Used only as a heuristic path during navigation.
[0022] A further improvement of the technical solution of the present invention is that: in step 4, after the guidance path is constructed, the complete guidance path data is used as the heuristic path to construct the navigation path, and the navigation process is completed using the complete guidance path data as the heuristic path. The circular domain at the guidance path node is the heuristic domain δ. The nodes of the heuristic domain play an attractive role in the navigation path generation process. When the navigation path enters the heuristic domain, it grows to the next node of the guidance path.
[0023] The further improvement of the technical solution of the present invention is that: Step 5 specifically includes: during the navigation process, the heuristic module and the obstacle detection module are turned on in real time, and when a moving obstacle C is encountered dobjThe collision risk detection module based on Gaussian distribution is enabled only when the local map is gridded. It is assumed that the moving obstacles will not overlap with the static obstacles. According to the positions of the static obstacles and the moving obstacles in the grid map and the motion prediction of the moving obstacles, the collision risks between the robot and the static obstacles and the moving obstacles in the planned path are calculated respectively, and a path with the minimum collision risk is selected as the navigation path; when the moving obstacle disappears, the navigation process grows rapidly towards the end point under the action of the heuristic module, the obstacle detection module and the heuristic domain δ of the nodes in the guidance path. The radius of the heuristic domain δ r is a time step distance d t When a moving obstacle is encountered, the above steps are executed until the end domain is reached. The radius of the end domain is one time step distance d t half.
[0024] Due to the adoption of the above technical solution, the technical progress achieved by the present invention is:
[0025] The guidance path is explored and generated by inserting auxiliary points, MT-RRT's multi-tree simultaneous exploration, and RRT-Connect's rapid subtree interconnection, which improves the generation speed of the guidance path. The existence of the guidance path L also accelerates the navigation process. In the navigation process, the guidance path is used as the heuristic path, that is, the reference of the heuristic module, avoiding the direct use of the guidance path without considering the solution of the two-point boundary value problem. The addition of the collision risk detection algorithm also improves the efficiency of the system in a dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative work.
[0027] Figure 1 A diagram of a robot navigation system in a dynamic environment;
[0028] Figure 2 A flow chart of a robot navigation method in a dynamic environment;
[0029] Figure 3 Location diagram of starting point, end point and insertion point;
[0030] Figure 4 The relationship diagram between each line segment and static obstacles after connecting each point;
[0031] Figure 5 Perform RRT exploration diagrams at each point where connection failure occurs;
[0032] Figure 6 During the exploration, the two trees merged into one tree and continued to explore;
[0033] Figure 7 The guidance path is generated and the obstacle map is moved;
[0034] Figure 8 Guidance path, navigation path and moving obstacle interaction diagram;
[0035] Fig. 9 heuristic bias map of guidance path to navigation path;
[0036] Fig.10 The navigation path map is planned under the combined influence of the guidance path, collision risk detection algorithm and different states of moving obstacles. DETAILED DESCRIPTION
[0037] The present invention is further described in detail below in conjunction with embodiments:
[0038] like Figure 1 As shown, the robot navigation system in a dynamic environment includes a map processing module, a guidance path generation module and a navigation module.
[0039] like Figure 2 As shown in Figure 1, it is a robot navigation method in a dynamic environment. The steps are as follows:
[0040] Step 1: Load the data in the map library to obtain the static obstacle space dataset C of the map sobj and the free space dataset C free ;
[0041] Step 2: Given the end point and the insertion point of the insertion-assisted guidance path L, determine the guidance path subset l n and nodes that need RRT exploration;
[0042] Step 3: Generate a complete guidance path based on the planning algorithm;
[0043] Step 4: After the guidance path is generated, it is used as a heuristic guidance path to complete the navigation path L nav generate;
[0044] Step 5: When encountering a moving obstacle C during navigation dobj When the collision risk detection module plans a feasible route, it selects a path with the lowest collision risk as the navigation path for obstacle avoidance.
[0045] Step 6: When the moving obstacle disappears, the navigation process grows rapidly toward the end point under the action of the heuristic module, obstacle detection module, and the heuristic domain of the nodes in the guidance path. When encountering a moving obstacle, it returns to step 5 until it reaches the end point domain, completing the navigation process.
[0046] Embodiment 1:
[0047] like Figure 3-10 As shown in the figure, the robot navigation process in a dynamic environment is performed by loading the data in the map library through the map processing module, binarizing it, or directly loading the binarized map data in the map library. Obtain the static obstacle space dataset C of the map sobj and the free space dataset C free . Figure 3 The black rectangle is a static obstacle, the blank area is free space, ① is the starting point, ② is the insertion point, and ③ is the end point. After the end point is given, the coordinates of the starting point and the end point in the map are known. The starting point coordinate X init (x init ,y init ), end point coordinate X end (x end ,y end ). Insert n( Figure 3 The n=2) nodes are used as auxiliary nodes for guiding path generation, so that the guiding path L can be generated more quickly. The coordinate calculation formula for inserting nodes is:
[0048]
[0049]
[0050] Indicates dividing the line segment into N equal parts. Determine the points (x n ,y n ) is in free space, that is, (x n ,y n )∈C free , if it exists, keep it, otherwise delete it.
[0051] Without considering dynamic obstacles, connect the remaining points from the starting point to the end point in sequence. Then determine the set l of each line segment n (n=1,2,3…) Is it in the free space C? free inside, if Then delete l n Collection. Figure 4 As shown in ④, the dotted line intersects with the obstacle, so it needs to be deleted. n ∈C freeThen keep this line segment as a spare subset l to construct the complete guidance path L n ,like Figure 4 The thin solid line in .
[0052] For each point where the connection fails, a random tree exploration is performed in the direction of the connection failure. Figure 5 As shown, when the Euclidean distance between the nodes of two trees is less than one time step distance d t Then connect and merge them into one tree and continue random tree exploration, such as Figure 5 neutralize Figure 6 ⑥ is shown in Figure 6. Repeat this step until the starting point and the end point are in the same tree (X init ∪X end )∈L. One time step distance d t is the distance explored by the random tree per unit time, here we set d t =1. When detecting (X init ∪X end )∈L means that a complete guidance path is constructed. Figure 7 As shown in ⑦. The guidance path L is not used as the navigation path L nav Used only as a heuristic path during navigation.
[0053] Before the guidance path is constructed, the navigation path at the starting point is constructed based on the subset l1 of the guidance path at the starting point as the heuristic path. When the guidance path is constructed, the complete guidance path data is used as the heuristic path to complete the navigation process. Fig. 9 As shown in . The thin solid line is the guidance path, and the thick solid line is the navigation path. The circular domain at the guidance path node is the heuristic domain δ, as shown in Fig. 9 The heuristic domain δ provides a heuristic bias in the process of generating the navigation path. The nodes in the heuristic domain δ play an attractive role in the process of generating the navigation path. When the navigation path enters the heuristic domain δ, it grows to the next node of the guidance path.
[0054] During the navigation process, the heuristic module and obstacle detection module are turned on in real time. When encountering a moving obstacle C dobj The collision risk detection module based on Gaussian distribution is enabled only when the local map is gridded. It is assumed that the moving obstacles and static obstacles will not overlap. According to the positions of static obstacles and moving obstacles in the grid map and the motion prediction of the moving obstacles, the collision risks between the robot and the static obstacles and the moving obstacles in the planned path are calculated respectively, and a path with the minimum collision risk is selected as the navigation path, such as Fig.10 As shown, thin solid line To guide the path, the triangle with an arrow is a moving obstacle C dobj like Figure 7 As shown in ⑧. Fig.10Thin solid line To guide the path, the triangle with an arrow is a moving obstacle C dobj like Figure 7 As shown in ⑧. Fig.10 middle The thick solid line is the feasible obstacle avoidance path generated by the system when the collision risk detection algorithm is in effect. When the moving obstacle disappears, the navigation process grows rapidly toward the end point under the action of the heuristic module, the obstacle detection module, and the heuristic domain δ of the nodes in the guidance path. The radius of the heuristic domain δ r is a time step distance d t When encountering a moving obstacle, the above steps are executed until the end domain is reached. The radius of the end domain is one time step distance d t half.
[0055] The navigation process ends after reaching the destination domain, e.g. Figure 8 As shown, the thin solid line is the guidance path, and the thick solid line is the navigation path. Figure 8 ⑨ in the middle is a feasible obstacle avoidance path generated by the system under the joint action of the heuristic module, obstacle detection module and collision risk detection module after encountering a moving obstacle during the navigation process.
[0056] The embodiments described above are merely descriptions of preferred implementation modes of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.
Claims
1. A robot navigation method in a dynamic environment, characterized in that: The navigation system includes a map processing module, a guidance path generation module and a navigation module. The navigation module includes a heuristic module, an obstacle detection module and a collision risk detection module. The steps are as follows: Step 1: Load the data in the map library to obtain the static obstacle space dataset C of the map sobj and the free space dataset C free ; Step 2: Given the end point and the insertion point of the insertion-assisted guidance path L, determine the guidance path subset l n and the nodes that need to be explored by RRT; Step 2: After the end point is given in the map, the coordinates of the starting point and the end point in the map are known, and the coordinates of the starting point X init (x init ,y init ), end point coordinate X end (x end ,y end ), insert n nodes on the line connecting the starting point and the end point on the map as auxiliary nodes for guiding path generation, so that the guiding path L can be generated faster. The coordinate calculation formula of the inserted node is: It means to divide the line segment into N equal parts and determine the points (x n ,y n ) is in free space, that is, (x n ,y n )∈C free , if it is, keep it, otherwise delete it; Step 2, without considering dynamic obstacles, connect the remaining points from the starting point to the end point in sequence, and then determine the set l of each line segment n (m=1,2,3…) Is it in free space C? free inside, if Then delete l n Set, if l n ∈V free Then keep this line segment as a spare subset l to construct the complete guidance path L n ; Step 2: For each point where the connection fails, perform a random tree exploration in the direction of the connection failure; Step 3: Generate a complete guidance path based on the planning algorithm; Step 4: After the guidance path is generated, it is used as a heuristic guidance path to complete the navigation path L nav generate; Step 5: When encountering a moving obstacle C during navigation dobj When the collision risk detection module plans a feasible route, it selects a path with the lowest collision risk as the navigation path for obstacle avoidance. Step 6: When the moving obstacle disappears, the navigation process grows rapidly toward the end point under the action of the heuristic module, obstacle detection module, and the heuristic domain of the nodes in the guidance path. When encountering a moving obstacle, it returns to step 5 until it reaches the end point domain, completing the navigation process.
2. The robot navigation method in a dynamic environment according to claim 1, characterized in that: In step 1, the data in the map library is loaded for binarization processing, or the binarized map data is directly loaded to obtain the static obstacle space dataset C of the map. sobj and the free space dataset C free .
3. The robot navigation method in a dynamic environment according to claim 1, characterized in that: In step 3, when the Euclidean distance between the nodes of the two trees is less than a time step distance d t Then connect and merge them into one tree and continue to explore the random tree. Repeat this step until the starting point and the end point are in the same tree, that is, (X init ∪X end )∈L; time step distance d t is the distance explored by the random tree per unit time, here we set d t =1; when detection (X init ∪X end )∈L, which means that a complete guidance path is constructed; the guidance path L is not used as the navigation path L nav Used only as a heuristic path during navigation.
4. The robot navigation method in a dynamic environment according to claim 1, characterized in that: In step 4, when the guidance path is constructed, the complete guidance path data is used as the heuristic path to construct the navigation path, and the navigation process is completed using the complete guidance path data as the heuristic path. The circular domain at the guidance path node is the heuristic domain δ. The nodes of the heuristic domain play an attractive role in the navigation path generation process. When the navigation path enters the heuristic domain, it grows to the next node of the guidance path.
5. The robot navigation method in a dynamic environment according to claim 1, characterized in that: The step 5 specifically includes: during the navigation process, the heuristic module and the obstacle detection module are turned on in real time, and when a moving obstacle C is encountered dobj The collision risk detection module based on Gaussian distribution is enabled only when the local map is gridded. It is assumed that the moving obstacles and static obstacles will not overlap. According to the positions of static obstacles and moving obstacles in the grid map and the motion prediction of the moving obstacles, the collision risks between the robot and the static obstacles and the moving obstacles in the planned path are calculated respectively, and a path with the minimum collision risk is selected as the navigation path. When the moving obstacle disappears, the navigation process grows rapidly towards the end point under the action of the heuristic module, the obstacle detection module and the heuristic domain δ of the nodes in the guidance path. The radius of the heuristic domain δ r is a time step distance d t When a moving obstacle is encountered, the above steps are executed until the end domain is reached. The radius of the end domain is one time step distance d t half.
Citation Information
Patent Citations
Improved dynamic RRT* mobile robot motion planning method
CN110703768A
Mobile robot path planning method based on A * algorithm and RRT * algorithm
CN112393728A
Method for planning optimal path for incremental environment information sampling of indoor mobile robot
CN106444769A
Improved RRT path planning method and system
CN110531770A
Myxomycetes RRT navigation method and system based on two-dimensional static path planning
CN110906947A
