Robot local path planning method, system and readable storage medium
By selecting a path that can avoid obstacles and is closest to the global path in the robot local path planning, and generating local paths through global path downsampling and curve fitting, the problems of path non-smoothness and inefficiency in the existing technology are solved, and the path is ineffective. More efficient and smoother local path planning.
Patent Information
- Application Number
- CN202110939144.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-08-16
- Publication Date
- 2024-12-03
- Estimated Expiration
- 2041-08-16
AI Technical Summary
Existing robot local path planning methods, such as RRT and lattice algorithms, can easily lead to path collisions with obstacles, paths not smooth, and inefficient, affecting the stability and real-time performance of robots.
By selecting the path that can avoid obstacles and is closest to the global path from the initial local path, and generating local paths through global path downsampling, translation, and curve fitting, filtering the optimal path using obstacle information, and directly generating locals from the global path Path, avoid re-searching of surrounding environments.
The generated local path is closer to the optimal path, has higher smoothness, faster computing efficiency, and has strong real-time performance, avoiding the risk of path oscillation and collision.
Smart Images

Figure CN113721608B_ABST
Abstract
Description
Technical Field
[0001] The present invention generally relates to the field of artificial intelligence. More specifically, the present invention relates to a robot local path planning method, system and computer readable storage medium. Background Art
[0002] With the development of artificial intelligence technology, robots with automatic route planning functions such as sweepers, drones, and driverless cars have become widely popular. Before moving, robots usually plan a global path and then move according to the global path. However, the obstacles and pedestrians encountered by the robot during the movement cannot be known in advance. Therefore, it is necessary to plan a reasonable local path in real time based on the obstacles and pedestrians encountered, so that the robot can move smoothly and complete the work.
[0003] Currently, the commonly used robot local path planning methods are: RRT (Rapidly-Exploring Random Tree, a planning algorithm based on random adoption) and lattice algorithm (lattice planning algorithm). Among them, RRT is a greedy algorithm. The path planned by RRT is usually close to obstacles, so it is easy for the robot to collide with obstacles. In addition, the paths planned by RRT are mostly far away from the optimal path and are not smooth enough, which affects the stability of the robot's driving. The path planning by the lattice algorithm is inefficient and may cause path oscillation (the robot repeatedly switches between different paths). Summary of the invention
[0004] In order to at least solve the above problems, the present invention proposes a robot local path planning method, by selecting a path that can avoid obstacles and is closest to the global path from several initial local paths as the local path, so that the local path is closer to the optimal path, and the local path is closer to the global path. In addition, the present invention only needs to fit the sampling points once, so the planning time can be saved.
[0005] In a first aspect, the present invention provides a robot local path planning method, comprising the following steps: obtaining a global path for the robot to reach a target position from an initial position, wherein the global path is a set of global path points; sampling the global path points to obtain global sampling points; translating each of the global sampling points at least twice in a direction perpendicular to the global path, and obtaining a group of local sampling points each time the translation is performed, wherein the translation distance is within a preset translation distance threshold range; performing curve fitting on each group of local sampling points to obtain multiple initial local paths; obtaining obstacle information, and evaluating each of the initial local paths according to the obstacle information to screen and output local paths.
[0006] In one embodiment, the method further includes: acquiring the current position of the robot, and planning a link path from the current position to the local path by a polynomial fitting method.
[0007] In one embodiment, the method of planning the link path from the current position to the local path by polynomial fitting includes the following steps: determining a first point in the local path that is closest to the current position; determining a second point whose distance from the first point is a preset link distance; fitting the current position and the second point into a path polynomial by a polynomial fitting method, wherein the path polynomial is the link path.
[0008] In one embodiment, performing curve fitting on each group of local sampling points to obtain multiple initial local paths includes the following steps: performing curve fitting on each group of local sampling points using a spline curve to obtain a curve equation corresponding to each group of local sampling points; interpolating a group of local sampling points corresponding to each curve equation to obtain multiple groups of interpolated local sampling points, and the set of interpolated local sampling points is the initial local path.
[0009] In one embodiment, the spline curve includes a cubic spline curve, a quintic spline curve and an S-type spline curve.
[0010] In one embodiment, the obtaining of obstacle information and evaluating each initial local path according to the obstacle information to screen and output the local paths includes the following steps: collecting first position information of the obstacle; calculating a first distance between the obstacle and each initial local path according to the first position information; and outputting the initial local path with the longest first distance as the local path.
[0011] In one embodiment, the evaluating each initial local path according to the obstacle information to obtain the local path also includes the following steps: determining whether a first distance between the obstacle and the initial local path is less than a preset obstacle avoidance threshold, and in response to a yes determination, deleting the corresponding initial local path.
[0012] In one embodiment, the preset translation distance threshold range is from the robot width to the robot width plus 0.5 meters.
[0013] In a second aspect, the present invention provides a robot local path planning system, the planning system comprising: one or more processors; a storage device for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the method described in the first aspect or any one of its multiple embodiments.
[0014] In a third aspect, the present invention provides a computer-readable storage medium comprising computer program instructions, which, when executed by one or more processors, implement the method described in the first aspect or any one of its multiple embodiments.
[0015] According to the technical solution of the present invention, by downsampling the global path points and then performing translation, curve fitting and interpolation operations, a set of sampling points with accurate positions and more dense is obtained, so that the planned local path is smoother. In addition, since the local path in the technical solution of the present invention is generated by downsampling, translation and curve fitting of the global path points, its curvature, smoothness, etc. are closer to the global path and the deviation is smaller. At the same time, the technical solution of the present invention uses the location information of the obstacle to select a path that can avoid the obstacle and is closest to the global path from several initial local paths as the local path, so the local path is closer to the optimal path. Furthermore, the technical solution of the present invention directly obtains the initial local path by translating on the basis of the global path, without the need to re-search the surrounding environment information to calculate and generate the local path like the lattice algorithm, so the technical solution of the present invention is faster and more efficient than the Lattice algorithm, and has strong real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS
[0016] By reading the following detailed description with reference to the accompanying drawings, the above and other objects, features and advantages of the exemplary embodiments of the present invention will become readily understood. In the accompanying drawings, several embodiments of the present invention are shown in an exemplary and non-limiting manner, and the same or corresponding reference numerals represent the same or corresponding parts, wherein:
[0017] Figure 1 is a flow chart showing a method for local path planning of a robot according to an embodiment of the present invention;
[0018] Figure 2 is a schematic diagram showing sampling of a global path according to an embodiment of the present invention;
[0019] Figure 3 is a schematic diagram showing a direction perpendicular to a global path corresponding to a global path point according to an embodiment of the present invention;
[0020] Figure 3a is a schematic diagram showing that a global path point is translated twice in a direction perpendicular to the global path according to an embodiment of the present invention;
[0021] Figure 3b is a schematic diagram showing a method of translating a global path point twice in a direction perpendicular to the global path according to another embodiment of the present invention;
[0022] Figure 3c is a schematic diagram showing a method of translating a global path point twice in a direction perpendicular to the global path according to yet another embodiment of the present invention;
[0023] Figure 4 is a flow chart showing a method for evaluating, screening and outputting each initial local path according to an embodiment of the present invention;
[0024] Figure 5 is a flow chart showing a method for planning a link path from a current position of a robot to a local path according to an embodiment of the present invention;
[0025] Figure 6 is a schematic diagram showing a link path from a current position of a robot to a local path according to an embodiment of the present invention; and
[0026] Figure 7 is a framework diagram showing a robot local path planning system according to an embodiment of the present invention. DETAILED DESCRIPTION
[0027] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. It should be understood that the embodiments described in this specification are only partial embodiments provided by the present invention to facilitate a clear understanding of the solutions and to comply with legal requirements, and are not all embodiments that can implement the present invention. Based on the embodiments disclosed in this specification, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.
[0028] Before the robot starts to move, it usually senses the operating environment through its own radar and builds a map of the operating environment; then, on the constructed map, the global path planning method is used to plan a global path from the robot's initial position to the target position. The global path is a pre-planned path, which is a global and optimal path, but has poor adaptability to errors and noise in the environmental model. When the environment changes, such as when unknown obstacles appear, the robot will no longer be able to move along the global path. In order to ensure that the robot can complete its work smoothly, the robot needs to plan a local path that can avoid obstacles. Compared with global path planning, local path planning focuses on considering the robot's current local working environment information so that the robot has good obstacle avoidance capabilities. It is usually necessary to detect the robot's local working environment through sensors to obtain information such as the location and geometric properties of obstacles, and the local path can be corrected at any time as the information is dynamically updated. Existing robot local path planning methods often have defects such as the planned path is too far from the global path, not smooth enough (the path curvature is too small), and the efficiency of planning the path is low.
[0029] The technical solutions in the embodiments of the present invention will be described clearly and completely below with reference to the accompanying drawings.
[0030] Figure 1 is a flow chart showing a robot local path planning method 100 according to an embodiment of the present invention; Figure 2 is a schematic diagram showing sampling of a global path according to an embodiment of the present invention; Figure 3 is a schematic diagram showing the direction perpendicular to the global path corresponding to the global path point according to an embodiment of the present invention. Figure 1 , Figure 2 and Figure 3 The process of the robot local path planning method is described in detail.
[0031] like Figure 1 As shown, the present invention provides a robot local path planning method, including steps S101-S105. At step S101, a global path for the robot to reach a target position from an initial position is obtained, and the global path is a collection of global path points. In an implementation scenario, the robot can perform global path planning based on the robot's initial position, target position and environmental map through its global path planner to obtain a global planning path. The environmental map is constructed based on the robot's own radar sensor, which can be a dTOF (Direct-Time of flight) radar or a laser radar, etc. Then, the robot can obtain a global path from the global path planner through its ROS (Robot Operating System) through a topic (a communication method in ROS).
[0032] After the global path is obtained, at step S102, the global path points are sampled to obtain a set of global sampling points. Figure 2 As shown, a global sampling point can be screened out at a certain distance (e.g., 5 cm) in the global path 1, and finally a group of global sampling points 2 is obtained. In step S103, each global sampling point is translated at least twice in a direction perpendicular to the global path. The direction perpendicular to the global path corresponding to each global sampling point is a direction perpendicular to its own direction. Figure 3 As shown, the directions of the global sampling point 21 and the global sampling point 22 are respectively indicated by arrows A and B. Therefore, there are two directions perpendicular to the global path corresponding to the global sampling point 21, namely the direction indicated by arrow A1 (i.e., the left side of the global sampling point 21) and the direction indicated by arrow A2 (i.e., the right side of the global sampling point 21); the directions perpendicular to the global path corresponding to the global sampling point 22 are the direction indicated by arrow B1 (i.e., the left side of the global sampling point 22) and the direction indicated by arrow B2 (i.e., the right side of the global sampling point 22).
[0033] Figure 3a is a schematic diagram showing that a global path point is translated twice in a direction perpendicular to the global path according to an embodiment of the present invention. Figure 3b is a schematic diagram showing the translation of a global path point twice in a direction perpendicular to the global path according to another embodiment of the present invention. Figure 3c FIG. 1 is a schematic diagram showing a method of translating a global path point twice in a direction perpendicular to the global path according to another embodiment of the present invention. Figure 3a , 3b 3c exemplifies the result of translating the global path point twice in a direction perpendicular to the global path.
[0034] In one embodiment, Figure 3a As shown, each of the aforementioned global sampling points can be translated to the left twice. After the first translation, a group of local sampling points 3-1 is obtained, and after the second translation, a group of local sampling points 3-2 is obtained. In another embodiment, as Figure 3b As shown, each of the aforementioned global sampling points 2 can be translated to the right twice. After the first translation, a group of local sampling points 3-3 is obtained, and after the second translation, a group of local sampling points 3-4 is obtained. In another embodiment, as Figure 3c As shown, each of the aforementioned global sampling points 2 can be translated to the left and right of the global sampling point 2, respectively, to obtain a group of local sampling points 3-5 and a group of local sampling points 3-6. The above translation distance can be pre-stored in the controller of the robot, and the distance can be adjusted according to the width of the robot. In different implementation scenarios, the translation distance can be any value between the width of the robot and the width of the robot plus 0.5 meters.
[0035] As described above, after translating each of the aforementioned global sampling points twice, two groups of local sampling points can be obtained, and the aforementioned group of global sampling points 2 can also be used as a group of local sampling points, so three groups of local sampling points are obtained. In an implementation scenario, the number of groups of local sampling points obtained can also be preset in the controller of the robot. In order to make the final local path closer to the global path, the aforementioned global sampling points can be translated to the left and right sides by the same number of times, and an even number of local sampling points can be obtained. At the same time, the aforementioned group of global sampling points 2 is also used as a group of local sampling points, so the number of groups of local sampling points is generally an odd number, such as 3, 5, 7, 9, etc.
[0036] After obtaining multiple groups of local sampling points as above, it is necessary to perform step S104, that is, select an appropriate curve type to perform spline curve fitting on each group of local sampling points to obtain multiple initial local paths. Commonly used curve types include cubic spline curves, quintic spline curves and S-type spline curves. The smoothness of the three curves is improved successively, but the CPU resources consumed are also increased successively, so it is necessary to select the corresponding curve according to the computing power of the robot's CPU. Because the robustness of the curve obtained by spline curve fitting is positively correlated with the degree of the spline curve, the robustness of the curve fitted by the S-type curve is the highest among the above three spline curves. And the robustness of the curve is positively correlated with the adaptability of the curve. Therefore, the degree of the spline curve is positively correlated with the adaptability of the curve it fits. Therefore, the adaptability of the curve fitted by the S-type curve is the highest. Therefore, if the computing power of the robot CPU is sufficient, a spline curve with a higher degree, such as an S-type curve type, is selected as much as possible.
[0037] After selecting a suitable spline curve, the spline curve is used to perform curve fitting on each of the above-mentioned groups of local sampling points to obtain the curve equation corresponding to each group of local sampling points. For example, if three groups of local sampling points are obtained, three groups of curve equations can be obtained. Then, each curve equation is used to interpolate the corresponding group of local sampling points to obtain multiple groups of interpolated local sampling points. The set of interpolated local sampling points is the initial local path. For example, the curve equation is y=2x 2 , the horizontal coordinates of two adjacent local sampling points are x1 and x2, where x1 = 1 and x2 = 10. If interpolation is performed between x1 and x2, the interpolation step can be set to 1. Therefore, the interpolated values of x can be set to 2, 3, 4, 5, 6, 7, 8, and 9, respectively. Finally, according to the curve equation: y = 2x 2 The y value corresponding to the inserted x is obtained. In this way, 8 sets of new coordinate data can be obtained, that is, 8 coordinate points are inserted, so as to obtain a smoother initial local path. Since the calculation speed of the polynomial equation obtained by curve fitting is faster than that of the polynomial equation obtained by solving the equation group, the calculation speed of the above local path planning is faster. At the same time, it also avoids the situation where the solution of the equation group is prone to fall into the local optimal solution, thereby improving the success rate of local path planning.
[0038] Next, step S105 is executed to obtain obstacle information, and each of the aforementioned initial partial paths is evaluated according to the obstacle information (objects or people that hinder the robot from moving forward during operation) to filter and output the partial paths. Figure 4 The flowchart of the method 400 for evaluating, screening and outputting each initial local path according to an embodiment of the present invention is specifically described. Figure 1 As can be seen from the description, method 400 is Figure 1The method 100 is a specific implementation mode shown in FIG. 4 , so the above description of the method 100 is also applicable to the method 400. Figure 4 As shown, at step S401, the first position information of the obstacle is collected. In one embodiment, the first position information may be the outline coordinate information of the obstacle, and the outline coordinate information is the coordinate information of each coordinate point of the obstacle edge line. The position and coverage of the obstacle can be obtained by the coordinates of the obstacle edge line. In one embodiment, the robot can obtain the above-mentioned first position information from the robot's sensor through the topic by its ROS. At step S402, the first distance between the obstacle and each initial local path is calculated according to the first position information. In an implementation scenario, the above-mentioned first distance can be calculated by the following method: first, a kd-tree (k-dimensional tree, high-dimensional index tree data structure) algorithm is used to generate a kd-tree for the local path, then the outline coordinate information of the obstacle is traversed, and then the outline coordinate information of the obstacle, that is, multiple groups of (x, y) information, is placed in the kd-tree of the local path, and the kd-tree algorithm will automatically calculate the distance between the coordinate points corresponding to each group (x, y) and the local path, and obtain the index of the route with the shortest distance. The index of the shortest route is the index of the nearest coordinate point, and the coordinate information of the nearest coordinate point can be obtained through the index of the nearest coordinate point. Finally, according to the formula: The first distance can be calculated. (x1, y1) represents the coordinates of the point on the local path closest to the obstacle, and (x2, y2) represents the coordinates of the point on the obstacle edge line closest to the local path.
[0039] At step S403, the initial local path is screened. Specifically, it is determined whether the first distance between the obstacle and the initial local path is less than the preset obstacle avoidance threshold. If so, the obstacle is considered to be on the corresponding initial local path, and the corresponding initial local path is deleted. In one embodiment, the initial local paths closest to the global path 1 can be screened one by one. If an initial local path whose distance from the obstacle is greater than the obstacle avoidance threshold is screened, the path can be output as a local path so that the robot can travel as close to the global path 1 as possible. In one embodiment, the above-mentioned obstacle avoidance threshold can be pre-stored in the controller, and its value can be arbitrarily selected within the range of 1 cm to 10 cm. At step S404, the initial local path with the longest first distance (i.e., the farthest from the obstacle) is used as the local path and output.
[0040] Combination of the above Figure 1 , 2 , 3, 3a, 3b, 3c, and 4 are exemplary descriptions of the robot local path planning method. It should be understood by those skilled in the art that: Figure 1 ,2 The planning methods shown in , 3, 3a, 3b, 3c, and 4 are exemplary rather than restrictive, and those skilled in the art may adjust the steps of the method as needed.
[0041] In actual operation, although the local path of the robot has been planned, there is still a certain distance between the local path and the current position of the robot. The specific reason is that the robot travels along the global path 1 by default, and the local path is obtained after sampling and translating the global path 1. Therefore, there is a certain distance between the robot and the local paths on the left and right sides of the global path 1. In order to move along the local path, the robot will move directly to the starting position of the local path, and the path during its movement may be uneven. In order to prevent the robot from losing balance due to the uneven path, it is necessary to plan a link path between the current position of the robot and the local path. Figure 5 is a flowchart showing a method 500 for planning a link path from the current position of the robot to a local path according to an embodiment of the present invention; Figure 1 As can be seen from the description, method 500 is Figure 1 The method 100 is shown as a specific implementation manner, so the above description of the method 100 is also applicable to the method 500. Figure 6 Schematic diagram showing the link path from the current position of the robot to the local path according to an embodiment of the present invention. Figure 5 and Figure 6 The process of planning a link path from the robot's current position to a local path is described.
[0042] It is understandable that in order to plan the link path from the current position to the local path, in addition to obtaining the local path of the robot, the current position of the robot must also be obtained. In different implementation scenarios, the current position can be the starting position of the robot or the real-time position during movement, which can be obtained in real time through radar. After obtaining the local path and current position of the robot, Figure 5 and Figure 6 As shown, a link path from the current position to the local path can be planned by a polynomial fitting method, specifically including steps S501 to S503. In step S501, the first point 4 closest to the current position in the local path is determined. In an application scenario, a kd-tree can be generated from the local path 5 by using a kd-tree algorithm, and then the distance between each coordinate point in the local path 5 and the current position is calculated, and finally the coordinate point with the shortest distance is determined as the first point 4.
[0043] At step S502, a second point 7 whose distance from the first point 4 is a preset link distance is determined. In one embodiment, after the first point 4 is determined, the second point 7 whose distance from the first point 4 is a preset link distance can be selected along the direction in which the robot 6 moves. The preset link distance can be set according to the length of the robot 6, and can be arbitrarily selected within the range of one-third of the length of the robot 6 to two-thirds of the length of the robot 6. The method for selecting the second point 7 is specifically: traverse each coordinate point on the connecting line between the robot 6 and the first point 4, and when the distance between the coordinate point and the first point 4 is the preset link distance, the coordinate point is determined as the second point 7, and the traversal is stopped at the same time. At step S503, the current position and the second point 7 are fitted into a path polynomial by a polynomial fitting method, and the path polynomial is the link path 8. In one embodiment, the coordinate information of the current position and the coordinate information of the second point 7 can be put into a matrix, and then the path polynomial, i.e., the link path 8, is obtained by QR decomposition.
[0044] After the link path 8 is successfully planned, the robot 6 will enter the local path 5 through the link path 8 and continue to move forward. However, the robot 6 may still encounter obstacles while moving along the local path 5. At this time, the local path 5 currently being used by the robot can be sampled, translated, fitted, evaluated and screened by the above-mentioned robot local path planning method to finally obtain a new local path. In one embodiment, when translating the sampling points of the local path 5, it can be translated preferentially in the direction of the global path to prevent the new local path from deviating too far from the global path. Further, the robot 6 can move as close to the global path as possible.
[0045] Combination of the above Figure 5 and Figure 6 The process of planning the link path from the current position of the robot to the local path is described exemplarily, and the process of the robot moving after entering the local path is also described exemplarily. It should be understood by those skilled in the art that Figure 5 and Figure 6 The planning method shown in is exemplary rather than restrictive, and those skilled in the art may adjust the steps of the method as needed.
[0046] Refer to the following Figure 7 To describe the framework diagram of the robot local path planning system according to an embodiment of the present invention. Figure 7 The structure shown is only an example, which should not bring any limitation to the functions and scope of use of the embodiments of the present invention. Figure 7As shown, the planning system is presented in the form of a general-purpose computing device, including but not limited to: at least one processor, at least one memory, and a communication bus connecting different system components. The communication bus represents one or more of several types of bus structures, including a memory bus or a memory controller, a peripheral bus, a graphics acceleration port, a processor, or a local bus using any of a variety of bus structures. The memory may include a readable medium in the form of a volatile memory, such as a random access memory (RAM) and / or a cache memory, and may further include a read-only memory (ROM). The memory may also include program modules, such program modules include but are not limited to: an operating system, one or more application programs, other program modules, and program data, each of these examples or some combination may include the implementation of a network environment.
[0047] In addition, the present invention also provides a computer-readable storage medium, including computer program instructions, which, when executed by one or more processors, implement the method described in the first aspect or any one of its multiple embodiments.
[0048] It should be noted that although the operations of the method of the present invention are described in a particular order in the accompanying drawings, this does not require or imply that the operations must be performed in this particular order, or that all the operations shown must be performed to achieve the desired results. On the contrary, the steps depicted in the flow chart can be performed in a different order. Additionally or alternatively, some steps can be omitted, multiple steps can be combined into one step, and / or one step can be decomposed into multiple steps.
[0049] It should be understood that when the terms "first", "second", "third" and "fourth" are used in the claims, description and drawings of the present invention, they are only used to distinguish different objects, rather than to describe a specific order. The terms "include" and "comprise" used in the description and claims of the present invention indicate the presence of the described features, wholes, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and / or their collections.
[0050] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification of the present invention and the claims, the singular forms of "a", "an" and "the" are intended to include the plural forms unless the context clearly indicates otherwise. It should also be further understood that the term "and / or" used in the specification of the present invention and the claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes these combinations.
[0051] Although the embodiments of the present invention are as described above, the contents are only examples used to facilitate understanding of the present invention, and are not intended to limit the scope and application scenarios of the present invention. Any technician in the technical field of the present invention can make any modifications and changes in the form and details of the implementation without departing from the spirit and scope disclosed by the present invention, but the scope of patent protection of the present invention shall still be based on the scope defined by the attached claims.
Claims
1. A robot local path planning method, characterized in that: The following steps are involved: Obtain a global path for the robot to reach a target position from an initial position, wherein the global path is a set of global path points; Sampling the global path points to obtain global sampling points; Each of the global sampling points is translated at least twice in a direction perpendicular to the global path, and a set of local sampling points is obtained each time the translation is performed, wherein the translation distance is within a preset translation distance threshold range; Performing curve fitting on each group of local sampling points to obtain multiple initial local paths; Obtaining obstacle information, and evaluating each of the initial partial paths according to the obstacle information to screen and output a partial path; Obtaining the current position of the robot and planning a link path from the current position to the local path by a polynomial fitting method, comprising the following steps: Determine a first point in the local path that is closest to the current position; Determine a second point whose distance from the first point is a preset link distance; Fitting the current position and the second point into a path polynomial by a polynomial fitting method, wherein the path polynomial is a link path; The method of performing curve fitting on each group of local sampling points to obtain a plurality of initial local paths comprises the following steps: performing curve fitting on each group of local sampling points using a spline curve to obtain a curve equation corresponding to each group of local sampling points; interpolating a group of local sampling points corresponding to each curve equation to obtain a plurality of groups of interpolated local sampling points, wherein the set of interpolated local sampling points is the initial local path; The current position of the robot is obtained, and a link path from the current position to the local path is planned by a polynomial fitting method, including: determining a first point in the local path that is closest to the current position; determining a second point whose distance from the first point is a preset link distance, wherein the method for determining the second point is specifically: traversing each coordinate point on the connecting line between the robot and the first point, and when the distance between the coordinate point and the first point is the preset link distance, determining the coordinate point as the second point and stopping the traversal; and fitting the current position and the second point into a path polynomial by a polynomial fitting method, wherein the path polynomial is the link path.
2. The method according to claim 1, characterized in that: The spline curves include cubic spline curves, quintic spline curves and S-type spline curves.
3. The method according to claim 1, characterized in that Obtaining obstacle information and evaluating each initial local path according to the obstacle information to screen and output the local path includes the following steps: Collecting first position information of the obstacle; Calculating a first distance between the obstacle and each of the initial local paths according to the first position information; The initial local path with the longest first distance is taken as the local path and outputted.
4. The method according to claim 3, characterized in that The step of evaluating each initial local path according to the obstacle information to obtain the local path further comprises the following steps: It is determined whether a first distance between the obstacle and the initial partial path is less than a preset obstacle avoidance threshold, and in response to a yes determination, the corresponding initial partial path is deleted.
5. The method according to claim 1, characterized in that The preset translation distance threshold range is from the robot width to the robot width plus 0.5 meters.
6. A robot local path planning system, characterized in that: The planning system comprises: one or more processors; A storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1-5.
7. A computer-readable storage medium, characterized in that: The method comprises computer program instructions, which, when executed by one or more processors, implement the method according to any one of claims 1 to 5.
Citation Information
Patent Citations
Local path planning method and system for automatic driving and computer readable storage medium
CN110262488A
Local path planning method and device
CN111750886A
