Multi-vehicle two-dimensional smooth path planning method based on improved wolf pack algorithm

Abstract

The invention discloses a multi-vehicle two-dimensional smooth path planning method based on an improved wolf pack algorithm, and belongs to the field of unmanned vehicle path planning, and the method comprises the following steps: S1, constructing a driving space of an unmanned vehicle group according to a driving environment of the unmanned vehicle group; s2, designing a driving cost function of the unmanned vehicle group; s3, iteratively updating the head wolf of the wolf pack path planning algorithm, and determining the position of the head wolf as the optimal path point of the unmanned vehicle group in the driving space; and S4, carrying out smoothing processing to obtain a multi-vehicle two-dimensional smooth path planning result. According to the multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm, the optimal path points, output by the improved wolf pack path planning algorithm, of the unmanned vehicle group are smoothed, the actual running footprint points of all the unmanned vehicles can be obtained, and the multi-vehicle two-dimensional smooth path planning result is obtained; and the method has the characteristics of simple algorithm implementation, good global convergence and high calculation robustness.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation, and particularly to a multi-vehicle two-dimensional smooth path planning method based on an improved wolf pack algorithm. Background Art

[0002] With the continuous development of driverless vehicle technology, the path planning of driverless vehicles has become an important part of the research field of driverless vehicles.

[0003] Specifically, the path planning of driverless vehicles refers to planning an optimal path that quickly and safely reaches the target point from the starting node in the target area under the conditions of meeting various constraints and objectives. Common path planning algorithms are divided into classical algorithms and swarm intelligence algorithms. Among them, classical algorithms include the A* algorithm, artificial potential field algorithm, and rapidly-exploring random tree algorithm, and swarm intelligence algorithms include classical genetic algorithms, ant colony algorithms, particle swarm algorithms, etc. However, classical algorithms themselves have problems such as slow convergence speed in the later stage of the algorithm and being easily trapped in local optima.

[0004] With the proposal of the wolf pack search algorithm, its advantages such as good global convergence and computational robustness can be utilized, making it suitable for solving optimization problems of complex functions. Therefore, compared with other algorithms, the wolf pack algorithm has more advantages in dealing with the path planning problem of driverless vehicles in complex environments.

[0005] However, traditional wolf pack algorithms also have problems such as too slow convergence speed in the later stage, decreased optimization accuracy, and being easily trapped in local minima. Summary of the Invention

[0006] To solve the above problems, the present invention provides a multi-vehicle two-dimensional smooth path planning method based on an improved wolf pack algorithm. By smoothing the optimal path points of the driverless vehicle group output by the improved wolf pack path planning algorithm, the footprint points of each driverless vehicle during actual driving can be obtained, and the multi-vehicle two-dimensional smooth path planning result can be obtained, and it has the characteristics of simple algorithm implementation, good global convergence, and high computational robustness.

[0007] To achieve the above object, the present invention provides a multi-vehicle two-dimensional smooth path planning method based on an improved wolf pack algorithm, including the following steps: S1. Construct the driving space of the driverless vehicle group according to the driving environment of the driverless vehicle group, and set the starting point and target point positions of the driverless vehicle group; S2. Based on the driving space described in step S1, design the driving cost function of the driverless vehicle group, and transform the path planning problem into an optimization problem; S3. Based on the driving cost function of the driverless vehicle group described in step S2, iteratively update the lead wolf of the wolf pack path planning algorithm, and determine the position of the lead wolf as the optimal path point of the driverless vehicle group in the driving space; S4. Smooth the optimal path points of the driverless vehicle group in the driving space output in step S3, obtain the footprint points of each driverless vehicle during actual driving, and obtain the multi-vehicle two-dimensional smooth path planning result.

[0008] Preferably, in step S1, a cylindrical function and a spherical function are used to perform three-dimensional environment modeling on the map environment to construct the driving space of the driverless vehicle group.

[0009] Preferably, in step S2, the driving cost function of the driverless vehicle group includes a driving distance cost function, a threat cost function, a time coordination cost function, and a collision cost function for the driverless vehicle driving.

