Path Optimization Method, Device, Storage Medium, and Equipment

Abstract

The present disclosure relates to a path optimization method and apparatus, an electronic device, and a storage medium. The method includes: downsampling a point sequence of a pre-planned path to obtain a set of sampled points, and calculating the minimum distance between each sampled point in the set of sampled points and an obstacle; updating the number and position of the sampled points according to the minimum distance between each sampled point and the obstacle and the distance between each sampled point and an adjacent sampled point; for the updated sampled points, determining the distance between the position of each sampled point after update and the position before update; in response to the distance between the position of each sampled point after update and the position before update being less than a preset threshold, using the updated sampled points as the determined sampled points; performing interpolation processing on the determined sampled points to obtain an optimized path, reducing the computational amount of path optimization through downsampling, meeting the requirements of a low-computation platform, and taking into account obstacles to ensure the smoothness of the path and avoid hitting obstacles.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robot navigation, and particularly to a path optimization method, device, storage medium, and equipment. Background Art

[0002] The robot navigation framework is divided into two parts, one is path planning and the other is path tracking.

[0003] For common planning algorithms (such as Dijkstra and A* etc.), a grid map is used as the basis, and the path with the minimum cost is searched on the grid map. Since the resolution of the grid map cannot be too high, too high will cause the computational amount to increase sharply, while too low resolution will result in an inaccurate path. Therefore, on the basis of weighing the computational amount and accuracy, a grid map with a certain resolution is used for path planning, but this leads to the generated path often not being smooth or the shortest, having certain bends, especially at narrow aisles.

[0004] And a curved path is a bad input for path tracking. The evaluation index of path tracking is to control whether the path the robot walks along fits the given path. The better the path tracking algorithm, the better the robot will be controlled to fit the given path. If the given path is curved, the path the robot walks out will also be curved. If the degree of path curvature is too large, it may exceed the control limit range of the robot, resulting in the robot coming to an emergency stop or drifting, and the user experience will be greatly reduced. In addition, if the path planned in an open space is also curved, it will cause the robot to walk along a curved route, giving the user a feeling that the robot is very unintelligent.

[0005] Can the accuracy of path tracking be reduced to meet the requirement of smooth robot movement? In fact, path tracking can filter the curved parts of the path, but this will necessarily lead to a reduced degree of path tracking fit, thereby increasing the risk of the robot hitting an obstacle.

[0006] Currently, most robots directly input the path into path tracking after path planning without path optimization processing. By adjusting the parameters of path tracking (such as look-ahead points etc.), the robot is made not to follow the curved parts of the path, which will necessarily lead to a decrease in the accuracy of path tracking. Moreover, the motion characteristics of the machine are generally not considered at the path planning level, and in specific cases, some curved and oscillating paths will be generated, resulting in violent robot movement.

[0007] In addition, some path optimization (path smoothing) methods smooth the path based on spline curves, and there is also the Floyd path smoothing algorithm for path smoothing. Among them, in the optimization method based on spline curves, a spline is a piecewise low-order polynomial approximation function that can be applied to functions with different nonlinearities or multiple extreme points. The smoothed path has the characteristic of nth-order continuity. The principle of the path smoothing method based on the Floyd path smoothing algorithm is simple, aiming to remove adjacent collinear points and redundant turning points.

[0008] However, for the smoothing method of spline curves, only the positions of the position points are considered during the optimization process, without considering the positions of obstacles, which is likely to cause the optimized path to hit obstacles or be insufficiently far from obstacles; for the path smoothing method based on the Floyd path smoothing algorithm, all path points of the entire path often need to be input into the optimization algorithm for optimization, resulting in a relatively large amount of computation and being not suitable for use on platforms with low computational power. Summary of the Invention

[0009] To solve the above technical problems or at least partially solve the above technical problems, embodiments of the present disclosure provide a path optimization method, apparatus, storage medium, and device.

[0010] In a first aspect, embodiments of the present disclosure provide a method, including:

[0011] Downsample the point sequence of the pre-planned path to obtain a set of sampled points, and calculate the minimum distance between each sampled point in the set of sampled points and the obstacle;

[0012] Update the number and position of the sampled points according to the minimum distance between each sampled point and the obstacle and the distance between each sampled point and the adjacent sampled point;

[0013] For the updated sampled points, determine the distance between the position of each sampled point after update and before update;

[0014] In response to the distance between the position of each sampled point after update and before update being less than a preset threshold, use the updated sampled points as the determined sampled points;

[0015] Perform interpolation processing on the determined sampled points to obtain an optimized path.

[0016] In a possible implementation manner, the method further includes:

