High-order Bezier curve optimization method for robot path planning

Abstract

The invention provides a high-order Bezier curve optimization method for robot path planning, and the method achieves the efficient and accurate robot path planning through the combination of an advanced sensor technology, an improved A * algorithm and a high-order Bezier curve, employs a laser radar or a camera to collect environment data, generates a three-dimensional environment map, and achieves the high-efficiency and accurate robot path planning. The method comprises the following steps of: rasterizing a three-dimensional environment map, dividing a feasible region and an obstacle region, planning a path by adopting an improved A * algorithm, introducing a dynamic adjustment factor mu to optimize a cost function, expanding a search neighborhood to 24 neighborhoods to find an optimal path, smoothing the path by utilizing a high-order Bezier curve, reducing the bending times, and improving the path planning efficiency. In addition, according to the real-time position of the robot and environment changes, the path is dynamically adjusted, the real-time performance and adaptability of the path are ensured, and the invention further relates to electronic equipment and a computer storage medium which are used for executing the path planning method.

Description

Technical Field

[0001] The present invention relates to the field of intelligent planning, and more specifically, to a high-order Bezier curve optimization method for robot path planning. Background Art

[0002] In recent years, with the development of intelligent control and machine learning technologies, path planning technology has begun to integrate these advanced technologies, for example, using deep learning networks to handle path planning problems in high-dimensional spaces, or using reinforcement learning methods to optimize the decision-making process of path planning.

[0003] In the practice of path planning, Bezier curves are widely used in path smoothing. Bezier curves define the shape of the curve through control points and can generate smooth and continuous paths, which is particularly important for path planning of robots and autonomous vehicles. High-order Bezier curves can provide more control points to achieve more complex path planning.

[0004] The robot path planning in the prior art is insufficient in progress and efficiency, and the number of bends in the path is too many, resulting in unstable operation of the robot. Summary of the invention

[0005] In order to overcome the problems of insufficient progress and efficiency of robot path planning in the prior art, too many bends in the path, resulting in unstable operation of the robot, etc., the present invention designs a high-order Bezier curve optimization method for robot path planning that can effectively solve the above technical problems.

[0006] In order to solve the above technical problems, the technical solution of the present invention is as follows:

[0007] A high-order Bezier curve optimization method for robot path planning, comprising the following steps:

[0008] Use laser radar or camera to collect environmental data and generate three-dimensional environmental maps;

[0009] rasterizing the three-dimensional environment map to divide it into feasible areas and obstacle areas;

[0010] The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N);

[0011] Expand the search neighborhood of the A* algorithm to 24 neighborhoods to find the optimal path;

[0012] Use high-order Bezier curves to smooth the path and reduce the number of bends;

[0013] Dynamically adjust the path according to the robot's real-time position and environmental changes.

[0014] Preferably, the specific steps of collecting environmental data using a laser radar or a camera to generate a three-dimensional environmental map include:

[0015] Install and calibrate lidar or cameras to obtain precise environmental data;

[0016] Scan the environment in real time to collect distance and angle data;

[0017] A three-dimensional environmental map is constructed using the collected environmental data, and the three-dimensional environmental map is updated to reflect environmental changes.

[0018] Preferably, the specific steps of rasterizing the three-dimensional environment map and dividing the feasible area and the obstacle area include: constructing a three-dimensional environment map

[0019] Convert the preprocessed environmental data into three-dimensional coordinate points;

[0020] The three-dimensional environment map is constructed using the three-dimensional coordinate points to reflect obstacles and terrain information in the environment;

[0021] Converting the three-dimensional environment map into a two-dimensional grid map to facilitate path planning algorithm processing;

[0022] Set the grid size, which is weighed based on the actual application scenario and computing resources;

[0023] Divide the grid coordinate system and assign unique coordinates to each grid;

[0024] Analyzing obstacle information in the three-dimensional environment map to determine the location of the obstacle in the two-dimensional grid map;

[0025] According to the location of the obstacle, the corresponding grid is marked as the obstacle area;

[0026] Mark the grids not marked as obstacle areas as feasible areas;