[0010] Preferably, the expression of the driving distance cost function is as follows: (1) In the formula, is the total driving distance of the driverless vehicle group; 、 are respectively the two-dimensional coordinate values of the th path point; 、 are respectively the two-dimensional coordinate values of the th path point; is the total number of path points; is the total number of driverless vehicles in the driverless vehicle group; The expression of the threat cost function is as follows: (2) (3) In the formula, is the total threat cost of the driverless vehicle group; is the threat penalty function; , are both weight coefficients; is the distance between the driverless vehicle and the radar; is the distance between the driverless vehicle and the threat; The expression of the time coordination cost function is as follows: (4) (5) In the formula, is the total time coordination cost of the driverless vehicle group; is the time coordination cost function; is the actual time for the th driverless vehicle to reach the end point; is the specified task time; and are respectively the actual time ranges for the th driverless vehicle to reach the end point; The collision cost function expression is as follows: (6) (7) In the formula, is the total collision cost of the driverless vehicle group; represents the number of collisions; represents the driverless vehicle and the driverless vehicle The Euclidean distance between them; The driving cost function expression of the driverless vehicle group is as follows: (8) In the formula, is the driving cost function of the driverless vehicle group; is the weight coefficient of the driving distance cost function; is the weight coefficient of the threat cost function; is the weight coefficient of the time coordination cost function; is the weight coefficient of the collision cost function.

[0011] Preferably, the actual time ranges and for the driverless vehicle to reach the end point are expressed as follows: (9) In the formula, and are respectively the minimum speed and the maximum speed of the th driverless vehicle; is the footprint length of the th driverless vehicle.

[0012] Preferably, step S3 specifically includes the following steps: S31. Initialize the wolf pack parameters, and according to the initialized parameters, generate multiple trajectories through the greedy generation method, and the number of trajectories is equal to the number of the wolf pack. The trajectories contain path point information, and the path point information includes two-dimensional coordinate values and the speed information corresponding to the two-dimensional coordinate values. At the same time, determine the current alpha wolf position and the alpha wolf fitness through the driving cost function of the driverless vehicle group; S32. Cluster the current wolf pack according to the fitness function, and start the exploring wolf wandering behavior for each sub-population according to the clustering result. The position of the exploring wolf is updated as follows: (10) Wherein, is the position of the nth exploration wolf in the m-dimensional space; The nth exploration wolf in the m-dimensional space; is the walking step length of the exploration wolf in the m-dimensional space; During the walking process, when the exploration wolf reaches the maximum number of walking times or the fitness is less than that of the leading wolf, the walking ends; S33. Select the original leading wolf or the exploration wolf with the minimum fitness as the leading wolf. Through the summoning behavior, make the fierce wolves move towards the leading wolf, and update the positions of the fierce wolves as follows: (11) Wherein, represents the position of the kth fierce wolf in the m-dimensional space after the kth iteration; in the m-dimensional space; represents the position of the kth fierce wolf in the m-dimensional space after the (k + 1)th iteration; in the m-dimensional space; represents the rushing step length of the fierce wolf in the m-dimensional space; represents the position of the leading wolf in the m-dimensional space after the kth iteration; in the S34. During the rushing process, when the fitness of the fierce wolf is less than that of the leading wolf, the fierce wolf becomes the new leading wolf and initiates the summoning behavior again; when the distance between the fierce wolf and the leading wolf is less than the distance threshold, transfer to the siege behavior, and during the siege stage, update the positions of the wolf pack as follows: (12) Wherein, is a random number uniformly distributed between [-1, 1], represents the siege step length of the wolf pack; During the siege process, when the fitness of the fierce wolf is less than that of the leading wolf, update the positions of the wolf pack again; S35. At the end of one iteration, record the position of the leading wolf of the sub-population, and regard the position of the leading wolf of the sub-population as the optimal path point of the multi-vehicle of the sub-population, and merge the populations. Then judge whether the termination condition is satisfied. If it is satisfied, output the optimal solution in the merged wolf pack, and regard the optimal solution in the merged wolf pack as the optimal path point of the driverless vehicle group in the driving space; otherwise, return to step S31 until the termination condition is satisfied.