[0017] In response to the distance between any updated sampling point and its position before update in the updated sampling points being greater than or equal to a preset threshold, the step of calculating the minimum distance between each sampling point and the obstacle is re-executed until the distance between each updated sampling point and its position before update is less than the preset threshold. Among them, for the newly added sampling points in the updated sampling points, the distance between the updated position and the position before update is infinite or greater than the preset threshold.

[0018] In a possible implementation manner, the minimum distance between each sampling point in the sampling point set and the obstacle is calculated through the following expression:

[0019] d = -ln(cost / 253) / a + r

[0020] Wherein, d is the minimum distance between the current sampling point and the obstacle, cost is the cost value of the grid on the cost map where the current sampling point is located, a is the attenuation coefficient, and r is the circumscribed circle radius of the robot for path tracking.

[0021] In a possible implementation manner, updating the number and position of the sampling points according to the minimum distance between each sampling point and the obstacle and the distance between each sampling point and its adjacent sampling point includes:

[0022] Calculating the size of the collision-free space range of the current sampling point according to the minimum distance between the current sampling point and the obstacle;

[0023] Based on the distance between the current sampling point and its adjacent sampling point and the size of the collision-free space ranges of the current sampling point and the adjacent sampling point, correcting the sampling point;

[0024] Adjusting the position of the current sampling point according to the distance between the current sampling point and its adjacent sampling point and the minimum distance between the current sampling point and the obstacle.

[0025] In a possible implementation manner, the size of the collision-free space range of the current sampling point is calculated through the following expression according to the minimum distance between the current sampling point and the obstacle:

[0026] ρ = d - r = -ln(cost / 253) / a

[0027] Wherein, ρ is the size of the collision-free space range of the current sampling point, d is the minimum distance between the current sampling point and the obstacle, r is the circumscribed circle radius of the robot for path tracking, cost is the cost value of the grid on the cost map where the current sampling point is located, and a is the attenuation coefficient.

[0028] In a possible implementation manner, the correcting the sampling point based on the distance between the current sampling point and its adjacent sampling point and the size of the collision-free space ranges of the current sampling point and the adjacent sampling point includes:

[0029] When two adjacent sampling points satisfy ||b i -b i+1 ||≥(ρ i +ρ i+1 ), a new sampling point is added between the two adjacent sampling points, where the position of the new sampling point is at the midpoint between the two adjacent sampling points;

[0030] When the (i - 1)-th sampling point and the (i + 1)-th sampling point satisfy ||b i-1 -b i+1 ||≤(ρ i-1 +ρ i+1 ), the i-th sampling point is deleted, where b i-1 is the position of the (i - 1)-th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point, ρ i-1 is the size of the collision-free space range of the (i - 1)-th sampling point, ρ i is the size of the collision-free space range of the i-th sampling point, ρ i+1 is the size of the collision-free space range of the (i + 1)-th sampling point.

[0031] In a possible implementation manner, adjusting the position of the current sampling point according to the distance between the current sampling point and the adjacent sampling points and the minimum distance between the current sampling point and the obstacle includes:

[0032] Step 1, calculate the gravitational force of the two adjacent sampling points on the i-th sampling point through the following expression:

[0033]

[0034] where f c is the gravitational force of the two adjacent sampling points on the i-th sampling point, k c is the gravitational coefficient, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point,

[0035] Step 2, calculate the repulsive force of the obstacle on the sampling point through the following expression:

[0036]

[0037] where f r is the repulsive force of the obstacle on the sampling point, k r is the gravitational coefficient, ρ0 is the threshold for using the repulsive force, ρ is the size of the collision-free space range of the sampling point, where, Calculated by the following expression:

[0038]

[0039] Where h is the step size, x is (10), y is (01),

[0040] Step 3, calculate the resultant force f according to the gravitational and repulsive forces received by the i-th sampling point total :

[0041] f total = f c + f r

[0042] Step 4, update the position of the sampling point through the resultant force:

[0043] b new = b old + αf total

[0044] Where b new is the new position of the sampling point, b old is the original position of the sampling point, α is the update step size, where αf total < ρ.

[0045] In a possible implementation, the determined sampling points are interpolated through the following expression:

[0046] p j = b i + j * k * (b i+1 - b i ) / ||b i+1 - b i || j * k ≤ ||b i+1 - b i ||

[0047] Where p j is the position of the j-th point inserted in the path between the i-th sampling point and the (j + 1)-th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point, k is the resolution of the interpolated path, and j is the j-th point inserted in the path between the i-th sampling point and the (i + 1)-th sampling point.

