Method for searching low-cost and multi-constraint smooth path on cost-assigned map

Abstract

The invention provides a method for searching a low-cost and multi-constraint smooth path on a cost-assigned map, and belongs to the technical field of path planning. Comprising the steps that a mathematical model is constructed and used for smoothing a path connecting a starting point and an end point to obtain a smooth path, and the smooth path is composed of line segments, arcs and easement curves; a two-stage algorithm is provided on the basis of a mathematical model, in the first stage, point sampling is conducted on a map, a map structure is constructed, and a Dijkstra algorithm is used for obtaining a connection sequence of approach points; in the second stage, on the basis of the first stage, the path points are processed one by one, a control point set P meeting the condition is generated, finally, an arc radius set R is obtained through calculation, and the path is uniquely determined under the sets P and R. According to the method, the problem of path planning of the cost map is solved, and an economical and feasible route can be generated on the cost map; and meanwhile, the operation time is short and the effect is better in the test.

Description

Technical Field

[0001] The invention belongs to the technical field of path planning, and in particular relates to a method for finding a low-cost, multi-constrained smooth path on a costed map. Background Art

[0002] With the rapid development of science and technology, robotics technology has been widely used in many fields such as industrial automation, service industry, and medical assistance. Robot navigation, as the basic technology for realizing autonomous robot movement and task execution, has always been one of the core contents in the field of robotics. Path planning is a crucial part of robot navigation, which involves how to design a reasonable and effective route for the robot from the starting point to the end point in a complex environment. Reasonable and effective path planning can not only improve mobility efficiency, but also ensure safety and save energy consumption.

[0003] At present, the research on the problem of path planning with cost map is not extensive and in-depth. Common processing methods are mostly focused on environment modeling and map preprocessing. For example, the actual scene is converted into a raster map, and each cell contains factors such as obstacle information and terrain features. For specific applications, it may also be necessary to assign different cost values ​​to certain cells. Classical path planning algorithms, such as Dijkstra's algorithm and A star search algorithm, are very effective in many cases, but they usually assume that the path is composed of straight segments and do not consider the curvature of the path. Some methods that rely on splines to generate smooth paths, such as Bezier curves, may not necessarily be compatible with the robot's own motion characteristics. In reality, there will be deviations from the route and errors. Heuristic algorithms use optimization techniques such as simulated annealing and genetic algorithms to make local adjustments to the path obtained from the preliminary search to meet constraints such as curvature continuity, but the algorithm depends on the selection of parameters and initial solutions, and is limited by probability parameters, so it converges slowly and is not very interpretable.

[0004] Most of the variant improvements of these methods only optimize for a specific constraint and rely on the assumptions of the original conditions, and cannot consider the application under actual conditions. Algorithm improvements will increase the computational complexity, making it difficult to meet the needs of real-time updates and fast responses in application scenarios with high real-time requirements.

[0005] Existing methods do not consider the situation where the robot incurs different costs when traveling the same distance in different areas due to terrain, material, etc.

[0006] Existing methods are difficult to take into account both the actual task requirements and their own motion characteristics. For example, in actual tasks, a series of path points are usually required to meet specific requirements. The generated route needs to ensure smoothness while ensuring the length of the line segments between curves to ensure precise control; controlling the maximum curvature of the curve reduces the wear of the instrument.

[0007] The optimization process of existing methods increases computational complexity, is time-consuming, and is difficult to respond quickly. Some algorithms can generate paths that meet basic constraints, but they still perform poorly in maintaining path smoothness and continuity. Summary of the invention

[0008] In order to solve the above technical problems, the present invention proposes a method for finding a low-cost, multi-constrained smooth path on a costed map. The technical solution of the present invention is as follows:

[0009] Constructing a mathematical model to smooth the path connecting the starting point and the end point to obtain a smooth path, wherein the smooth path is composed of line segments, circular arcs and transition curves;

