Method for generating distance map and escape method based on distance map
Through the methods of distance map generation and escape path calculation, the problem of intelligent robots being trapped in complex scenarios is solved, and efficient escape path generation is achieved, which is suitable for intelligent robots in complex scenarios.
Patent Information
- Application Number
- CN202411215958.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-02
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2044-09-02
AI Technical Summary
Intelligent robots are easily trapped in complex scenarios, and the existing methods of getting out of trouble are inefficient, making it difficult to effectively solve the problem of robots getting out of trouble.
The distance map generation method is used to map the reference path to the raster map, fill the raster by comparing the distance between points, generate a distance map, and calculate the escape path based on the map, using the A* algorithm and cubic spline motion primitives The collection generates a path to escape.
It effectively helps the robot to get out of trouble, improves the efficiency and accuracy of escape, and is especially suitable for square robots in complex scenarios.
Smart Images

Figure CN118730122B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of intelligent robot technology, and in particular, to a method for generating a distance map and a method for escaping from a predicament based on the distance map. Background Art
[0002] With the continuous improvement of living standards, smart homes have gradually been integrated into people's daily lives, and the presence of robots has also increased day by day. They have emerged in many channels such as catering and cleaning, providing more convenience for people's lives. Whether it is a catering environment with a large flow of people or a family scene, the working scenes of robots are mostly complex and changeable, which makes the new types of obstacles in the working scenes of robots diverse, making them often trapped in a certain area during work. Summary of the invention
[0003] Some robot escape methods require the use of distance maps. This application provides a method for generating a distance map, including the following steps:
[0004] Mapping the reference path into the first map to obtain a mapping of the reference path in the first map;
[0005] The size of the first map is the same as the size of the second grid map;
[0006] Fill the grid where the mapped points constituting the reference path in the first map are located with the indexes of the mapped points constituting the reference path in the first map;
[0007] Compare the distance between a point of the mapping that does not belong to the reference path in the first map and all the points of the mapping that constitute the reference path in the first map, and fill the index of the point among all the points of the mapping that constitute the reference path in the first map that is closest to the point of the mapping that does not belong to the reference path in the first map into the grid where the point of the mapping that does not belong to the reference path in the first map is located; fill all the grids in the first map according to this rule, and the resulting map is the distance map.
[0008] A reference path is generated based on the robot's position when the robot selects an escape path after being trapped and the robot's target position.
[0009] The A* algorithm (i.e., A star algorithm) can be used to generate a reference path based on the robot's position when the robot is trapped and the robot's target position when it chooses an escape path.
[0010] The following specific methods can be used to create a second grid map:
[0011] Based on the two-dimensional workspace, multiple first discrete points are evenly set , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axis; , , θ u ∈[-π , π] , and All are positive values;
[0012] Based on the first grid map, using Axis and The symmetry of the axis obtains the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant, and the second grid map is formed by the first grid map and the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant.
[0013] The present application also provides a method for escaping from a difficult situation based on a distance map, comprising the following steps:
[0014] S1, generating a first motion primitive set;
[0015] S2, performing coordinate transformation on all motion primitives in the first motion primitive set to obtain a second motion primitive set;
[0016] S3, forming a third motion primitive set from all motion primitives in the second motion primitive set that meet the second condition;
[0017] S4, generating a reference path based on the position of the robot when the robot is trapped and the target position of the robot when selecting an escape path;
[0018] S5, generating a distance map;
[0019] S6, calculating the first integrals of all motion primitives in the third motion primitive set, and taking the motion primitive with the highest first integral as the escape path.
[0020] The robot can escape from trouble by moving along the selected escape path.
[0021] The specific method of generating the first motion primitive set includes:
[0022] S11, based on the two-dimensional workspace, evenly set multiple first discrete points , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axis; , , θ u ∈[-π , π] , and All are positive values;
[0023] S12, based on cubic spline, the origin With each first discrete Connect to obtain multiple motion primitives, any motion primitive is composed of multiple second discrete points; the expression of the motion primitive is as follows:
[0024] ;
[0025] in, Represents a second discrete point of a motion primitive and the origin The length of the arc path between is a motion primitive, is a constant, is the coefficient of the first-order term, is the coefficient of the quadratic term, is the cubic coefficient;
[0026] S13, calculating the curvatures at all second discrete points of the motion primitives; the curvature calculation formula at the second discrete points of the motion primitives is as follows:
[0027] ;
[0028] ;
[0029] ;
[0030] in, is the curvature at the second discrete point of the motion primitive;
[0031] S14, forming a fourth motion primitive set from all motion primitives that meet the first condition;
[0032] S15, initializing the fourth motion primitive set to generate the first motion primitive set.
[0033] The first condition is that the curvatures at all the second discrete points of the motion primitive are less than or equal to the maximum curvature of the robot; the maximum curvature of the robot may be preset.
[0034] The specific method of initializing the fourth motion primitive set and generating the first motion primitive set is as follows: according to the fourth motion primitive set, using Axis and The symmetry of the axis obtains the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant. The fourth motion primitive set together with the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant constitute the first motion primitive set.
[0035] The following specific method can be used to perform coordinate transformation on all motion primitives in the first motion primitive set to obtain the second motion primitive set:
[0036] Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive;
[0037] The motion primitives obtained by performing coordinate transformation on all the motion primitives in the first motion primitive set constitute the second motion primitive set.
[0038] The specific formula for coordinate transformation is as follows:
[0039] ;
[0040] ;
[0041] ;
[0042] in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axis; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axis; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0043] In some other embodiments of the present application, coordinate transformation is performed on all motion primitives in the first motion primitive set to obtain the second motion primitive set. The following specific method may also be used:
[0044] Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive;
[0045] All motion primitives satisfying the third condition among the motion primitives obtained after coordinate transformation of all the motion primitives in the first motion primitive set constitute a second motion primitive set.
[0046] The specific formula for coordinate transformation is as follows:
[0047] ;
[0048] ;
[0049] ;
[0050] in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axis; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axis; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0051] The third condition is that all second discrete points constituting a certain motion primitive are within the second grid map.
[0052] The motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
[0053] To determine whether a motion primitive will collide with an obstacle, methods such as the circumscribed circle collision detection algorithm or the separating axis theorem collision detection algorithm can be used.
[0054] The A* algorithm (i.e., A star algorithm) can be used to generate a reference path based on the robot's position when the robot is trapped and the robot's target position when it chooses an escape path.
[0055] The calculation method of the first integral of the motion primitives in the third motion primitive set is as follows:
[0056] ;
[0057] in, The end point is The first integral of the motion primitives; For this end point The distance from the end point to all points that constitute the reference path Euclidean distance between the closest points; For this end point The distance from the end point to all points that constitute the reference path The angular difference between the closest points; is the distance weight, is the angle weight.
[0058] End point The distance from the end point to all points that constitute the reference path The Euclidean distance between the closest points is obtained with the help of the generated distance map.
[0059] The horizontal coordinate of a point, the vertical coordinate of the point, and the direction of the robot at the point can be used to The angle between the axes determines the point. The distance from the end point to all points that constitute the reference path The angular difference between the closest points can be the robot's The direction and The angle between the axes and the distance of the robot from the end point among all the points that make up the reference path The direction of the nearest point is The angular difference between the angles of the axes.
[0060] Compared with the prior art, the embodiments of the present application have at least the following advantages or beneficial effects:
[0061] The distance map generation method and the escape method based on the distance map of the present application can effectively help the robot escape from trouble. BRIEF DESCRIPTION OF THE DRAWINGS
[0062] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.
[0063] Figure 1 A flowchart of a method for generating a distance map provided in an embodiment of the present application.
[0064] Figure 2 Schematic diagram of a square robot being trapped in the distance map-based escape method provided in an embodiment of the present application.
[0065] Figure 3 A flow chart of a method for escaping distress based on a distance map provided in an embodiment of the present application.
[0066] Figure 4 A map obtained by filling the index of the mapped point of the reference path into the first map in the distance map generation method provided in the embodiment of the present application.
[0067] Figure 5 It is a distance map generated in the distance map generation method provided in the embodiment of the present application. DETAILED DESCRIPTION
[0068] In order to make the purpose, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments.
[0069] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.
[0070] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.
[0071] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.
[0072] In the description of the embodiments of the present application, "plurality" means at least 2.
[0073] Some embodiments of the present application are described in detail below in conjunction with the accompanying drawings. In the absence of conflict, the following embodiments and the various features in the embodiments can be combined with each other.
[0074] like Figure 1 As shown, an embodiment of the present application provides a method for generating a distance map, comprising the following steps:
[0075] The reference path is mapped into the first map to obtain a mapping of the reference path in the first map; the size of the first map is the same as the size of the second grid map.
[0076] The grid where the mapped points constituting the reference path in the first map are located is filled with the indexes of the mapped points constituting the reference path in the first map.
[0077] For example, Figure 4 As shown, the mapped points of the reference path are "W1", "W2", "W3", "W4" (not shown in the figure), and the corresponding indexes are "0", "1", "2", "3", respectively. Then the corresponding indexes are filled into the grids where the mapped points constituting the reference path are located in the first map. The number "0" is filled into the grid where the mapped point "W1" of the reference path is located, the number "1" is filled into the grid where the mapped point "W2" of the reference path is located, the number "2" is filled into the grid where the mapped point "W3" of the reference path is located, and the number "3" is filled into the grid where the mapped point "W4" of the reference path is located.
[0078] Compare the distance between a point of the mapping that does not belong to the reference path in the first map and all the points of the mapping that constitute the reference path in the first map, and fill the grid where the point of the mapping that does not belong to the reference path in the first map is located with the index of the point closest to the point of the mapping that does not belong to the reference path in the first map among all the points of the mapping that constitute the reference path in the first map.
[0079] For example, Figure 5 As shown, the point corresponding to the first grid in the upper left corner is V1 (not shown in the figure). For the mapped point V1 that does not belong to the reference path, the distance between point V1 and all the mapped points of the reference path (i.e., point W1, point W2, point W3, and point W4) is compared, and the index "1" of point W2 (in this example, point W2 is closest to point V1) that is closest to the mapped point V1 that does not belong to the reference path in the first map among all the points that constitute the mapping of the reference path in the first map (i.e., point W1, point W2, point W3, and point W4) is filled into the grid where the mapped point V1 that does not belong to the reference path in the first map is located.
[0080] According to the above rules, all the grids in the first map are filled, and the resulting map is the distance map.
[0081] A reference path is generated based on the robot's position when the robot selects an escape path after being trapped and the robot's target position.
[0082] In some embodiments of the present application, an A* algorithm (i.e., A star algorithm) may be used to generate a reference path based on the robot's position when the robot selects an escape path after being trapped and the robot's target position.
[0083] In some embodiments of the present application, the following specific method may be used to create the second grid map:
[0084] Based on the two-dimensional workspace, multiple first discrete points are evenly set , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axis; , , θ u ∈[-π , π] , and All are positive values;
[0085] Based on the first grid map, using Axis and The symmetry of the axis obtains the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant, and the second grid map is formed by the first grid map and the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant.
[0086] An embodiment of the present application also provides a method for escaping from a difficult situation based on a distance map. Figure 2 As shown in the figure, the square robot (robot) is too close to the wall during cleaning in order to achieve a better cleaning effect, causing itself to be trapped in the corner. In this case, the distance map-based escape method in this application can be used to escape. Figure 3 As shown, an embodiment of the present application provides a method for escaping from a difficult situation based on a distance map, comprising:
[0087] S1, generating a first motion primitive set;
[0088] S2, performing coordinate transformation on all motion primitives in the first motion primitive set to obtain a second motion primitive set;
[0089] S3, forming a third motion primitive set from all motion primitives in the second motion primitive set that meet the second condition;
[0090] S4, generating a reference path based on the position of the robot when the robot is trapped and the target position of the robot when selecting an escape path;
[0091] S5, generating a distance map using the distance map generation method provided in the embodiment of the present application;
[0092] S6, calculating the first integrals of all motion primitives in the third motion primitive set, and taking the motion primitive with the highest first integral as the escape path.
[0093] The robot can escape from trouble by moving along the selected escape path.
[0094] In some embodiments of the present application, the specific method of generating the first motion primitive set is:
[0095] S11, based on the two-dimensional workspace, evenly set multiple first discrete points , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axis; , , θ u ∈[-π , π] , and All are positive values;
[0096] S12, based on cubic spline, the origin With each first discrete Connect to obtain multiple motion primitives, any motion primitive is composed of multiple second discrete points; the expression of the motion primitive is as follows:
[0097] ;
[0098] in, Represents a second discrete point of a motion primitive and the origin The length of the arc path between is a motion primitive, is a constant, is the coefficient of the first-order term, is the coefficient of the quadratic term, is the cubic coefficient;
[0099] S13, calculating the curvatures at all second discrete points of the motion primitives; the curvature calculation formula at the second discrete points of the motion primitives is as follows:
[0100] ;
[0101] ;
[0102] ;
[0103] in, is the curvature at the second discrete point of the motion primitive;
[0104] S14, forming a fourth motion primitive set from all motion primitives that meet the first condition;
[0105] S15, initializing the fourth motion primitive set to generate the first motion primitive set.
[0106] The first condition is that the curvatures at all second discrete points of the motion primitive are less than or equal to the maximum curvature of the robot; the maximum curvature of the robot may be preset.
[0107] The specific method of initializing the fourth motion primitive set and generating the first motion primitive set is as follows: according to the fourth motion primitive set, using Axis and The symmetry of the axis obtains the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant. The fourth motion primitive set together with the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant constitute the first motion primitive set.
[0108] In some embodiments of the present application, coordinate transformation is performed on all motion primitives in the first motion primitive set to obtain the second motion primitive set. The following specific method can be used:
[0109] Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive;
[0110] For example: motion primitive A in the first motion primitive set is composed of three points A1, A2, and A3. After the coordinate transformation of point A1, point B1 is obtained. After the coordinate transformation of point A2, point B2 is obtained. After the coordinate transformation of point A3, point B3 is obtained. The three points B1, B2, and B3 constitute motion primitive B obtained after the coordinate transformation of motion primitive A in the first motion primitive set.
[0111] The motion primitives obtained by performing coordinate transformation on all the motion primitives in the first motion primitive set constitute the second motion primitive set.
[0112] The specific formula for coordinate transformation is as follows:
[0113] ;
[0114] ;
[0115] ;
[0116] in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axis; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axis; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0117] In some other embodiments of the present application, coordinate transformation is performed on all motion primitives in the first motion primitive set to obtain the second motion primitive set. The following specific method may also be used:
[0118] Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive;
[0119] All motion primitives satisfying the third condition among the motion primitives obtained after coordinate transformation of all the motion primitives in the first motion primitive set constitute a second motion primitive set.
[0120] The specific formula for coordinate transformation is as follows:
[0121] ;
[0122] ;
[0123] ;
[0124] in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axis; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axis; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0125] The third condition is that all second discrete points constituting a certain motion primitive are within the second grid map.
[0126] The motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
[0127] In some embodiments of the present application, whether a certain motion primitive will collide with an obstacle may be determined by using a circumscribed circle collision detection algorithm or a separating axis theorem collision detection algorithm.
[0128] In some embodiments of the present application, an A* algorithm (i.e., A star algorithm) may be used to generate a reference path based on the robot's position when the robot selects an escape path after being trapped and the robot's target position.
[0129] In some embodiments of the present application, the calculation method of the first integral of the motion primitives in the third motion primitive set is as follows:
[0130] ;
[0131] in, The end point is The first integral of the motion primitives; For this end point The distance from the end point to all points that constitute the reference path Euclidean distance between the closest points; For this end point The distance from the end point to all points that constitute the reference path The angular difference between the closest points; is the distance weight, is the angle weight.
[0132] End point The distance from the end point to all points that constitute the reference path The Euclidean distance between the closest points can be obtained with the help of the generated distance map.
[0133] In some embodiments of the present application, the horizontal coordinate of a point, the vertical coordinate of the point, and the direction of the robot at the point are used. The angle between the axes determines the point.
[0134] A certain end point The distance from the end point to all points that constitute the reference path The angular difference between the closest points can be the robot's The direction and The angle between the axes and the distance of the robot from the end point among all the points that make up the reference path The direction of the nearest point is The angular difference between the angles of the axes.
[0135] The distance map generation method and the escape method based on the distance map of the present application can effectively help robots (especially square robots) escape from trouble.
[0136] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.
[0137] It will be apparent to those skilled in the art that the present application is not limited to the details of the exemplary embodiments described above, and that the present application can be implemented in other specific forms without departing from the spirit or essential features of the present application. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the present application is defined by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims be included in the present application. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.
Claims
1. A method for generating a distance map, characterized in that: The following steps are involved: Mapping the reference path into a first map to obtain a mapping of the reference path in the first map; the size of the first map is the same as the size of the second grid map; Fill the grid where the mapped points constituting the reference path in the first map are located with the indexes of the mapped points constituting the reference path in the first map; Compare the distance between a certain point of the mapping that does not belong to the reference path in the first map and all the points of the mapping that constitutes the reference path in the first map, and fill the index of the point of the mapping that constitutes the reference path in the first map and has the shortest distance to the certain point of the mapping that does not belong to the reference path in the first map into the grid where the certain point of the mapping that does not belong to the reference path in the first map is located; fill all the grids in the first map according to this rule, and the resulting map is the distance map; The second grid map is obtained by the following method: Based on the two-dimensional workspace, multiple first discrete points are evenly set , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axes; , , , and All are positive values; Based on the first grid map, using Axis and The symmetry of the axis obtains the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant, and the second grid map is formed by the first grid map and the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant.
2. The method for generating a distance map according to claim 1, characterized in that: The reference path is generated based on the position of the robot when the robot selects an escape path after being trapped and the target position of the robot.
3. A method for escaping from a difficult situation based on a distance map, characterized in that: The following steps are involved: S1, generating a first motion primitive set; S2, performing coordinate transformation on all motion primitives in the first motion primitive set to obtain a second motion primitive set; S3, forming a third motion primitive set from all motion primitives in the second motion primitive set that meet the second condition; S4, generating a reference path based on the position of the robot when the robot is trapped and the target position of the robot when selecting an escape path; S5, generating a distance map using the distance map generation method described in any one of claims 1 to 2; S6, calculating the first integrals of all motion primitives in the third motion primitive set, and taking the motion primitive with the highest first integral as the escape path.
4. The method for escaping from a difficult situation based on a distance map according to claim 3, characterized in that: The specific method of generating the first motion primitive set includes: S11, based on the two-dimensional workspace, evenly set multiple first discrete points , create the first raster map; where, is the horizontal coordinate of the first discrete point, is the ordinate of the first discrete point, is the orientation of the robot at the first discrete point and The angle of the axes; , , , and All are positive values; S12, based on cubic spline, the origin With each first discrete Connect to obtain multiple motion primitives, any motion primitive is composed of multiple second discrete points; the expression of the motion primitive is as follows: ; in, Represents a second discrete point of a motion primitive and the origin The length of the arc path between is a motion primitive, is a constant, is the coefficient of the first-order term, is the coefficient of the quadratic term, is the cubic coefficient; S13, calculating the curvatures at all second discrete points of the motion primitives; the curvature calculation formula at the second discrete points of the motion primitives is as follows: ; ; ; in, is the curvature at the second discrete point of the motion primitive; S14, forming a fourth motion primitive set from all motion primitives that meet the first condition; S15, initializing the fourth motion primitive set to generate the first motion primitive set; The first condition is that the curvatures at all second discrete points of the motion primitive are less than or equal to the maximum curvature of the robot.
5. The method for escaping from a difficult situation based on a distance map according to claim 4, characterized in that: The specific method of initializing the fourth motion primitive set to generate the first motion primitive set is: according to the fourth motion primitive set, using Axis and The symmetry of the axis obtains the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant. The fourth motion primitive set together with the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant constitute the first motion primitive set.
6. The method for escaping from a difficult situation based on a distance map according to claim 3, characterized in that: The specific method of performing coordinate transformation on all motion primitives in the first motion primitive set to obtain the second motion primitive set includes: Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive; The motion primitives obtained by coordinate transformation of all the motion primitives in the first motion primitive set constitute a second motion primitive set; The specific formula for coordinate transformation is as follows: ; ; ; in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axes; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axes; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
7. The method for escaping from a difficult situation based on a distance map according to claim 3, characterized in that: The specific method of performing coordinate transformation on all motion primitives in the first motion primitive set to obtain the second motion primitive set includes: Performing coordinate transformation on all points constituting a certain motion primitive in the first motion primitive set, and the points obtained after the coordinate transformation of these points constitute the motion primitive obtained after the coordinate transformation of the motion primitive; All motion primitives satisfying the third condition among the motion primitives obtained after coordinate transformation of all motion primitives in the first motion primitive set constitute a second motion primitive set; The specific formula for coordinate transformation is as follows: ; ; ; in, is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, is the horizontal coordinate of the point, is the ordinate of the point, is the orientation of the robot at this point and The angle of the axes; It is the coordinates of the robot's location when it is trapped and chooses a path to escape. It is the horizontal coordinate of the robot's position when it is trapped and choosing an escape path. The vertical coordinate of the robot's position when it is trapped and choosing an escape path. is the orientation of the robot at this position and The angle of the axes; It is the coordinate of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
8. The method for escaping from a difficult situation based on a distance map according to claim 3, characterized in that: The motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
9. The method for escaping from a difficult situation based on a distance map according to claim 3, characterized in that: The calculation method of the first integral of the motion primitive is as follows: ; in, The end point is The first integral of the motion primitives; For this end point The distance from the end point to all points that constitute the reference path Euclidean distance between the closest points; For this end point The distance from the end point to all points that constitute the reference path The angular difference between the closest points; is the distance weight, is the angle weight.
Citation Information
Patent Citations
Method and device for determining occupied grid map
CN112785909A
Multi-target path planning method based on two-dimensional grid map
CN116625395A
