Path planning method and system based on target guide angle

Abstract

The invention discloses a path planning method and system based on a target guide angle, and relates to path planning, and the method comprises the steps: initializing parameters, and creating a grid map; the parameters comprise a starting point, a target point and the number N of obstacles; calculating a neighborhood obstacle rate according to the number N of the obstacles, and adjusting a search neighborhood range according to the neighborhood obstacle rate; adjusting the search direction according to the target guide angle, and determining the expansion range of the current search node; obtaining a to-be-expanded node meeting a preset safety condition in the determined expansion range through an inter-node safety margin evaluation strategy; traversing and searching the nodes to be expanded by using the improved GOA-A * algorithm, and generating a planned path from the starting point to the target point; wherein the improved GOA-A * algorithm is obtained by setting an angle factor and a distance factor in a heuristic function of an A * algorithm; for the problem that in the prior art, the A * algorithm is low in search efficiency, the angle factor, the distance factor and the like are introduced into the heuristic function, and the search efficiency is improved.

Description

[0001] Technology Neighborhood

[0002] The present application relates to a path planning neighborhood, and more specifically, to a path planning method and system based on a target guidance angle. Background Art

[0003] With the rapid development of artificial intelligence technology, the autonomy and intelligence level of mobile robots have been improved. Whether it is a robot providing services in an indoor environment or a robot performing inspection tasks in an outdoor environment, its movement is inseparable from autonomous navigation technology. Path planning is the key to realizing mobile robot navigation. It mainly means that in an environment with obstacles, the mobile robot can find a collision-free path from the starting point to the target point safely and efficiently according to certain evaluation indicators.

[0004] At present, common path planning algorithms include A* algorithm, DijkstrA* algorithm, RRT algorithm, genetic algorithm, ant colony algorithm, etc. Among them, A* algorithm has been widely used in game development, robot navigation, map routing, logistics and transportation due to its high efficiency, flexibility, intuitiveness and easy implementation, and is suitable for path planning problems in different scenarios. The working principle of A* algorithm is to model the static environment of the robot in the form of a grid map, and then convert the grid map into a weighted graph structure, and realize global path planning by finding an optimal combination of edges and nodes from the starting point to the target node in the graph structure.

[0005] Compared with other graph-based path planning algorithms such as depth-first search and breadth-first search, the A* algorithm has the characteristics of heuristic search. It can quickly and effectively search for the optimal path from the start node to the target node based on known environmental information and guided by the heuristic function. However, the traditional A* algorithm still has the following defects: the algorithm can only search up to 8 adjacent nodes around the node, the path selection has limitations, and it is easy to produce many turning points, which is not conducive to the fast and smooth driving of the mobile robot; affected by obstacles, the estimated value and the true value of the heuristic function have a large deviation, the algorithm will expand a large number of redundant nodes, and reduce the search efficiency; the path planned by the algorithm may collide with surrounding obstacles when expanding obliquely, and the safety is low, which is not conducive to the safe driving of the robot in the actual environment.

[0006] Due to the above defects, the planning results of the traditional A* algorithm are often not ideal and need further improvement. Summary of the invention

[0007] 1. Technical problems to be solved

[0008] In order to solve the problem of low search efficiency of the A* algorithm in the prior art, the present application provides a path planning method and system based on the target guidance angle, which improves the search efficiency by introducing angle factors and distance factors in the heuristic function.

[0009] 2. Technical solution

[0010] The purpose of this application is achieved through the following technical solutions.

[0011] One aspect of the present specification provides a path planning method based on a target steering angle, including: initializing parameters and creating a grid map; the parameters include a starting point, a target point and the number of obstacles N; calculating a neighborhood obstacle rate according to the number of obstacles N, and adjusting the search neighborhood range according to the neighborhood obstacle rate; and adjusting the search direction according to the target steering angle to determine the expansion range of the current search node; obtaining nodes to be expanded that meet preset safety conditions within a determined expansion range through an inter-node safety margin evaluation strategy; using an improved GOA-A* algorithm to traverse and search for nodes to be expanded, and generating a planned path from the starting point to the target point; wherein the improved GOA-A* algorithm is obtained by setting an angle factor and a distance factor in the heuristic function of the A* algorithm.

[0012] On the one hand, the traditional A* algorithm only considers the distance from the node to the target point in the heuristic function, without considering the density of obstacles around the node. In areas with dense obstacles, even if the node is close to the target, the actual path cost will be large, causing the search direction to deviate from the optimal path. On the other hand, the traditional A* algorithm tends to search in the direction of the target point, ignoring other directions that may be better. This single search strategy may fall into the local optimum and miss the global optimal solution.

