CN112428271B - Real-time robot motion planning method based on multimodal information feature tree - Google Patents

Abstract

The invention discloses a real-time motion planning method for a robot based on a multimodal information feature tree, which includes acquiring an environment map, extracting the pose information of the robot in real time, and judging whether the representation degree of the feature point on the environment map is complete, and if not If it is complete, use angular velocity and distance fusion to perform preliminary feature extraction on the node pose of the robot, until the representation degree of the feature points is complete to obtain the optimal feature points, obtain the final feature point set, and update the feature map; if it is complete, use Euclidean distance to evaluate the environment. The map is updated to obtain the feature map; then the feature matrix is obtained according to the feature point set, and the multi-modal information feature tree of the feature points is generated according to the starting point, the end point and the feature matrix, and the heuristic path is obtained. The invention quickly finds out candidate nodes in feasible regions by constructing a multimodal information feature tree and extracting feature points in real time to optimize path planning based on random sampling, solve trap space and improve the intelligence of mobile robots.

Description

Real-time robot motion planning method based on multimodal information feature tree

technical field

The invention relates to the technical field of robot motion planning, in particular to a real-time robot motion planning method based on a multimodal information feature tree.

Background technique

The development of artificial intelligence technology has brought opportunities for the research of mobile service robots. At present, mobile service robots such as guiding robots, sweeping robots, shopping guide robots, and cargo handling robots have been successfully applied to airports, supermarkets, museums, families, etc. surroundings. Path planning for mobile robots refers to finding a collision-free path between a given initial point and a target point that satisfies the specified constraints without human intervention. Currently, the main path planning methods can be roughly divided into four types: grid-based path planning, artificial potential field-based path planning, reward-based path planning, and random sampling-based motion planning. Among these four methods, the path planning algorithm based on random sampling has a small amount of calculation, flexible obstacle avoidance and no need to model the environment, which greatly reduces the planning time and memory cost, ensures the real-time performance of path planning, and is more suitable for Solve path planning problems in dynamic environments. However, in a complex dynamic human-machine integration environment, such as mazes, S-curves, slits and other scenes, there are dynamic obstacles such as pedestrians at the same time, the robot does not simply move in a purely static environment, but also takes into account Dynamic pedestrians in random movements. Since the proportion of feasible areas in the entire map is greatly reduced, the probability of randomly sampling feasible areas decreases, and the local planning algorithm is prone to fall into trap space, resulting in longer path planning time or even planning failure. At the same time, the robot is not given the concept of "brain memory". The robot has walked through the same scene countless times, and the next time it walks, it needs to re-plan the path and waste computing resources.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a real-time motion planning method for a robot based on a multimodal information feature tree that solves the trap space problem, realizes node multiplexing, ensures real-time performance and saves time and cost.

In order to solve the above-mentioned technical problems, the present invention provides a real-time motion planning method for a robot based on a multimodal information feature tree, comprising the following steps:

Step 1: Obtain the environment map, extract the node pose information of the robot in real time, and determine whether the representation of the feature point to the environment map is complete, if not, go to step 2, and if it is complete, go to step 4;

Step 2: Use the angular velocity channel and distance fusion to perform preliminary feature extraction on the node pose of the robot, obtain the initial feature point set, and update the feature map at the same time;

Step 3: Use distance fusion again to further extract the initial feature point set, until the representation degree of the feature points is complete to obtain the optimal feature point, and obtain the final feature point set;

Step 4: Use the Euclidean distance to update the environment map, obtain the feature map, and complete the real-time feature extraction;

Step 5: Obtain a feature matrix according to the feature point set, generate a multimodal information feature tree of the feature points according to the starting point, the ending point and the feature matrix, and obtain a heuristic path on the feature map.

Further, in the step 1, the method for judging whether the representation degree of the feature point on the environmental map is complete is to check whether the environmental map has a corresponding feature file each time before opening the terminal for path planning, and if there is a corresponding feature file. If there is no corresponding feature file, the characterization degree is incomplete; when the robot completes the path planning on the environment map and closes the terminal, the environment map will be generated. corresponding feature file.

Further, the specific process of the step 1 is:

Step 1-1: Given a navigation target, the robot starts path planning based on the Risk-RRT algorithm, and obtains the node pose information G _m =(x _m , y _m , ω _m ) of the robot in the process of path planning in real time, where x , y represents the position of the node, and ω represents the angular velocity of the node;

Step 1-2: Determine whether the feature file exists every time you open the terminal, and calculate the ratio of the number of corresponding feature point numbers to all the points in the barrier-free area by calculating the points in the barrier-free area on the map through the feature file. representative, used to represent the degree of representation;

If there is no feature file, set representative=0, initialize the feature map _MF as a two-dimensional zero matrix of the same size as the environment map, initialize the feature point set _GFP as an empty set, and execute step 2;

If there is a feature file but the degree of representation is incomplete, store the node G _new = (x _n , y _n ) stored in the feature file into the feature point set _GFP , and execute step 2;

If the feature file exists and the degree of representation is complete, store the node G _new =(x _n , _yn ) stored in the feature file into the feature point set G _FP , and execute step 4 .

Further, the specific process of the step 2 is:

Step 2-1: Use the angular velocity channel to extract nodes whose angular velocity is greater than the preset threshold, denoted as G _new =(x _n , y _n );

Step 2-2: If the degree of characterization is incomplete, go to step 2-3; if the degree of characterization is complete, go to step 2-5;