[0010] Based on the mathematical model, a two-stage algorithm is proposed. In the first stage, point sampling is performed on the map to construct a graph structure, and the Dijkstra algorithm is used to obtain the connection order of the waypoints. In the second stage, based on the first stage, the waypoints are processed one by one to generate a control point set P that meets the conditions, and finally the arc radius set R is calculated. Under the sets P and R, the path is uniquely determined.

[0011] The present invention has the following beneficial effects:

[0012] 1. The method of the present invention solves the path planning problem assigned to the cost map. The method can generate an economically feasible route on the cost map; at the same time, the running time is short and the effect is better in the test.

[0013] 2. The method of the present invention overcomes the shortcoming of the traditional smooth path algorithm that is composed of multiple smooth paths superimposed on each other. It realizes the seamless transition between line segments and circular arcs through the transition curve, thus ensuring the continuity of the overall path curvature; at the same time, it takes into account the actual mission waypoints and obstacle avoidance requirements.

[0014] 3. The new mathematical model proposed in the present invention takes into account the dynamic factors of the robot and the accuracy requirements of the system control, and introduces constraints such as the minimum length of the line segments between curves and the maximum curvature of the curves. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] FIG1( a ) is a schematic diagram of a line segment type between curves provided by the present invention;

[0016] FIG1( b ) is a schematic diagram of another type of line segments between curves provided by the present invention;

[0017] Figure 2 It is the mathematical model explanation diagram and generation path schematic diagram of the problem provided by the present invention;

[0018] Figure 3 It is a model diagram of a relaxation curve involved in the present invention;

[0019] Figure 4 It is a schematic diagram of the smoothing method involved in the present invention;

[0020] Figure 5 It is a flow chart of the proposed algorithm provided by the present invention;

[0021] Figure 6 It is a map point selection, composition method and path schematic diagram provided by the present invention;

[0022] Figure 7 is a flow chart of a method for calculating the order of waypoints provided by the present invention;

[0023] Figure 8 It is a schematic diagram of a method for completing the transition of waypoints in a line segment manner according to a connection sequence provided by the present invention;

[0024] Fig. 9 The present invention provides a point-by-point inspection, and then obtains a control point set schematic diagram in a post-processing manner. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical scheme and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in each embodiment of the present invention described below can be combined with each other as long as they do not conflict with each other. To achieve the above-mentioned purpose, the present invention adopts the following technical scheme.

[0026] The present invention proposes a method for finding a low-cost, multi-constrained smooth path on a costed map, which specifically includes:

[0027] Constructing a mathematical model to smooth the path connecting the starting point and the end point to obtain a smooth path, wherein the smooth path is composed of line segments, circular arcs and transition curves;

[0028] Based on the mathematical model, a two-stage algorithm is proposed. In the first stage, point sampling is performed on the map to construct the graph structure, and the Dijkstra algorithm is used to obtain the connection order of the waypoints. In the second stage, based on the first step, the waypoints are processed one by one to generate a control point set P that meets the conditions, and finally the arc radius set R is calculated. Under the sets P and R, the path is uniquely determined.

[0029] Furthermore, the mathematical model is used to accurately describe and solve the following problem: according to the given cost map, find a smooth path connecting the starting point and the end point, that is, smoothing, where the smooth path should meet the following optimization criteria and constraints: (1) pass through all given waypoints; (2) avoid given obstacles; (3) the path meets the requirement of curvature continuity; (4) due to the design of the machine, the path has a maximum curvature constraint, that is, there is a minimum radius restriction on the arc; (5) the length of the line segment between the curves is greater than the minimum length of the line segment of this type; (6) under the above criteria and constraints, reduce the cost of the path. The objective function of the mathematical model is:

[0030] ;

[0031] st ;

[0032] ;

[0033] ;

[0034] ;

[0035] ;

[0036] ;