[0013] In this application, on the one hand, an angle factor is introduced into the heuristic function, the search direction of the node is compared with the direction of the target point, and the node with the same direction as the target is searched first. This direction-based heuristic can effectively avoid the search from deviating from the optimal path and improve the search efficiency. On the other hand, a distance factor is introduced into the heuristic function, and the distance from the node to the starting point and the target point is comprehensively considered to balance the global path cost. By taking into account the two dimensions of distance and direction, the priority of the node can be evaluated more accurately to avoid the local optimal trap. In addition, the search neighborhood range of the node is dynamically adjusted according to the density of obstacles around the node. A larger search neighborhood is used in the obstacle sparse area to accelerate the search process; a smaller search neighborhood is used in the obstacle dense area to improve the search accuracy. This adaptive neighborhood adjustment strategy can realize efficient and accurate path search in environments of different complexity. Finally, when expanding the node, the unsafe nodes that are too close to the obstacle are filtered out by evaluating the safety margin between nodes. This safety constraint not only ensures the feasibility of the path, but also reduces the search of invalid nodes, further improving the efficiency of the algorithm.

[0014] Furthermore, the neighborhood obstacle rate is calculated according to the number of obstacles N, including: calculating the current node p i The number of obstacles N within the preset range; according to the number of obstacles N, calculate the neighborhood obstacle rate; set the search step length according to the neighborhood obstacle rate, when the neighborhood obstacle rate is greater than the preset threshold, set the step length L1, otherwise set the step length L2; according to the set step length L1 or L2, set the current node p i The search range of L2 is greater than L1. The higher the obstacle rate, the more obstacles there are in the area, the greater the search difficulty, and a smaller search step size L1 should be used. Conversely, when the obstacle rate is low, a larger search step size L2 can be used. Dynamically adjusting the search step size can quickly expand the search range in areas with sparse obstacles and reduce unnecessary node traversal. In areas with dense obstacles, a small step size is used to ensure the accuracy and safety of the search. This adaptive search strategy can improve overall efficiency while ensuring search quality.

[0015] According to the current node, adjacent node V c and the target node V g The target steering angle α is calculated; the range of the target steering angle α is divided into multiple regions; each region is traversed, an angle threshold ε0 is set, and the target steering angle α is compared with the angle threshold ε0. If the target steering angle α is less than the angle threshold ε0, the corresponding node p is j As the current node p iIf not, adjust the angle threshold ε0 and continue searching; the adjacent nodes whose target guidance angle is less than the angle threshold are preferentially selected as the expansion nodes. This means that the search will proceed in the general direction of the target node, reducing invalid attempts in the wrong direction.

[0016] For the current node p i Repeat the above expansion steps for all adjacent nodes until the target guidance angle α is less than the angle threshold ε0, and the current node p is obtained. i If there is no node that meets the angle threshold, adjust the threshold and continue searching until a suitable node is found. This method introduces the guidance of target information in the expansion direction, making the search more targeted. Combining the set search neighborhood and expansion range, we can get the current node p i The ultimate expansion range.

[0017] Furthermore, the neighborhood obstacle rate obs rate The calculation formula is:

[0018] Furthermore, the calculation formula of the target guidance angle α is:

[0019] V c =(c x -n x ,c y -n y ), V g =(g x -n x ,g y -n y ), In the formula (n x ,n y ) is the current point coordinate, (g x ,g y ) is the target point coordinate, are the coordinates of adjacent nodes; represents the x-coordinate of the ith adjacent node, represents the y coordinate of the i-th adjacent node;

[0021] Furthermore, through the safety margin evaluation strategy between nodes, the nodes to be expanded that meet the preset safety conditions are obtained within the determined expansion range, including: according to each child node in the determined expansion range, the child node is connected with the corresponding parent node to generate a straight line L; the vertical distance d from each obstacle node in the grid map to the generated straight line L is calculated; it is determined whether the obtained vertical distance is greater than the preset distance D, if so, the corresponding child node is used as the node to be expanded of the parent node, if not, the corresponding child node is deleted; all child nodes within the expansion range are traversed to obtain the final node to be expanded.

[0022] Specifically, the present application connects each child node within the extended range with the corresponding parent node to generate a straight line L. The vertical distance d from each obstacle node in the grid map to the straight line L is calculated. It is possible to quickly determine whether the line between the child node and the parent node collides with the obstacle without traversing all the points on the line. By simplifying the calculation of collision detection, the time overhead of safety margin evaluation is reduced and the evaluation efficiency is improved.

[0023] Furthermore, the vertical distance d from each obstacle node in the grid map to the generated straight line L is calculated using the following formula: d = vertical dif ×cos(θ),vertical dif =|y o -(k*x o +b)|, b=y p -k*x p ,y cp =kx+b, where (x p ,y p ) is the parent node coordinate, (x c ,y c ) is the child node coordinate, (x o ,y o ) is the obstacle coordinate, θ represents the angle between the straight line L and the horizontal line; vertical dif The y value of the obstacle o Coordinates and x on line L o The absolute value of the corresponding y-coordinate difference; k represents the slope of the straight line equation formed by the child node and the parent node; b represents the intercept of the straight line equation formed by the child node and the parent node; y cp Represents the equation of the line formed by the child node and the parent node;

[0024] Furthermore, an improved GOA-A* algorithm is obtained, including: calculating the distance dn from the current node to the target node; calculating the distance ds from the start node to the target node; and setting a heuristic function f(n) according to dn and ds to obtain the improved GOA-A* algorithm.