Step 2-3: Determine whether the feature point set G _FP is empty, if it is empty, directly store the node G _new in the feature point set G _FP , and return to step 2-1; if it is not empty, execute step 2-4 ;

Step 2-4: Use the Euclidean distance channel to calculate the Euclidean distance between the node G _new and all the nodes G _i =(x _i ,y _i ) in the feature point set G _FP in turn, and determine whether the Euclidean distance is less than or equal to the preset The first distance fusion threshold of , if yes, return to step 2-1;

If the distance between node G _new and all nodes in feature point set G _FP is greater than the preset first distance fusion threshold, add node G _new into feature point set G _FP , bring G _FP into step 3 and execute ;

Step 2-5: Calculate the Euclidean distance between the node G _new and all nodes G _i =(x _i ,y _i ) in the feature point set G _FP in turn, and determine whether the Euclidean distance is less than or equal to the preset first time Distance fusion threshold, if yes, go to step 2-6, if not, go to step 4;

Step 2-6: Perform collision detection between the grid points in the feature map _MF and the node G _new in turn, if all the collision detections pass and the sum of the distances between the grid points in the feature map _MF and the node G _new is smaller than the feature map The _sum of the distances between the grid points in MF and the original feature nodes, then the node G _new replaces the original feature node G _i =(x _i , y _i ), updates the feature map, brings the G _FP into step 4 and executes , otherwise go to step 5.

Further, when the angular velocity channel is used to extract the nodes whose angular velocity is greater than the preset threshold in the step 2-1, the nodes are extracted at intervals of a certain number to reduce the redundancy of the nodes.

Further, the specific process of the step 3 is:

Step 3-1: Initialize the count c to 0, initialize the feature point set _GR to be saved as an empty set; extract the adjacent feature nodes of each node G _i =(x _i ,y _i ) in the feature point set _GFP gather;

Step 3-2: Initialize i to 1;

Step 3-3: Determine whether i is less than or equal to the number n of the feature point set G _FP , if so, go to step 3-4, if not, go to step 3-6;

Step 3-4: Determine whether the number of adjacent feature node sets of the i-th node is less than or equal to 3, if so, go to step 3-5, if not, set i=i+1 and return to step 3-3;

Step 3-5: Add the node G _i =(x _i , y _i ) itself and the nodes in the adjacent feature node set into the point set _GR ; determine whether the number of adjacent feature node sets is less than or equal to 1, If so, count c+1; if i is not equal to the number n of the feature point set G _FP , then set i=i+1 and return to step 3-3, otherwise perform step 3-6;

Step 3-6: Perform deduplication processing on the point set _GR , if the number n of the feature point set G _FP is greater than or equal to the number n of the point set _GR -1, update the feature map, and bring the feature point set G _FP into Step 4 and execute, otherwise execute steps 3-7;

Step 3-7: Add all the remaining nodes from the feature point set _GFP except the point set _GR to the point set _GD , perform the second distance fusion to delete the feature points, and update the feature map. At this time, the feature point set _GFP That is, the final feature point set G _FP ', bring G _FP ' into step 4 and execute.

Further, the specific method of the distance fusion is:

Take the node G _x in the point set, connect it with the remaining nodes G _x1 in the point set, and perform collision detection and Euclidean distance calculation on the map. If the collision detection passes and the distance is less than the artificially set distance fusion distance threshold, then from The node G _x1 is deleted from the point set until all points in the point set have been detected.

Further, the specific process of the step 4 is:

Step 4-1: The initialization flag bit is 0, which is used to indicate whether the points in the obstacle-free area on the map have found their corresponding feature point numbers, and convert all the node coordinates in the G _FP ' into grid coordinates, and record Let G _i1 =(x _i1 , y _i1 );

Step 4-2: Traverse all grid points (i _k , j _k ) in the obstacle-free area in the two-dimensional grid map MP, initialize count to 0, and initialize _{min_dist} to map length × map width; i _k , j _k ) are sequentially connected with the feature node G _i1 in the _GFP ' to perform collision detection on the map. If the collision detection passes, the Euclidean distance dist ₁ between the two points is calculated. If dist ₁ is less than min_dist, Then update the corresponding value of the grid point ( _ik , _jk ) in the feature map MF to the number i of the feature node and update _{min_dist} to the value of dist ₁ ; if the collision detection fails, count count+1; until G The traversal of all nodes in _FP ' ends, if count=the number n of G _FP ', set the flag bit to 1, and exit the loop, and return to step 2; if count ≠ the number n of G _FP ', repeat step 4 -2 until all grid points in the obstacle-free area in the two-dimensional grid map M _p are traversed, and the feature map M _F is obtained;

Step 4-3: If the flag bit is equal to 0, the representation degree is complete, and let the representative be equal to 1, otherwise, let the representative be equal to 0.

Further, the specific process of the step 5 is:

Step 5-1: Select a node G _FPi in G _FP ', and check the connection between the remaining nodes G _FPx and node G _FPi on the map for collision detection. The distance is stored in the feature matrix P _F (i, x), otherwise 0 is stored in P _F (i, x);

Step 5-2: Find the feature node number corresponding to the starting point in the feature map MF, and add it to the multimodal information feature tree _T , complete the initialization of the multimodal information feature tree T, and initialize the set of open nodes as: T _open = T;

Step 5-3: determine whether the number of open nodes is greater than zero, if so, go to step 5-4; if not, go to step 5-10;

Step 5-4: extract the first open node t _open , and remove t _open from the open node set T _open ;

Step 5-5: Extract the _AOI of the _open area of interest from the feature map MF;