[0037] Among them, argmin represents the parameter value (the value of the independent variable) of a function to achieve the minimum value in its domain, and the objective function represents finding the control point set P and radius set R at the minimum cost. Indicates that the curvature is continuous, and n represents the number of curved parts of the entire path; , Indicates order, such as i means the next one in the i-1 part; represents the function that assigns cost to the map, Represents a set of path curves The radius under the control point is The arc and the corresponding transition curve, Represents a set of path segments The radius of the control point and the adjacent arc and The line segment below. represents the smooth path generated by the path curve set C and the path segment set L, Indicates all regions, represents the feasible area, that is, the obstacle-free area, Represents the waypoints that need to be passed. represents the set of all waypoints, Indicates the radius of the path at the control point, Represents the minimum radius constraint after smoothing, which controls the maximum curvature of the generated route. represents the supplementary angle of the extended line at the i-th control point, It represents the outward displacement value (calculated value) of the tangent point of the i-th curve. The tangent point of the arc and the line segment before adding the transition curve becomes the tangent point of the transition curve and the line segment after adding the transition curve. Indicates the external displacement value of its change, Indicates the minimum line segment length constraint at this location, and the specific value can be set based on experience. It can be divided into two categories: the same direction curve and the line segment between the opposite direction curves. Please refer to Figure 1 (a) and Figure 1 (b) for details. , , For vector The coordinates of For vector The direction is determined by the vector cross product, that is, if , then point A is On the left; Similarly, calculate point B to determine whether point A and point B are Same side or opposite side, as shown in Figure 1 (a) and Figure 1 (b). 1 , C 2 , C 3 , C 4 Indicates the tangent point between the line segment and the arc. For details on the effect of connecting the start and end points, please refer to Figure 2 As shown, Q represents the connection point between the line segment and the transition curve, H represents the connection point between the transition curve and the arc, P represents the intersection point of the line segment extension, T represents the tangent point of the arc and the line segment if the transition curve is not inserted, α represents the supplementary angle of the line segment extension angle, R represents the arc radius, L represents the length of the transition curve, i-1 and i represent the i-1 and i parts, which are consistent with the mathematical model. Figure 2 The area between point Q and point H represents a transition curve, the area between two adjacent points H is an arc, and the rest is a line segment. The entire smooth path is composed of line segments, arcs, and transition curves. A transition curve can be uniquely determined by the following parameters:

[0038] (1) Length of the transition curve L;

[0039] (2) Coordinates and direction angles at the starting point ;

[0040] (3) Two parameters that determine its linear curvature function .

[0041] Given these parameters, the position of the curve at any point s in the transition curve [0,L] is calculated by solving the following equation: , and direction angle :

[0042] ;

[0043] Obviously, the angle between the line segments The number of curves (a curve consists of two transition curves and a circular arc) n is uniquely determined by the control point set P.

[0044] Derivation of related properties of the transition curve: Figure 3 As shown, line segment OQ is tangent to the long dashed arc at point Q, and the radius of the circle is Equal to R. Insert a transition curve of length L between line segment OQ and the long dashed arc, that is, the curve in the figure , the line segment and arc tangent point Q needs to be moved outward, and the arc needs to be moved inward. Through two moves, the arc and line segment tangent points are converted into arc and transition curve tangent points, and line segment and transition curve tangent points. For ease of processing, assume that the starting point of the transition curve is at the origin, and the initial direction angle is 0, which can be easily obtained , It represents the supplementary angle of the extended line. According to mathematical properties, it is also equal to the central angle of the circle, which is consistent with the model constraints in the previous article. The meaning is the same, assuming , according to the properties of the transition curve, we can get and The values ​​of L and R are functions of L and R. The values ​​of the two are recorded as and , respectively represent the arc inward displacement value and the tangent point outward displacement value, where v is the same as the model constraint in the previous text The meaning is the same, indicating the outward displacement value. Here, when the relaxation curve From the properties of the curve, we can know that the radius of curvature of any point on the curve is The length of the curve at that point The product is a constant, Then when , we can get:

[0045] ;

[0046] At this time, the length of the transition curve is L, and we get:

[0047] ;

[0048] , It is the result of integrating the transition curve and represents the position.