[0025] Furthermore, the heuristic function f(n) is expressed as follows:

[0026] f(n)=g(n)+(weight+β*angle weight +γ*dis weight )*h(n), Ax+By+C=0, Among them, angle weight represents the target steering angle weight function; weight Represents the distance function from the neighboring node to the optimal path; β and γ are angle weight and dis weight The weight coefficient of ; g(n) represents the actual cost function; h(n) represents the evaluation function; weight represents the distance weight function from the current node to the target node; A, B, and C represent the independent variable coefficient, dependent variable coefficient, and constant term of the straight line equation formed by the starting node and the target node, respectively.

[0027] Further, a planned path from the starting point to the target point is generated, including: using the improved GOA-A* algorithm to traverse and search for nodes to be expanded to generate an initial path path0; traversing the vector composed of three adjacent nodes in the initial path path0, determining the node collinearity by calculating the vector angle, removing the intermediate nodes with an angle of zero, and obtaining the path path1 after the first removal of redundant points; taking the starting point of the path path1 as the starting node, and determining whether the node connection passes through the obstacle by calculating the distance between the connecting line of all nodes between the starting node and the end point of the path and the obstacle; obtaining the node closest to the starting node and not passing through the obstacle as the new starting node; removing the redundant nodes between the original starting node and the new starting node, and obtaining the path path2 after the second removal of redundant points; reversing the initial path path0, repeating the first and second steps of removing redundant nodes, and obtaining the path path after the reverse removal of redundant points 2,reverse ; Extract path2 and path 2,reverse The common nodes of the nodes are used to form a new sequence. By comparing the lengths of the forward and reverse sections between adjacent common nodes in the new sequence and the number of nodes, the sections with fewer nodes or less than the threshold value are selected to replace the original node sequence to obtain the final planned path.

[0028] Another aspect of the present specification provides a path planning system based on a target guidance angle, characterized in that it includes: a map module, which initializes parameters and creates a grid map; the parameters include a starting point, a target point and the number of obstacles N; an extension range module, which calculates the neighborhood obstacle rate according to the number of obstacles N, and adjusts the neighborhood search range according to the neighborhood obstacle rate; and adjusts the neighborhood search direction according to the target guidance angle; determines the extension range of the current search node according to the adjusted neighborhood search range and search direction; an extension node module, which obtains nodes to be extended that meet preset safety conditions within the determined extension range; a path generation module, which uses an improved GOA-A* algorithm to traverse the nodes to be extended and generate an initial path; sets an angle factor and a distance factor, and improves the A* algorithm to obtain the improved GOA-A* algorithm; and a path correction module, which processes redundant points on the initial path to obtain the final planned path.

[0029] 3. Beneficial effects

[0030] Compared with the prior art, the advantages of this application are:

[0031] (1) This application avoids the limitation of the traditional A* algorithm that can only search 8 adjacent nodes around by setting the target steering angle to dynamically adjust the expanded search range, effectively reducing the number of redundant nodes expanded during the search process, thereby improving the efficiency of path search. In the node cost function, the node azimuth deviation cost and distance deviation cost are comprehensively considered to balance the path length and direction changes, so that the turning points of the generated initial path are reduced. By removing redundant points from the path twice in the forward and reverse directions, the environmental information obtained in the forward and reverse search processes is fully utilized, and common nodes are extracted to construct a better path, effectively eliminating the local path redundancy caused by the search direction, and improving the efficiency of path planning.

[0032] (2) The node safety margin assessment strategy is introduced to select nodes that meet the safety distance threshold conditions as nodes to be expanded by calculating the distance between the node connection and the obstacle. This ensures that the generated path maintains a sufficient safety distance from the obstacle, effectively improving the safety performance of the planned path and avoiding the risk of collision between the robot and the obstacle. It also improves the efficiency of path planning.

[0033] (3) Path search is performed based on grid maps. The local environment map is constructed in real time through perception information, which can adapt to dynamically changing complex environments. At the same time, by setting the step sizes L1 and L2 to divide different search neighborhoods, the algorithm's adaptability in different environments is enhanced. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 This is a flow chart of a path planning method based on a target steering angle according to this embodiment;

[0035] Figure 2 This is a schematic diagram of the eight-neighborhood expansion of this application;

[0036] Figure 3 This is a schematic diagram of the sixteen-direction expansion of this application;

[0037] Figure 4 This is a schematic diagram of the target steering angle of this application;

[0038] Figure 5 This is a range diagram corresponding to different angles of this application;

[0039] Figure 6 This is the safety inspection diagram for this application;

[0040] Figure 7 This is the forward secondary elimination of redundant point graph of this application;

[0041] Figure 8 This is the reverse secondary elimination of redundant point graph of this application;

[0042] Fig. 9 A schematic diagram of the common nodes of this application;

[0043] Fig.10 This is the bidirectional path diagram after removing redundant nodes in this application;