Step 5-6: Extract all adjacent feature nodes X _new of t _open in the area of interest AOI;

Step 5-7: Determine whether the adjacent feature node set X _new is empty, if it is empty, go to step 5-3; if it is not empty, go to step 5-8;

Step 5-8: Take the first node x _new of X _new , find the nearest neighbor x _parent of x _new on the multimodal information feature tree T, and judge whether x _{parent exists} . Remove from X _new , and return to step 5-7; if it exists, execute step 5-9;

Step 5-9: add x _new to the multimodal information feature tree T and the open node set T _open , remove x _new from X _new , and return to step 5-7;

Step 5-10: Find the feature node corresponding to the end position in the feature map MF, and find the path from the start point to the end point by reverse searching the multimodal information feature tree _T ;

Step 5-11: Return to step 2-1.

Further, the specific method for extracting the area of interest _AOI of t _open from the feature map MF in the step 5-5 is,

In the feature map _MF , search for all points with a value of t _open , record the limit values x _min , x _max , y _min , and y _max of the coordinates in all points, and select the square area corresponding to the coordinate limit value as the area of interest AOI.

By constructing a multimodal information feature tree and extracting feature points in real time, the invention can quickly find out the candidate nodes of the feasible area to optimize the path planning based on random sampling, solve the trap space, and improve the intelligence of the mobile robot, so that in complex dynamic The optimal motion trajectory of the mobile robot can be found efficiently and quickly in the human-machine integrated environment.

The above description is only an overview of the technical solution of the present invention. In order to understand the technical means of the present invention more clearly, and implement it according to the content of the description, the preferred embodiments of the present invention are described in detail below with the accompanying drawings.

Description of drawings

Figure 1 is a flow chart of the present invention.

FIG. 2 is a schematic diagram of extracting feature points through angular velocity channel and distance fusion in an embodiment of the present invention.

FIG. 3 is a schematic diagram of a second distance fusion in an embodiment of the present invention.

FIG. 4 is a schematic diagram of finally extracted feature points in an embodiment of the present invention.

FIG. 5 is a schematic diagram of a multimodal information feature tree and a heuristic path in an embodiment of the present invention.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

In the description of the present invention, it should be understood that the terms "first" and "second" are only used for description purposes, and cannot be interpreted as indicating or implying relative importance or the number of indicated technical features. Thus, a feature defined as "second" or "first" may expressly or implicitly include one or more of that feature. The term "comprising" is intended to cover non-exclusive inclusion, such as a process, method, system, product or device comprising a series of steps or elements, not limited to the listed steps or elements but optionally also including no listed out steps or units, or optionally also include other steps or units inherent to these processes, methods, products or devices.

1-5, an embodiment of a real-time robot motion planning method based on a multimodal information feature tree of the present invention includes the following steps:

Step 1: Obtain the environment map, extract the node pose information of the robot in real time, and determine whether the representation degree of the feature point on the environment map is complete (the premise of the complete representation degree is that the feature map must be calculated for each qualified feature point, The feature map will calculate whether all the points on the map have found their corresponding feature points. Only when each point on the map finds its corresponding feature point, the characterization degree is complete). If it is not complete, go to step 2. , if it is complete, go to step 4. The method for judging whether the representation degree of the feature point on the environmental map is complete is to check whether the environmental map has a corresponding feature file each time before opening the terminal for path planning. If there is a corresponding feature file, judge by the feature file. Whether the characterization degree is complete, if there is no corresponding feature file, the characterization degree is incomplete; when the robot completes the path planning on the environment map and closes the terminal, the corresponding feature file of the environment map is generated.

In this embodiment, three feature files, fp.txt, feature_map.txt, and feature_matrix.txt are set, wherein fp.txt is the feature point file, which stores the x and y coordinate information of the feature points, and the fp.txt file has two columns and n lines. , n is the number of feature points, the first column is the x-coordinate of the feature point, and the second column is the y-coordinate of the feature point; feature_map.txt is the feature map file, which stores the capabilities of each grid point on the environment map. The number of the closest feature points connected; feature_matrix.txt is the feature matrix file, which stores the connection of n feature points and the distance between two neighboring nodes. Every time you open the terminal, determine whether the three files fp.txt, feature_map.txt, and feature_matrix.txt exist. If there is no corresponding fp.txt feature point file, it is the first time that the robot performs motion planning on the environment map, and the degree of representation is incomplete; if there is a corresponding feature point file fp.txt but no corresponding feature_map.txt, feature_matrix. txt file, it is not the first time that the robot performs motion planning on the environment map and the characterization is incomplete; if there are corresponding fp.txt, feature_map.txt, feature_matrix.txt, the characterization is complete. The feature node set G _FP at this time is written into the fp.txt file only every time the terminal is closed, which can reduce the calculation amount of the running process and improve the running speed. Moreover, the feature map file and feature matrix file corresponding to the environment map will be generated only when the terminal is closed when the characterization degree is complete, that is, the feature map feature_map.txt file and feature matrix will be updated every time the terminal is closed when the characterization degree is complete. The feature_matrix.txt file is written to the corresponding txt file.