[0013] Preferably, in step S34, the distance threshold is calculated as follows: (13) Wherein, represents the spatial dimension, is the distance judgment factor. and respectively represent the maximum and minimum values of the variables to be optimized in the - dimensional space.

[0014] Preferably, in step S4, the cubic spline interpolation method is used to smooth the footprint points of multiple vehicles; It specifically includes the following steps: For the known data points , the interpolation function of each dimension is expressed as a cubic polynomial: (14) (15) In the formula, 、 are respectively , dimensional spline interpolation functions; 、 、 、 , 、 、 、 are all coefficients to be solved; When performing cubic spline interpolation, the following conditions are satisfied: 1) At adjacent data points, the first - order and second - order derivatives are continuous: (16) (17) 2) Pass through each data point: (18) The following conditions are satisfied: 1) At adjacent data points, the first - order and second - order derivatives are continuous: (19) (20) 2) Pass through each data point: (21) Meet the following conditions: 1) At adjacent data points, the first and second derivatives are continuous: (22) (23) 2) Pass through each data point: (24) Substitute formulas (16)-(26) into formulas (14)-(15) for solution to obtain the fitting path.

[0015] The present invention has the following beneficial effects: By smoothing the optimal path points of the driverless vehicle group output by the improved wolf pack path planning algorithm, the footprint points of each driverless vehicle during actual driving can be obtained, and the two-dimensional smooth path planning result of multiple vehicles can be obtained, and it has the characteristics of simple algorithm implementation, good global convergence, and high computational robustness.

[0016] The technical solution of the present invention will be further described in detail below with reference to the drawings and embodiments. Brief Description of the Drawings

[0017] Figure 1 It is a schematic structural diagram of the multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm of the present invention. Detailed Embodiments

[0018] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the embodiments of the present invention will be further described in detail below with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the embodiments of the present invention, and are not used to limit the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in this application without creative efforts belong to the scope of protection of this application. The examples of the embodiments are shown in the drawings, where the same or similar reference numerals represent the same or similar elements or elements with the same or similar functions from beginning to end.

[0019] It should be noted that the terms "include" and "have" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or server that includes a series of steps or units does not necessarily have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0020] Like reference numerals and letters refer to like items in the following figures; thus, once an item is defined in one figure, it need not be further defined or explained in subsequent figures.

[0021] As Figure 1 shown, the multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm includes the following steps: S1. Construct the driving space of the driverless vehicle group according to the driving environment of the driverless vehicle group, and set the starting point and target point positions of the driverless vehicle group; In step S1, a cylindrical function and a spherical function are used to perform three-dimensional environment modeling on the map environment to construct the driving space of the driverless vehicle group.

[0022] S2. Based on the driving space described in step S1, design the driving cost function of the driverless vehicle group, and transform the path planning problem into an optimization problem; In step S2, the driving cost function of the driverless vehicle group includes a driving distance cost function, a threat cost function, a time coordination cost function, and a collision cost function for the driverless vehicle.

[0023] The driving distance cost refers to the length of the driving path, and the expression of the driving distance cost function is as follows: (1) In the formula, is the total driving distance of the driverless vehicle group; 、 are respectively the two-dimensional coordinate values of the th path point; 、 are respectively the two-dimensional coordinate values of the th path point; is the total number of path points; is the total number of driverless vehicles in the driverless vehicle group; The threat cost refers to radar, missiles, atmospheric threats, etc. during the driving process of the driverless vehicle group, and the expression of the threat cost function is as follows: (2) (3) In the formula, is the total threat cost of the driverless vehicle group; is the threat penalty function; , are both weight coefficients; is the distance between the driverless vehicle and the radar; is the distance between the driverless vehicle and the threat; The time coordination cost refers to the time cost for each driverless vehicle in a driverless vehicle fleet to complete a task. The expression of the time coordination cost function is as follows: (4) (5) In the formula, is the total time coordination cost of the driverless vehicle fleet; is the time coordination cost function; is the actual time for the th driverless vehicle to reach the end point; is the specified task time; and are respectively the actual time ranges for the th driverless vehicle to reach the end point; The actual time range for the driverless vehicle to reach the end point and are expressed as follows: (6) In the formula, and are respectively the minimum speed and the maximum speed of the th driverless vehicle; is the footprint length of the th driverless vehicle.