[0049] The present invention smoothes and inserts a transition curve into a line segment and an arc. The method is as follows: Figure 4 As shown: First, insert an arc with a radius of R between two line segments. The intersection point of the line segments is P, and the tangent points are and , then the arc center angle is , which also represents the supplementary angle of the extended straight line. Assuming that the center of the circle is , the arc along Direction of movement units. At the same time, the central angle decreases from both ends , so that the processed arc is obtained; the tangent point and Along and move Units get points and As the starting point of the transition curve, the smooth transition between the line segment and the arc is completed by inserting the transition curve. Figure 2 Middle It represents the outward displacement value of the tangent point. Assuming that the starting point and the end point of the path are both passed by the line segment, that is, , then the number of line segments is one more than the number of transition curves.

[0050] The objective function in the mathematical model represents minimizing the cost of the path on the map under the control of the control point set P and the arc radius set R. The first three constraints indicate that the path should be in the feasible space and cover all the waypoints under the condition of curvature continuity. The fourth constraint indicates that the path has a maximum curvature limit. The fifth constraint indicates that the line segment length has a minimum length constraint, where the z value is the minimum line segment z i Length constraints are mainly divided into two types: the length of the line segment between the same-direction curve and the reverse-direction curve. Therefore, when the control point is determined, the z value of each line segment is uniquely determined, and the last constraint ensures that the arc can be smoothly inserted into the transition curve.

[0051] The present invention proposes a two-stage algorithm based on the above mathematical model. Figure 5 As shown, the input is: map information including boundaries and obstacles, constraint data such as z, , algorithm parameters such as the minimum distance between points (default , is the barrier-free area), incremental cost threshold (default 1) and the position coordinates of the waypoints. In the first stage, point sampling is performed on the map, the graph structure is constructed, and the connection order of the waypoints is obtained using the Dijkstra algorithm; in the second stage, based on the first step, the waypoints are processed one by one to generate a control point set P that meets the conditions, and finally the arc radius set R is calculated. Under the sets P and R, the path is uniquely determined.

[0052] Specifically, in the first stage of the algorithm, the map information is first sampled, where it is necessary to ensure that the distance between any two points (including the starting and ending points, and the waypoints) is greater than the preset parameter , that is, any two points , The distance should satisfy: , the effect is as follows Figure 6 As shown in (a), the bold points are waypoints (which may include the starting and ending points and waypoints); based on these points, the growth method is used to generate a graph structure based on the Delaunay triangulation network, which is recorded as , where V is the set of points and E is the set of edges. Specifically, first find the pair of points with the smallest distance between any two points among all the points, connect these two points as the initial baseline; then find the third point to create the first triangle, use the two endpoints of the baseline as the starting point of the vector, and use the remaining unconstructed points as the end point of the vector, and calculate the cosine value of the angle between the two vectors. .in and are the two endpoints of the baseline, For the remaining unnetworked points. You need to find the point with the smallest cosine value to construct the first triangle. Take the three sides of the first triangle as the baseline and find possible extension points that form a triangle with the baseline. Similar to Figure 1, ensure that the extendable points are outside the triangle. Find the point with the smallest cosine value among all extendable points and the formed edge cannot be a repeated extension edge. Repeat the steps of finding points and constructing edges until all baselines are processed. The effect is as follows Figure 6 As shown in (b), Delaunay triangulation is selected as the basic graph structure because it can more effectively represent the irregular characteristics of map cost compared to other structures such as raster. Any two points Edge The weight of its cost function in the map The line integral of .