[0048] In a second aspect, an embodiment of the present disclosure provides a path optimization device, including:

[0049] A calculation module, configured to downsample a point sequence of a pre-planned path to obtain a set of sampling points, and calculate the minimum distance between each sampling point in the set of sampling points and an obstacle;

[0050] An update module, configured to update the number and position of sampling points according to the minimum distance between each sampling point and an obstacle and the distance between each sampling point and its adjacent sampling point;

[0051] A determination module, configured to determine the distance between the position of each updated sampling point after update and before update for the updated sampling points;

[0052] A response module, configured to use the updated sampling points as the determined sampling points in response to the distance between the position of each updated sampling point after update and before update being less than a preset threshold;

[0053] An interpolation module, configured to perform interpolation processing on the determined sampling points to obtain an optimized path.

[0054] In a third aspect, an embodiment of the present disclosure provides an electronic device, including a processor, a communication interface, a memory, and a communication bus, where the processor, the communication interface, and the memory communicate with each other through the communication bus;

[0055] The memory is configured to store a computer program;

[0056] The processor is configured to implement the above path optimization method when executing the program stored on the memory.

[0057] In a fourth aspect, an embodiment of the present disclosure provides a computer-readable storage medium, on which a computer program is stored, and characterized in that the computer program implements the above path optimization method when executed by a processor.

[0058] The above technical solutions provided by the embodiments of the present disclosure have at least some or all of the following advantages compared with the prior art:

[0059] For the path optimization method described in the embodiments of the present disclosure, downsampling is performed on the point sequence of a pre-planned path to obtain a set of sampling points, and the minimum distance between each sampling point in the set of sampling points and an obstacle is calculated; the number and position of the sampling points are updated according to the minimum distance between each sampling point and the obstacle and the distance between each sampling point and its adjacent sampling point; for the updated sampling points, the distance between the position of each updated sampling point after update and before update is determined; in response to the distance between the position of each updated sampling point after update and before update being less than a preset threshold, the updated sampling points are used as the determined sampling points; interpolation processing is performed on the determined sampling points to obtain an optimized path. By downsampling, the computational amount of path optimization is reduced, meeting the requirements of a low-computation platform, and taking the distance between the sampling points and the obstacle into account when optimizing the path, ensuring the smoothness of the path and solving the risk problem of hitting an obstacle. Description of the Drawings

[0060] The accompanying drawings here are incorporated into the specification and form a part of this specification, showing embodiments consistent with the present disclosure, and are used together with the specification to explain the principles of the present disclosure.

[0061] To more clearly illustrate the technical solutions in the embodiments of the present disclosure or in the prior art, the following will briefly introduce the accompanying drawings required for use in the description of the embodiments or related technologies. Obviously, for those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0062] Figure 1 Schematically shows a flowchart of a path optimization method according to an embodiment of the present disclosure;

[0063] Figure 2 Schematically shows a flowchart of another path optimization method according to an embodiment of the present disclosure;

[0064] Figure 3 Schematically shows a structural block diagram of a path optimization device according to an embodiment of the present disclosure;

[0065] Figure 4 Schematically shows a structural block diagram of an electronic device according to an embodiment of the present disclosure. Detailed implementation manners

[0066] To make the objectives, technical solutions, and advantages of the embodiments of the present disclosure clearer, the following will clearly describe the technical solutions in the embodiments of the present disclosure with reference to the accompanying drawings in the embodiments of the present disclosure. Obviously, the described embodiments are part of the embodiments of the present disclosure, rather than all of them. Based on the embodiments in the present disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present disclosure.

[0067] See Figure 1 , embodiments of the present disclosure provide a path optimization method, including the following steps:

[0068] S1. Downsample the point sequence of the pre-planned path to obtain a set of sampled points;

[0069] In some embodiments, downsampling the point sequence of the pre-planned path means starting from the starting point, selecting one point every several points and adding it to the set of sampled points. The purpose is to reduce the number of points on the path. For example, if the original number of points on the path is n, and a new path is formed by selecting one point every 1 point, the number of points in the set of sampled points is n / 2.

[0070] S2. Calculate the minimum distance between each sampled point in the set of sampled points and the obstacle;

[0071] S3. Update the number and position of the sampling points according to the minimum distance between each sampling point and the obstacle and the distance between each sampling point and its adjacent sampling point;

[0072] S4. For the updated sampling points, determine the distance between the position of each sampling point after update and before update;

[0073] S5. Determine whether the distance between the position of each sampling point after update and before update is less than a preset threshold. For the newly added sampling points among the updated sampling points, the distance between the position after update and before update is infinity or greater than the preset threshold:

[0074] If so, execute step S6;

[0075] If not, return to step S2;

[0076] S6. Take the updated sampling points as the determined sampling points;

[0077] S7. Perform interpolation processing on the determined sampling points to obtain an optimized path.

[0078] In some embodiments, on the cost map, through the following expression, the cost value on each grid is obtained from the minimum distance between the grid and the obstacle.

[0079] cost = e -a*(d-r) *253

[0080] where d is the minimum distance between the current sampling point and the obstacle, cost is the cost value of the grid where the current sampling point is located on the cost map, a is the attenuation coefficient, and r is the radius of the circumscribed circle of the robot for path tracking. If the robot is circular, r is the radius of the robot.

[0081] Therefore, in step S1, through the following expression, calculate the minimum distance between each sampling point in the sampling point set and the obstacle:

[0082] d = -ln(cost / 253) / a + r

[0083] where d is the minimum distance between the current sampling point and the obstacle, cost is the cost value of the grid where the current sampling point is located on the cost map, a is the attenuation coefficient, and r is the radius of the circumscribed circle of the robot for path tracking.

[0084] In this embodiment, in step S2, the updating the number and position of the sampling points according to the minimum distance between each sampling point and the obstacle and the distance between each sampling point and its adjacent sampling point includes:

[0085] Calculate the size of the collision-free space range of the current sampling point based on the minimum distance between the current sampling point and the obstacle;

[0086] Modify the sampling point based on the distance between the current sampling point and the adjacent sampling point and the size of the collision-free space range of the current sampling point and the adjacent sampling point;

[0087] Adjust the position of the current sampling point according to the distance between the current sampling point and the adjacent sampling point and the minimum distance between the current sampling point and the obstacle.

[0088] In this embodiment, through the following expression, calculate the size of the collision-free space range of the current sampling point according to the minimum distance between the current sampling point and the obstacle:

[0089] ρ = d - r = -ln(cost / 253) / a

[0090] Where ρ is the size of the collision-free space range of the current sampling point, d is the minimum distance between the current sampling point and the obstacle, r is the radius of the circumscribed circle of the robot for path tracking, cost is the cost value of the grid on the cost map where the current sampling point is located, and a is the attenuation coefficient.

[0091] In this embodiment, the modifying the sampling point based on the distance between the current sampling point and the adjacent sampling point and the size of the collision-free space range of the current sampling point and the adjacent sampling point includes:

[0092] When two adjacent sampling points satisfy ||b i -b i+1 ||≥(ρ i +ρ i+1 ), add a new sampling point between the two adjacent sampling points, where the position of the new sampling point is at the midpoint between the two adjacent sampling points;

[0093] When the (i - 1)-th sampling point and the (i + 1)-th sampling point satisfy ||b i-1 -b i+1 ||≤(ρ i-1 +ρ i+1 ), delete the i-th sampling point, where b i-1 is the position of the (i - 1)-th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point, ρ i-1 is the size of the collision-free space range of the (i - 1)-th sampling point, ρ i is the size of the collision-free space range of the i-th sampling point, ρ i+1 is the size of the collision-free space range of the (i + 1)-th sampling point.

[0094] In this embodiment, adjusting the position of the current sampling point according to the distance between the current sampling point and the adjacent sampling point and the minimum distance between the current sampling point and the obstacle includes:

[0095] Step 1: Calculate the gravitational force of the two adjacent sampling points on the i-th sampling point through the following expression:

[0096]

[0097] where, f c is the gravitational force of the two adjacent sampling points on the i-th sampling point, k c is the gravitational coefficient, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point,

[0098] Step 2: Calculate the repulsive force of the obstacle on the sampling point through the following expression:

[0099]

[0100] where, f r is the repulsive force of the obstacle on the sampling point, k r is the gravitational coefficient, ρ0 is the threshold for using the repulsive force, ρ is the size of the collision-free space range of the sampling point, where, is calculated through the following expression:

[0101]

[0102] where, h is the step size, x is (10), y is (01),

[0103] Step 3: Calculate the resultant force f total of the i-th sampling point according to the gravitational force and the repulsive force:

[0104] f total = f c + f r

[0105] Step 4: Update the position of the sampling point through the resultant force:

[0106] b new = b old + αf total

[0107] where, b new is the new position of the sampling point, b old is the original position of the sampling point, α is the update step size, where, αf total < ρ.

[0108] In this embodiment, in step S1, the determined sampling points are interpolated through the following expression:

[0109] p j = b i + j * k * (b i+1 - b i ) / ||b i+1 - b i || where j * k ≤ ||b i+1 - b i ||