[0024] The collision cost refers to that the minimum distance between each driverless vehicle in the driverless vehicle fleet is not less than the minimum safe driving distance. The expression of the collision cost function is as follows: (7) (8) In the formula, is the total collision cost of the driverless vehicle fleet; represents the number of collisions; represents the Euclidean distance between the driverless vehicle and the driverless vehicle ; represents the safe distance between driverless vehicles; The expression of the driving cost function of the driverless vehicle fleet is as follows: (9) In the formula, is the driving cost function of the driverless vehicle fleet; is the weight coefficient of the driving distance cost function; is the weight coefficient of the threat cost function; is the weight coefficient of the time coordination cost function; is the weight coefficient of the collision cost function.

[0025] S3. Based on the driving cost function of the driverless vehicle swarm described in step S2, iteratively update the lead wolf of the wolf pack path planning algorithm to determine that the position of the lead wolf is the optimal path point of the driverless vehicle swarm in the driving space; Step S3 specifically includes the following steps: S31. Initialize the parameters of the wolf pack, and generate multiple trajectories through the greedy generation method according to the initialized parameters. The number of trajectories is equal to the number of the wolf pack. The trajectories contain path point information, and the path point information includes two-dimensional coordinate values and speed information corresponding to the two-dimensional coordinate values. At the same time, determine the current position of the lead wolf and the fitness of the lead wolf through the driving cost function of the driverless vehicle swarm; S32. Cluster the current wolf pack according to the fitness function, and start the exploring wolf wandering behavior according to the clustering results. The position of the exploring wolf is updated as follows: (10) where is the th exploring wolf's position in the dimensional space; is the wandering step size of the exploring wolf in the dimensional space; During the wandering process, when the exploring wolf reaches the maximum number of wandering times or the fitness is less than that of the lead wolf (the smaller the cost), the wandering ends; S33. Select the original lead wolf or the exploring wolf with the minimum fitness as the lead wolf, and through the summoning behavior, make the fierce wolves move towards the lead wolf. The position of the fierce wolves is updated as follows: (11) where represents the position of the th fierce wolf after the th iteration in the dimensional space; represents the position of the th fierce wolf after the th iteration in the dimensional space; represents the rushing step size of the fierce wolf in the dimensional space; represents the position of the lead wolf in the th iteration in the dimensional space; S34. During the rushing process, when the fitness of the fierce wolf is less than that of the lead wolf, the fierce wolf becomes the new lead wolf and initiates the summoning behavior again; when the distance between the fierce wolf and the lead wolf is less than the distance threshold, transfer to the siege behavior. During the siege stage, the position of the wolf pack is updated as follows: (12) Wherein, is a random number uniformly distributed between [-1, 1], representing the siege step length of the wolf pack; During the siege process, when the fitness of the fierce wolf is less than that of the leading wolf, the position of the wolf pack is updated again; S35. At the end of one iteration, record the position of the leading wolf of the sub-population, and regard the position of the leading wolf of the sub-population as the optimal path point of multiple vehicles in the sub-population, and merge the populations. Then judge whether the termination condition is satisfied. If it is satisfied, output the optimal solution in the merged wolf pack, and regard the optimal solution in the merged wolf pack as the optimal path point of the driverless vehicle group in the driving space; otherwise, return to step S31 until the termination condition is satisfied.

[0026] Preferably, in step S34, the distance threshold is calculated as follows: (13) Wherein, represents the dimensionality of the space, is the distance judgment factor. and respectively represent the maximum and minimum values of the variables to be optimized in the -dimensional space.

