A method for smoothing the path of a mobile robot
By constructing a turning transition path larger than the minimum turning radius and optimizing Euclidean distance, the problem of mobile robots entering dead ends was solved, achieving efficient path smoothing and path fitting, and adapting to complex environments.
Patent Information
- Application Number
- CN202410922570.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-10
- Publication Date
- 2026-01-30
- Estimated Expiration
- 2044-07-10
AI Technical Summary
Existing technologies cannot effectively prevent mobile robots constrained by minimum turning radius from entering dead ends, leading to path planning failures or inefficiencies.
By constructing a turning transition path with a radius of circular arc greater than or equal to the minimum turning radius of the mobile robot, the A* point-to-point path planning algorithm is used to find the shortest collision-free path in a narrow space. Combined with Euclidean distance optimization, the turning transition path that is closest to the original path is selected to avoid dead ends.
This enables mobile robots to move smoothly while meeting minimum turning radius constraints, avoiding dead ends, improving the fit and efficiency of path smoothing, and adapting to complex path environments.
Smart Images
Figure CN118913272B_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the technical field of mobile robots, and more particularly, to a path smoothing method for mobile robots. BACKGROUND
[0002] The path generated by a mobile robot path planning algorithm is composed of a number of path points, which presents as straight line segments and various types of turns, including obtuse turns, back-and-forth turns and acute turns. For mobile robots with differential steering and Ackerman steering, such paths are not ideal choices. Although the differential steering model allows the mobile robot to turn after stopping, sharp turns will increase the time cost, reduce efficiency and disrupt the continuity of task execution. For mobile robots with Ackerman steering model, back-and-forth turns and sharp turns are kinematically infeasible, which will make the mobile robot deviate from the predetermined area when trying to follow the path, reduce efficiency and even cause path following failure. Therefore, the path needs to be specially smoothed to improve the motion efficiency of the mobile robot.
[0003] Path smoothing is a key link in path planning post-processing, which requires the smoothed path to be not only continuous and safe, but also to meet the kinematic constraints of the mobile robot. In addition, in order to ensure a high success rate, the smoothed path should be as close to the original path as possible. In order to reduce the path length, the path obtained by planning usually needs to be close to the obstacle boundary, which increases the difficulty of path smoothing. In the face of the smoothing needs of point-to-point paths and coverage paths, the smoothing process needs to meet multiple requirements, including close to the original path, path continuity and safety assurance, and consideration of the kinematic constraints of the mobile robot.
[0004] In the process of processing the obtained path, most smoothing algorithms are implemented according to interpolation methods or predetermined templates. However, the polynomial interpolation smoothing method causes deviation between the smoothed path and the original path due to the Runge phenomenon caused by too many path points in practical applications. Although the Bezier curve and B-spline curve smoothing methods show good results in point-to-point path application scenarios, these methods do not fully consider the maximum curvature limit when turning, and are not suitable for Ackerman mobile robots with curvature constraints. Although the Dubins path algorithm can generate the shortest path under the condition of meeting the maximum curvature constraint, the algorithm cannot ensure that the path completely fits the original path or effectively avoids obstacles. In addition, for mobile robots with minimum turning radius constraints, the existing path planning methods cannot effectively avoid the mobile robot from entering a dead end. Once the mobile robot with minimum turning radius constraints enters the dead end, it will be unable to generate a feasible path due to the minimum turning radius constraint, resulting in being trapped in the dead end. SUMMARY
[0005] In view of the problem that the prior art cannot avoid the mobile robot constrained by the minimum turning radius from entering a dead end and being trapped in the dead end, the application provides a mobile robot path smoothing method, which can avoid the mobile robot constrained by the minimum turning radius from entering a dead end.
[0006] To solve the above technical problems, the application provides the following technical scheme:
[0007] A mobile robot path smoothing method comprises the following steps:
[0008] S1: determining a two-dimensional map, a kinematic constraint condition and a minimum turning radius, and determining an original path according to the two-dimensional map; the original path is a passable area in the two-dimensional map; wherein the minimum turning radius can be equal to a minimum turning radius limit value of the mobile robot, or can be greater than the minimum turning radius limit value of the mobile robot;
[0009] S2: judging whether there is a path point in the original path that does not satisfy the kinematic constraint condition, if yes, selecting one of them as a to-be-smoothed path point, and entering step S3, otherwise entering step S10;
[0010] S3: determining a first backtracking point and a second backtracking point, and entering step S4; wherein a path before the to-be-smoothed path point on the original path is a front path, a path after the to-be-smoothed path point on the original path is a rear path, and the first backtracking point and the second backtracking point are respectively points at which a circle with a same radius equal to the minimum turning radius is tangent to the front path and the rear path respectively;
[0011] S4: constructing a first center point set and a third center point set, constructing a first auxiliary circle and a third auxiliary circle according to the first center point set and the third center point set, and then entering step S5; wherein a path between the first backtracking point and the to-be-smoothed path point is a front backtracking path, a vertical distance between each first center on the first center point set and the front backtracking path is equal to the minimum turning radius; a path between the second backtracking point and the to-be-smoothed path point is a rear backtracking path, a vertical distance between each third center on the third center point set and the rear backtracking path is equal to the minimum turning radius; the first center is a center of a circle with a radius equal to the minimum turning radius, and the circle is the first auxiliary circle; the third center is a center of a circle with a radius equal to the minimum turning radius, and the circle is the third auxiliary circle;
[0012] S5: constructing a second circle center point set, constructing a second auxiliary circle according to the second circle center point set, and then entering step S6; wherein the second circle center is the intersection point of circles with the first circle center and the third circle center as the circle center and the radius equal to twice the minimum turning radius; the second auxiliary circle is a circle with the second circle center as the circle center and the radius equal to the minimum turning radius;
[0013] S6: determining a turning transition path according to each of the second circle centers, respectively, and entering step S7; wherein the turning transition path includes three sections of connecting arcs, wherein the first section is located on the first auxiliary circle tangent to the second auxiliary circle, the starting point is located on the front backtracking path, and the ending point is the tangent point of the first auxiliary circle and the second auxiliary circle; the second section is located on the second auxiliary circle, the starting point is the tangent point of the first auxiliary circle and the second auxiliary circle, and the ending point is the tangent point of the third auxiliary circle and the second auxiliary circle; the third section is located on the third auxiliary circle tangent to the second auxiliary circle, the starting point is the tangent point of the third auxiliary circle and the second auxiliary circle, and the ending point is located on the rear backtracking path;
[0014] S7: removing the second circle center corresponding to the turning transition path that intersects with the obstacle in the two-dimensional map, and entering step S8; in this way, the mobile robot can be prevented from colliding with the obstacle during movement;
[0015] S8: determining whether the second circle center point set is empty, if not, selecting a turning transition path closest to the original path to replace the corresponding part in the original path, and then entering step S10; if yes, entering step S9; it can be understood that the second circle center point set is not empty, which means that there is a collision-free turning transition path between the front backtracking path and the rear backtracking path, and then the second circle center needs to be optimized to select a turning transition path closest to the original path; if the second circle center point set is empty, it means that it is impossible to find a turning transition path meeting the minimum turning radius requirement, and the positions of the first backtracking point and the second backtracking point need to be updated by executing a backtracking strategy;
[0016] S9: constructing a collision-free shortest path connecting the first backtracking point and the second backtracking point, replacing the original path section between the first backtracking point and the second backtracking point with the collision-free shortest path, and entering step S10;
[0017] S10: end.
[0018] In the technical solution, a turning transition path with a radius greater than or equal to the minimum turning radius of the mobile robot is constructed, and the turning transition path is used to smooth the area with the dead end in the original path, so that the mobile robot limited by the minimum turning radius can move smoothly in the original path, and the mobile robot limited by the minimum turning radius can avoid being trapped in the dead end.
[0019] Preferably, in the step S1, the two-dimensional map is a binary two-dimensional map. It can be understood that the binary two-dimensional map refers to simplifying the information on the map into binary values without considering specific physical features or detailed terrain information. Using the binary two-dimensional map is advantageous for simplifying the calculation, thereby quickly determining the original path and the obstacle.
[0020] Preferably, the kinematic constraint condition is that the turning curvature and the turning angle corresponding to the path point in the original path do not exceed the maximum turning curvature and the maximum turning angle of the mobile robot; in the step S2, the turning curvature and the turning angle corresponding to each path point in the original path are calculated first, and then a path point with a turning curvature greater than the maximum turning curvature and / or a turning angle greater than the maximum turning angle is selected as a path point to be smoothed. The path point that does not meet the requirement of the maximum turning curvature and the maximum turning angle is selected as the path point to be smoothed, so as to ensure that the smoothed path meets the constraint requirement of the maximum turning curvature and the maximum turning angle of the mobile robot. It can be understood that the minimum turning radius and the maximum turning curvature are inversely related, and a smaller turning radius corresponds to a larger turning curvature, and the maximum turning angle determines the maximum turning curvature that the mobile robot can reach when turning. If the minimum turning radius and the maximum turning curvature corresponding to the path point do not meet the set requirement, it indicates that there is a dead end near the path point that can trap the robot.
[0021] Preferably, in the step S8, before entering the step S10, the step S2 is entered first. In this way, all the path points to be smoothed in the original path can be traversed, and all the path points to be smoothed can be smoothed one by one, so that a complete smoothed path can be obtained.
[0022] Preferably, in the step S2, all the path points to be smoothed that do not meet the kinematic constraint condition are arranged along the forward direction of the original path; and then the first path point that does not meet the kinematic constraint condition is selected as the path point to be smoothed. That is, after the smoothing processing of a path point to be smoothed is completed, all the path points that do not meet the kinematic constraint condition in the original path are searched again, and the first path point that does not meet the kinematic constraint condition is smoothed, so that continuous and efficient smoothing processing of the original path is realized.
[0023] Preferably, before the step S8, a first auxiliary point is determined on the front path and a second auxiliary point is determined on the rear path; then the Euclidean distance sum of each second circle center and the to-be-smoothed path point, the first auxiliary point, the second auxiliary point, the first backtracking point and the second backtracking point is calculated; in the step S8, the turning transition path corresponding to the second circle center corresponding to the minimum Euclidean distance sum is the turning transition path closest to the original path. The Euclidean distance sum of these points is used as the cost of the turning transition path close to the original path, and the smaller the cost means the lower the deviation of the turning transition path from the original path. Therefore, the turning transition path corresponding to the second circle center corresponding to the minimum Euclidean distance sum is closer to the original path, and the fitting degree of the smoothed path to the original path is higher.
[0024] wherein the number of the first auxiliary point and the second auxiliary point can be set to one, two or more according to the relative position of the to-be-smoothed path point, the first backtracking point and the second backtracking point.
[0025] Therefore, preferably, the first auxiliary point and the second auxiliary point are at least one, respectively.
[0026] wherein the A* point-to-point path planning algorithm or the Dijkstra algorithm can be used to determine a shortest path without collision between the first backtracking point and the second backtracking point, that is, a shortest path without collision with the virtual obstacle.
[0027] Preferably, in the step S9, the A* point-to-point path planning algorithm is used to construct a shortest path without collision connecting the first backtracking point and the second backtracking point. The A* point-to-point path planning algorithm combines the breadth-first search and the greedy optimal search of the Dijkstra algorithm, and can more quickly and effectively find the shortest path meeting the requirement of no collision.
[0028] Preferably, in the step S9, before entering the step S10, the step S2 is entered first. After a collision-free shortest path between the first backtracking point and the second backtracking point is constructed, the first backtracking point and the second backtracking point may become new path points that do not satisfy the kinematic constraint condition, so before entering the step S10, it is needed to return to the step S2, and if after a collision-free shortest path between the first backtracking point and the second backtracking point is constructed, the first backtracking point or the second backtracking point does not satisfy the kinematic constraint condition, the path point that does not satisfy the kinematic constraint condition is smoothed, and then the step S10 is entered. By introducing such an automatic backtracking strategy, when the first backtracking point and / or the second backtracking point does not satisfy the kinematic constraint condition, the first backtracking point and / or the second backtracking point is smoothed as a path point to be smoothed in this way, so that the mobile robot always has the ability to find an effective alternative path to avoid being trapped in a dead end when facing a narrow space, thereby adapting to a more complex original path.
[0029] Preferably, in the step S9, the collision-free shortest path does not collide with the virtual obstacle and does not collide with the actual obstacle in the two-dimensional map.
[0030] The beneficial effects of the present application are:
[0031] (1) By constructing a turning transition path with a radius greater than or equal to the minimum turning radius of the mobile robot, the area in the original path where the dead end exists is smoothed by using the turning transition path, so that the mobile robot limited by the minimum turning radius can move smoothly in the original path, and the mobile robot limited by the minimum turning radius can be prevented from being trapped in the dead end.
[0032] (2) By calculating the Euclidean distance between the plurality of points in the original path and the second center corresponding to the turning transition path, the distance is used as a cost to measure the fitting degree of the turning transition path and the original path, and the turning transition path with the minimum cost is selected to smooth the original path, so that the fitting degree of the smoothed path and the original path can be greatly improved.
[0033] (3) The automatic backtracking strategy is introduced, when no effective turning transition path can be found in the area formed between the previous backtracking path and the subsequent backtracking path, a collision-free shortest path between the first backtracking point and the second backtracking point is constructed to replace the original path, and if the first backtracking point or the second backtracking point does not satisfy the kinematic constraint condition, the first backtracking point or the second backtracking point is smoothed as a path point to be smoothed, so that the mobile robot always has the ability to find an alternative path to avoid being trapped in a dead end when facing a narrow space, thereby adapting to a more complex original path.
[0034] (4) Using the A* point-to-point path planning algorithm, the mobile robot can quickly find a collision-free shortest path in a short time from a narrow space, thereby further improving the smoothing efficiency of the original path. BRIEF DESCRIPTION OF DRAWINGS
[0035] Figure 1 is a schematic diagram of the extraction process of the first backtracking point and the second backtracking point;
[0036] Figure 2 is a schematic diagram of the extraction process of the first circle center point set and the third circle center point set;
[0037] Figure 3 is a schematic diagram of the extraction process of the second circle center point set;
[0038] Figure 4 is a schematic diagram of the extraction process of the turning transition path;
[0039] Figure 5 is a schematic diagram of the extraction process of the shortest path between the first backtracking point and the second backtracking point that does not collide with the virtual obstacle;
[0040] Figure 6 is a schematic diagram of the path obtained after the dead-end area is smoothed by the path smoothing method of the prior art;
[0041] Figure 7 is a schematic diagram of the path obtained after the dead-end area is smoothed by the mobile robot path smoothing method of the present application;
[0042] Figure 8 is a schematic diagram of the path obtained after the point-to-point original path is smoothed by the path smoothing method of the prior art;
[0043] Figure 9 is a schematic diagram of the path obtained after the point-to-point original path is smoothed by the mobile robot path smoothing method of the present application;
[0044] Figure 10 is a schematic diagram of the path obtained after the large-area coverage original path is smoothed by the path smoothing method of the prior art;
[0045] Figure 11 is a schematic diagram of the path obtained after the large-area coverage original path is smoothed by the mobile robot path smoothing method of the present application;
[0046] Figure 12 is a schematic diagram of the process of determining the sum of the Euclidean distances between the second circle center and the to-be-smoothed path point, the first auxiliary point, the second auxiliary point, the first backtracking point, and the second backtracking point. DETAILED DESCRIPTION
[0047] The technical solution of the present invention will be further described in detail below through specific embodiments and in conjunction with the accompanying drawings:
[0048] Example 1
[0049] A method for smoothing the path of a mobile robot includes the following steps:
[0050] S1: Determine the 2D map, kinematic constraints, and minimum turning radius γ. min The original path is determined based on the two-dimensional map; the original path is the passable area in the two-dimensional map; in this embodiment, the minimum turning radius γ is... min The value is equal to the minimum turning radius limit of the mobile robot;
[0051] S2: As Figure 1 As shown, determine if there are any path points in the original path that do not meet the kinematic constraints. If so, select one of them as the path point p to be smoothed. m If yes, proceed to step S3; otherwise, proceed to step S10.
[0052] S3: Determine the first backtracking point p icf Second backtracking point p icr Proceed to step S4; where p is the path point to be smoothed on the original path. m The previous path is the previous path, specifically p. m The path to p1, the path point p on the original path to be smoothed. m The subsequent path is the later path, specifically p. m The path to p4; the first backtracking point p icf Second backtracking point p icr They are the same radius equal to the minimum turning radius γ min The points where the circle is tangent to the front path and the back path, respectively.
[0053] S4: As Figure 2 As shown, construct the first set of center points O. c1 and the third center point set O c3 Construct a first auxiliary circle and a third auxiliary circle based on the first and third center point sets, then proceed to step S5; construct the first auxiliary circle with the first center c1 as the center, and construct the third auxiliary circle with the third center c3, then proceed to step S5; wherein, the first backtracking point p icf With the path point p to be smoothed m The path between them is a forward backtracking path, with the first set of center points O. c1 The perpendicular distance between each first circle center c1 and the previous backtracking path is equal to the minimum turning radius γ. min Second backtracking point p icr With the path point p to be smoothedm The path between them is a backtracking path, and the third center point set O c3 The perpendicular distance between each third center c3 and the backtracking path is equal to the minimum turning radius γ. min The first center c1 is the center of the circle and its radius is equal to the minimum turning radius γ. min The circle is the first auxiliary circle; the third circle has center c3 and its radius is equal to the minimum turning radius γ. min The circle is the third auxiliary circle;
[0054] S5: As Figure 3 As shown, construct the second set of center points O. c2 According to the second center point set O c2 Construct a second auxiliary circle, then proceed to step S6; wherein, the center c2 of the second circle is a circle with the center c1 of the first circle and the center c3 of the third circle as its centers, and the radius of each circle is equal to twice the minimum turning radius γ. min The intersection of the circles; the second auxiliary circle is centered at the second circle's center c2 and has a radius equal to the minimum turning radius γ. min A circle;
[0055] S6: As Figure 4 As shown, a turning transition path is determined based on each second circle center c2, and the process proceeds to step S7. The turning transition path includes three connecting arcs. The first arc is located on the first auxiliary circle tangent to the second auxiliary circle, with its starting point on the forward backtracking path and its ending point at the tangent point between the first and second auxiliary circles. The second arc is located on the second auxiliary circle, with its starting point at the tangent point between the first and second auxiliary circles and its ending point at the tangent point between the third and second auxiliary circles. The third arc is located on the third auxiliary circle tangent to the second auxiliary circle, with its starting point at the tangent point between the third and second auxiliary circles and its ending point on the backward backtracking path.
[0056] S7: Move the second center c2 corresponding to the turning transition path that interacts with obstacles in the 2D map from the second center point set O. c2 Remove from the list and proceed to step S8; this prevents the mobile robot from colliding with obstacles while moving.
[0057] S8: Determine the set of the second center point O c2 If the value is not empty, select the turning transition path that best matches the original path to replace the corresponding part of the original path, and then proceed to step S10; otherwise, proceed to step S9. It can be understood that the second center point set O... c2 If the value is not empty, it means that there is a collision-free turning transition path between the previous and subsequent backtracking paths. Therefore, it is necessary to continue optimizing the second center c2 to select a turning transition path that best approximates the original path. If the second center point set O... c2is empty, indicating that no turning transition path satisfying the minimum turning radius requirement can be found, and a backtracking strategy needs to be performed to update the first backtracking point p icf and the position of the second backtracking point p icr ;
[0058] S9: A collision-free shortest path connecting the first backtracking point p icf and the second backtracking point p icr is constructed, and the original road segment between the first backtracking point p icf and the second backtracking point p icr is replaced by the collision-free shortest path, and step S10 is entered;
[0059] S10: End.
[0060] Further, in step S1, the two-dimensional map is a binary two-dimensional map. It can be understood that the binary two-dimensional map refers to simplifying the information on the map to binary values without considering specific physical features or detailed terrain information. Using the binary two-dimensional map is beneficial for simplifying calculations, thereby quickly determining the original path and obstacles.
[0061] Further, the kinematic constraint condition is that the turning curvature and the turning angle corresponding to the path points in the original path do not exceed the maximum turning curvature and the maximum turning angle of the mobile robot; in step S2, the turning curvature and the turning angle corresponding to each path point in the original path are first calculated, and then a path point with a turning curvature greater than the maximum turning curvature and / or a turning angle greater than the maximum turning angle is selected as the to-be-smoothed path point p m . Taking the path point that does not satisfy the maximum turning curvature and the maximum turning angle requirement as the to-be-smoothed path point p m can ensure that the smoothed path satisfies the constraint requirement of the maximum turning curvature and the maximum turning angle of the mobile robot. It can be understood that the minimum turning radius and the maximum turning curvature are inversely related, and a smaller turning radius corresponds to a larger turning curvature, and the maximum turning angle determines the maximum turning curvature that the mobile robot can reach when turning. If the minimum turning radius and the maximum turning curvature corresponding to the path point do not satisfy the set requirement, it indicates that there is a dead end near the path point that will trap the robot.
[0062] Further, in step S8, before entering step S10, step S2 is first entered. In this way, all to-be-smoothed path points p m in the original path can be traversed, and all to-be-smoothed path points p m are smoothed one by one, thereby obtaining a complete smoothed path.
[0063] Further, in step S2, all to-be-smoothed path points p mArrange the points along the original path's forward direction; then select the first path point that does not satisfy the kinematic constraints as the path point p to be smoothed. m In other words, at the end of a path to be smoothed point p... m After smoothing, all path points that do not meet the kinematic constraints are searched again in the original path. Then, the first path point that does not meet the kinematic constraints is smoothed. In this way, continuous and efficient smoothing of the original path is achieved.
[0064] Verification of the effect of this embodiment: as follows Figures 6 to 7 As shown, the shaded area represents the set of obstacle pixels in the 2D map, and the white area represents the passable path, i.e., the original path. It is evident that when a dead end exists in the original path, the method in this embodiment can prevent the mobile robot, constrained by the minimum turning radius, from entering the dead end. Figures 8 to 11 As shown, similarly, the shaded areas in the figure represent the set of obstacle pixels in the 2D map, while the white areas represent the passable paths, i.e., the original paths. It is evident that the method in this embodiment has a good smoothing effect whether dealing with point-to-point original paths or original paths covering large areas, enabling the mobile robot to meet the minimum turning radius requirement and preventing it from getting stuck in dead ends.
[0065] The beneficial effects of this embodiment are as follows: By constructing a turning transition path with an arc segment radius greater than or equal to the minimum turning radius of the mobile robot, the area with dead ends in the original path can be smoothed using this turning transition path. This allows the mobile robot, which is limited by the minimum turning radius, to move smoothly in the original path, avoiding the mobile robot being trapped in a dead end.
[0066] Example 2
[0067] This embodiment further explains step S8 based on embodiment 1.
[0068] like Figure 12 As shown, before step S8, a first auxiliary point p2 is determined on the front path, and a second auxiliary point p3 is determined on the back path; then, the relationship between each second circle center c2 and the point p on the path to be smoothed is calculated. m First auxiliary point p2, second auxiliary point p3, first backtracking point p icf Second backtracking point p icrthe second circle center c2 corresponding to the minimum Euclidean distance sum is the turning transition path closest to the original path. The Euclidean distance sum of these points is used as the cost of the turning transition path closest to the original path, and the smaller the cost means the lower the deviation of the turning transition path from the original path. Therefore, the turning transition path corresponding to the second circle center c2 corresponding to the minimum Euclidean distance sum is closer to the original path, and the fitting degree of the smoothed path to the original path is higher.
[0069] wherein the number of the first auxiliary point p2 and the second auxiliary point p3 can be set to one, two or more according to the relative position of the to-be-smoothed path point p m , the first backtracking point p icf and the second backtracking point p icr .
[0070] Specifically, in the embodiment, the first auxiliary point p2 and the second auxiliary point p3 are both one.
[0071] The other features, working principles and beneficial effects of the embodiment are consistent with those of embodiment 1.
[0072] Embodiment 3
[0073] Based on embodiment 2, further, in step S9, an A* point-to-point path planning algorithm is used to determine a collision-free shortest path between the first backtracking point p icf and the second backtracking point p icr as the turning transition path, that is, the shortest path without collision with the virtual obstacle. The A* point-to-point path planning algorithm combines the breadth-first search and greedy optimal search features of the Dijkstra algorithm, and can more quickly and effectively find the shortest path.
[0074] Further, in step S9, before entering step S10, step S2 is entered first. After a collision-free shortest path is constructed between the first backtracking point p icf and the second backtracking point p icr , the first backtracking point p icf and the second backtracking point p icr may become new path points that do not satisfy the kinematic constraint condition, so before entering step S10, it is necessary to return to step S2. If a collision-free shortest path is constructed between the first backtracking point p ic f and the second backtracking point p icr , the first backtracking point p icf or the second backtracking point p icrIf the kinematic constraint condition is not satisfied, the point not satisfying the condition is smoothed, and the process returns to step S10. By introducing such an automatic backtracking strategy, when the first backtracking point p icf and / or the second backtracking point p icr does not satisfy the kinematic constraint condition, the first backtracking point p icf and / or the second backtracking point p icr is smoothed as a path point to be smoothed, so that the mobile robot always has the ability to find an effective alternative path to avoid being trapped in a dead end when facing a narrow space, thereby adapting to more complex original paths.
[0075] Further, in step S9, the collision-free shortest path does not collide with the virtual obstacle and does not collide with the actual obstacle in the two-dimensional map.
[0076] In actual applications, the collision-free shortest path between the first backtracking point p icf and the second backtracking point p icr may be a straight line connecting the first backtracking point p icf and the second backtracking point p icr , or any curve between the two points. Referring to Figure 6 , in this embodiment, the A* point-to-point path planning algorithm is used to calculate a collision-free shortest path between the first backtracking point p icf and the second backtracking point p icr , which is a straight line connecting the two points.
[0077] The other features, working principles, and beneficial effects of this embodiment are consistent with those of embodiment 2.
[0078] Obviously, the above embodiments of the present application are merely examples for clearly illustrating the present application, and are not intended to limit the implementation modes of the present application. Based on the above description, those skilled in the art can make other different forms of changes or modifications, which do not need to be and cannot be exhaustively listed here. Any modification, equivalent replacement, and improvement within the spirit and principles of the present application shall be included in the protection scope of the claims of the present application.
Claims
1. A mobile robot path smoothing method, characterized by, The method comprises the following steps: S1: determining a two-dimensional map, kinematic constraints and a minimum turning radius, and determining an original path according to the two-dimensional map; S2: judging whether there is a path point in the original path that does not satisfy the kinematic constraints, if yes, selecting one of them as a to-be-smoothed path point and entering step S3, otherwise entering step S10; S3: determining a first backtracking point and a second backtracking point, and entering step S4; wherein a path before the to-be-smoothed path point on the original path is a front path, a path after the to-be-smoothed path point on the original path is a rear path, and the first backtracking point and the second backtracking point are respectively points at which a circle with a same radius equal to the minimum turning radius is tangent to the front path and the rear path respectively; S4: constructing a first center point set and a third center point set, constructing a first auxiliary circle and a third auxiliary circle according to the first center point set and the third center point set respectively, and then entering step S5; wherein a path between the first backtracking point and the to-be-smoothed path point is a front backtracking path, a vertical distance between each first center on the first center point set and the front backtracking path is equal to the minimum turning radius; a path between the second backtracking point and the to-be-smoothed path point is a rear backtracking path, a vertical distance between each third center on the third center point set and the rear backtracking path is equal to the minimum turning radius; the first auxiliary circle is a circle with the first center as a center and a radius equal to the minimum turning radius; and the third auxiliary circle is a circle with the third center as a center and a radius equal to the minimum turning radius; S5: constructing a second center point set, constructing a second auxiliary circle according to the second center point set, and then entering step S6; wherein the second center is an intersection point of circles with the first center and the third center as centers and a radius equal to twice the minimum turning radius; and the second auxiliary circle is a circle with the second center as a center and a radius equal to the minimum turning radius; S6: determining a turning transition path according to each second center respectively, and entering step S7; wherein the turning transition path comprises three connected circular arcs, a first segment of which is located on the first auxiliary circle tangent to the second auxiliary circle, a starting point of which is located on the front backtracking path and an ending point of which is a tangent point of the first auxiliary circle and the second auxiliary circle; a second segment of which is located on the second auxiliary circle, a starting point of which is the tangent point of the first auxiliary circle and the second auxiliary circle and an ending point of which is a tangent point of the third auxiliary circle and the second auxiliary circle; and a third segment of which is located on the third auxiliary circle tangent to the second auxiliary circle, a starting point of which is the tangent point of the third auxiliary circle and the second auxiliary circle and an ending point of which is located on the rear backtracking path; S7: removing a second center corresponding to the turning transition path that intersects an actual obstacle in the two-dimensional map, and entering step S8; S8: determining a first auxiliary point on the front path and a second auxiliary point on the rear path, and then calculating the sum of Euclidean distances between each second circle center and the to-be-smoothed path point, the first auxiliary point, the second auxiliary point, the first backtracking point and the second backtracking point; the turning transition path corresponding to the second circle center corresponding to the minimum sum of Euclidean distances is the turning transition path closest to the original path; determining whether the set of second circle centers is empty, if not, selecting a turning transition path closest to the original path to replace the corresponding part of the original path, and then entering step S10; if yes, entering step S9; S9: constructing a collision-free shortest path connecting the first backtracking point and the second backtracking point, and replacing the original path segment between the first backtracking point and the second backtracking point with the collision-free shortest path, and then entering step S10; S10: ending.
2. The method of claim 1, wherein, In step S1, the two-dimensional map is a binary two-dimensional map.
3. The method of claim 1, wherein, The kinematic constraint condition is that the turning curvature and the turning angle corresponding to the path point in the original path do not exceed the maximum turning curvature and the maximum turning angle of the mobile robot; in step S2, the turning curvature and the turning angle corresponding to each path point in the original path are calculated, and then a path point with a turning curvature greater than the maximum turning curvature and / or a turning angle greater than the maximum turning angle is selected as a to-be-smoothed path point.
4. The method of claim 1, wherein, In step S8, before entering step S10, step S2 is entered first.
5. The method of claim 4, wherein, In step S2, all to-be-smoothed path points that do not meet the kinematic constraint condition are arranged along the forward direction of the original path; and then the first path point that does not meet the kinematic constraint condition is selected as a to-be-smoothed path point.
6. The method of claim 1, wherein The first auxiliary point and the second auxiliary point each have at least one.
7. The method of claim 1, wherein In step S9, an A* point-to-point path planning algorithm is used to construct a collision-free shortest path connecting the first backtracking point and the second backtracking point.
8. The method of claim 1 to 7, wherein In step S9, before entering step S10, step S2 is entered first.
9. The method of claim 8, wherein, In step S9, the collision-free shortest path does not collide with virtual obstacles and actual obstacles in the two-dimensional map.
Citation Information
Patent Citations
Obstacle-avoiding route planning and control method for unmanned agricultural machine driving
CN106681335A
Robot path planning method based on elliptical tangent line structure
CN106774329A