[0053] Then determine the connection order of the waypoints, such as Figure 6 As shown in the thick line (c), the cost between two path points is determined by The shortest distance determined by Dijkstra's algorithm is determined on the graph structure. In , for two set points, a shortest path can be found. First, start from one point, set the distance of the point to 0, set the adjacent nodes to the weight of the edge, and set the distance of other nodes to ∞, indicating that the path to the node has not been found. Then, from the unvisited nodes, select the node closest to the starting point and mark it as visited. For all neighbor nodes of the selected node, check whether the path from the starting point through the current node to the neighbor node is shorter than the currently known shortest path. If so, update the shortest path value of the neighbor node. Repeat the above steps until all nodes are visited, or the remaining unvisited nodes are not connected to the source node. After the algorithm is completed, the shortest path between the two points is determined. Figure 7 As shown in the figure, a Hamiltonian circuit is constructed by adding virtual nodes, that is, going from the specified starting point to the specified end point, passing through all other nodes only once. The distance from the virtual point to the starting and ending points is set to 0, and the distances of other nodes are infinite. This step ensures that the starting and ending points in the route are adjacent, and the Hamiltonian circuit can determine the connection order of each waypoint. Figure 7 The middle triangle is the virtual point, the square is the starting and ending point, and the circle is the waypoint.

[0054] In the second stage of the algorithm, the connection order obtained in the first stage will be processed one by one. First, according to the trend of the waypoint (determined by the connection order in the first stage), the point is smoothly transitioned in the form of a line segment. That is, if after determining the order, there are three points whose connection order is , The middle point B is transitioned by a line segment passing through it, and the direction of the line segment is , see Figure 8 As shown in (a) and (b), the dotted lines represent the connection order and the solid lines represent The direction trend of the point indicates that the trend is generated in the order of connection, and the path points complete the transition through the line segments, that is, two information nodes are generated in the trend direction. , where the direction of the trend line is controlled by the position of the obstacle and the length of the line segment. If the original direction leads to an obstacle, the direction will be adjusted. The adjustment method is as follows: The trend line is swung until the transition line segment does not pass through obstacles. The swinging method is: , direction angle , hour, 0. The swing adjustment angle is , and keep trying within the range. The segment length parameter is determined by the minimum segment length and maximum curvature in the constraint, and is generally set to 2 . At this time, Synchronously join the graph structure Specifically: For example, (1) if Falling on point set Hit the mark On, will Replace with , the rest remain unchanged; (2) If it falls on the edge set The medium weight is Edge Then add points to the graph structure , remove The edge of and The weights of the two edges are: , ( for distance between two points); (3) If it falls to A triangle with vertices Then add points to the graph structure , generating three new edges , and , weights and the way to construct the initial graph structure are the same, which is the cost function in the map The line integrals of and .

[0055] The focus of the second stage is to find beacon points and key points. If the starting point is S and the end point is E, the connection order of the waypoints is , After the transition in the previous step, the waypoints are now connected in line segments. The nodes are currently connected in the order of S, , , , E. Among them , , is the information node generated according to the trend line in the previous step. The total number of information nodes is 2 means there are two starting points and two ending points. Indicates the number of waypoints. Since each waypoint generates two information nodes, the total number must be an even number. Here, we process them in groups of two at a time, that is, the groups are: , , at this time, the information nodes generated by the same waypoint are as follows , , the transition has been completed by line segment, so it does not need to be considered. For example, the Dijkstra algorithm generates a graph The shortest path on , from the initial point S to the end point Point-by-point connection test. If the cost increases significantly after connecting with line segments, the cost calculation method is the same as the graph structure, which is the cost function in the map. If the increment exceeds the set threshold of the original cost times, the preceding node of the check node is set as both the beacon point and the new starting point, and the check continues point by point until the end. Obviously, if the line passes through the obstacle area, the line integral is infinite. See Fig. 9 As shown in (a), the polygonal area represents the high-cost area or the prohibited area, the dotted line indicates the detection of the cost increase node (the dotted line passes through the polygonal area), and the five-pointed star indicates the node before the increase node, that is, the beacon point. After all the beacon points are obtained, all beacon points are checked to see whether they meet the smoothing generation conditions and whether they pass through the prohibited area after generation. If not, they will be adjusted, that is, they will be moved back along the obstacle direction until they do not pass through the obstacle after smoothing, and the final key point will be generated. Fig. 9 The solid line in (b) represents the generated result after verification.