[0027] Check the adjacent grids. If there is a narrow passage between the obstacle area and the feasible area, make appropriate adjustments to ensure the accuracy of path planning.

[0028] Smoothing the grid map, monitoring environmental changes in real time, and when environmental changes are detected, timely updating the two-dimensional grid map and re-marking the feasible area and the obstacle area;

[0029] Ensure that the grid map always reflects the current environmental conditions and provides an accurate basis for path planning.

[0030] The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N) including:

[0031] Create two sets, an open set and a closed set. The open set is used to store the nodes to be checked, and the closed set is used to store the nodes that have been checked.

[0032] Set g(N)=0 and h(N) for the starting point;

[0033] Calculate the cost function of the starting point f(N) = g(N) + μ*h(N), where μ is a dynamic adjustment factor;

[0034] If the open set is empty, no path is found and the algorithm ends;

[0035] Find the node with the lowest f(N) value from the open set and record it as the current node N;

[0036] Moving the current node from the open set to the closed set;

[0037] If the current node is the target point, the path planning is completed and all neighbor nodes of the current node are traversed;

[0038] Dynamically adjust the value of μ according to environmental changes and requirements during path planning;

[0039] If special areas are encountered during path planning, increase the value of μ so that the algorithm is more inclined to avoid these areas;

[0040] If a faster path search is needed, the value of μ can be reduced to make the algorithm more inclined to use heuristic information;

[0041] Starting from the target point, trace back to the starting point along the parent node of each node to build a complete path;

[0042] The path is reversed to obtain a path from the starting point to the target point.

[0043] Output the constructed path and smooth the path to improve the actual driving efficiency of the robot.

[0044] Preferably, traversing all neighbor nodes of the current node further includes:

[0045] If the neighbor node is in the closed set, skip the neighbor node;

[0046] Calculate the movement cost from the current node to the neighbor node, denoted as tentative_g_score;

[0047] If the neighbor node is not in the open set, or the tentative_g_score is less than the g(N) of the neighbor node, update the information of the neighbor node:

[0048] Set the parent node of the neighbor node to be the current node;

[0049] Update g(N) of the neighbor node to tentative_g_score;

[0050] Recalculate h(N) of the neighbor node;

[0051] Update the cost function of the neighbor node f(N)=g(N)+μ*h(N);

[0052] If the neighbor node is not in the open set, add it to the open set.

[0053] Preferably, the method of smoothing the path using a high-order Bezier curve to reduce the number of bends includes:

[0054] According to the improved A* algorithm or other path planning algorithms, obtain the preliminary path planning results of the robot;

[0055] Extracting key nodes on the preliminary path to form a path point sequence;

[0056] Using a high-order Bezier curve to fit the path point sequence;

[0057] Determine the control points of the Bezier curve, which will determine the shape of the curve;

[0058] Using an optimization algorithm to adjust the position of the control point;

[0059] Set optimization goals to minimize path length, maximize path smoothness, or both;

[0060] Until a set of control points is found so that the fitting degree between the Bezier curve and the path point sequence reaches a preset standard.

[0061] An electronic device, comprising:

[0062] A memory storing executable program code;

[0063] a processor coupled to the memory;

[0064] The processor calls the executable program code stored in the memory to execute the high-order Bezier curve optimization method for robot path planning as described above.

[0065] A computer storage medium stores computer instructions, which, when called, are used to execute the above-mentioned high-order Bezier curve optimization method for robot path planning.

