Mobile robot path smoothing method
By constructing a turning transition path and introducing an automatic backtracking strategy, the problem of mobile robots entering a dead end is solved, the smooth fit and movement efficiency of paths are improved, and kinematic constraints and obstacle avoidance requirements are met.
Patent Information
- Application Number
- CN202410922570.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-10
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-07-10
AI Technical Summary
The prior art cannot effectively avoid mobile robots constrained by the minimum turning radius entering dead ends, resulting in path planning failure, and the existing path smoothing algorithm cannot meet kinematic constraints and obstacle avoidance requirements.
By determining the two-dimensional map, kinematic constraints, and minimum turning radius, a turning transition path is constructed to avoid dead ends, a smooth path is generated using arc segments and auxiliary center points sets, and an automatic backtracking strategy is introduced to optimize the path.
It effectively avoids mobile robots entering dead ends, improves the fit between the path and the original path, and ensures smooth movement and path planning efficiency of mobile robots in narrow spaces.
Smart Images

Figure CN118913272A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of mobile robots, and more specifically, to a mobile robot path smoothing method. Background Art
[0002] The path generated by the mobile robot path planning algorithm consists of several path points, presenting straight segments and various types of turns, including obtuse turns, reciprocating turns, and sharp turns. For mobile robots using differential steering and Ackerman steering, such a path is not an ideal choice. Although the differential steering model allows the mobile robot to turn after stopping, sharp turns will increase time consumption, reduce efficiency, and destroy the continuity of task execution. For mobile robots with the Ackerman steering model, reciprocating turns and sharp turns are kinematically infeasible, which will cause the mobile robot to deviate from the predetermined area when trying to follow the path, reducing efficiency or even causing path following failure. Therefore, the path needs to be specially smoothed to improve the movement efficiency of the mobile robot.
[0003] Path smoothing is a key step in path planning post-processing. The smoothed path must not only be continuous and safe, but also conform to 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 planned path usually needs to be close to the obstacle boundary, which increases the difficulty of path smoothing. Faced with the smoothing requirements of point-to-point paths and coverage paths, the smoothing process must meet multiple requirements at the same time, including being close to the original path, ensuring path continuity and safety, and considering the kinematic constraints of the mobile robot.
[0004] When processing the planned path, most smoothing algorithms are implemented based on interpolation methods or predetermined templates. However, in practical applications, the polynomial interpolation smoothing method has a deviation from the original path due to the Runge phenomenon caused by too many path points. Although the Bezier curve and B-spline curve smoothing methods show good results in point-to-point path application scenarios, these methods fail to fully consider the maximum curvature limit when turning and are not suitable for curvature-constrained Ackerman mobile robots. Although the Dubins path algorithm can generate the shortest path under the condition of satisfying the maximum curvature constraint, the algorithm cannot ensure that the path completely fits the original path or effectively bypasses obstacles. In addition, for mobile robots constrained by the minimum turning radius, the existing path planning methods cannot effectively prevent the mobile robot from entering a dead end. Once a mobile robot constrained by the minimum turning radius enters a dead end, it will not be able to generate a feasible path due to the constraint of the minimum turning radius, resulting in being trapped in the dead end. Summary of the invention
[0005] In view of the problem that the above-mentioned prior art cannot prevent a mobile robot subject to a minimum turning radius constraint from entering a dead end and being trapped in a dead end, the present invention provides a mobile robot path smoothing method, which can prevent a mobile robot subject to a minimum turning radius constraint from entering a dead end.
[0006] In order to solve the above technical problems, the technical solution provided by the present invention is:
[0007] A mobile robot path smoothing method comprises the following steps:
[0008] S1: Determine a two-dimensional map, kinematic constraints and a minimum turning radius, and determine an original path according to the two-dimensional map; the original path is the traversable area in the two-dimensional map; wherein the value of the minimum turning radius may be equal to or greater than the minimum turning radius limit value of the mobile robot;
[0009] S2: Determine whether there are path points in the original path that do not meet the kinematic constraint condition. If so, select one of them as the path point to be smoothed and go to step S3; otherwise, go to step S10;
[0010] S3: Determine a first backtracking point and a second backtracking point, and proceed to step S4; wherein the path on the original path before the path point to be smoothed is the front path, and the path on the original path after the path point to be smoothed is the back path, and the first backtracking point and the second backtracking point are points where the same circle having a radius equal to the minimum turning radius is tangent to the front path and the back path respectively;
[0011] S4: construct a first circle center point set and a third circle center point set, construct a first auxiliary circle and a third auxiliary circle according to the first circle center point set and the second circle center point set, and then enter step S5; wherein, the path between the first backtracking point and the path point to be smoothed is the front backtracking path, and the vertical distance between each first circle center on the first circle center point set and the front backtracking path is equal to the minimum turning radius; the path between the second backtracking point and the path point to be smoothed is the backtracking path, and the vertical distance between each third circle center on the third circle center point set and the back backtracking path is equal to the minimum turning radius; the circle with the first circle center as the center and the radius equal to the minimum turning radius is the first auxiliary circle; the circle with the third circle center as the center and the radius equal to the minimum turning radius is the third auxiliary circle;
[0012] S5: construct a second circle center point set, construct a second auxiliary circle according to the second circle center point set, and then proceed to step S6; wherein the second circle center is the intersection of circles with the first circle center and the third circle center as the circle center and the radius of each circle being 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 being equal to the minimum turning radius;
[0013] S6: Determine a turning transition path according to each of the second circle centers, and proceed to step S7; wherein the turning transition path includes three connecting arcs, wherein the first segment 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 end point is the tangent point between the first auxiliary circle and the second auxiliary circle; the second segment is located on the second auxiliary circle, the starting point is the tangent point between the first auxiliary circle and the second auxiliary circle, and the end point is the tangent point between the third auxiliary circle and the second auxiliary circle; the third segment is located on the third auxiliary circle tangent to the second auxiliary circle, the starting point is the tangent point between the third auxiliary circle and the second auxiliary circle, and the end point is located on the rear backtracking path;
[0014] S7: Eliminate the second circle center corresponding to the turning transition path that is in contact with the obstacle in the two-dimensional map, and enter step S8; this can prevent the mobile robot from colliding with the obstacle when moving;
[0015] S8: Determine whether the second circle center point set is not empty. If so, select a turning transition path that is closest to the original path to replace the corresponding part of the original path, and then enter step S10; if not, enter step S9; it can be understood that if the second circle center point set is not empty, it means that there is a turning transition path without collision between the front backtracking path and the rear backtracking path, and it is necessary to continue to optimize the second circle center to select a turning transition path that is closest to the original path; if the second circle center point set is empty, it means that a turning transition path that meets the minimum turning radius requirement cannot be found, and it is necessary to execute the backtracking strategy to update the positions of the first backtracking point and the second backtracking point;
[0016] S9: construct a collision-free shortest path connecting the first backtracking point and the second backtracking point, and use the collision-free shortest path to replace the original section between the first backtracking point and the second backtracking point, and then proceed to step S10;
[0017] S10: End.
[0018] In the above technical scheme, 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 turning transition path is used to smooth the area where there is a dead end in the original path, so that the mobile robot restricted by the minimum turning radius can move smoothly in the original path, and the mobile robot restricted by the minimum turning radius can be prevented from entering a dead end and being trapped in the dead end.
[0019] Preferably, in step S1, the two-dimensional map is a binary two-dimensional map. It is understood that a 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 a binary two-dimensional map is conducive to simplifying calculations, thereby quickly determining the original path and obstacles.
[0020] Preferably, the kinematic constraint condition is that the turning curvature and turning angle corresponding to the path points in the original path do not exceed the maximum turning curvature and maximum turning angle of the mobile robot respectively; in the step S2, the turning curvature and turning angle corresponding to each path point in the original path are first calculated, and then a path point whose turning curvature is greater than the maximum turning curvature and / or whose maximum turning angle is greater than the maximum turning angle is selected as the path point to be smoothed. By taking the path points that do not meet the maximum turning curvature and maximum turning angle requirements as the path points to be smoothed, it can be ensured that the smoothed path meets the constraint requirements of the maximum turning curvature and maximum turning angle of the mobile robot. It can be understood that the minimum turning radius and the maximum turning curvature are inversely correlated, and a smaller turning radius corresponds to a larger turning curvature, while the maximum turning angle determines the maximum turning curvature that the mobile robot can achieve when turning. If the minimum turning radius and the maximum turning curvature corresponding to the path point do not meet the set requirements, it means that there is a dead end near the path point that will trap the robot.
[0021] Preferably, in step S8, before entering step S10, step S2 is entered first. In this way, all path points to be smoothed in the original path can be traversed, and all path points to be smoothed can be smoothed one by one, so as to obtain a complete smoothed path.
[0022] Preferably, in step S2, all the path points to be smoothed that do not satisfy the kinematic constraint are arranged along the advancing direction of the original path; then the first path point that does not satisfy the kinematic constraint is selected as the path point to be smoothed. That is, after the smoothing process of a path point to be smoothed is completed, all path points that do not satisfy the kinematic constraint are searched again in the original path, and then the first path point that does not satisfy the kinematic constraint is smoothed, so that continuous and efficient smoothing of the original path is achieved in this way.
[0023] Preferably, before 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 sum of the Euclidean distances between each of the second center of the circle and the path point to be smoothed, the first auxiliary point, the second auxiliary point, the first backtracking point and the second backtracking point is calculated; in step S8, the turning transition path corresponding to the second center of the circle with the smallest sum of Euclidean distances is the turning transition path closest to the original path. The sum of the Euclidean distances of these points is used as the cost of the turning transition path being close to the original path. The smaller the cost, the lower the deviation between the turning transition path and the original path. Therefore, the turning transition path corresponding to the second center of the circle with the smallest sum of Euclidean distances is closer to the original path, and the smoothed path is more closely aligned with the original path.
[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 positions of the path point to be smoothed, the first backtracking point and the second backtracking point.
[0025] Therefore, preferably, there is at least one first auxiliary point and at least one second auxiliary point respectively.
[0026] Among them, the A* point-to-point path planning algorithm or the Dijkstra algorithm can be used to determine a collision-free shortest path between the first backtracking point and the second backtracking point, that is, the shortest path that does not collide with the virtual obstacle.
[0027] Preferably, 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. The A* point-to-point path planning algorithm combines the characteristics of the breadth-first search and greedy optimal search of the Dijkstra algorithm, and can more quickly and effectively find the shortest path that meets the collision-free requirement.
[0028] Preferably, in step S9, before entering step S10, step S2 is entered first. After constructing a collision-free shortest path with the second backtracking point between the first backtracking point and the second backtracking point, the first backtracking point and the second backtracking point may become new path points that do not satisfy the kinematic constraints. Therefore, before entering step S10, it is necessary to return to step S2. If the first backtracking point or the second backtracking point satisfies the non-kinematic constraints after constructing a collision-free shortest path with the second backtracking point between the first backtracking point and the second backtracking point, the unsatisfied points are smoothed, and then step S10 is entered. By introducing such an automatic backtracking strategy, when the first backtracking point and / or the second backtracking point do not satisfy the kinematic constraints, the first backtracking point and / or the second backtracking point are smoothed as path points to be smoothed using this method. This allows the mobile robot to always be able to find an effective alternative path to avoid itself from getting stuck in a dead end when facing a narrow space, thereby adapting to a more complex original path.
[0029] Preferably, 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.
[0030] Beneficial effects of the present invention:
[0031] (1) 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 turning transition path is used to smooth the dead end area in the original path. This allows the mobile robot restricted by the minimum turning radius to move smoothly in the original path, and prevents the mobile robot restricted by the minimum turning radius from entering a dead end and being trapped in the dead end.
[0032] (2) By calculating the Euclidean distance between multiple points in the original path and the center of the second circle corresponding to the turning transition path, the size of the distance is used as the cost to measure the degree of fit between the turning transition path and the original path. The turning transition path with the smallest cost is selected to smooth the original path, which can greatly improve the fit between the smoothed path and the original path.
[0033] (3) An automatic backtracking strategy is introduced. When no effective turning transition path can be found in the area formed by the front backtracking path and the rear backtracking path, a collision-free shortest path is first constructed between the first backtracking point and the second backtracking point to replace the original path accordingly. If the first backtracking point or the second backtracking point does not meet the kinematic constraints, the first backtracking point or the second backtracking point is used as a path point to be smoothed. In this way, the mobile robot can always find an alternative path to avoid getting stuck 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 narrow space in a short time, thereby further improving the smoothing efficiency of the original path. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 It is a schematic diagram of the extraction process of the first backtracking point and the second backtracking point;
[0036] Figure 2 It 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 This is a schematic diagram of the extraction process of the second circle center point set;
[0038] Figure 4 It is a schematic diagram of the extraction process of the turning transition path;
[0039] Figure 5 is a schematic diagram of a process of extracting a shortest path between a first backtracking point and a second backtracking point that does not collide with the virtual obstacle;
[0040] Figure 6 It is a path schematic diagram obtained by smoothing the dead-end area using the path smoothing method of the prior art;
[0041] Figure 7 A schematic diagram of a path after a dead-end area is smoothed by a mobile robot path smoothing method of the present invention;
[0042] Figure 8 It is a path schematic diagram obtained by smoothing the original point-to-point path using the path smoothing method of the prior art;
[0043] Fig. 9 A path schematic diagram obtained by smoothing a point-to-point original path using a mobile robot path smoothing method of the present invention;
[0044] Fig.10 It is a path schematic diagram obtained by smoothing the original path covering a large area using the path smoothing method in the prior art;
[0045] Fig.11 A path schematic diagram obtained by smoothing an original path covering a large area according to a mobile robot path smoothing method of the present invention;
[0046] Fig.12 Schematic diagram of the process of determining the sum of the Euclidean distances between the second circle center and the path point to be smoothed, 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 is further described in detail below through specific embodiments and in conjunction with the accompanying drawings:
[0048] Example 1
[0049] A mobile robot path smoothing method comprises the following steps:
[0050] S1: Determine the 2D map, kinematic constraints and minimum turning radius γ min , and determine the original path according to the two-dimensional map; the original path is the passable area in the two-dimensional map; in this embodiment, the minimum turning radius γ min The value of is equal to the minimum turning radius limit of the mobile robot;
[0051] S2: Figure 1 As shown, it is determined whether there are path points in the original path that do not meet the kinematic constraints. If so, one of them is selected as the path point to be smoothed p m , go to step S3; otherwise go to step S10;
[0052] S3: Determine the first backtracking point p icf and the second backtracking point p icr , enter step S4; wherein the path point p to be smoothed on the original path m The previous path is the front path, specifically p m To p1, the path point p on the original path to be smoothed m The subsequent path is the post-path, specifically p m The path to p4; the first backtracking point p icf and the 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: Figure 2 As shown, construct the first circle center point set O c1 and the third circle center point set O c3 , construct the first auxiliary circle and the third auxiliary circle according to the first circle center point set and the second circle center point set, and then enter step S5; construct the first auxiliary circle with the first circle center c1 as the circle center, and construct the third auxiliary circle with the third circle center c3, and then enter step S5; wherein the first backtracking point p icf and the path point p to be smoothed m The path between is the forward backtracking path, and the first circle center point set O c1 The vertical distance between each first circle center c1 and the previous backtracking path is equal to the minimum turning radius γ min ; The second backtracking point p icr and the path point p to be smoothedm The path between is the backtracking path, and the third circle center point set O c3 The vertical distance between each third circle center c3 on the backtracking path is equal to the minimum turning radius γ min ; The first circle center c1 is the center of the circle and the radius is equal to the minimum turning radius γ min The circle is the first auxiliary circle; the third circle center c3 is the center of the circle and the radius is equal to the minimum turning radius γ min The circle is the third auxiliary circle;
[0054] S5: Figure 3 As shown, construct the second circle center point set O c2 , according to the second circle center point set O c2 Construct a second auxiliary circle, and then proceed to step S6; wherein the second circle center c2 is respectively centered on the first circle center c1 and the third circle center c3 and the radius is equal to twice the minimum turning radius γ min The second auxiliary circle is centered on the second center c2 and has a radius equal to the minimum turning radius γ min circle;
[0055] S6: Figure 4 As shown, a turning transition path is determined according to each second circle center c2, and step S7 is entered; wherein the turning transition path includes three connecting arcs, wherein the first segment 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 end point is the tangent point between the first auxiliary circle and the second auxiliary circle; the second segment is located on the second auxiliary circle, the starting point is the tangent point between the first auxiliary circle and the second auxiliary circle, and the end point is the tangent point between the third auxiliary circle and the second auxiliary circle; the third segment is located on the third auxiliary circle tangent to the second auxiliary circle, the starting point is the tangent point between the third auxiliary circle and the second auxiliary circle, and the end point is located on the rear backtracking path;
[0056] S7: Remove the second circle center c2 corresponding to the turning transition path that has to negotiate with the obstacle in the two-dimensional map from the second circle center point set O c2 The mobile robot is removed from the process and the process goes to step S8; this can prevent the mobile robot from colliding with obstacles while moving;
[0057] S8: Determine the second circle center point set O c2 Is it not empty? If so, a turning transition path closest to the original path is selected to replace the corresponding part of the original path, and then the process goes to step S10; if not, the process goes to step S9; It can be understood that the second center point set O c2 is not empty, indicating that there is a collision-free turning transition path between the front backtracking path and the back backtracking path, and it is necessary to continue to optimize the second circle center c2 to select a turning transition path that is closest to the original path; if the second circle center point set O c2If it is empty, it means that a turning transition path that meets the minimum turning radius requirement cannot be found, and the backtracking strategy needs to be executed to update the first backtracking point p icf and the second backtracking point p icr location;
[0058] S9: Construct a connection to the first backtracking point p icf and the second backtracking point p icr The collision-free shortest path is used to find the first backtracking point p icf and the second backtracking point p icr Replace the original road section between them and proceed to step S10;
[0059] S10: End.
[0060] Further, in step S1, the two-dimensional map is a binary two-dimensional map. It is understood that a 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 a binary two-dimensional map is conducive to simplifying calculations, thereby quickly determining the original path and obstacles.
[0061] Furthermore, the kinematic constraint condition is that the turning curvature and 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 respectively; in step S2, the turning curvature and turning angle corresponding to each path point in the original path are first calculated, and then a path point whose turning curvature is greater than the maximum turning curvature and / or whose maximum turning angle is greater than the maximum turning angle is selected as the path point to be smoothed p m The path points that do not meet the maximum turning curvature and maximum turning angle requirements are taken as the path points to be smoothed p m , which can ensure that the smoothed path meets the constraints of the maximum turning curvature and maximum turning angle of the mobile robot. It can be understood that the minimum turning radius and the maximum turning curvature are inversely correlated, 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 achieve when turning. If the minimum turning radius and maximum turning curvature corresponding to a path point do not meet the set requirements, it means that there is a dead end near the path point that will trap the robot.
[0062] Furthermore, in step S8, before entering step S10, step S2 is entered first. In this way, all path points p to be smoothed in the original path can be traversed. m , for all path points p to be smoothed m Smoothing is performed one by one to obtain a complete smooth path.
[0063] Furthermore, in step S2, all path points to be smoothed that do not satisfy the kinematic constraints are mArrange along the forward direction of the original path; then select the first path point that does not meet the kinematic constraints as the path point to be smoothed p m That is to say, at the end of a path point p to be smoothed m After smoothing, all path points that do not meet the kinematic constraints are re-searched in the original path, and 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] The effect verification of this embodiment: Figure 6 to Figure 7 As shown, the shaded part is the obstacle pixel set in the two-dimensional map, and the white part is the passable path, that is, the original path. It can be seen that when there is a dead end 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, the shaded part in the figure is the obstacle pixel set in the two-dimensional map, and the white part is the passable path, that is, the original path. It can be seen that the method of this embodiment has a good smoothing effect whether it is for a point-to-point original path or an original path covering a large area, which can enable the mobile robot to meet the minimum turning radius restriction requirement and avoid the mobile robot being trapped in a dead end.
[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, and using the turning transition path to smooth the area where a dead end exists in the original path, the mobile robot restricted by the minimum turning radius can move smoothly in the original path, and the mobile robot restricted by the minimum turning radius can be prevented from entering a dead end and being trapped in the dead end.
[0066] Example 2
[0067] This embodiment further illustrates step S8 based on embodiment 1.
[0068] like Fig.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 path point to be smoothed p3 is calculated. m , first auxiliary point p2, second auxiliary point p3, first backtracking point p icf and the second backtracking point p icrThe sum of the Euclidean distances of the second circle center c2 corresponding to the minimum sum of the Euclidean distances is the turning transition path closest to the original path in step S8. The sum of the Euclidean distances of these points is used as the cost of the turning transition path being close to the original path. The smaller the cost, the lower the deviation between the turning transition path and the original path. Therefore, the turning transition path corresponding to the second circle center c2 corresponding to the minimum sum of the Euclidean distances is closer to the original path, and the smoothed path has a higher degree of fit with the original path.
[0069] Among them, according to the path point p to be smoothed m 、The first backtracking point p icf and the second backtracking point p icr According to the relative position situation, the number of the first auxiliary point p2 and the second auxiliary point p3 are set to one, two or more.
[0070] Specifically, in this 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 this embodiment are consistent with those of Embodiment 1.
[0072] Example 3
[0073] Based on Example 2, this embodiment further uses the A* point-to-point path planning algorithm at the first backtracking point p in step S9. icf and the second backtracking point p icr A collision-free shortest path is determined as the turning transition path, that is, the shortest path that does not collide with virtual obstacles. The A* point-to-point path planning algorithm combines the characteristics of the breadth-first search and greedy optimal search of the Dijkstra algorithm, and can find the shortest path more quickly and effectively.
[0074] Further, in step S9, before entering step S10, step S2 is entered first. At the first backtracking point p icf and the second backtracking point p icr After constructing a collision-free shortest path between icf and the second backtracking point p icr All of them may become new path points that do not satisfy the kinematic constraints. Therefore, before entering step S10, it is necessary to return to step S2. If at the first backtracking point p ic f and the second backtracking point p icr After constructing a collision-free shortest path between icf or the second backtracking point p icrIf the kinematic constraints are not satisfied, the points that do not meet the constraints are smoothed, and then the process goes 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 When the kinematic constraints are not met, this method is used to calculate the first backtracking point p icf and / or the second backtracking point p icr Smoothing is performed on the path points to be smoothed, so that the mobile robot can always find an effective alternative path to avoid getting into a dead end when facing a narrow space, thereby adapting to a more complex original path.
[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 practical applications, at the first backtracking point p icf and the second backtracking point p icr The shortest collision-free path between them may be the first backtracking point p icf and the second backtracking point p icr A straight line connecting the two points may be a curve of any shape between the two points. Figure 6 In this embodiment, the A* point-to-point path planning algorithm is used to calculate the first backtracking point p icf and the second backtracking point p icr A collision-free shortest path between the two points is the 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 invention are merely examples for clearly explaining the present invention, and are not intended to limit the implementation methods of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description, and it is not necessary and impossible to list all the implementation methods here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.
Claims
1. A mobile robot path smoothing method, characterized in that: The following steps are involved: 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: Determine whether there are path points in the original path that do not meet the kinematic constraint condition. If so, select one of them as the path point to be smoothed and go to step S3; otherwise, go to step S10; S3: Determine a first backtracking point and a second backtracking point, and proceed to step S4; wherein the path on the original path before the path point to be smoothed is the front path, and the path on the original path after the path point to be smoothed is the back path, and the first backtracking point and the second backtracking point are points where the same circle having a radius equal to the minimum turning radius is tangent to the front path and the back path respectively; S4: Construct a first circle center point set and a third circle center point set, construct a first auxiliary circle and a third auxiliary circle according to the first circle center point set and the second circle center point set respectively, and then enter step S5; wherein, the path between the first backtracking point and the path point to be smoothed is a front backtracking path, and the vertical distance between each first circle center on the first circle center point set and the front backtracking path is equal to the minimum turning radius; the path between the second backtracking point and the path point to be smoothed is a backtracking path, and the vertical distance between each third circle center on the third circle center point set and the back backtracking path is equal to the minimum turning radius; the first auxiliary circle is a circle with the first circle center as the center and a radius equal to the minimum turning radius; the third auxiliary circle is a circle with the third circle center as the center and a radius equal to the minimum turning radius; S5: construct a second circle center point set, construct a second auxiliary circle according to the second circle center point set, and then proceed to step S6; wherein the second circle center is the intersection of circles with the first circle center and the third circle center as the circle center and the radius of each circle being 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 being equal to the minimum turning radius; S6: Determine a turning transition path according to each of the second circle centers, and proceed to step S7; wherein the turning transition path includes three connecting arcs, wherein the first segment 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 end point is the tangent point between the first auxiliary circle and the second auxiliary circle; the second segment is located on the second auxiliary circle, the starting point is the tangent point between the first auxiliary circle and the second auxiliary circle, and the end point is the tangent point between the third auxiliary circle and the second auxiliary circle; the third segment is located on the third auxiliary circle tangent to the second auxiliary circle, the starting point is the tangent point between the third auxiliary circle and the second auxiliary circle, and the end point is located on the rear backtracking path; S7: Eliminate the second circle center corresponding to the turning transition path that is in contact with the obstacle in the two-dimensional map, and proceed to step S8; S8: Determine whether the second circle center point set is not empty. If so, select a turning transition path that is closest to the original path to replace the corresponding part of the original path, and then go to step S10; if not, go to step S9; S9: construct a collision-free shortest path connecting the first backtracking point and the second backtracking point, and use the collision-free shortest path to replace the original section between the first backtracking point and the second backtracking point, and then proceed to step S10; S10: End.
2. A mobile robot path smoothing method according to claim 1, characterized in that: In the step S1, the two-dimensional map is a binarized two-dimensional map.
3. A mobile robot path smoothing method according to claim 1, characterized in that: The kinematic constraint condition is that the turning curvature and turning angle corresponding to the path points in the original path do not exceed the maximum turning curvature and maximum turning angle of the mobile robot respectively; in the step S2, the turning curvature and turning angle corresponding to each path point in the original path are first calculated, and then a path point whose turning curvature is greater than the maximum turning curvature and / or whose maximum turning angle is greater than the maximum turning angle is selected as the path point to be smoothed.
4. A mobile robot path smoothing method according to claim 1, characterized in that: In the step S8, before entering the step S10, the step S2 is entered first.
5. A mobile robot path smoothing method according to claim 4, characterized in that: In step S2, all the path points to be smoothed that do not satisfy the kinematic constraint conditions are arranged along the forward direction of the original path; then the first path point that does not satisfy the kinematic constraint conditions is selected as the path point to be smoothed.
6. A mobile robot path smoothing method according to claim 1, characterized in that: Before 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 sum of the Euclidean distances between each of the second circle centers and the path point to be smoothed, 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 sum of Euclidean distances is the turning transition path closest to the original path.
7. A mobile robot path smoothing method according to claim 6, characterized in that: There is at least one first auxiliary point and at least one second auxiliary point.
8. A mobile robot path smoothing method according to claim 1, characterized in that: In the 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.
9. A mobile robot path smoothing method according to any one of claims 1 to 8, characterized in that: In the step S9, before entering the step S10, the step S2 is entered first.
10. A mobile robot path smoothing method according to claim 9, characterized in that: 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.
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
Path generating and following method of robot with non-zero turning radius
CN115167479A