Step 1-1: Given a navigation target, the robot is based on the Risk-RRT method (see the literature "C.Fulgenzi, A.Spalanzani, C.Laugier, and C.Tay,"Risk based motion planning and navigationinuncertain dynamic environment,"Res. Rep., pp.1–14, 2010.”) Start path planning and extract the node pose information of the robot in real time by subscribing to the topics /amcl_pose and /robot_0/odom to obtain the node pose of the robot during the path planning process in real time information. The /amcl_pose topic records the real-time x and y coordinate information of the robot; the /robot_0/odom topic records the robot angular velocity information corresponding to the aforementioned position, which is denoted as G _m =(x _m , y _m , ω _m ), where, x and y represent the position of the node, and ω represents the angular velocity of the node.

Step 1-2: Under the ROS system (Robot Operating System, ROS, a robot operating system, which is a highly flexible software architecture for writing robot software programs), determine whether the feature file exists every time the terminal is opened , and calculate the point in the barrier-free area on the map through the feature file to find the ratio of the number of the corresponding feature point numbers to all the points in the barrier-free area. The representative is used to represent the degree of representation; when calculating the representative, for simplicity, take All cases where the ratio is less than 1 are set to representative=0; when the ratio is equal to 1, it means that the representation degree is complete, at this time, representative=1 is set, and only when the representation degree is complete, the feature map and feature matrix are generated and written separately. feature_map.txt file and feature_matrix.txt file.

If the three files _fp.txt , feature_map.txt, and feature_matrix.txt do not exist, it means that it is the first time to perform motion planning on this map, then let representative=0, and initialize the feature map MF to a size of the same size as the environment map The two-dimensional zero matrix (the pixel of the environment map is 504*434, the initialization feature map is a two-dimensional matrix of 504*434), the initialization feature point set _GFP is an empty set, and step 2 is performed. In this embodiment, step 2- 1;

If the feature file exists but the characterization degree is incomplete, that is, the fp.txt file exists but the other two files do not exist, it means that motion planning has been done on this map before, but the characterization degree is not complete, then the feature point file fp.txt The stored node G _new = (x _n , y _n ) is stored in the feature point set GFP, and is stored in Rviz ( _Rviz is a graphical tool that comes with ROS, which can easily perform graphical operations on ROS programs. The invention mainly visualizes the feature points in the Rviz platform for the simulation experiment), and executes step 2, and in this embodiment, executes step 2-2;

If the feature file exists and the representation degree is complete, that is, the fp.txt, feature_map.txt and feature_matrix.txt files all exist, it means that the representation of the environment map is complete, and the node G _new = (x _n , y _n ) are stored in the feature point set G _FP , and step 4 is executed.

Step 2: Use the angular velocity channel and distance fusion to perform preliminary feature extraction on the node pose of the robot, obtain the initial feature point set G _FP , and update the feature map at the same time.

Step 2-1: Use the angular velocity channel to extract the nodes whose angular velocity is greater than the preset threshold, denoted as G _new =(x _n , y _n ); when extracting the nodes whose angular velocity is greater than the preset threshold, extract nodes at intervals of a certain number, Used to reduce the redundancy of nodes. In this embodiment, the quantity interval is set to 15, the frequency of messages published by the /amcl_pose and /robot_0/odom topics is high, and the gap between points is small. Go to the node with an effective gap to reduce the redundancy of the node.

Step 2-2: If the degree of representation is incomplete, perform step 2-3; if the degree of representation is complete, perform step 2-5, that is, replace the original feature point according to the optimal distance.

Step 2-3: Determine whether the feature point set G _FP is empty, if it is empty, directly store the node G _new in the feature point set G _FP , and return to step 2-1; if it is not empty, execute step 2-4 .

Step 2-4: Use the Euclidean distance channel to connect the node G _new to all nodes in the feature point set G _FP G _i =(x _i ,y _i )(i=1, 2, 3,...n, where n is The number of feature point sets G _FP ) performs the Euclidean distance calculation in turn (that is, the distance fusion operation in step 2, in order to distinguish it from the distance fusion performed again in step 3, here is called the first distance fusion, step 3 is the second distance fusion, and the two distance fusions are Euclidean distance channels), denoted as d_once, and determine whether the Euclidean distance d_once is less than or equal to the preset first distance fusion threshold (in this embodiment, the The threshold is 50. 50 is the actual 2.7m divided by the resolution 0.054), if yes, return to step 2-1; if the distance between the node G _new and all nodes in the feature point set G _FP is greater than the preset first If the second distance fusion threshold is set, the node G _new is added to the feature point set G _FP , and the G _FP is brought into step 3 and executed.

Step 2-5: This step is to adjust the position of the feature points according to the optimal distance, which is only performed when the feature point set G _FP has a complete representation of the environmental map, that is, representative=1, and the node G _new is connected to the feature point set. All nodes in G _FP G _i =(x _i ,y _i )(i=1,2,3,...n) perform Euclidean distance calculation in turn, denoted as d_once, and judge whether the Euclidean distance d_once is less than or equal to the predetermined distance. Set the first distance fusion threshold, if yes, go to step 2-6, if not go to step 4.

Adjust the position of the feature points according to the optimal distance. The optimal distance is to select the optimal feature point by calculating the sum of the distances, which means that the new nodes acquired in real time will not be added at this time, but to adjust the position of the original feature points. That is, to calculate the sum of the distances between the original feature point and all points on the map corresponding to it, and to calculate the sum of the distances between the new node and all the points on the map corresponding to the original feature point, compare the two The sum of the distances selects the one with the smaller distance to replace the original one as the optimal feature point. At the same time, the premise of calculating the sum of the distance is that the new node can be connected to all the points on the map corresponding to the original feature point, which also ensures that once the representation degree is complete, how to adjust the position of the feature point , the degree of representation will not be reduced to incomplete, but will iteratively find the optimal feature points that a certain area can represent.

如果碰撞检测全部都通过且特征地图M_F中栅格值为i的栅格点与节点G_new的距离之和小于特征地图M_F中栅格值为i的栅格点与原先的特征节点的距离之和，则节点G_new替换原有特征节点G_i＝(x_i,y_i)，更新特征地图，将G_FP带入步骤4并执行，否则执行步骤5。如果能替换更优的特征点就替换，更新特征地图；如果没找到更优的特征点，那就还依据原来的那些特征点生成启发式路径，执行步骤5。Step 2-6: Perform collision detection on the grid point with grid value i in the feature map _MF and the node G _new in turn (if there is an obstacle in the connection between the two points, the collision detection will fail, otherwise, it will pass) , the formula is:

If all collision detections pass and the sum of the distances between the grid point with grid value i in the feature map _MF and the node G _new is less than the distance between the grid point with grid value i in the feature map _MF and the original feature node The sum of the distances, the node G _new replaces the original feature node G _i =( _xi , y _i ), updates the feature map, and brings the _GFP into step 4 and executes, otherwise, execute step 5. If a better feature point can be replaced, replace it and update the feature map; if no better feature point is found, generate a heuristic path based on the original feature points, and perform step 5.

As shown in Figure 2, the trajectory of the robot is represented by a dotted line, the flags represent the feature points extracted through the angular velocity channel and the first distance fusion, and the black rectangle represents the obstacle area.

Step 3: Use distance fusion again to further extract the initial feature point set G _FP , until the representation degree of the feature points is complete to obtain the optimal feature point, and obtain the final feature point set G _FP '. The first distance fusion is to add new feature points, and the second distance fusion is to delete redundant feature points for the newly added feature points.

Step 3-1: Initialize the count c (the number of neighbor feature points that c is used to calculate the feature point ≤ 1) to 0, and initialize the feature point set _GR to be saved as an empty set ( _GR is used to store the necessary Stored feature point information); _extract each node G _i =(x _i , y _i ) in the feature point set GFP (i=1, 2, 3, ... n, where n is the feature point set _GFP The number of adjacent feature nodes set.

Step 3-2: Initialize i (number of cycles) to 1.

Step 3-3: Determine whether i is less than or equal to the number n of the feature point set G _FP , if yes, go to step 3-4, if not, go to step 3-6.

Step 3-4: Determine whether the number of adjacent feature node sets of the i-th node is less than or equal to 3 (if the number of neighbor feature points of a feature point is less than 1, it means that the feature point is an outlier, and he cannot participate In the generation of the feature tree, in order to ensure the connectivity between points, we save the feature points with the number of neighbor feature points less than or equal to 3 and their neighbors based on experience, and do not participate in the second distance fusion. point to participate in the second distance fusion), if yes, go to step 3-5, if not, set i=i+1 and return to step 3-3.

Step 3-5: Add the node G _i =(x _i , y _i ) itself and the nodes in the adjacent feature node set into the point set _GR ; determine whether the number of adjacent feature node sets is less than or equal to 1, If yes, count c+1; if i is not equal to the number n of the feature point set G _FP , set i=i+1 and return to step 3-3, otherwise, execute step 3-6.

Step 3-6: Delete and deduplicate the point set _GR (in step 3-5, some feature points are repeatedly added to _GR , so it is necessary to delete duplicates. The method of deduplication is judged by the iterator in C++) , if the number n of the feature point set G _FP is greater than or equal to (the number of point sets G _R - 1) (indicating that only one feature point can participate in the second distance fusion, it is equivalent to not participating), then Update the feature map, bring the feature point set _GFP into step 4 and execute, otherwise execute steps 3-7.

Step 3-7: Add all the remaining nodes from the feature point set _GFP except the point set _GR to the point set _GD (that is, the feature point set that can participate in the second distance fusion), and perform the second distance fusion to delete features point. The process of performing the second distance fusion to delete the feature points is to take the xth node G _Dx of G _D (x=1, 2, ... n ₁ -1, where n ₁ is the number of point sets G _D ), and sequentially Connect with the node G _Dx1 (x1=x+1, x+2,...n ₁ ) in G _D to perform collision detection and Euclidean distance calculation on the map. If the collision detection passes and the distance is less than the artificially set two The distance threshold of the second fusion (in this embodiment, the threshold is 80, which is a little larger than the 50 of the first distance fusion, and 80 is also obtained by dividing the actual number of meters by 0.054), then delete the node G from the feature point set G _FP . _Dx1 until all points in the point set G _D have been detected. After the second distance fusion deletes the feature points, the feature map is updated, and the feature point set G _FP at this time is the final feature point set G _FP ', and the G _FP ' is brought into step 4 and executed.

As shown in Figure 3, r ₁ represents the threshold (50) of the first distance fusion, r ₂ represents the threshold (80) of the second distance fusion, and the cross sign represents the redundant feature points deleted by the second distance fusion , the solid circle represents the final extracted feature points, and the rectangle represents the obstacle area.

Step 4: Use the Euclidean distance to update the environment map to obtain a feature map and complete real-time feature extraction. When opening the terminal, if the degree of representation is complete, use the feature points in fp.txt to update the feature point set; if the degree of representation is incomplete, use the final feature point set G _FP ' obtained in step 3 ' Update the feature point set.

Step 4-1: Initialize the flag grid_not_find=0, which is used to indicate whether the points in the obstacle-free area on the map have found their corresponding feature point numbers, and convert all the node coordinates in the G _FP ' into grid coordinates, Denoted as G _i1 =(x _i1 , y _i1 ) (i1=1, 2,...n, where n is the number of feature point sets G _FP '), the conversion formula is:

Grid coordinate x _i1 = (x coordinate - x coordinate of the origin of the map)/map resolution,

Grid coordinate y _i1 = (y coordinate - y coordinate of the origin of the map)/map resolution.

Step _4-2 : _Traverse all grid points ( _{ik k} , j _k ), initialize count (count is used to count, if (i _k , j _k ) cannot find its corresponding feature point number, then set count=count+1) to 0, and initialize min_dist as map length × map Width; connect the grid points (i _k , j _k ) to the feature node G _i1 in the _GFP ' in order to perform collision detection on the map, if the collision detection passes, calculate the Euclidean distance between the two points dist ₁ , if dist ₁ is less than _{min_dist} , update the corresponding value of grid point (i _k , j _k ) in feature map MF to the number i of feature node and min_dist is updated to the value of dist ₁ ; if the collision detection fails, then Count count+1; until the traversal of all nodes in G _FP ' ends, if count=the number n of G _FP ', then set grid_not_find=1, and exit the loop, and return to step 2 (this process is as long as it is found that there is a If the point and all points in G _FP ' fail to pass the collision detection, then exit the loop to save the amount of calculation, indicating that the degree of representation is not complete enough, and you need to go back to step 2 to continue adding new feature points); if count≠ _GFP ' If the number n is n, repeat step 4-2 until all grid points in the obstacle-free area in the two-dimensional grid map M _p are traversed.