[0066] Compared with the prior art, the beneficial effects of the technical solution of the present invention are as follows: the present invention obtains environmental data in real time through high-precision sensors to ensure the accuracy and real-time nature of environmental information, constructs a three-dimensional environmental map that can more realistically reflect the actual environment, including terrain, obstacles, etc., and provides an accurate basis for subsequent path planning, converts the three-dimensional environmental map into a two-dimensional grid map, simplifies the processing difficulty of the path planning algorithm, improves the calculation efficiency, clearly distinguishes between feasible areas and obstacle areas, provides clear boundaries for path planning, reduces the blindness of path search, monitors environmental changes in real time, updates the grid map, ensures the accuracy and real-time nature of path planning, introduces a dynamic adjustment factor μ to optimize the cost function f(N), enables the algorithm to dynamically adjust the search strategy according to different situations, and improves Improve the efficiency of path planning, expand the search neighborhood to 24 neighborhoods, increase the flexibility of the search, help find a better path, dynamically adjust the value of μ according to environmental changes and needs, make the algorithm more flexible, and be able to cope with complex and changing environments, use high-order Bezier curves to smooth the path, reduce the number of bends in the path, make the path smoother, and improve the driving stability of the robot. Adjust the control point position of the Bezier curve through the optimization algorithm to ensure that the fit between the curve and the path point sequence meets the preset standard, and optimize the length and smoothness of the path. Dynamically adjust the path according to the real-time position of the robot and environmental changes: During the robot's driving process, it perceives environmental changes in real time and dynamically adjusts the path according to the new environmental information to ensure the real-time and accuracy of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] In order to more clearly illustrate the implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings in the following description are only exemplary, and for ordinary technicians in this field, other implementation drawings can be derived based on the provided drawings without paying any creative work.

[0068] Figure 1 A step-by-step diagram of a high-order Bezier curve optimization method for robot path planning. DETAILED DESCRIPTION

[0069] The drawings are for illustrative purposes only and should not be construed as limiting the present patent;

[0070] In order to better illustrate the present embodiment, some parts in the drawings may be omitted, enlarged or reduced, and do not represent the size of the actual product;

[0071] It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0072] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0073] Example

[0074] A high-order Bezier curve optimization method for robot path planning, such as Figure 1 As shown, the following steps are included:

[0075] Use laser radar or camera to collect environmental data and generate three-dimensional environmental maps;

[0076] rasterizing the three-dimensional environment map to divide it into feasible areas and obstacle areas;

[0077] The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N);

[0078] Expand the search neighborhood of the A* algorithm to 24 neighborhoods to find the optimal path;

[0079] Use high-order Bezier curves to smooth the path and reduce the number of bends;

[0080] Dynamically adjust the path according to the robot's real-time position and environmental changes.

[0081] Bezier curve is a mathematical parameterized curve that defines the shape of the curve through a set of control points. In path planning, Bezier curve is widely used because of its smooth and natural characteristics. By adjusting the position and number of control points, the shape of the curve can be flexibly controlled to meet the needs of robot path planning.

[0082] The specific steps of using laser radar or camera to collect environmental data and generate a three-dimensional environmental map include:

[0083] Install and calibrate lidar or cameras to obtain precise environmental data;

[0084] Scan the environment in real time to collect distance and angle data;

[0085] A three-dimensional environmental map is constructed using the collected environmental data, and the three-dimensional environmental map is updated to reflect environmental changes.

[0086] The specific steps of rasterizing the three-dimensional environment map and dividing the feasible area and the obstacle area include: constructing a three-dimensional environment map

[0087] Convert the preprocessed environmental data into three-dimensional coordinate points;

[0088] The three-dimensional environment map is constructed using the three-dimensional coordinate points to reflect obstacles and terrain information in the environment;

[0089] Converting the three-dimensional environment map into a two-dimensional grid map to facilitate path planning algorithm processing;

[0090] Set the grid size, which is weighed based on the actual application scenario and computing resources;

[0091] Divide the grid coordinate system and assign unique coordinates to each grid;

[0092] Analyzing obstacle information in the three-dimensional environment map to determine the location of the obstacle in the two-dimensional grid map;

[0093] According to the location of the obstacle, the corresponding grid is marked as the obstacle area;

[0094] Mark the grids not marked as obstacle areas as feasible areas;

[0095] Check the adjacent grids. If there is a narrow passage between the obstacle area and the feasible area, make appropriate adjustments to ensure the accuracy of path planning.

[0096] Smoothing the grid map and monitoring environmental changes in real time, such as obstacle movement, new obstacles, etc.; when environmental changes are detected, timely updating the two-dimensional grid map and re-marking the feasible area and the obstacle area;