[0110] where p j is the position of the j-th point inserted in the path between the i-th sampling point and the (i + 1)-th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point, k is the resolution of the interpolated path, and j is the j-th point inserted in the path between the i-th sampling point and the (i + 1)-th sampling point.

[0111] In this embodiment, in step S1, the interpolation processing of the determined sampling points to obtain an optimized path includes:

[0112] Performing linear interpolation processing on the determined sampling points according to a preset path resolution requirement to obtain a sequence of interpolated sampling points, and using the sequence of interpolated sampling points as the optimized path.

[0113] In another embodiment, in step S2, the updating of the number and position of sampling points according to the minimum distance between each sampling point and an obstacle and the distance between each sampling point and its adjacent sampling point includes:

[0114] Through the following expression, a bubble is formed for each sampling point with the position of the sampling point as the center and the size of the collision-free space range of the sampling point as the radius:

[0115] B(b) = {q: ||b - q|| < ρ(b)}

[0116] where q is a point in the bubble, b is the center position of the bubble, and ρ is the radius of the bubble.

[0117] The set of bubbles formed by the bubbles of each sampling point is used as an elastic band that does not contain obstacles;

[0118] Adjust the position, size, and number of bubbles according to the force on each bubble to form a new elastic band. Among them, referring to Figure 2 , the steps of adjusting the bubbles include:

[0119] First step, define that two adjacent bubbles within the elastic band must overlap;

[0120] It is defined that within the elastic band, except for the two bubbles at the head and tail, each bubble within the elastic band must overlap with adjacent bubbles. The more bubbles there are within the band, the easier it is to meet the condition of bubble overlap. However, the more bubbles there are, the greater the computational amount during the update of the elastic band. Therefore, on the premise of meeting the overlap, the number of bubbles needs to be reduced.

[0121] In the second step, if two adjacent bubbles of a bubble overlap, then this bubble can be deleted.

[0122] Specifically, for bubble i, if bubble i - 1 overlaps with bubble i + 1, then bubble i can be deleted. The specific judgment conditions are as follows:

[0123] ||b i-1 -b i+1 ||≤(ρ i-1 +ρ i+1 )

[0124] Where b i-1 is the center position of the (i - 1)-th bubble, b i+1 is the center position of the (i + 1)-th bubble, ρ i-1 is the radius of the (i - 1)-th bubble, and ρ i+1 is the radius of the (i + 1)-th bubble.

[0125] If bubble i meets the above conditions, then bubble i can be deleted. After performing the above inspection operation on each bubble within the band, proceed to the third step.

[0126] In the third step, if adjacent bubbles do not overlap, then add a bubble in the middle.

[0127] For bubble i and bubble i + 1, if there is no overlap, then a new bubble can be inserted between bubble i and bubble i + 1. The specific judgment conditions are as follows:

[0128] ||b i -b i+1 ||≥(ρ i +ρ i+1 )

[0129] If bubble i meets the above conditions, then add a new bubble. The center position of the new bubble is the midpoint of the line connecting the centers of bubble i and bubble i + 1, and the radius of the new bubble is the size of the collision-free space range of the center of the new bubble.

[0130] In the fourth step, iterate the second step and the third step until there is no bubble addition or deletion, and then proceed to the fifth step;

[0131] In the fifth step, introduce virtual forces. Adjacent bubbles form gravitational forces, and obstacles provide repulsive forces.

[0132] For the i-th bubble, the gravitational forces exerted on it by the two adjacent bubbles (the (i - 1)-th and (i + 1)-th bubbles) are as follows:

[0133]

[0134] where k c is the gravitational coefficient.

[0135] The bubble is also subject to a repulsive force generated by the surrounding obstacles. The magnitude of the repulsive force f r is as follows:

[0136]

[0137] where k r is the gravitational coefficient, ρ0 is the threshold for using the repulsive force, which is given by the following:

[0138]

[0139] where h is the step size, x(1, 0), y(0, 1).

[0140] Step 6: Update the position of the bubble through the resultant force;

[0141] Calculate the resultant force f through the following expression total :

[0142] f total = f c + f r

[0143] Update the position of the bubble through the resultant force:

[0144] b new = b old + αf total

[0145] where b new is the new center position of the bubble, b old is the original center position of the bubble, α is the update step size. The larger α is, the faster it converges to the equilibrium position, but it is also more likely to generate oscillations. To ensure that the updated position is collision-free, i.e., b new is inside the bubble, the following condition needs to be satisfied:

[0146] αf total < ρ.

[0147] Step 7: Update the size of the bubble through the following expression;

