A mobile robot path planning method for optimizing safety distance

Abstract

This invention relates to a mobile robot path planning method for optimizing safe distance, belonging to the field of mobile robot path planning technology. The method introduces a soft and hard constraint point judgment method to expand new nodes in the traditional A* path planning method, and adds turning penalty cost and soft and hard constraint point cost during the cost calculation process. After obtaining the path nodes, the path curve is smoothed and optimized using Bézier curves to achieve the optimal planned path while ensuring motion safety.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning technology, and relates to a method for optimizing the path planning of mobile robots with a safe distance. Background Technology

[0002] With the rapid development of the economy and the booming progress of the robotics industry, more and more people hope to have robots replace them in their work. The emergence of mobile robots perfectly meets this demand. Robots move in their environment, performing functions such as grasping, transporting, obstacle avoidance, and path planning. Among these, path planning in mobile robots is an important research direction. Path planning methods for mobile robots are mainly divided into traditional path planning algorithms, sampling-based path planning algorithms, and intelligent biomimetic algorithms.

[0003] Path planning can be broadly divided into two directions: Local Path Planning and Global Path Planning. Global path planning, as the name suggests, decomposes the entire map and finds the optimal path to the target point according to an initially set function. However, it requires accurate prior knowledge of the environment, and it becomes ineffective when encountering unexpected obstacles. As a form of pre-planning, global path planning algorithms do not require high real-time computing power from the robot, and its planning results are optimal and comprehensive. Local path planning algorithms, on the other hand, focus on unknown environments, enabling the robot to have good obstacle avoidance capabilities. They use sensors to detect the surrounding environment and obtain information such as the location and size of obstacles. This type of planning requires continuous collection of environmental data, thus demanding higher real-time computing power from the robot. It can also effectively avoid and correct obstacles that appear suddenly. However, because it lacks global information, the planned path may not be optimal, and it may even fail to find the correct path and target point. During the search process, the robot body is simplified to a point mass, ignoring its actual size; although the planned path effectively avoids impassable obstacle areas, the drawback is that it is too close to the obstacles, and in practical applications, the robot cannot track the path well and is prone to collisions; the planned path has poor smoothness.

[0004] Therefore, a mobile robot path planning method is urgently needed to solve the above problems. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide a mobile robot path planning method that optimizes the safety distance, which makes the mobile robot path planning smoother and is applicable to narrow spaces to reduce the occurrence of failures, while satisfying the path planning task of the mobile robot.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A mobile robot path planning method for optimizing safe distance is proposed. This method introduces a soft and hard constraint point judgment method to expand new nodes in the traditional A* path planning method, and adds turning penalty cost and soft and hard constraint point cost in the cost calculation process. After obtaining the path nodes, the path curve is smoothed and optimized using Bézier curves to achieve the optimal planned path while ensuring motion safety.

[0008] Furthermore, the method for determining soft and hard constraint points specifically includes the following steps:

[0009] 1) When expanding a new node, it is necessary to determine whether there are obstacles around the node. If there are, the node cannot be expanded.

[0010] 2) When determining constraint points, strong constraint points should be determined first, followed by weak constraint points;

[0011] 3) First, search around the node to determine if it intersects with the inner circle; if it does not intersect with the inner circle, then determine if it intersects with the outer circle and add these nodes to the search list.

[0012] 4) Expand the search to the next circle, determine which grids intersect with the circle, and add them to the search list. If the node does not intersect with the inner circle, then determine whether the node intersects with the outer circle.

[0013] 5) Continue to expand the outer circle nodes until the entire circle no longer intersects with the outer circle.

[0014] Furthermore, the formula for calculating the search expansion is:

[0015]

[0016] Where (x,y) represents the two-dimensional coordinates of the node, and n represents the coordinate value;

[0017] To ensure computational precision and accuracy, the given two-dimensional coordinate point (x, y) is fine-tuned by increasing the coordinate increments (add_x, add_y). Specifically, if add_x is less than 0, it is incremented by 0.5; if add_x is greater than 0, it is subtracted by 0.5. Similarly, if add_y is less than 0, it is incremented by 0.5; if add_y is greater than 0, it is subtracted by 0.5. This corrects the values ​​of add_x and add_y, gradually bringing them closer to the nearest integer and eliminating the effects of small numerical changes, resulting in a smoother and more comfortable motion trajectory.