Step 4-3: If grid_not_find is equal to 0, the degree of representation is complete (indicating that all grid points on the map can find the number of its corresponding feature point), let representative be equal to 1, otherwise let representative be equal to 0.

As shown in Figure 4, the dotted line represents the node extracted in the robot path planning, the solid circle and the cross represent the feature points extracted by the first distance fusion, where: the cross represents the redundant feature points deleted by the second distance fusion , the solid circles represent the feature nodes finally extracted after the second distance fusion; through the angular velocity channel and the two distance fusions, the number of feature nodes is significantly reduced.

Step 5: Obtain a feature matrix P _F according to the feature point set _GFP ', generate a multimodal information feature tree of the feature points according to the starting point, the end point and the feature matrix P _F , and obtain a heuristic path on the feature map.

Step 5-1: Select a certain node _{G FPi (} i=1, 2, ... n, where n is the number of feature point sets _G , 2,...n and x≠i) and the node G _FPi for collision detection on the map, if the collision detection passes, the Euclidean distance between the two points is stored in the feature matrix P _F (i, x ), otherwise 0 is stored in _PF (i,x).

Step 5-2: Find the feature node number corresponding to the starting point in the feature map MF, and add it to the multimodal information feature tree _T , complete the initialization of the multimodal information feature tree T, and initialize the set of open nodes as: T _open =T.