[0148] ρ = d - r = -ln(cost / 253) / a

[0149] Among them, ρ is the size of the collision-free space range of the current sampling point, d is the minimum distance between the current sampling point and the obstacle, r is the radius of the circumscribed circle of the robot for path tracking, cost is the cost value of the grid on the cost map where the current sampling point is located, and a is the attenuation coefficient.

[0150] Step 8: Through continuous iteration, a new elastic band is formed;

[0151] Starting from the first bubble to the last bubble, steps 5 to 7 are sequentially executed for each bubble. After execution, steps 2 to 7 are performed on the entire elastic band. The condition for exiting the loop is that in the last iteration, the distance moved by each bubble in the elastic band is less than the threshold, and it is considered that the entire elastic band is in a stable state, the loop ends, and the number and positions of the centers of the bubbles of the new elastic band are used as the number and positions of the updated sampling points.

[0152] The path optimization method of the present disclosure reduces the overall computational amount through downsampling, straightens the path by constructing virtual gravity, and keeps away from obstacles by constructing virtual repulsive forces to ensure that the optimized path will not hit obstacles. At the same time, the positions of the sampling points are updated by using virtual forces, so that the positions of the sampling points are continuous rather than discrete. Therefore, the optimized path is smooth.

[0153] The elastic band of the present disclosure is composed of bubbles. The bubbles of the elastic band are subjected to virtual forces. Adjacent bubbles generate gravitational forces to straighten the elastic band. The bubbles are also simultaneously subjected to repulsive forces generated by obstacles, so that the elastic band keeps away from obstacles. After multiple iterations, finally the bubbles are in a position where the gravitational force and the repulsive force are balanced. Overall, the elastic band is a band that is away from obstacles, smooth and "taut", ensuring the smoothness of the path followed by the robot.

[0154] The path optimization method of the present disclosure optimizes the curved part of the original path on the premise of no collision with obstacles, so as to ensure the smoothness of the path followed by the robot, so as to ensure the smoothness of the control quantity output for path tracking. The overall feeling for the user is that the robot moves smoothly, and it is ensured that the robot walks along a straight line as much as possible in an open space, thereby avoiding the phenomenon of "twisting left and right" of the robot in an open space, achieving the purpose of balancing the contradiction between path planning and path tracking.

[0155] See Figure 3 , the embodiment of the present disclosure provides a path optimization device, including:

[0156] A calculation module 11, configured to perform downsampling on the point sequence of the pre-planned path to obtain a sampling point set, and calculate the minimum distance between each sampling point in the sampling point set and the obstacle;

[0157] An update module 12, configured to update the number and positions of the sampling points according to the minimum distance between each sampling point and an obstacle and the distance between each sampling point and its adjacent sampling point;

[0158] A determination module 13, configured to determine, for each updated sampling point, the distance between its position after update and its position before update;

[0159] A response module 14, configured to, in response to the distance between each sampling point's position after update and its position before update being less than a preset threshold, use the updated sampling points as the determined sampling points;

[0160] An interpolation module 15, configured to perform interpolation processing on the determined sampling points to obtain an optimized path.

[0161] For the implementation processes of the functions and roles of each unit in the above device, refer specifically to the implementation processes of the corresponding steps in the above method, which will not be elaborated here.

[0162] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial descriptions of the method embodiments. The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of the present invention. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0163] In the second embodiment described above, any combination of the calculation module 11, the update module 12, the determination module 13, the response module 14, and the interpolation module 15 can be implemented in one module, or any one of them can be split into multiple modules. Alternatively, at least part of the functions of one or more of these modules can be combined with at least part of the functions of other modules and implemented in one module. At least one of the calculation module 11, the update module 12, the determination module 13, the response module 14, and the interpolation module 15 can be at least partially implemented as a hardware circuit, such as a field-programmable gate array (FPGA), a programmable logic array (PLA), a system on chip, a system on a substrate, a system in a package, an application-specific integrated circuit (ASIC), or any other reasonable way of integrating or packaging circuits, etc., implemented by hardware or firmware, or implemented in any one of the three implementation manners of software, hardware, and firmware, or in an appropriate combination of any several of them. Alternatively, at least one of the calculation module 11, the update module 12, the determination module 13, the response module 14, and the interpolation module 15 can be at least partially implemented as a computer program module, which can execute corresponding functions when the computer program module is run.

[0164] Referring to Figure 4 As shown, the electronic device provided by the embodiment of the present disclosure includes a processor 1110, a communication interface 1120, a memory 1130, and a communication bus 1140. Among them, the processor 1110, the communication interface 1120, and the memory 1130 complete communication with each other through the communication bus 1140;