[0018] Furthermore, the specific method for setting the cost of soft and hard constraint points is as follows: Based on the robot's radius r, set the corresponding soft and hard constraint ranges; with the robot's center as the origin, traverse all ranges with a radius of 2r+1, calculating the distance dis of each cell relative to the center; when dis < r, it indicates that the cell is a hard constraint point and must be avoided, therefore its cost is set to a number significantly larger than the total path cost, ensuring that the algorithm avoids hard constraint points during path planning; when dis ∈ (r, 2r), it indicates that the cell is within the range of soft constraint points and should be avoided as much as possible, but if it must be passed through, the cost should be minimized; its cost is set to 1 / dis, and during path planning, cells closer to the robot's center should be prioritized for passage; by changing the search domain according to the robot's radius, it is ensured that robots of different sizes can obtain safe passage paths.

[0019] Furthermore, by using a coefficient slightly larger than the step size to correct the Euclidean distance, the heuristic function is made more realistic. The formula for calculating the turning penalty cost is as follows:

[0020]

[0021] Where h represents the evaluation function, the current point coordinates are (x1, y1), the target point coordinates are (x2, y2), and s represents the step size that the robot increases after each movement.

[0022] The beneficial effects of this invention are as follows: This invention, in addition to traditional A * The path planning method introduces hard and soft constraint points, and sets corresponding weights and priorities (i.e., turning penalty cost and soft and hard constraint point cost) according to the actual situation. Through comprehensive consideration, under the premise of ensuring motion safety, the path curve is smoothed by using Bézier curves, which can make the planned path of the mobile robot smoother and is suitable for narrow spaces to reduce the occurrence of failures, while meeting the path planning task of the mobile robot.

[0023] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0024] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0025] Figure 1This is a flowchart of the mobile robot path planning method for optimizing safety distance according to the present invention;

[0026] Figure 2 Schematic diagram of soft and hard constraint points for robots with different radii;

[0027] Figure 3 A schematic diagram of a smooth optimization strategy for right-angle turns;

[0028] Figure 4 This is a schematic diagram of a smooth optimization strategy for obtuse-angle turns. Detailed Implementation

[0029] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0030] Please see Figures 1-4 This invention provides a method for optimizing the path planning of a mobile robot to improve safety distance, which is an improvement over traditional A * The path planning method introduces hard and soft constraint points (because in robot motion planning, constraint points are generally divided into hard and soft constraint points. Hard constraint points are constraints that must be satisfied; failure to satisfy them will lead to planning failure. Soft constraint points are points that do not need to be satisfied but will affect the planning result), and sets corresponding weights and priorities according to the actual situation. Through comprehensive consideration, the optimal planned path is achieved while ensuring motion safety.

[0031] The process for determining soft and hard constraint points when expanding a new node is as follows:

[0032] 1) When expanding a new node, it is necessary to determine whether there are obstacles around the node. If there are, the node cannot be expanded.

[0033] 2) When determining constraint points, strong constraint points should be determined first, followed by weak constraint points;

[0034] 3) First, search around the node to determine if it intersects with the inner circle; if it does not intersect with the inner circle, then determine if it intersects with the outer circle and add these nodes to the search list.

[0035] 4) Expand the search to the next circle, determine which grids intersect with the circle, and add them to the search list. If the node does not intersect with the inner circle, then determine whether the node intersects with the outer circle.

[0036] 5) Continue expanding the outer circle nodes until the entire circle no longer intersects with the outer circle. The calculation formula for the search expansion is:

[0037]

[0038] Where (x,y) represents the two-dimensional coordinates of the node, and n represents the coordinate value;

[0039] To ensure computational precision and accuracy, the given two-dimensional coordinate point (x, y) is fine-tuned by increasing the coordinate increments (add_x, add_y). Specifically, if add_x is less than 0, it is incremented by 0.5; if add_x is greater than 0, it is subtracted by 0.5. Similarly, if add_y is less than 0, it is incremented by 0.5; if add_y is greater than 0, it is subtracted by 0.5. This corrects the values ​​of add_x and add_y, gradually bringing them closer to the nearest integer and eliminating the effects of small numerical changes, resulting in a smoother and more comfortable motion trajectory. Figure 2 As shown, the robot's radii from left to right are 0.5, 1.0, and 1.5 respectively. The solid circles in the figure represent the robot, the dark gray areas represent hard constraint points, and the light gray areas represent soft constraint points.

[0040] Based on the robot's radius, set corresponding hard and soft constraint ranges. Using the robot's center as the origin, traverse all ranges with a radius of 2r+1, calculating the distance *dis* of each cell relative to the center. When *dis* < r, it indicates a hard constraint point, which must be avoided. Therefore, its cost is set to a number significantly larger than the overall path cost to ensure the algorithm avoids hard constraint points during path planning. When *dis* ∈ (r, 2r), it indicates a soft constraint point, which should be avoided as much as possible, but if passage is necessary, the cost should be minimized. Its cost is set to 1 / *dis*, prioritizing cells closer to the robot's center during path planning. By varying the search domain according to the robot's radius, it ensures safe passage paths for robots of different sizes.