[0056] By extending the trend line and finding key points one by one, we can get the key point set of the model. , and then generate a smooth path through the point set. The size of each segment insertion radius is determined by the constraints and map information. Here, the particle swarm PSO algorithm is used to determine the optimal radius. PSO is initialized with a set of random solutions, and then the optimal solution is found through iteration. In each iteration, it updates itself by referring to the individual optimal pbest of each radius and the global optimal gbest of all radii. After finding these two optimal values, it updates its own speed and position through the following formula.

[0057] ;

[0058] in, In the present invention, N does not usually need to be too large to obtain a better solution, and the value of N can be set to 15; is the inertia factor, set it to a constant between 0.8 and 1; Indicates the update speed of the radius, set to That's it; A random number between 0 and 1; Indicates the solution of each current radius; and All are equal to 1. To update the current solution. The PSO algorithm can quickly find a set of high-quality radius solutions, which are used as the arc radius of each part of the smoothing. After smoothing, the final route can be obtained (the smoothing method is at the front). Obviously, if the map cost is uniform, the path with the lowest cost is the shortest route, and the insertion radius should be the minimum radius.

Claims

1. A method for finding a low-cost, multi-constrained smooth path on a costed map, characterized in that: The method specifically comprises: Constructing a mathematical model to smooth the path connecting the starting point and the end point to obtain a smooth path, wherein the smooth path is composed of line segments, circular arcs and transition curves; Based on the mathematical model, a two-stage algorithm is proposed. In the first stage, point sampling is performed on the map to construct a graph structure, and the Dijkstra algorithm is used to obtain the connection order of the waypoints. In the second stage, based on the first stage, the waypoints are processed one by one to generate a control point set P that meets the conditions, and finally the arc radius set R is calculated. Under the sets P and R, the path is uniquely determined.

2. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 1, characterized in that: The mathematical model is used to describe and solve the following problem: based on a given cost map, find a smooth path connecting the start point and the end point, where the smooth path should satisfy the following optimization criteria and constraints: (1) pass through all given waypoints; (2) avoid given obstacles; (3) the path meets the requirement of curvature continuity; (4) the path has a maximum curvature constraint, that is, there is a minimum radius restriction on the arc; (5) the length of the line segment between the curves is greater than the minimum length of this type of line segment; (6) under the above criteria and constraints, reduce the cost of the path.

3. A method for finding a low-cost, multi-constrained smooth path on a cost map according to claim 2, characterized in that: The objective function of the mathematical model is: ； Among them, st ; ； ； ； ； ； Among them, argmin represents the parameter value of a function to achieve the minimum value in its domain, and the objective function represents finding the control point set P and radius set R at the minimum cost. Indicates that the curvature is continuous, and n represents the number of curved parts of the entire path; , Indicates order, represents the function that assigns cost to the map, Represents a set of path curves The radius under the control point is The arc and the corresponding transition curve, Represents a set of path segments The radius of the control point and the adjacent arc and The line segment below, represents the smooth path generated by the path curve set C and the path segment set L, Indicates all regions, represents the feasible area, that is, the obstacle-free area, Represents the waypoints that need to be passed. represents the set of all waypoints, Indicates the radius of the path at the control point, Represents the minimum radius constraint after smoothing, which controls the maximum curvature of the generated route. represents the supplementary angle of the extended line at the i-th control point, It indicates the outward displacement value of the tangent point of the i-th curve. The tangent point of the arc and the line segment before adding the transition curve becomes the tangent point of the transition curve and the line segment after adding the transition curve. Indicates the external displacement value of its change, represents the minimum line segment length constraint at that location, It is divided into two categories: line segments between curves in the same direction and curves in opposite directions.

4. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 3, characterized in that: The entire smooth path is composed of line segments, arcs, and transition curves. A transition curve can be uniquely determined by the following parameters: (1) Length of the transition curve L; (2) Coordinates and direction angles at the starting point ; (3) Two parameters that determine its linear curvature function ; Given these parameters, the position of the curve at any point s in the transition curve [0,L] is calculated by solving the following equation: , and direction angle : ； Angle between each line segment The number of curves n is uniquely determined by the control point set P, and a curve consists of two transition curves and an arc.