[0044] Fig.11 A comparison chart of planning results of the A* algorithm and the GOA-A* algorithm in an environment of this application;

[0045] Fig.12 This is a comparison chart of planning results between the traditional A* algorithm and the GOA-A* algorithm in another environment of this application. DETAILED DESCRIPTION

[0046] The present application is described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0047] like Figure 1As shown, in this embodiment, firstly, data structures such as openList and closedList are initialized, and a grid map is created; node expansion is performed based on the openList queue. The f value is calculated according to the g value from the current node to the starting point and the h value to the target point, and the node with the smallest f value is selected as the expansion object. The nodes obtained by the expansion are screened. First, the angle between the nodes is used to calculate whether they are collinear, and redundant nodes are eliminated; then the distance between the node connection line and the obstacle is calculated to determine whether the connection line passes through the obstacle, and unsafe nodes are eliminated. Using the screened nodes, the initial path from the starting point to the target point is generated by backtracking the parent node pointer. The initial path is further optimized. Including forward elimination of redundant nodes, reverse elimination of redundant nodes, extraction of common nodes eliminated in both positive and negative directions, comparison and selection of the optimal section, etc., finally a non-redundant, safe and efficient planning path is obtained. The nodes in the search process are added to the closedList, and the planned path is passed to the motion control module to guide the robot movement.

[0048] Specifically, the search neighborhood range is adjusted according to the neighborhood obstacle rate, and the search direction is adjusted according to the target guidance angle to determine the expansion range of the current search node, including: traversing all grids within the neighborhood range and counting the number of obstacle grids N. According to the number of obstacles N, the neighborhood obstacle rate is calculated; the neighborhood obstacle rate is calculated by the following formula: If the neighborhood obstacle rate is greater than the preset threshold, the search step size is reduced to L1, and the neighborhood range is reduced to the 8-neighborhood of the current node, such as Figure 2 As shown in the figure, the red circle in the grid map environment is the current position, and the eight arrows point to the adjacent 8 neighborhoods in the following directions: upper left, upper, upper right, left, right, lower left, lower, and lower right; otherwise, the search step size is increased to L2, and the neighborhood range is expanded to 16 directions of the current node. Figure 3 The red circle in the grid map environment shown is the current position, and the sixteen arrows point to the neighborhood range that is further expanded on the basis of the 8 neighborhoods, involving a total of 24 neighborhood nodes in 16 directions; in this application, a neighborhood search range is obtained according to the neighborhood obstacle rate to ensure that the search within this range is safe and efficient. A set of angular expansion ranges is obtained according to the target guidance angle to ensure that the expansion direction is in the target direction. The two ranges are combined and their intersection is taken as the final expansion range. This combination strategy constrains the search range in both spatial and directional dimensions, eliminating a large number of unnecessary expansion nodes, thereby greatly improving the search efficiency.

[0049] like Figure 4 As shown, in the grid map environment, the red circle is the current node position Start, the green circle is the target node position Goal, Child is the child node position, and the vector formed by the child node and the current node is V c, the vector formed by the current node and the target node is V g , vector V c and vector V g The angle between the current node and the adjacent node V is the target steering angle α. Traverse all grids in the neighborhood, and for each grid p, c and the target node V g The target steering angle α is calculated by the following formula: V c =(c x -n x ,c y -n y ), V g =(g x -n x ,g y -n y ),

[0050] In the formula (n x ,n y ) is the current point coordinate, (g x ,g y ) is the target point coordinate, i∈[1,24] is the coordinate of the adjacent node; V c Represents the vector formed by the current position and the adjacent nodes, V g Represents the vector formed by the current position and the target position.

[0051] like Figure 5 As shown in the figure, the red circle is the current node Start, the green circle is the target node position Goal, the target guidance angle α includes nodes ①② in the range of [0, 22.5], and includes nodes ③④⑤⑥⑦⑧ in the range of (22.5, 45]. Guided by the target point, the node expansion range is controlled by controlling the size of the target guidance angle to reduce the expansion of redundant nodes. The sixteen-direction search divides the environment into sixteen parts. According to the symmetry, the value range of α is divided into eight intervals: {[0, 22.5], (22.5, 45], (45, 67.5], (67.5, 90], (90, 112.5], (112.5, 135], (135, 157.5], (157.5, 180]}. The initial search sets α∈[0, 22.5]. If it is passable, continue to the next step of the algorithm; otherwise, adjust α∈(22.5, 45] and repeat the above steps until a passable sequence is obtained.