[0165] The memory 1130 is used to store a computer program;

[0166] When the processor 1110 executes the program stored on the memory 1130, the following path optimization method is implemented:

[0167] Downsample the point sequence of the pre-planned path to obtain a set of sampled points, and calculate the minimum distance between each sampled point in the set of sampled points and the obstacle;

[0168] Update the number and position of the sampled points according to the minimum distance between each sampled point and the obstacle and the distance between each sampled point and its adjacent sampled point;

[0169] For the updated sampled points, determine the distance between the position of each sampled point after update and its position before update;

[0170] In response to the distance between the position of each sampled point after update and its position before update being less than a preset threshold, use the updated sampled points as the determined sampled points;

[0171] Perform interpolation processing on the determined sampled points to obtain an optimized path.

[0172] The above-mentioned communication bus 1140 can be a Peripheral Component Interconnect (PCI) bus, an Extended Industry Standard Architecture (EISA) bus, or the like. The communication bus 1140 can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience of representation, only a thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0173] The communication interface 1120 is used for communication between the above-mentioned electronic device and other devices.

[0174] The memory 1130 can include a Random Access Memory (RAM), and can also include a non-volatile memory, such as at least one disk memory. Optionally, the memory 1130 can also be at least one storage device located far away from the aforementioned processor 1110.

[0175] The above-mentioned processor 1110 can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it can also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0176] The embodiments of the present disclosure also provide a computer-readable storage medium. A computer program is stored on the above-mentioned computer-readable storage medium, and when the computer program is executed by a processor, the path optimization method as described above is implemented.

[0177] The computer-readable storage medium can be included in the device / device described in the above embodiments; it can also exist alone and not be assembled into the device / device. The above-mentioned computer-readable storage medium carries one or more programs, and when the one or more programs are executed, the path optimization method according to the embodiments of the present disclosure is implemented.

[0178] According to an embodiment of the present disclosure, the computer-readable storage medium may be a non-volatile computer-readable storage medium, which may include, for example, but not limited to: portable computer disks, hard disks, random access memories (RAMs), read-only memories (ROMs), erasable programmable read-only memories (EPROMs or flash memories), portable compact disk read-only memories (CD-ROMs), optical storage devices, magnetic storage devices, or any suitable combination of the above. In the present disclosure, the computer-readable storage medium may be any tangible medium that contains or stores a program, and the program may be used by or in combination with an instruction execution system, apparatus, or device.

[0179] It should be noted that, in this document, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements but also includes other elements not expressly listed, or further includes elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "including a..." does not exclude the existence of additional identical elements in the process, method, article or device including the said element.

[0180] The above are only specific embodiments of the present disclosure, enabling those skilled in the art to understand or implement the present disclosure. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present disclosure. Therefore, the present disclosure will not be limited to these embodiments shown herein, but rather should be accorded the widest scope consistent with the principles and novel features claimed herein.

Claims

1. A path optimization method, characterized in that, The method includes: Downsample the point sequence of the pre-planned path to obtain a set of sampled points, and calculate the minimum distance between each sampled point in the set of sampled points and the obstacle; Update the number and position of the sampled points according to the minimum distance between each sampled point and the obstacle and the distance between each sampled point and its adjacent sampled points; For the updated sampled points, determine the distance between the position of each sampled point after update and before update; In response to the distance between the position of each sampled point after update and before update being less than a preset threshold, use the updated sampled points as the determined sampled points; Perform interpolation processing on the determined sampled points to obtain an optimized path.

2. The method according to claim 1, characterized in that, The method further includes: In response to the distance between the position of any one of the updated sampled points after update and before update being greater than or equal to the preset threshold, re-execute the step of calculating the minimum distance between each sampled point and the obstacle until the distance between the position of each sampled point after update and before update is less than the preset threshold, where, for the newly added sampled points among the updated sampled points, the distance between the position after update and before update is infinite or greater than the preset threshold.

3. The method according to claim 1, characterized in that, Calculate the minimum distance between each sampled point in the set of sampled points and the obstacle through the following expression: d = -ln(cost / 253) / a + r where d is the minimum distance between the current sampled point and the obstacle, cost is the cost value of the grid on the cost map where the current sampled point is located, a is the attenuation coefficient, and r is the circumscribed circle radius of the robot for path tracking.