[0097] Ensure that the grid map always reflects the current environmental conditions and provides an accurate basis for path planning.

[0098] The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N) including:

[0099] Create two sets, OpenSet and ClosedSet. The OpenSet is used to store the nodes to be checked, and the ClosedSet is used to store the nodes that have been checked.

[0100] Set g(N) = 0 (the cost from the starting point to the current node) and h(N) (the heuristic cost, the estimated cost from the current node to the target point) for the starting point;

[0101] Calculate the cost function of the starting point f(N) = g(N) + μ*h(N), where μ is a dynamic adjustment factor;

[0102] If the open set is empty, no path is found and the algorithm ends;

[0103] Find the node with the lowest f(N) value from the open set and record it as the current node N;

[0104] Moving the current node from the open set to the closed set;

[0105] If the current node is the target point, the path planning is completed and all neighbor nodes of the current node are traversed;

[0106] Dynamically adjust the value of μ according to environmental changes and requirements during path planning;

[0107] If special areas are encountered during path planning, such as dynamic obstacles and dangerous areas, increase the value of μ so that the algorithm is more inclined to avoid these areas;

[0108] If a faster path search is needed, the value of μ can be reduced to make the algorithm more inclined to use heuristic information;

[0109] Starting from the target point, trace back to the starting point along the parent node of each node to build a complete path;

[0110] The path is reversed to obtain a path from the starting point to the target point.

[0111] Output the constructed path and smooth the path to improve the actual driving efficiency of the robot.

[0112] The traversing of all neighbor nodes of the current node further includes:

[0113] If the neighbor node is in the closed set, skip the neighbor node;

[0114] Calculate the moving cost from the current node to the neighboring node, recorded as tentative_g_score; in the A* algorithm, tentative_g_score is a key variable, which represents the temporary or tentative path cost (g(n)) from the starting node through the current node to the neighboring node. This value is dynamically calculated and used to determine whether there is a better path to reach a certain node.

[0115] If the neighbor node is not in the open set, or the tentative_g_score is less than the g(N) of the neighbor node, update the information of the neighbor node:

[0116] Set the parent node of the neighbor node to be the current node;

[0117] Update g(N) of the neighbor node to tentative_g_score;

[0118] Recalculate h(N) of the neighbor node;

[0119] Update the cost function of the neighbor node f(N)=g(N)+μ*h(N);

[0120] If the neighbor node is not in the open set, add it to the open set.

[0121] The method of smoothing the path by using a high-order Bezier curve to reduce the number of bends includes:

[0122] According to the improved A* algorithm or other path planning algorithms, obtain the preliminary path planning results of the robot;

[0123] Extracting key nodes on the preliminary path, including a starting point, a target point, and a turning point on the path, to form a path point sequence;

[0124] A suitable high-order Bezier curve, such as a cubic or quartic Bezier curve, at least cubic, to fit the sequence of path points;

[0125] Determine the control points of the Bezier curve, which will determine the shape of the curve; usually, they need to be determined by an optimization algorithm to minimize the error between the curve and the original path points.

[0126] Using an optimization algorithm, such as a least squares method, a genetic algorithm, or other machine learning algorithms, to adjust the positions of the control points;

[0127] Set optimization goals to minimize path length, maximize path smoothness, or both;

[0128] Until a set of control points is found so that the fitting degree between the Bezier curve and the path point sequence reaches a preset standard.

[0129] An electronic device, comprising:

[0130] A memory storing executable program code;

[0131] a processor coupled to the memory;

[0132] The processor calls the executable program code stored in the memory to execute the high-order Bezier curve optimization method for robot path planning as described above.

[0133] A computer storage medium stores computer instructions, which, when called, are used to execute the above-mentioned high-order Bezier curve optimization method for robot path planning.

[0134] During the specific implementation process, use installed and calibrated lidar or camera equipment to scan the surrounding environment in real time, collect distance and angle data, and ensure that the sensor accuracy meets the requirements to obtain accurate environmental information.