[0052] like Figure 6 As shown, the red rectangle is the current node position (x par ,y par), the gray node is a neighborhood node (x chil ,y chil ), the black rectangle is an obstacle (x obn ,y obn ), the distance d from the straight line L formed by connecting the current node and the center of the neighboring node to the center of the obstacle is used to determine whether the node can be used as a node to be expanded. S3, through the node-to-node safety margin evaluation strategy, obtain the nodes to be expanded that meet the preset safety conditions within the determined expansion range; according to each child node in the determined expansion range, connect the child node with the corresponding parent node to generate a straight line L; calculate the vertical distance d from each obstacle node in the grid map to the generated straight line L; calculate the vertical distance d from each obstacle node in the grid map to the generated straight line L, through the following formula: d = vertical dif ×cos(θ),vertical dif =|y o -(k*x o +b)|, b=y p -k*x p ,y cp =kx+b, where (x p ,y p ) is the parent node coordinate, (x c ,y c ) is the child node coordinate, (x o ,y o ) is the obstacle coordinate, θ represents the angle between the straight line L and the horizontal line; determine whether the obtained vertical distance is greater than the preset distance D, if so, the corresponding child node is used as the node to be expanded of the parent node, if not, it is considered that the child node and the obstacle do not meet the safety margin condition, and the child node is deleted from the list of nodes to be expanded, and the expansion of the child node is no longer considered. In this application, the line equation between the parent and child nodes is obtained according to the coordinates of the two nodes, and then the vertical distance from the obstacle node to the line is calculated, and a safety threshold is set to determine whether the safety margin condition is met between the nodes. The vertical distance can be calculated by the method of space vectors or by the method of analytic geometry.

[0053] By setting a safety distance threshold, the present application can retain as many alternative nodes as possible while ensuring the safety of the path, avoiding the loss of feasible solutions due to excessive pruning. By traversing all child nodes within the extension range, safety margin evaluation and screening are performed in turn. This exhaustive traversal method can ensure that all nodes to be expanded that meet the safety conditions are found, and no possible alternative solutions are missed. Although the time complexity of the exhaustive search is high, due to the previous extension range limitation and fast collision detection, the number of nodes actually searched has been greatly reduced, so the overall time cost is acceptable.

[0054] The angle factor and distance factor are set in the heuristic function of the A* algorithm to obtain the improved GOA-A* algorithm; including: calculating the distance dn from the current node to the target node; calculating the distance ds from the start node to the target node; according to dn and ds, setting the heuristic function f(n) to obtain the improved GOA-A* algorithm; the heuristic function f(n) is expressed as follows:

[0055] f(n)=g(n)+(weight+β*angle weight +γ*dis weight )*h(n), Ax+By+C=0, Among them, β and γ are angle weight and dis weight The weight coefficient; g(n) represents the actual cost function; h(n) represents the evaluation function; weight represents the distance weight function from the current node to the target node; angle weight represents the target steering angle weight function; weight represents the distance function from the neighboring node to the optimal path; A, B, and C represent the independent variable coefficient, dependent variable coefficient, and constant term of the straight line equation formed by the starting node and the target node, respectively.

[0056] On the one hand, the angle factor angle weight , which indicates the degree of deviation between the direction from the current node to the target node and the direction from the start node to the target node. The greater the deviation, the greater the angle weight The larger the value, the larger the corresponding heuristic function value f(n). In the node expansion process of the A* algorithm, nodes with smaller f(n) values ​​will be expanded first. weight Afterwards, nodes that deviate less from the target direction will be assigned smaller f(n) values, and thus be expanded first. This strategy can guide the search direction to the target direction in the early stages of the search process, reduce invalid searches in the wrong direction, and improve the algorithm's directionality and search efficiency.

[0057] On the other hand, the distance factor dis weight , represents the estimated distance from the current node to the optimal path. The optimal path can be roughly regarded as the connection between the starting node and the target node. weight The larger the value, the farther the current node deviates from the optimal path, and the larger the corresponding f(n) value. weightAfter that, the search process will give priority to expanding those nodes that are closer to the optimal path, so that the search is carried out as much as possible along the direction from the starting node to the target node, reducing detours. This strategy can shorten the length of the search path, making it closer to the shortest path, and improving the quality of path planning. At the same time, the search range will be more compact, reducing the number of expanded nodes and improving search efficiency. weight Designed as a function of the distance from the node to the optimal path, it is more in line with the physical meaning of path planning than the traditional Euclidean distance heuristic function. This design can more accurately estimate the actual cost of the node, making the heuristic function more accurate and effective.

[0058] like Figures 7 to 10 As shown, the red rectangle in the lower left corner of the grid map is the starting node, the green rectangle in the upper right corner is the target node, and the solid line is the original path. Figure 7 The blue dotted line in the figure is the result of the secondary elimination of redundant points. In addition to the start and target nodes, only three path nodes in the original path are retained, which are labeled 1, 2, and 3 respectively; Figure 8 The purple dotted line in the figure is the result of reverse secondary elimination of redundant points. In addition to the start and target nodes, four path nodes in the original path are retained, which are numbered 1, 2, 3 and 4 respectively. Fig. 9 Schematic diagram of the common nodes of the forward elimination results and the reverse elimination results. The centers of the nodes contained in the ellipse are the common nodes, which are labeled as p1, p2 and p3; Fig.10 The figure is the path result after simplification in both the forward and reverse directions. The dotted line is the global optimal simplified path. The improved GOA-A* algorithm is used to traverse and search for the nodes to be expanded to generate a planned path from the starting point to the target point.

