Robot escape method
By generating and filtering a collection of motion primitives, and generating reference paths based on the robot position and target position, the problem of robot being trapped during the cleaning process is solved, safe and effective escape paths are achieved, and cleaning efficiency is improved.
Patent Information
- Application Number
- CN202411215956.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-02
- Publication Date
- 2024-12-27
AI Technical Summary
The robot is trapped because it is too close to an obstacle during the cleaning process, and the path planner finds it difficult to plan effective paths, resulting in difficulties in getting out of difficulties.
By generating the first set of motion primitives, performing coordinate transformation and filtering, a set of motion primitives that do not collide with obstacles, and generating a reference path based on the robot position and target position, and computing the motion primitive with the highest integral as the escape path.
Effectively help the robot get out of trouble, ensure safe paths and avoid collisions with obstacles, improving the robot's cleaning efficiency in complex environments.
Smart Images
Figure CN119197519A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the technical field of intelligent robots, and in particular to a method for a robot to escape from trouble. Background Art
[0002] In actual engineering applications, the robot may get too close to obstacles for some reasons (for example, to ensure that it can clean most of the area). In this case, it is difficult for the path planner to plan an effective path, and the robot may be trapped. Summary of the invention
[0003] The present application provides a robot escape method, which is used to solve the technical problem of how to get the robot out of trouble.
[0004] The embodiment of the present application is implemented as follows:
[0005] A robot escape method comprises the following steps: 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 primitives that meet a 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, calculating the first integral of all motion primitives in the third motion primitive set, and taking the motion primitive with the highest first integral as the escape path. The robot moves along the selected escape path to escape.
[0006] The specific method of generating the first motion primitive set includes:
[0007] S11, based on the two-dimensional workspace, uniformly set a plurality of first discrete points (X u , Y u ,θ u ), create a raster map; where X u is the horizontal coordinate of the first discrete point, Y u is the ordinate of the first discrete point, θ u is the angle between the robot's orientation at the first discrete point and the X axis; u ∈[0,g),Y u ∈[0,h),θ u ∈[-π,π], g and h are both positive;
[0008] S12, based on the cubic spline, the origin (0, 0, 0) is respectively connected to each first discrete point (X u , Y u ,θ u ) are connected to obtain multiple motion primitives, and any motion primitive is composed of multiple second discrete points; the expression of the motion primitive is as follows:
[0009] k(s)=j+ls+ts 2 +qs 3 ;
[0010] Wherein, s represents the length of the arc path between a second discrete point of a certain motion primitive and the origin (0, 0, 0), k(s) is a certain motion primitive, j is a constant, l is the coefficient of the first-order term, t is the coefficient of the second-order term, and q is the coefficient of the third-order term;
[0011] 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:
[0012]
[0013] k(s)′=l+2ts+3qs 2 ;
[0014] k(s)″=2t+6qs;
[0015] Where cur(s) is the curvature at the second discrete point of the motion primitive;
[0016] S14, forming a fourth motion primitive set from all motion primitives that meet the first condition;
[0017] S15, initializing the fourth motion primitive set to generate the first motion primitive set.
[0018] 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.
[0019] The specific method for initializing the fourth motion primitive set and generating the first motion primitive set is: based on the fourth motion primitive set, the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant are obtained by utilizing the symmetry of the X-axis and the Y-axis; the fourth motion primitive set and the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant together constitute the first motion primitive set.
[0020] 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:
[0021] 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;
[0022] The motion primitives obtained by coordinate transformation of all the motion primitives in the first motion primitive set constitute the second motion primitive set.
[0023] The specific formula for coordinate transformation is as follows:
[0024] X m =(X n -X r )cos(θ r -θ n )-(Y n -Y r )sin(θ r -θ n )+X r ;
[0025] Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ;
[0026] θ m =θ r -θ n ;
[0027] Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when it is trapped and chooses a path to escape. r Y is the horizontal coordinate of the robot's position when it is trapped and chooses a path to escape. r is the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0028] 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:
[0029] 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;
[0030] 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;
[0031] The specific formula for the transformation is as follows:
[0032] X m =(X n -X r )cos(θ r -θ n )-(Y n -Y r )sin(θ r -θ n )+X r ;
[0033] Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ;
[0034] θ m =θ r -θ n ;
[0035] Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when the robot is trapped and chooses a way to escape, Xr is the horizontal coordinate of the robot's position when the robot is trapped and chooses a way to escape, Y ris the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0036] The third condition is that all second discrete points constituting a certain motion primitive are within the second grid map.
[0037] The following specific method can be used to create the second grid map: based on the first grid map, the symmetry of the X-axis and the Y-axis is used to obtain the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant, and 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 together constitute the second grid map.
[0038] The motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
[0039] To determine whether a moving container 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.
[0040] 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 selecting an escape path.
[0041] The calculation method of the first integral of the motion primitives in the third motion primitive set is as follows:
[0042] score(p i )=λ1*dist(p i , ref m )+λ2*angle(p i , ref m );
[0043] Among them, score(p i ) is the end point p i The first integral of the motion primitive; dist(p i , ref m ) is the end point p i The distance from the end point p among all the points constituting the reference path i The Euclidean distance between the closest points; angle(p i , ref m ) is the end point p i The distance between all points on the reference path and the end point p iThe angular difference between the nearest points; λ1 represents the distance weight and λ2 represents the angle weight.
[0044] A certain end point p i The distance from the end point p among all the points constituting the reference path i The angular difference between the closest points can be the robot at the end point p i The angle between the orientation and the X axis and the distance from the robot to the end point p among all the points constituting the reference path i The angle difference between the direction of the nearest point and the X-axis.
[0045] The point can be determined by the horizontal coordinate of a point, the vertical coordinate of the point, and the angle between the robot's orientation at the point and the X-axis. i The distance from the end point p among all the points constituting the reference path i The angular difference between the closest points can be the robot at the end point p i The angle between the orientation and the X axis and the distance from the robot to the end point p among all the points constituting the reference path i The angle difference between the direction of the nearest point and the X-axis.
[0046] A robot escape method of the present application is suitable for escaping a robot from a distress state. BRIEF DESCRIPTION OF THE DRAWINGS
[0047] 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.
[0048] Figure 1 A flowchart of a robot escape method provided in an embodiment of the present application.
[0049] Figure 2 Schematic diagram of a square robot being trapped in the robot escape method provided in an embodiment of the present application. DETAILED DESCRIPTION
[0050] 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.
[0051] 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.
[0052] 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.
[0053] 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.
[0054] In the description of the embodiments of the present application, "plurality" means at least 2.
[0055] 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.
[0056] like Figure 2 As shown in the figure, in order to obtain a better cleaning effect, the square robot (robot) is too close to the wall during cleaning, causing itself to be trapped in the corner. In this case, the robot escape method in this application can be used to escape. Figure 1 As shown, a robot escape method provided by an embodiment of the present application includes:
[0057] S1, generating a first motion primitive set;
[0058] S2, performing coordinate transformation on all motion primitives in the first motion primitive set to obtain a second motion primitive set;
[0059] S3, forming a third motion primitive set from all motion primitives in the second motion primitive set that meet the second condition;
[0060] 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;
[0061] S5, 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.
[0062] The robot can escape from trouble by moving along the selected escape path.
[0063] As a preferred implementation, the specific method for generating the first motion primitive set includes:
[0064] S11, based on the two-dimensional workspace, uniformly set a plurality of first discrete points (X u , Y u ,θ u ), create the first grid map; where X u is the horizontal coordinate of the first discrete point, Y u is the ordinate of the first discrete point, θ u is the angle between the robot's orientation at the first discrete point and the X axis; u ∈[0,g),Y u ∈[0,h),θ u ∈[-π, π], g and h are both positive values, and the sizes of g and h can be selected according to business requirements. For example, g and h can be set to 1m;
[0065] S12, based on the cubic spline, the origin (0, 0, 0) is respectively connected to each first discrete point (X u , Y u ,θ u ) are connected to obtain multiple motion primitives, and any motion primitive is composed of multiple second discrete points; the expression of the motion primitive is as follows:
[0066] k(s)=j+ls+ts 2 +qs 3 ;
[0067] Wherein, s represents the length of the arc path between a second discrete point of a certain motion primitive and the origin (0, 0, 0), k(s) is a certain motion primitive, j is a constant, l is the coefficient of the first-order term, t is the coefficient of the second-order term, and q is the coefficient of the third-order term;
[0068] 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:
[0069]
[0070] k(s)′=l+2ts+3qs 2 ;
[0071] k(s)″=2t+6qs;
[0072] Where cur(s) is the curvature at the second discrete point of the motion primitive;
[0073] S14, forming a fourth motion primitive set from all motion primitives that meet the first condition;
[0074] S15, initializing the fourth motion primitive set to generate the first motion primitive set.
[0075] As a preferred implementation, 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.
[0076] As a preferred implementation, the specific method of initializing the fourth motion primitive set and generating the first motion primitive set is: according to the fourth motion primitive set, the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant are obtained by using the symmetry of the X-axis and the Y-axis, and the fourth motion primitive set and the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant together constitute the first motion primitive set. It is worth noting that this step will only be initialized once, that is, it is executed when constructing the relevant class.
[0077] As a preferred implementation, 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:
[0078] 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;
[0079] For example: the A motion primitive 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 B motion primitive obtained after the coordinate transformation of the A motion primitive in the first motion primitive set is composed of three points B1, B2, and B3.
[0080] The motion primitives obtained by coordinate transformation of all the motion primitives in the first motion primitive set constitute the second motion primitive set.
[0081] The specific formula for coordinate transformation is as follows:
[0082] X m =(X n -X r )cos(θ r -θ n )-(Y n -Yr )sin(θ r -θ n )+X r ;
[0083] Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ;
[0084] θ m =θ r -θ n ;
[0085] Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when it is trapped and chooses a path to escape. r Y is the horizontal coordinate of the robot's position when it is trapped and chooses a path to escape. r is the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0086] 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:
[0087] 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;
[0088] 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.
[0089] The specific formula for coordinate transformation is as follows:
[0090] X m =(X n -X r )cos(θ r -θ n )-(Y n -Y r )sin(θ r -θ n )+X r ;
[0091] Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ;
[0092] θ m =θ r -θ n ;
[0093] Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when it is trapped and chooses a path to escape. r Y is the horizontal coordinate of the robot's position when it is trapped and chooses a path to escape. r is the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
[0094] As a preferred implementation, the third condition is that all second discrete points constituting a certain motion primitive are within the second grid map.
[0095] As a preferred implementation method, the following specific method can be used to create the second grid map: based on the first grid map, the symmetry of the X-axis and the Y-axis is used to obtain the grid map of the second quadrant, the grid map of the third quadrant and the grid map of the fourth quadrant, and 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 together constitute the second grid map.
[0096] As a preferred implementation manner, the motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
[0097] 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.
[0098] 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 selecting an escape path.
[0099] As a preferred implementation, the specific method for calculating the first integral of the motion primitives in the third motion primitive set is:
[0100] score(p i )=λ1*dist(p i , ref m )+λ2*angle(p i , ref m );
[0101] Among them, score(p i ) is the end point p i The first integral of the motion primitive; dist(p i , ref m ) is the end point p i The distance from the end point p among all the points constituting the reference path i The Euclidean distance between the closest points; angle(p i , ref m ) is the end point p i The distance from the end point p among all the points constituting the reference path i The angular difference between the nearest points; λ1 is the distance weight and λ2 is the angle weight.
[0102] The point can be determined by the horizontal coordinate of a point, the vertical coordinate of the point, and the angle between the robot's orientation at the point and the X-axis.i The distance from the end point p among all the points constituting the reference path i The angular difference between the closest points can be the robot at the end point p i The angle between the orientation and the X axis and the distance from the robot to the end point p among all the points constituting the reference path i The angle difference between the direction of the nearest point and the X-axis.
[0103] A robot escape method provided in an embodiment of the present application can effectively help a robot (especially a square robot) escape from trouble.
[0104] 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.
[0105] 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 robot escape method, 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, 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.
2. A robot escape method according to claim 1, characterized in that: The specific method of generating the first motion primitive set includes: S11, based on the two-dimensional workspace, uniformly set a plurality of first discrete points (X u , Y u ,θ u ), create the first raster map; Among them, X u is the horizontal coordinate of the first discrete point, Y u is the ordinate of the first discrete point, θ u is the angle between the robot's orientation at the first discrete point and the X axis; u ∈[0,g),Y u ∈[0,h),θ u ∈[-π,π], g and h are both positive; S12, based on the cubic spline, the origin (0, 0, 0) is respectively connected to each first discrete point (X u , Y u ,θ u ) to obtain a plurality of motion primitives, wherein any motion primitive is composed of a plurality of second discrete points; The expression of the motion primitive is as follows: k(s)=j+ls+ts 2 +qs 3 ; Wherein, s represents the length of the arc path between a second discrete point of a certain motion primitive and the origin (0, 0, 0), k(s) is a certain motion primitive, j is a constant, l is the coefficient of the first-order term, t is the coefficient of the second-order term, and q is the coefficient of the third-order term; S13, calculating the curvatures at all second discrete points of the motion primitive; The curvature calculation formula of the second discrete point of the motion primitive is as follows: k(s)′=l+2ts+3qs 2 ; k(s)″=2t+6qs; Where cur(s) is the curvature at the second discrete point of the motion primitive; S14, forming a fourth motion primitive set from the motion primitives that meet the first condition; S15, initializing the fourth motion primitive set to generate the first motion primitive set.
3. A robot escape method according to claim 2, characterized in that: The first condition means that the curvatures at all second discrete points of the motion primitive are less than or equal to the maximum curvature of the robot.
4. A robot escape method according to claim 2, characterized in that: The specific method of initializing the fourth motion primitive set to generate the first motion primitive set is: based on the fourth motion primitive set, the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant are obtained by utilizing the symmetry of the X-axis and the Y-axis, and the first motion primitive set is formed by the fourth motion primitive set and the motion primitives of the second quadrant, the motion primitives of the third quadrant and the motion primitives of the fourth quadrant.
5. A robot escape method according to claim 1, 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: X m =(X n -X r )cos(θ r -θ n )-(Y n -Y r )sin(θ r -θ n )+X r ; Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ; i m =θ r -θ n ; Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when the robot is trapped and chooses a way to escape, Xr is the horizontal coordinate of the robot's position when the robot is trapped and chooses a way to escape, Y r is the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
6. A robot escape method according to claim 1, 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: X m =(X n -X r )cos(θ r -θ n )-(Y n -Y r )sin(θ r -θ n )+X r ; Y m =(X n -X r )sin(θ r -θ n )+(Y n -Y r )cos(θ r -θ n )+X r ; i m =θ r -θ n ; Among them, (X n , Y n ,θ n ) is the coordinate of a point constituting a certain motion primitive in the first motion primitive set before coordinate transformation, X n is the horizontal coordinate of the point, Y n is the ordinate of the point, θ n is the angle between the robot's orientation at that point and the X-axis; (X r , Y r ,θ r ) is the coordinate of the robot's position when the robot is trapped and chooses a way to escape, Xr is the horizontal coordinate of the robot's position when the robot is trapped and chooses a way to escape, Y r is the vertical coordinate of the robot's position when it is trapped and chooses a path to escape, θ r is the angle between the robot's orientation at this position and the X-axis; (X m , Y m ,θ m ) are the coordinates of a point obtained after coordinate transformation of a point constituting a certain motion primitive in the first motion primitive set.
7. A robot escape method according to claim 6, characterized in that: The third condition is that all second discrete points constituting a certain motion primitive are within the second grid map.
8. A robot escape method according to claim 7, characterized in that: The second grid map is obtained by the following method: Based on the first grid map, the grid maps of the second quadrant, the grid maps of the third quadrant and the grid maps of the fourth quadrant are obtained by utilizing the symmetry of the X-axis and the Y-axis. The second grid map is constituted by the first grid map and the grid maps of the second quadrant, the grid maps of the third quadrant and the grid maps of the fourth quadrant.
9. A robot escape method according to claim 1, characterized in that: The motion primitive that meets the second condition refers to a motion primitive that will not collide with an obstacle.
10. A robot escape method according to claim 1, characterized in that: The specific method for calculating the first integral of all motion primitives in the third motion primitive set is: score(p i )=λ1*dist(p i ,refm)+λ2*angle(p i ,refm); Among them, score(p i ) is the end point p i The first integral of the motion primitive; dist(p i , refm) is the end point p i The distance from the end point p among all the points constituting the reference path i The Euclidean distance between the closest points; angle(p i , ref m ) is the end point p i The distance from the end point p among all the points constituting the reference path i The angular difference between the nearest points; λ1 is the distance weight and λ2 is the angle weight.