Step 5-3: Determine whether the number of open nodes is greater than zero, if yes, go to Step 5-4; if not, go to Step 5-10.

Step 5-4: Extract the first open node t _open , and remove t _open from the open node set T _open .

Step 5-5: Extract the _AOI of the t _open area of interest from the feature map _MF , search for all points with the value t _open in the feature map MF, and record the limit values x _min , x _max , y of the coordinates in all the points _min , y _max , select the square area corresponding to the coordinate limit value as the area of interest AOI.

Step 5-6: Extract all adjacent feature nodes X _new of t _open in the area of interest AOI.

Step 5-7: Determine whether the adjacent feature node set X _new is empty, if it is empty, go to step 5-3; if not, go to step 5-8.

Step 5-8: Take the first node x _new of X _new , find the nearest neighbor x _parent of x _new on the multimodal information feature tree T, and judge whether x _{parent exists} . X _new , and go back to step 5-7; if it exists, go to step 5-9.

Step 5-9: Add x _new to the multimodal information feature tree T and the open node set T _open , remove x _new from X _new , and return to step 5-7.

Step 5-10: Find the feature node corresponding to the end position in the feature map _MF , and find the path from the start point to the end point by reverse searching the multimodal information feature tree T. The original environment map has only two values of 0 and 255, 0 represents the area without obstacles, 255 represents the area with obstacles, and the feature map fills the raster value with the value of 0 in the environment map with the number of its corresponding feature point. Yes, the 255 remains the same. In this way, given an end point, you can quickly find the feature point number filled by the end point in the feature map, and the same is true for the starting point. When the end point is given, take the position of the robot at that time as the starting point, and find the starting point in the feature map. Corresponding feature point numbers, and then use feature points to replace start/end points to generate heuristic paths. The advantage of this is that it only needs to traverse the feature points of the order of ten, instead of traversing all the grid values of the order of hundreds of thousands as in the traditional method, which greatly reduces the complexity of the traversal and ensures the real-time performance of path planning.

Step 5-11: Return to step 2-1, which is used to update the feature point set. When the terminal is closed, the stored feature file is updated to be the adjusted feature point. When the map is opened next time, it can be directly Performing step 5 to generate a heuristic path saves a lot of time compared to performing steps 2 to 4 while finding feature points and updating the feature map.

As shown in Figure 5, given the end point, the real-time position of the robot is used as the starting point, and the candidate nodes are continuously added to the multi-modal information feature tree through the collision detection module to complete the creation of the multi-modal information feature tree. The search tree shown, and then the heuristic path shown by the shadow.

Beneficial effects of the present invention:

(1) Real-time feature point extraction is used to characterize the environmental structure, which not only solves the shortcomings of traditional GVD that need to extract feature points in advance offline, but also greatly saves time and cost. The feature point information is saved in the file, and checked and updated before and after the path planning, which occupies a small memory and solves the problem of node reuse and saving computing resources. After the extraction and creation are completed, it can be reused in the path planning of different starting points and ending points. , which is equivalent to giving the robot the function of "brain memory".

(2) The present invention expresses the map more concisely by extracting feature points. The expressed feature map retains the structural information of the environment and removes redundant information. In the search process of path planning, it is only necessary to traverse the feature points. , instead of traversing all map grid points, which greatly reduces the complexity of traversal and ensures real-time path planning.

(3) On the basis of the Risk-RRT motion planning method, a multi-modal information feature tree is generated. The multi-modal information feature tree is composed of feature points extracted based on nodes at all times of Risk-RRT, and the feature points do not contain timestamps. information. It is not only suitable for dynamic and complex environments, but also the nodes are concise and representative, and can represent the structural information of the environment. Given the starting point and the end point, a heuristic path can be quickly generated, and the robot can perform motion planning in sequence according to the sub-goals on the heuristic path, and finally reach the target point, which solves the trap space problem in the motion planning process based on random sampling.

The above-mentioned embodiments are only preferred embodiments for fully illustrating the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or transformations made by those skilled in the art on the basis of the present invention are all within the protection scope of the present invention. The protection scope of the present invention is subject to the claims.

Claims (7)