5. A method for finding a low-cost, multi-constrained smooth path on a cost map according to claim 4, characterized in that: In the transition curve, line segment OQ is tangent to the long dashed arc at point Q. The radius of the circle is Equal to R, insert a transition curve of length L between line segment OQ and the long dashed arc, that is, the curve , move the tangent point Q of the line segment and the arc outward, and move the arc inward. By moving, the tangent point of the arc and the line segment is converted into the tangent point of the arc and the transition curve, and the tangent point of the line segment and the transition curve. Assuming that the starting point of the transition curve is at the origin and the initial direction angle is 0, it is easy to get , It represents the supplementary angle of the extended line. It is also equal to the central angle of the circle according to mathematical properties, which is consistent with the model constraints in the previous article. The meaning is the same, assuming , according to the properties of the transition curve, we can get and The value of is a function of L and R, and the values ​​of the two are respectively recorded as and , respectively represent the arc inward displacement value and the tangent point outward displacement value, where v is the same as the model constraint in the previous text The meaning is the same, indicating the outward displacement value. Here, when the relaxation curve From the properties of the curve, we know that the radius of curvature of any point on the curve is The length of the curve at that point The product is a constant, , then when ,get: ； At this time, the length of the transition curve is L, and we get: ； , It is the result of integrating the transition curve and represents the position.

6. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 5, characterized in that: The specific steps of smoothing are: inserting a transition curve between the line segment and the arc, first inserting an arc with a radius of R between the two line segments, the intersection point of the line segments is P, and the tangent points are and , then the arc center angle is , It represents the supplementary angle of the extended straight line. Assuming that the center of the circle is , the arc along Direction of movement units, and the central angle decreases from both ends , so as to obtain the processed arc; the tangent point and Along and move Units get points and As the starting point of the transition curve, the smooth transition between the line segment and the arc is completed by inserting the transition curve.

7. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 1, characterized in that: The two-stage algorithm inputs are: map information including boundaries and obstacles, constraint data, and location coordinates of waypoints. Constraint data includes z, , algorithm parameters take the minimum distance between points , incremental cost threshold , where the default , is the barrier-free area, the incremental cost threshold The default value is 1.

8. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 7, characterized in that: In the first stage the algorithm includes: Step 1: Collect points on the map information, ensuring that the distance between any two points is greater than the preset parameter , that is, any two points , The distance should satisfy: ; Based on these points, the growth method is used to generate a graph structure based on the Delaunay triangulation network, denoted as , where V is the set of points and E is the set of edges; Step 2: Determine the order of connecting the waypoints. The cost between two waypoints is determined by Determined by the shortest distance determined using Dijkstra's algorithm.

9. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 8, characterized in that: Step 1 includes: first, find the point pair with the smallest distance between any two points among all the points, connect these two points as the initial baseline; second, find the third point to create the first triangle, use the two endpoints of the baseline as the starting point of the vector, and use the remaining unconstructed points as the end point of the vector in turn, and calculate the cosine value of the angle between the two vectors. ,in and are the two endpoints of the baseline, For the remaining points that have not been constructed, find the point with the smallest cosine value to construct the first triangle. Take the three sides of the first triangle as the baseline, and find the expandable points that form a triangle with the baseline. The expandable points are outside the triangle. Find the point with the smallest cosine value among all the expandable points. The formed edge cannot be a repeated expansion edge. Repeat the steps of finding points and constructing edges until all baselines are processed.

10. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 9, characterized in that: Step 2 includes: the cost between two waypoints is given by The shortest distance determined by Dijkstra's algorithm is determined on the graph structure. In , for setting two points to find a shortest path, first start from one point, set the distance of the point to 0, set the adjacent nodes to the weight of the edge, and set the distance of other nodes to ∞, indicating that the path to the node has not been found. Then, from the unvisited nodes, select the node closest to the starting point and mark it as visited. For all neighbor nodes of the selected node, check whether the path from the starting point through the current node to the neighbor node is shorter than the currently known shortest path. If so, update the shortest path value of the neighbor node and repeat the above steps until all nodes are visited or the remaining unvisited nodes are not connected to the source node. After the algorithm is completed, the shortest path between the two points is determined.

11. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 7, characterized in that: The second stage of the algorithm includes: processing the connection order obtained in the first stage one by one; first, according to the trend of the waypoint, the point is smoothly transitioned in the form of a line segment. After determining the order, there are three points whose connection order is , , the direction of the line segment is , the trend line indicates that the trend is generated in the order of connection, and the waypoints complete the transition through the line segment, that is, two information nodes are generated in the direction of the trend line , where the direction of the trend line is controlled by the position of the obstacle and the length of the line segment. When the original trend line direction leads to an obstacle, the direction will be adjusted. The adjustment method is as follows: The trend line is swung until the transition line segment does not pass through obstacles. The swinging method is: , direction angle , hour, 0, the swing adjustment angle is The line segment length parameter is determined by the minimum line segment length and the maximum curvature in the constraint and is set to 2 , at this time will Synchronously join the graph structure middle.

12. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 11, characterized in that: Will Synchronously join the graph structure The specific method is: For example, (1) if Falls on a point in the point set V On, will Replace with , the rest remain unchanged; (2) If it falls in the edge set E, the weight is Edge Then add points to the graph structure , remove The edge of and The weights of the two edges are: , , for The distance between two points; (3) If it falls to A triangle with vertices Then add points to the graph structure , generating three new edges , and , weights and the way to construct the initial graph structure are the same, which is the cost function in the map The line integrals of and .

13. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 12, characterized in that: In the second stage, the specific method for finding beacon points and key points is: if the starting point is S and the end point is E, the connection order of the waypoints is , After the transition in the previous stage, the waypoints are connected in the form of line segments. The current nodes are connected in the order of S, , , , E, where , , is the information node generated according to the trend line in the previous step. The total number of information nodes is , 2 means there are two starting points and end points, Indicates the number of waypoints. Each waypoint will generate two information nodes, so the total number must be an even number. Here, they are processed in groups of two at a time, that is, the groups are: , ,by For example, the Dijkstra algorithm generates a graph The shortest path Localpath on the group is composed of the initial point Towards the end point of the group Point-by-point connection test. If the cost increases significantly after connecting with line segments, the cost calculation method is the same as the graph structure, which is the cost function in the map. If the increment exceeds the set threshold of the original cost, times, the preceding node of the check node is set as the beacon point and the new starting point at the same time, and the point-by-point inspection is continued until the end. If the line passes through the obstacle area, the line integral is infinite. After all beacon points are obtained, all beacon points are checked to see if they meet the smoothing generation conditions and whether they pass through the prohibited area after generation. If not, they will be adjusted, that is, they will be moved back along the obstacle direction until they do not pass through the obstacle after smoothing to generate the final key point.

14. A method for finding a low-cost, multi-constrained smooth path on a costed map according to claim 13, characterized in that: By extending the trend line and finding key points one by one, we can get the key point set of the model. , and then generate a smooth path through the point set. The size of each segment insertion radius is determined by constraints and map information. Here, the particle swarm PSO algorithm is used to determine the optimal radius. The PSO algorithm is initialized as a set of random solutions, and then the optimal solution is found through iteration. In each iteration, it updates itself by referring to the individual optimal pbest of each radius and the global optimal gbest of all radii. After finding these two optimal values, it updates its speed and position through the following formula: ； in, , let N be 15; is the inertia factor, set it to a constant between 0.8 and 1; Indicates the update speed of the radius, set to That's it; A random number between 0 and 1; Indicates the solution of each current radius; and All equal to 1, To update the current solution, use the PSO algorithm to find a set of high-quality radius solutions, which are used as the arc radius for smoothing each part. After smoothing, the final route can be obtained.

Images