[0027] S4. Smooth the optimal path points of the driverless vehicle group output in step S3 in the driving space to obtain the footprint points of each driverless vehicle actually traveling, and obtain the two-dimensional smooth path planning result of multiple vehicles.

[0028] In step S4, cubic spline interpolation is used to smooth the footprint points of multiple vehicles to reduce the discreteness between the footprint points; It specifically includes the following steps: For the known data points , represent the interpolation function of each dimension as a cubic polynomial: (14) (15) Wherein, 、 are respectively , the spline interpolation functions of the dimension; 、 、 、 and 、 、 、 All are coefficients to be solved; When performing cubic spline interpolation, Satisfy the following conditions: 1) At adjacent data points, the first-order and second-order derivatives are continuous: (16) (17) 2) Pass through each data point: (18) Satisfy the following conditions: 1) At adjacent data points, the first-order and second-order derivatives are continuous: (19) (20) 2) Pass through each data point: (21) Satisfy the following conditions: 1) At adjacent data points, the first-order and second-order derivatives are continuous: (22) (23) 2) Pass through each data point: (24) Substitute formulas (16) - (24) into formulas (14) - (15) for solution to obtain the fitting path.

[0029] Therefore, the present invention adopts the above multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm. By smoothing the optimal path points of the driverless vehicle group output by the improved wolf pack path planning algorithm, the footprint points of each driverless vehicle during actual driving can be obtained, and the multi-vehicle two-dimensional smooth path planning result can be obtained, and it has the characteristics of simple algorithm implementation, good global convergence, and high computational robustness.

[0030] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that they can still modify or equivalently replace the technical solutions of the present invention, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A multi-vehicle two-dimensional smooth path planning method based on an improved wolf pack algorithm, characterized in that: The following steps are involved: S1. Construct a driving space for the unmanned vehicle group according to the driving environment of the unmanned vehicle group, and set the starting point and target point positions of the unmanned vehicle group; S2. Based on the driving space described in step S1, a driving cost function of the unmanned vehicle group is designed to transform the path planning problem into an optimization problem; S3, based on the unmanned vehicle group driving cost function described in step S2, iteratively updating the leader of the wolf group path planning algorithm, and determining the leader position as the optimal path point of the unmanned vehicle group in the driving space; S4. Smoothing the optimal path points of the unmanned vehicle group in the driving space outputted in step S3, obtaining the actual driving footprints of each unmanned vehicle, and obtaining a two-dimensional smooth path planning result for multiple vehicles.

2. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 1 is characterized in that: In step S1, a three-dimensional environment modeling is performed on the map environment using cylindrical functions and spherical functions to construct a driving space for the unmanned vehicle group.

3. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 2 is characterized in that: In step S2, the driving cost function of the unmanned vehicle group includes the driving distance cost function, threat cost function, time coordination cost function and collision cost function of the unmanned vehicle.

4. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 3 is characterized in that: The driving distance cost function expression is as follows: （1） In the formula, is the total driving distance of the driverless vehicle group; 、 Respectively The two-dimensional coordinate values ​​of the path points; 、 Respectively The two-dimensional coordinate values ​​of the path points; is the total number of path points; is the total number of driverless cars in the driverless car group; The threat cost function expression is as follows: （2） （3） In the formula, is the total threat cost of the driverless car group; is the threat penalty function; , All are weight coefficients; is the distance between the driverless car and the radar; The distance between the driverless car and the threat; The time coordination cost function expression is as follows: （4） （5） In the formula, is the total time coordination cost of the driverless vehicle group; is the time coordination cost function; For the The actual time it takes for the driverless car to reach the destination; To set the time for the task; and Respectively The actual time frame for the driverless car to reach the destination; The collision cost function expression is as follows: （6） （7） In the formula, is the total collision cost of the driverless car group; Indicates the number of collisions; Represents driverless car and driverless cars The Euclidean distance between Indicates the safe distance between driverless cars; The driving cost function expression of the unmanned vehicle group is as follows: （8） In the formula, is the driving cost function of the driverless car group; is the weight coefficient of the travel distance cost function; is the weight coefficient of the threat cost function; is the weight coefficient of the time collaboration cost function; is the weight coefficient of the collision cost function.

5. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 4 is characterized in that: The actual time range for the driverless car to reach the destination and The expression is as follows: （9） In the formula, and Respectively The minimum and maximum speeds of the autonomous vehicles; For the The length of a driverless car's footprint.

6. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 5 is characterized by: Step S3 specifically includes the following steps: S31, initializing wolf pack parameters, and generating multiple trajectories by a greedy generation method according to the initialized parameters, and the number of trajectories is equal to the number of wolf packs, and the trajectories include path point information, and the path point information includes two-dimensional coordinate values ​​and speed information corresponding to the two-dimensional coordinate values, and at the same time, determining the current alpha wolf position and the alpha wolf fitness through the unmanned vehicle group driving cost function; S32, cluster the current wolf pack according to the fitness function, and start exploring the wolf's wandering behavior and location according to the clustering results. Updated as follows: （10） In the formula, For the Only the wolf is The position of the dimensional space; Indicates the number of walking directions, The value is The constant of For the wolf The walking step length in dimensional space; During the wandering process, when the scout wolf reaches the maximum wandering times or the fitness is less than that of the leader wolf, the wandering ends; S33, select the original leader wolf or the scout wolf with the smallest fitness as the leader wolf, and make the fierce wolf move towards the leader wolf through the calling behavior. The position of the fierce wolf is updated as follows: （11） In the formula, Indicates After iterations, the wolf exist Position in dimensional space; Indicates After iterations, the wolf exist Position in dimensional space; express The running stride length of the fierce wolf in the dimensional space; Represents iteration After The position of the alpha wolf in the dimensional space; S34: During the raid, when the fitness of a fierce wolf is less than that of the leader wolf, the fierce wolf becomes the new leader wolf and initiates the summoning behavior again; when the distance between the fierce wolf and the leader wolf is less than the distance threshold, the siege behavior is switched, and during the siege phase, the wolf pack position is updated as follows: （12） In the formula, is a random number uniformly distributed between [-1,1]. Indicates the siege step length of the wolf pack; During the siege, when the fitness of the fierce wolf is less than that of the leader wolf, the position of the wolf pack is updated; S35. After one iteration is completed, the position of the head wolf of the sub-population is recorded, and the position of the head wolf of the sub-population is regarded as the optimal path point for multiple vehicles in the sub-population. The populations are merged, and then it is determined whether the termination condition is met. If so, the optimal solution in the merged wolf group is output, and the optimal solution in the merged wolf group is used as the optimal path point for the unmanned vehicle group in the driving space; otherwise, return to step S31 until the termination condition is met.

7. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 6 is characterized by: In step S34, the distance threshold The calculation formula is as follows: （13） In the formula, represents the dimension of space, is the distance judgment factor; and Respectively The maximum and minimum values ​​of the variables to be optimized in the dimensional space.

8. The multi-vehicle two-dimensional smooth path planning method based on the improved wolf pack algorithm according to claim 7 is characterized in that: In step S4, the multiple vehicle footprint points are smoothed using cubic spline interpolation method; It specifically includes the following steps: For the known Data points , expressing the interpolation function in each dimension as a cubic polynomial: (14) (15) In the formula, 、 They are , Dimensional spline interpolation function; 、 、 、 , 、 、 、 These are coefficients to be solved; When performing cubic spline interpolation, The following conditions are met: 1) At adjacent data points, the first-order and second-order derivatives are continuous: （16） （17） 2) Going through each data point: （18） The following conditions are met: 1) At adjacent data points, the first-order and second-order derivatives are continuous: （19） （20） 2) Going through each data point: （21） The following conditions are met: 1) At adjacent data points, the first-order and second-order derivatives are continuous: （22） （23） 2) Going through each data point: （24） Substitute formula (16)-formula (24) into formula (14)-formula (15) to solve and obtain the fitting path.

Images