1. a robot real-time motion planning method based on multimodal information feature tree, is characterized in that, comprises the following steps: Step 1: Obtain the environment map, extract the node pose information of the robot in real time, and determine whether the representation of the feature point to the environment map is complete, if not, go to step 2, and if it is complete, go to step 4; Step 2: Use the angular velocity channel and distance fusion to perform preliminary feature extraction on the node pose of the robot, obtain the initial feature point set, and update the feature map at the same time, including; Step 2-1: Use the angular velocity channel to extract nodes whose angular velocity is greater than the preset threshold, denoted as G _new =(x _n , y _n ); Step 2-2: If the degree of characterization is incomplete, go to step 2-3; if the degree of characterization is complete, go to step 2-5; Step 2-3: Determine whether the feature point set G _FP is empty, if it is empty, directly store the node G _new in the feature point set G _FP , and return to step 2-1; if it is not empty, execute step 2-4 ; Step 2-4: Use the Euclidean distance channel to calculate the Euclidean distance between the node G _new and all the nodes G _i =(x _i ,y _i ) in the feature point set G _FP in turn, and determine whether the Euclidean distance is less than or equal to the preset The first distance fusion threshold of , if yes, return to step 2-1; If the distance between node G _new and all nodes in feature point set G _FP is greater than the preset first distance fusion threshold, add node G _new into feature point set G _FP , bring G _FP into step 3 and execute ; Step 2-5: Calculate the Euclidean distance between the node G _new and all nodes G _i =(x _i ,y _i ) in the feature point set G _FP in turn, and determine whether the Euclidean distance is less than or equal to the preset first time Distance fusion threshold, if yes, go to step 2-6, if not, go to step 4; Step 2-6: Perform collision detection between the grid points in the feature map _MF and the node G _new in turn, if all the collision detections pass and the sum of the distances between the grid points in the feature map _MF and the node G _new is smaller than the feature map The _sum of the distances between the grid points in MF and the original feature nodes, then the node G _new replaces the original feature node G _i =(x _i , y _i ), updates the feature map, brings the G _FP into step 4 and executes , otherwise go to step 5; Step 3: Use distance fusion again to further extract the initial feature point set, until the representation degree of the feature points is complete to obtain the optimal feature point, and obtain the final feature point set, which specifically includes: Step 3-1: Initialize the count c to 0, initialize the feature point set _GR to be saved as an empty set; extract the adjacent feature nodes of each node G _i =(x _i ,y _i ) in the feature point set _GFP gather; Step 3-2: Initialize i to 1; Step 3-3: Determine whether i is less than or equal to the number n of the feature point set G _FP , if so, go to step 3-4, if not, go to step 3-6; Step 3-4: Determine whether the number of adjacent feature node sets of the i-th node is less than or equal to 3, if so, go to step 3-5, if not, set i=i+1 and return to step 3-3; Step 3-5: Add the node G _i =(x _i , y _i ) itself and the nodes in the adjacent feature node set into the point set _GR ; determine whether the number of adjacent feature node sets is less than or equal to 1, If so, count c+1; if i is not equal to the number n of the feature point set G _FP , then set i=i+1 and return to step 3-3, otherwise perform step 3-6; Step 3-6: Perform deduplication processing on the point set _GR , if the number n of the feature point set G _FP is greater than or equal to the number n of the point set _GR -1, update the feature map, and bring the feature point set G _FP into Step 4 and execute, otherwise execute steps 3-7; Step 3-7: Add all the remaining nodes from the feature point set _GFP except the point set _GR to the point set _GD , perform the second distance fusion to delete the feature points, and update the feature map. At this time, the feature point set _GFP That is, the final feature point set G _FP ', bring G _FP ' into step 4 and execute; Step 4: Use the Euclidean distance to update the environment map, obtain the feature map, and complete the real-time feature extraction; Step 5: Obtain a feature matrix according to the feature point set, generate a multimodal information feature tree of the feature points according to the starting point, the ending point and the feature matrix, and obtain a heuristic path on the feature map, including: Step 5-1: Select a node G _FPi in G _FP ', and check the connection between the remaining nodes G _FPx and node G _FPi on the map for collision detection. The distance is stored in the feature matrix P _F (i, x), otherwise 0 is stored in P _F (i, x); Step 5-2: Find the feature node number corresponding to the starting point in the feature map MF, and add it to the multimodal information feature tree _T , complete the initialization of the multimodal information feature tree T, and initialize the set of open nodes as: T _open = T; Step 5-3: determine whether the number of open nodes is greater than zero, if so, go to step 5-4; if not, go to step 5-10; Step 5-4: extract the first open node t _open , and remove t _open from the open node set T _open ; Step 5-5: Extract the _AOI of the _open area of interest from the feature map MF; Step 5-6: Extract all adjacent feature nodes X _new of t _open in the area of interest AOI; Step 5-7: Determine whether the adjacent feature node set X _new is empty, if it is empty, go to step 5-3; if it is not empty, go to step 5-8; Step 5-8: Take the first node x _new of X _new , find the nearest neighbor x _parent of x _new on the multimodal information feature tree T, and judge whether x _{parent exists} . Remove from X _new , and return to step 5-7; if it exists, execute step 5-9; Step 5-9: add x _new to the multimodal information feature tree T and the open node set T _open , remove x _new from X _new , and return to step 5-7; Step 5-10: Find the feature node corresponding to the end position in the feature map MF, and find the path from the start point to the end point by reverse searching the multimodal information feature tree _T ; Step 5-11: Return to step 2-1. 2. the robot real-time motion planning method based on the multimodal information feature tree according to claim 1, is characterized in that: in the described step 1, the method for judging whether the characterization degree of the feature point to this environmental map has been complete is that each Before opening the terminal for path planning, check whether there is a corresponding feature file in this environment map. If there is a corresponding feature file, judge whether the characterization degree is complete through the feature points in the feature file. If there is no corresponding feature file, the characterization degree Incomplete; when the robot completes the path planning on the environment map and closes the terminal, the corresponding feature file of the environment map is generated. 3. the robot real-time motion planning method based on multimodal information feature tree according to claim 2, is characterized in that, the concrete process of described step 1 is: Step 1-1: Given a navigation target, the robot starts path planning based on the Risk-RRT algorithm, and obtains the node pose information G _m =(x _m , y _m , ω _m ) of the robot in the process of path planning in real time, where x , y represents the position of the node, and ω represents the angular velocity of the node; Step 1-2: Determine whether the feature file exists every time you open the terminal, and calculate the ratio of the number of corresponding feature point numbers to all the points in the barrier-free area by calculating the points in the barrier-free area on the map through the feature file. representative, used to represent the degree of representation; If there is no feature file, set representative=0, initialize the feature map _MF as a two-dimensional zero matrix of the same size as the environment map, initialize the feature point set _GFP as an empty set, and execute step 2; If there is a feature file but the degree of representation is incomplete, store the node G _new = (x _n , y _n ) stored in the feature file into the feature point set _GFP , and execute step 2; If the feature file exists and the degree of representation is complete, store the node G _new =(x _n , _yn ) stored in the feature file into the feature point set G _FP , and execute step 4 . 4. the robot real-time motion planning method based on multimodal information feature tree according to claim 1, is characterized in that: when using angular velocity channel in described step 2-1 to extract the node that angular velocity is greater than preset threshold, every A certain number of interval extraction nodes are used to reduce the redundancy of nodes. 5. the robot real-time motion planning method based on multimodal information feature tree according to claim 1, is characterized in that: the concrete method of described distance fusion is: Take the node G _x in the point set, connect it with the remaining nodes G _x1 in the point set, and perform collision detection and Euclidean distance calculation on the map. If the collision detection passes and the distance is less than the artificially set distance fusion distance threshold, then from The node G _x1 is deleted from the point set until all points in the point set have been detected. 6. the robot real-time motion planning method based on multimodal information feature tree according to claim 1, is characterized in that: the concrete process of described step 4 is: Step 4-1: The initialization flag bit is 0, which is used to indicate whether the points in the obstacle-free area on the map have found their corresponding feature point numbers, and convert all the node coordinates in the G _FP ' into grid coordinates, and record Let G _i1 =(x _i1 , y _i1 ); Step 4-2: Traverse all grid points (i _k , j _k ) in the obstacle-free area in the two-dimensional grid map MP, initialize count to 0, and initialize _{min_dist} to map length × map width; i _k , j _k ) are sequentially connected with the feature node G _i1 in the _GFP ' to perform collision detection on the map. If the collision detection passes, the Euclidean distance dist ₁ between the two points is calculated. If dist ₁ is less than min_dist, Then in the feature map MF, the corresponding value of the grid point ( _ik , _jk ) is updated to the number i of the feature node and _{min_dist} is updated to the value of dist ₁ ; if the collision detection fails, count count+1; until G The traversal of all nodes in _FP ' ends, if count=the number n of G _FP ', set the flag bit to 1, and exit the loop, and return to step 2; if count ≠ the number n of G _FP ', repeat step 4 -2 until all grid points in the obstacle-free area in the two-dimensional grid map M _p are traversed, and the feature map M _F is obtained; Step 4-3: If the flag bit is equal to 0, the representation degree is complete, and let the representative be equal to 1, otherwise, let the representative be equal to 0. 7. the robot real-time motion planning method based on multimodal information feature tree according to claim 1, is characterized in that: in described step 5-5, the concrete method that extracts the interest area _AOI of t _open from feature map MF for, In the feature map _MF , search for all points whose value is t _open , record the limit values x _min , x _max , y _min , and y _max of the coordinates in all points, and select the square area corresponding to the coordinate limit value as the area of interest AOI.

Images