4. The method according to claim 1, characterized in that, The updating the number and position of the sampled points according to the minimum distance between each sampled point and the obstacle and the distance between each sampled point and its adjacent sampled points includes: Calculate the size of the collision-free space range of the current sampled point according to the minimum distance between the current sampled point and the obstacle; Correct the sampled points based on the distance between the current sampled point and its adjacent sampled points and the size of the collision-free space ranges of the current sampled point and the adjacent sampled points; Adjust the position of the current sampled point according to the distance between the current sampled point and its adjacent sampled points and the minimum distance between the current sampled point and the obstacle.

5. The method according to claim 4, characterized in that, Calculate the size of the collision-free space range of the current sampled point according to the minimum distance between the current sampled point and the obstacle through the following expression: ρ = d - r = -ln(cost / 253) / a where ρ is the size of the collision-free space range of the current sampled point, d is the minimum distance between the current sampled point and the obstacle, r is the circumscribed circle radius of the robot for path tracking, cost is the cost value of the grid on the cost map where the current sampled point is located, and a is the attenuation coefficient.

6. The method according to claim 4, characterized in that, The correcting the sampled points based on the distance between the current sampled point and its adjacent sampled points and the size of the collision-free space ranges of the current sampled point and the adjacent sampled points includes: When two adjacent sampling points satisfy ||b i -b i+1 ||≥(ρ i +ρ i+1 ), a new sampling point is added between the two adjacent sampling points, where the position of the new sampling point is at the midpoint between the two adjacent sampling points; When the i-1th sampling point and the i+1th sampling point satisfy ||b i-1 -b i+1 || ≤ (ρ i-1 + ρ i+1 ), delete the i-th sampling point, where b i-1 is the position of the i-1th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the i+1th sampling point, ρ i-1 is the size of the collision-free space range of the i-1th sampling point, ρ i is the size of the collision-free space range of the i-th sampling point, ρ i+1 is the size of the collision-free space range of the i+1th sampling point.

7. The method according to claim 4, characterized in that, The adjusting the position of the current sampled point according to the distance between the current sampled point and its adjacent sampled points and the minimum distance between the current sampled point and the obstacle includes: Step 1, calculate the gravitational force of the two adjacent sampled points on the i-th sampled point through the following expression: Among them, f c is the gravitational force exerted on the i-th sampling point by the two adjacent sampling points, and k c is the gravitational coefficient, b i is the position of the i-th sampling point, and b i+1 is the position of the (i + 1)-th sampling point. Step 2, calculate the repulsive force of the obstacle on the sampled point through the following expression: Among them, f r is the repulsive force on the sampling point by the obstacle, k r is the gravitational coefficient, ρ0 is the threshold for using the repulsive force, and ρ is the size of the collision-free space range of the sampling point. Among them, is calculated by the following expression: where h is the step size, x is (1, 0), and y is (0, 1). Step 3: Calculate the resultant force f based on the gravitational and repulsive forces acting on the i-th sampling point total : f total = f c + f r Step 4: Update the positions of the sampling points through the resultant force: b new = b old + αf total where b new is the new position of the sampling point, b old is the original position of the sampling point, α is the update step size, where αf total <ρ.

8. The method according to claim 1, wherein Interpolate the determined sampling points through the following expression: p j = b i + j * k * (b i+1 - b i ) / ||b i+1 - b i || j * k ≤ ||b i+1 - b i || where p j is the position of the j-th point inserted in the path between the i-th sampling point and the (i + 1)-th sampling point, b i is the position of the i-th sampling point, b i+1 is the position of the (i + 1)-th sampling point, k is the resolution of the interpolated path, and j is the j-th point inserted in the path between the i-th sampling point and the (i + 1)-th sampling point.

9. A path optimization device, characterized in that Including: A calculation module for downsampling the point sequence of the pre-planned path to obtain a set of sampling points and calculating the minimum distance between each sampling point in the set of sampling points and the obstacle; An update module for updating the number and positions of the sampling points according to the minimum distance between each sampling point and the obstacle and the distance between each sampling point and its adjacent sampling point; A determination module for determining the distance between the position of each sampling point after update and its position before update for the updated sampling points; A response module for, in response to the distance between the position of each sampling point after update and its position before update being less than a preset threshold, taking the updated sampling points as the determined sampling points; An interpolation module for interpolating the determined sampling points to obtain an optimized path.

10. An electronic device, characterized in that Including a processor, a communication interface, a memory, and a communication bus. Among them, the processor, the communication interface, and the memory complete communication with each other through the communication bus; The memory is used for storing a computer program; The processor, when executing the program stored on the memory, implements the path optimization method described in any one of claims 1-8.

11. A computer-readable storage medium, on which a computer program is stored, characterized in that The computer program, when executed by the processor, implements the path optimization method described in any one of claims 1-8.

Images