[0135] The collected environmental data is converted into three-dimensional coordinate points and these points are used to construct a three-dimensional environmental map.

[0136] Obstacles (such as walls, furniture, etc.) and terrain information (such as slopes, steps, etc.) must be clearly identified in the map.

[0137] The 3D environment map is updated in real time to reflect changes in the environment, such as moving obstacles or newly appearing obstacles.

[0138] Convert the 3D environment map into a 2D grid map to simplify the processing of the path planning algorithm.

[0139] Set a reasonable grid size to balance the relationship between computing resources and map accuracy.

[0140] Grid coordinate system division

[0141] Assign unique coordinates to each raster and construct a raster coordinate system.

[0142] Analyze the obstacle information in the three-dimensional environment map and determine its position in the two-dimensional grid map.

[0143] The grid containing obstacles is marked as the obstacle area, and the remaining grids are marked as the feasible area.

[0144] Check adjacent grids. If there is a narrow passage between the obstacle area and the feasible area, make appropriate adjustments to ensure the accuracy of path planning.

[0145] Smooth the raster map to remove false marks due to noise or errors.

[0146] Create open and closed sets to store nodes during path planning.

[0147] Set g(N)=0 and heuristic function h(N) as the starting point.

[0148] A dynamic adjustment factor μ is introduced to calculate the cost function of the starting point f(N)=g(N)+μ*h(N).

[0149] Dynamically adjust the value of μ to adapt to different environmental changes and path planning requirements.

[0150] Find the node with the lowest f(N) value from the open set as the current node and move it to the closed set.

[0151] Traverse all neighbor nodes of the current node, update their g(N), h(N) and f(N) values, and add qualified neighbor nodes to the open set.

[0152] When the target node is found, start from the target node and trace back to the starting point along the parent node of each node to build a complete path.

[0153] Reverse the path to get the path from the starting point to the target point.

[0154] Extract key nodes from the path generated by the A* algorithm to form a path point sequence.

[0155] Use a high-order Bezier curve to fit the path point sequence and determine the control points of the Bezier curve.

[0156] Set an optimization goal such as minimizing path length, maximizing path smoothness, or both.

[0157] Use optimization algorithms (such as gradient descent method, genetic algorithm, etc.) to adjust the positions of control points to improve the fit between the Bezier curve and the path point sequence.

[0158] The optimization process is repeated until a preset goodness-of-fit criterion is achieved.

[0159] While the robot is driving, it can perceive environmental changes in real time, such as new obstacles or road changes.

[0160] Based on new environmental information, the planned path is dynamically adjusted to ensure that the robot can reach its destination safely and efficiently.

[0161] The same or similar reference numerals correspond to the same or similar components;

[0162] The terms used in the drawings to describe positional relationships are only used for illustrative purposes and should not be construed as limiting this patent;

[0163] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not limitations on the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made on the basis of the above description. 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 high-order Bezier curve optimization method for robot path planning, characterized in that: The following steps are involved: Use laser radar or camera to collect environmental data and generate three-dimensional environmental maps; rasterizing the three-dimensional environment map to divide it into feasible areas and obstacle areas; The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N); Expand the search neighborhood of the A* algorithm to 24 neighborhoods to find the optimal path; Use high-order Bezier curves to smooth the path and reduce the number of bends; Dynamically adjust the path according to the robot's real-time position and environmental changes.

2. The high-order Bezier curve optimization method for robot path planning according to claim 1, characterized in that: The specific steps of using laser radar or camera to collect environmental data and generate a three-dimensional environmental map include: Install and calibrate lidar or cameras to obtain precise environmental data; Scan the environment in real time to collect distance and angle data; A three-dimensional environmental map is constructed using the collected environmental data, and the three-dimensional environmental map is updated to reflect environmental changes.