[0041] Traditional A* path planning algorithms only consider the estimated path from the starting node to the target node, resulting in a relatively simple cost function. However, this simple heuristic function leads to a stepped, uneven path. Furthermore, robots need to make frequent turns during actual movement, which consumes more energy, increases wear and tear, and raises the risk of failure. Therefore, this invention introduces a turning penalty mechanism, adding a corresponding penalty cost to nodes that require turning, effectively reducing the number of turns during the journey.

[0042] When performing path planning, in addition to considering the shortest path length, factors such as path smoothness and feasibility also need to be ensured. To better adapt to real-world conditions, this invention uses a coefficient slightly larger than the step size to correct the Euclidean distance, making the heuristic function more realistic. Furthermore, an additional turning penalty term is added to the traditional evaluation function h to penalize turning nodes.

[0043] By using a coefficient slightly larger than the step size to correct the Euclidean distance, the heuristic function becomes more realistic. The specific formula for calculating the turning penalty cost is as follows:

[0044]

[0045] Where h represents the evaluation function, the current point coordinates are (x1, y1), the target point coordinates are (x2, y2), and s represents the step size that the robot increases after each movement.

[0046] Bézier curves, derived from Bernstein polynomials, are generally used to smooth and optimize the polyline portion of a path. Due to their ease of operation and excellent representation of images, they are frequently used in path planning. A Bézier curve is a curve defined by a set of control points and has the following characteristics:

[0047] Symmetry: The coefficients of a Bézier curve are symmetrical, meaning that the i-th coefficient of the curve is the same as the i-th to circum-i coefficient.

[0048] Convex hull property: Bézier curves are always contained within the convex hull of the polygon defined by all control points. This means that a Bézier curve will not leave the convex polygon formed by its control points.

[0049] Endpoint attribute: The first and last control points on the curve are exactly the start and end points of the Bézier curve, which means that the Bézier curve always passes through its start and end points.

[0050] Recursiveness: This means that the coefficients of the Bézier curve satisfy the following formula.

[0051] B i,n (t)=(1-t)B i,n-1 (t)+tB i-1,n-1 (t),{i=1,n,···,n}

[0052] The radius of curvature of a Bézier curve changes smoothly from its starting point to its ending point because it has continuous high-order derivatives. An nth-degree Bézier curve is a parametric curve composed of nth-degree Bernstein basis polynomials. The nth-degree Bernstein polynomial has the following form:

[0053]

[0054] Where i = 0, 1, ..., n, and It is the coefficient of the binomial.

[0055] The expression for an nth-degree Bézier curve is:

[0056]

[0057] Where P(i) represents n+1 control points, and P(0) and P(1) are the starting position and the target position, respectively.

[0058] In two-dimensional space, the curvature of a Bézier curve with respect to t is expressed as:

[0059]

[0060] During the movement of a mobile robot, the turning step length increases when the turning angle is greater than or equal to 90°. This type of turn generally consists of three steps: 1) deceleration and stopping; 2) turning and selecting the direction of travel; 3) continuing forward. The deceleration and acceleration of the mobile robot both require engagement of servo motors, which significantly reduces the robot's speed.

[0061] To improve the practicality of paths, reduce the number of turns, and increase the speed of mobile robots, this invention aims to optimize the turning problem during path planning. The basic idea is to use inflection points in the path as control points, thereby improving the smoothness of the planned path. However, high-order Bézier curves suffer from high computational cost and poor numerical stability. Therefore, this invention sets the robot radius as the influence weight of the Bézier curve order, with the two being negatively correlated. In other words, when the robot radius is large, the order of the Bézier curve function used for smoothing optimization will decrease, ensuring that the mobile robot can smoothly turn at inflection points without affecting its safe distance.

[0062] The following are the specific implementation steps of the mobile robot path planning method of the present invention:

[0063] Step 1: Create an environmental map using the raster method.

[0064] Defining A* nodes: First, an inner class named "Node" is defined to represent nodes in the A* algorithm. Each node has a coordinate, a parent node, a g value (the actual cost from the starting point to the current node), and an h value (the estimated cost from the current node to the end point, calculated using Euclidean distance).

[0065] Step 2: Initialize A* Search: In the initialization method of the "AStar" class, an open list (used to store nodes to be checked) and a close list (used to store checked nodes) are defined. At the same time, a start and end point are set, and traversable markers are defined.

[0066] Step 3: Unit cost, or step size: The algorithm sets different unit costs based on the node's position relative to its parent node (horizontal, vertical, or diagonal). This is to make the algorithm more inclined to choose straight paths rather than diagonal paths. Furthermore, the algorithm also considers the cost of turning; if the current node changes direction compared to its parent node, an additional cost is incurred.