[0059] First, we determine node collinearity by vector angles and remove redundant nodes: initialize path 0, add the starting node t0 to path0, and the initial path path0 starts from the starting point t0 (x0, y0). t1 (x1, y1) is the child node of t0, and t2 (x2, y2) is the next node of t1. and The angle between the three points is used to determine whether they are collinear: if the angle is 0, it means that the three points t0, t1, and t2 are collinear, and t1 is a collinear redundant point and needs to be removed from the path. If the angle is not 0, connect t0 and t2 to form a new path, and check whether it complies with the safety expansion strategy. If it complies, t1 is a redundant inflection point and needs to be removed; if it does not comply, retain the t1 node. Repeat the above steps until a redundant point removal is completed from the starting node to the target node to obtain a new path path1. In this application, by comparing the vector angle formed by three adjacent points, it is determined whether they are collinear. If they are collinear, the middle node is a redundant point and can be removed. For the case of non-collinearity, try to directly connect the two end points. If the safety margin condition is met, the middle node is a redundant inflection point and can also be removed. Redundant straight line segments and unnecessary inflection points in the path can be removed to make the path simpler without affecting the safety of the path. This redundant node removal method based on geometric features is simple to calculate and efficient, and can quickly simplify the path without affecting the feasibility of the path.

[0060] Then, by calculating the distance between the node connection and the obstacle, remove the nodes that pass through the obstacle: set the starting point of path1 to the parent node p0 (x0, y0), the next node of the child node is p2 (x2, y2), and connect the nodes between p0 and p2 points to the target node in sequence to form a new path, and judge whether the path complies with the safety expansion strategy, find the node in the last connected path in path1 that does not pass through the obstacle as the next node, and remove the intermediate redundant nodes, and judge the next group of nodes, repeat the above steps until the second redundant point removal process from the starting node to the target node is completed to obtain path2. Specifically, the present application, on the basis of one removal, further checks whether there are non-adjacent nodes that can be directly connected on the path. Starting from the starting point, try to connect the current node and the second node after it. If the safety margin is met, all nodes between the starting point and the second node are removed. The redundant inflection points remaining after one removal can be removed, and the path can be further simplified. By judging the distance from the obstacle, it is ensured that the simplified path is still safe. This node elimination method based on obstacle distance can further simplify the path, ensure the safety and accessibility of the path, and reduce the difficulty and time cost of subsequent optimization.

[0061] Furthermore, the initial path is processed in reverse order to obtain the reverse elimination result. The node set of the initial path path0 is arranged in reverse order and then the node collinearity and obstacle distance are judged, that is, the redundant points are reversely eliminated to obtain the path 2,reverseSpecifically, the initial path is processed in reverse order, that is, redundant nodes are eliminated again from the target point to the starting point. In this way, the path can be simplified from another direction, making up for the shortcomings of forward elimination and obtaining a more optimized path. By eliminating in both positive and negative directions, redundant nodes can be removed more comprehensively, improving the simplicity and efficiency of the path.

[0062] Finally, extract the common points of the positive and negative elimination results and select the optimal path. Specifically, find two paths, path2 and path 2,reverse Common node sequence in the path new path , if the path connecting adjacent common nodes p1 and p2 passes through an obstacle, then compare the common elements in path2 and path 2,reverse The length of the path and the number of nodes in the path, insert the node sequence with shorter path length or the same path length but fewer nodes into new path Then repeat the above operation from node p2 until all nodes new are traversed. path all nodes in . Specifically, the present application performs one-time and two-time redundant point elimination processing on the original path and the reverse path, respectively, to obtain a forward simplified path and a reverse simplified path. The common nodes of the two simplified paths are extracted to form a new path. For the continuous common nodes on the new path, the corresponding sub-paths on the forward path and the reverse path are compared, and the sub-path with fewer nodes or shorter length is selected and inserted between the two common nodes. By comprehensively considering the simplification results in both the forward and reverse directions, a globally optimal simplified path can be obtained. This method can effectively deal with the problem of local simplification failure caused by different traversal directions, so that the final path is as concise as possible. Through this section selection method based on the positive and negative elimination results, the path can be optimized from both local and global perspectives to obtain a more concise and efficient final path.

[0063] Furthermore, the B-spline curve smoothing process is specifically as follows: the definition of the k-order B-spline curve is: Where P i is the control point, B i,k (u) is the i-th k-order B-spline basis function, and the control point P i Correspondingly, u is the independent variable. The basis function of the B-spline curve has the following de Boer-Cox recursion formula, which is as follows: It is agreed that 0 / 0 = 0. In the formula, u i is a set of continuously changing values ​​called a non-decreasing sequence of node vectors, whose definition interval is [0, 1]. The sequence is as follows: [u0 u1 … u k u k+1 … u n u n+1 … un+k ], the expression and basis function of the cubic uniform B-spline curve are:

[0064] Substituting the path nodes after removing redundant points as control points, a cubic B-spline curve is obtained in which the broken line at the turning point is replaced by a smooth curve, thus completing the smooth optimization of the original path. This application obtains a concise and safe planning path by removing redundant points twice and comparing sub-paths between common nodes, and then smoothes it with a B-spline curve, finally obtaining a non-redundant and relatively smooth robot motion trajectory.

[0065] Specifically, the B-spline curve has local controllability, that is, each control point only affects a small section of the curve around it, which makes the shape of the curve smoother at the turning point without sharp corners. By adjusting the order and node vector of the B-spline curve, the degree of smoothness and the shape of the curve can be controlled, and a balance can be achieved between path smoothing and approximation to the original path. The smoothed path has better continuity and is more suitable for robot motion control.

[0066] like Fig.11 and Fig.12 As shown in the figure, the yellow grid in the lower left corner of the grid map is the starting node, the rose-red grid in the upper right corner is the target node, the black grid is the non-collisionable obstacle, the white grid is the feasible area, the red solid line is the global planning path of the A* algorithm, and the blue solid line is the global planning path of the GOA-A* algorithm. Fig.11 This is a comparison of the planning results of the traditional A* algorithm and the GOA-A* algorithm in a simple environment. (a) is the planning result of the traditional A* algorithm in a grid map environment. Due to the limitations of the A* algorithm, the path has more turning angles and the path length needs to be optimized. (b) is the planning result of the GOA-A* algorithm in a grid map environment. The above improvements improve the path smoothness of the GOA-A* algorithm and have a shorter path length. Fig.12 Comparison of the planning results of the traditional A* algorithm and the GOA-A* algorithm in a complex environment. (a) is the planning result of the traditional A* algorithm in a grid map environment. In a narrow and tortuous environment, the traditional A* algorithm needs to go through multiple turns, which is not conducive to the stability of the actual operation of the mobile robot and increases energy consumption. (b) is the planning result of the GOA-A* algorithm in a grid map environment. In a complex environment, the path planned by the GOA-A* algorithm is smoother and reduces unnecessary direction changes, effectively improving the quality of the planned path.

[0067] The invention of the present application and its implementation methods are described schematically above. The description is not restrictive. Without departing from the spirit or basic features of the present application, the present application can be implemented in other specific forms. What is shown in the accompanying drawings is only one of the implementation methods of the invention of the present application. The actual structure is not limited to this, and any figure mark in the claims should not limit the claims involved. Therefore, if ordinary technicians in this field are inspired by it, without departing from the purpose of the present invention, they can design structural methods and embodiments similar to the technical solution without creativity, which should all fall within the scope of protection of this patent. In addition, the word "including" does not exclude other elements or steps, and the word "one" before the element does not exclude the inclusion of "multiple" elements. The multiple elements stated in the product claim can also be implemented by one element through software or hardware. The words first, second, etc. are used to indicate names, and do not indicate any specific order.

Claims

1. A path planning method based on target guidance angle, characterized in that: include: Initialize parameters and create a grid map; the parameters include the starting point, the target point and the number of obstacles N; The neighborhood obstacle rate is calculated according to the number of obstacles N, and the search neighborhood range is adjusted according to the neighborhood obstacle rate; and the search direction is adjusted according to the target guidance angle to determine the expansion range of the current search node; Through the inter-node safety margin assessment strategy, the nodes to be expanded that meet the preset safety conditions are obtained within the determined expansion range; The improved GOA-A* algorithm is used to traverse and search for nodes to be expanded, and a planning path from the starting point to the target point is generated; wherein the improved GOA-A* algorithm is obtained by setting the angle factor and the distance factor in the heuristic function of the A* algorithm.

2. The path planning method based on target guidance angle according to claim 1, characterized in that: The specific steps of calculating the neighborhood obstacle rate according to the number of obstacles N include: Calculate the current node p i The number of obstacles N within the preset range; According to the number of obstacles N, calculate the neighborhood obstacle rate; The search step length is set according to the neighborhood obstacle rate. When the neighborhood obstacle rate is greater than the preset threshold, the step length L1 is set. When the neighborhood obstacle rate is less than the preset threshold, the step length L2 is set. L2 is greater than L1. According to the set step length L1 or L2, the current node p is set. i Search scope; According to the current node p i , the coordinates of the adjacent nodes and the target node, and calculate the target guidance angle α; Divide the value range of the target steering angle α into multiple regions; Traverse each area, set the angle threshold ε0, compare the target steering angle α with the angle threshold ε0, if the target steering angle α is less than the angle threshold ε0, then the corresponding node p j As the current node p i If not, adjust the angle threshold ε0 and continue searching until the target guidance angle α is less than the angle threshold ε0, and the current node p is obtained. i The scope of expansion; Combine the set search range and expansion range to get the current node p i The ultimate expansion range.

3. The path planning method based on target guidance angle according to claim 2, characterized in that: Neighborhood obstacle rate obs rate The calculation formula is:

4. The path planning method based on target steering angle according to claim 2, characterized in that: The calculation formula of the target guidance angle α is: V c =(c x -n x ,c y -n y ) V g =(g x -n x ,g y -n y ) Among them, (n x ,n y ) is the current point coordinate, (g x ,g y ) is the coordinate of the target point, are the coordinates of adjacent nodes; represents the x-coordinate of the ith adjacent node, represents the y coordinate of the i-th adjacent node; V c Represents the vector formed by the current position and the adjacent nodes, V g Represents the vector formed by the current position and the target position, c indicates that the end of the vector is the adjacent node, and g indicates that the end of the vector is the target node.

5. The path planning method based on target guidance angle according to claim 1, characterized in that: The specific steps of obtaining the nodes to be expanded that meet the preset safety conditions within the determined expansion range include: According to each child node within the determined extension range, connect the child node with the corresponding parent node to generate a straight line L; Calculate the vertical distance d from each obstacle node in the grid map to the generated straight line L; Determine whether the obtained vertical distance is greater than the preset distance D. If so, use the corresponding child node as the node to be expanded of the parent node. If not, delete the corresponding child node. Traverse all child nodes within the expansion range to obtain the final node to be expanded.

6. The path planning method based on target steering angle according to claim 5, characterized in that: Calculate the vertical distance d from each obstacle node in the grid map to the generated straight line L using the following formula: d=vertical dif ×cos(θ) verticaldif=|yo-(k*xo+b)| b=y p -k*x p yes cp =kx+b Among them, (x p ,y p ) is the parent node coordinate, (x c ,y c ) is the child node coordinate, (x o ,y o ) is the obstacle coordinate, θ represents the angle between the straight line L and the horizontal line; vertical dif The y value of the obstacle o Coordinates and x on line L o The absolute value of the corresponding y-coordinate difference; k represents the slope of the straight line equation formed by the child node and the parent node; b represents the intercept of the straight line equation formed by the child node and the parent node; y cp Represents the equation of the line formed by the child node and the parent node; dif represents the y value of the obstacle o Coordinates and x on line L o The corresponding y-coordinate difference, cp represents the child node and the parent node.

7. The path planning method based on target steering angle according to claim 1, characterized in that: The specific steps of obtaining the improved GOA-A* algorithm include: Calculate the distance dn from the current node to the target node; Calculate the distance ds from the starting node to the target node; According to dn and ds, the heuristic function f(n) is set to obtain the improved GOA-A* algorithm.

8. The path planning method based on target steering angle according to claim 7, characterized in that: The heuristic function f(n) is expressed as follows: f(n)=g(n)+(weight+β*angle weight +γ*dis weight )*h(n) Ax+By+C=0 Among them, angle weight represents the target steering angle weight function; weight Represents the distance function from the neighboring node to the optimal path; β and γ are angle weight and dis weight The weight coefficient of ; g(n) represents the actual cost function; h(n) represents the evaluation function; weight represents the distance weight function from the current node to the target node; A, B, and C represent the independent variable coefficient, dependent variable coefficient, and constant term of the straight line equation formed by the starting node and the target node, respectively.

9. The path planning method based on target steering angle according to any one of claims 2 to 8, characterized in that: The specific steps to generate a planned path from the starting point to the target point include: Use the improved GOA-A* algorithm to traverse and search for nodes to be expanded, and generate the initial path path0; Traverse the vectors composed of three adjacent nodes in the initial path path0, determine the collinearity of the nodes by calculating the vector angle, remove the intermediate nodes with zero angle, and obtain the path path1 after the first removal of redundant points; Take the starting point of path 1 as the starting node, and determine whether the node connection passes through the obstacle by calculating the distance between the connection line of all nodes between the starting node and the end point of the path and the obstacle; Get the node closest to the starting node and not passing through obstacles as the new starting node; Eliminate the redundant nodes between the original starting node and the new starting node to obtain the path path2 after the second elimination of redundant points; The initial path path0 is processed in reverse order, and the first and second steps of removing redundant nodes are repeated to obtain the path path after removing redundant points in reverse order. 2,reverse ; Extract path2 and path 2,reverse The common nodes of the nodes are used to form a new sequence. By comparing the lengths of the forward and reverse sections between adjacent common nodes in the new sequence and the number of nodes, the sections with fewer nodes or less than the threshold value are selected to replace the original node sequence to obtain the final planned path.

10. A path planning system based on target guidance angle, characterized in that: include: Map module, initializes parameters and creates raster maps; The parameters include a starting point, a target point and the number of obstacles N; The extended range module calculates the neighborhood obstacle rate according to the number of obstacles N, and adjusts the neighborhood search range according to the obstacle rate; and adjusts the neighborhood search direction according to the target guidance angle; and determines the extended range of the current search node according to the adjusted neighborhood search range and search direction; The expansion node module obtains the nodes to be expanded that meet the preset safety conditions within the determined expansion range; The path generation module uses the improved GOA-A* algorithm to traverse the nodes to be expanded and generate the initial path; Setting an angle factor and a distance factor to improve the A* algorithm to obtain the improved GOA-A* algorithm; The path correction module processes the redundant points of the initial path to obtain the final planned path.

Images