3. The high-order Bezier curve optimization method for robot path planning according to claim 1, characterized in that: The specific steps of rasterizing the three-dimensional environment map and dividing the feasible area and the obstacle area include: constructing a three-dimensional environment map Convert the preprocessed environmental data into three-dimensional coordinate points; The three-dimensional environment map is constructed using the three-dimensional coordinate points to reflect obstacles and terrain information in the environment; Converting the three-dimensional environment map into a two-dimensional grid map to facilitate path planning algorithm processing; Set the grid size, which is weighed based on the actual application scenario and computing resources; Divide the grid coordinate system and assign unique coordinates to each grid; Analyzing obstacle information in the three-dimensional environment map to determine the location of the obstacle in the two-dimensional grid map; According to the location of the obstacle, the corresponding grid is marked as the obstacle area; Mark the grids not marked as obstacle areas as feasible areas; Check the adjacent grids. If there is a narrow passage between the obstacle area and the feasible area, make appropriate adjustments to ensure the accuracy of path planning. Smoothing the grid map, monitoring environmental changes in real time, and when environmental changes are detected, timely updating the two-dimensional grid map and re-marking the feasible area and the obstacle area; Ensure that the grid map always reflects the current environmental conditions and provides an accurate basis for path planning.

4. The high-order Bezier curve optimization method for robot path planning according to claim 1, characterized in that: The improved A* algorithm is used for path planning, and the dynamic adjustment factor μ is introduced to optimize the cost function f(N) including: Create two sets, an open set and a closed set. The open set is used to store the nodes to be checked, and the closed set is used to store the nodes that have been checked. Set g(N)=0 and h(N) for the starting point; Calculate the cost function of the starting point f(N) = g(N) + μ*h(N), where μ is a dynamic adjustment factor; If the open set is empty, no path is found and the algorithm ends; Find the node with the lowest f(N) value from the open set and record it as the current node N; Moving the current node from the open set to the closed set; If the current node is the target point, the path planning is completed and all neighbor nodes of the current node are traversed; Dynamically adjust the value of μ according to environmental changes and requirements during path planning; If special areas are encountered during path planning, increase the value of μ so that the algorithm is more inclined to avoid these areas; If a faster path search is needed, the value of μ can be reduced to make the algorithm more inclined to use heuristic information; Starting from the target point, trace back to the starting point along the parent node of each node to build a complete path; Reversing the path to obtain a path from the starting point to the target point; Output the constructed path and smooth the path to improve the actual driving efficiency of the robot.

5. The high-order Bezier curve optimization method for robot path planning according to claim 4, characterized in that: The traversing of all neighbor nodes of the current node further includes: If the neighbor node is in the closed set, skip the neighbor node; Calculate the movement cost from the current node to the neighbor node, denoted as tentative_g_score; If the neighbor node is not in the open set, or the tentative_g_score is less than the g(N) of the neighbor node, update the information of the neighbor node: Set the parent node of the neighbor node to be the current node; Update g(N) of the neighbor node to tentative_g_score; Recalculate h(N) of the neighbor node; Update the cost function of the neighbor node f(N)=g(N)+μ*h(N); If the neighbor node is not in the open set, add it to the open set.

6. The high-order Bezier curve optimization method for robot path planning according to claim 1, characterized in that: The method of smoothing the path by using a high-order Bezier curve to reduce the number of bends includes: According to the improved A* algorithm or other path planning algorithms, obtain the preliminary path planning results of the robot; Extracting key nodes on the preliminary path to form a path point sequence; Using a high-order Bezier curve to fit the path point sequence; Determine the control points of the Bezier curve, which will determine the shape of the curve; Using an optimization algorithm to adjust the position of the control point; Set optimization goals to minimize path length, maximize path smoothness, or both; Until a set of control points is found so that the fitting degree between the Bezier curve and the path point sequence reaches a preset standard.

7. An electronic device, characterized in that: The device comprises: A memory storing executable program code; a processor coupled to the memory; The processor calls the executable program code stored in the memory to execute the high-order Bezier curve optimization method for robot path planning as described in any one of claims 1-6.

8. A computer storage medium, characterized in that: The computer storage medium stores computer instructions, which, when called, are used to execute the high-order Bezier curve optimization method for robot path planning as described in any one of claims 1-6.

Images