[0067] Step 4: Find the node with the smallest F value: In the "getMinNode" method, find the node with the smallest F value (i.e., the sum of the g and h values) from the openList.

[0068] Step 5: Determine the feasibility of a node: Several methods are provided to determine whether a node is in the closed list (nodes that have already been considered). The "pointInCloseList" and "pointInOpenList" methods are used to check whether a point is already in the closed list or open list, whether it is in the open list (nodes to be considered), and whether the endpoint is in the closed list.

[0069] Step 6: Search for neighboring nodes: The "searchNear" method searches for points around a specified node and checks whether these points are passable (non-obstacle-free) and whether they are already in the closed table. If a neighboring node is passable and not in the closed table, the g value from the starting point to this node is calculated, and this node is added to the open table.

[0070] Step 7: Pathfinding Begins: In the "start" method, first check if the destination is reachable. Then, add the starting point to the open list. Next, a main loop is entered, in which the node with the smallest F value is found, its neighboring nodes are checked, and then this node is added to the closed list. This process continues until a path to the destination is found, or there are no nodes in the open list (meaning there is no feasible path).

[0071] Step 8: Find constraint points: The function cal_dis, used to calculate the Euclidean distance between two points, calculates the distance between the current node and the target node. The "find_two_circle" function finds constraint points and assigns corresponding costs to determine which node is the best next node.

[0072] Step 9: Bézier Curves: The "bezier_normal" function is used to generate Bézier curves at given control points. This completes path smoothing optimization.

[0073] Step 10: Termination Condition: If the destination is added to the closed list, a path has been found, and this path is returned. If the open list is empty, no path leads to the destination, and the algorithm returns. In each iteration, the algorithm searches a new set of nodes and updates their g and h values. Once the destination is found, the algorithm returns the path from the start point to the destination. If no path is found, the algorithm returns an error message.

[0074] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for optimizing the path planning of a mobile robot to ensure safe distance, characterized in that, In the traditional A* path planning method, a method for judging soft and hard constraint points is introduced to expand new nodes, and turning penalty cost and soft and hard constraint point cost are added in the cost calculation process. After obtaining the path nodes, the path curve is smoothed and optimized using Bézier curves to achieve the optimal planned path while ensuring motion safety. The method for determining soft and hard constraint points specifically includes the following steps: 1) When expanding a new node, it is necessary to determine whether there are obstacles around the node. If there are, the node cannot be expanded. 2) When determining constraint points, hard constraint points should be determined first, followed by soft constraint points; 3) First, search around the node and determine if it intersects with the inner circle; if it does not intersect with the inner circle, determine if it intersects with the outer circle and add these nodes to the search list. 4) Expand the search to the next circle, determine which grids intersect with the circle, and add them to the search list. If the node does not intersect with the inner circle, then determine whether the node intersects with the outer circle. 5) Continue to expand the outer circle nodes until the entire circle no longer intersects with the outer circle.

2. The mobile robot path planning method according to claim 1, characterized in that, The formula for calculating search expansion is: in,( x , y () represents the two-dimensional coordinates of the node. n Represents coordinate values; To ensure calculation accuracy and precision, for a given two-dimensional coordinate point ( x , y Fine-tune the coordinates by increasing the coordinate increment (add_x, add_y). Specifically, if add_x is less than 0, add 0.5 to it; if add_x is greater than 0, subtract 0.5 from it. If add_y is less than 0, add 0.5 to it; if add_y is greater than 0, subtract 0.5 from it.

3. The mobile robot path planning method according to claim 1, characterized in that, The specific method for setting the cost of soft and hard constraint points is as follows: Based on the robot's radius r, set the corresponding soft and hard constraint ranges; with the robot's center as the origin, traverse all ranges with a radius of 2r+1, calculating the distance dis of each cell relative to the center; when dis < r, it indicates that the cell is a hard constraint point and must be avoided, therefore its cost is set to a number significantly larger than the overall path cost, ensuring the algorithm avoids hard constraint points during path planning; when dis ∈ (r, 2r), it indicates that the cell is within the soft constraint point range and should be avoided as much as possible, but if it must be passed through, the cost should be minimized; its cost is set to 1 / dis, and during path planning, cells closer to the robot's center should be prioritized for passage; by changing the search domain according to the robot's radius, it ensures that robots of different sizes can obtain safe passage paths.

4. The mobile robot path planning method according to claim 1, characterized in that, The formula for calculating the turning penalty cost is as follows: in, h This represents the evaluation function, where the current point coordinates are (x1, y1) and the target point coordinates are (x2, y2). This indicates the step size that the robot increases after each movement.

Images