CN110501020A - A multi-objective 3D path planning method - Google Patents

Abstract

The invention discloses a multi-objective three-dimensional path planning method. By establishing a total estimated cost function F(n), F(n) is an estimated cost function G(n) from a current point to an adjacent point and an estimated cost from an adjacent point to an end point. The function H(n) is summed to form, wherein: G(n) is used to calculate the energy consumption and distance cost from the current point to the neighboring point in the three-dimensional space; H(n) includes the total energy consumption and the distance, and the total energy consumption is calculated by calculating the The energy consumption between the point and the target node is obtained, and the distance is obtained by calculating the straight-line distance and the curved distance between the adjacent point and the target; finally, the multi-objective chaos optimization algorithm is used to solve the multi-objective optimization problem of the path planning method, which can Realize three-dimensional planning that takes into account both energy consumption and distance, find an optimized path to reduce energy consumption in mountainous terrain, and improve the cruising range of smart mobile devices.

Description

A multi-objective 3D path planning method

technical field

The invention belongs to the field of path planning and relates to a multi-objective three-dimensional path planning method, which can be used for path planning in fields such as intelligent navigation and unmanned driving.

Background technique

At present, battery energy storage is very limited, and improving battery life is an important research topic. Relevant research mainly includes two aspects: improving the storage capacity of the battery and reducing the unit energy consumption of the battery. Since the energy consumption coefficient of uphill is significantly greater than that of flat roads, for a mountainous city, a flat and moderately long road may save energy much more than a relatively short road with many ups and downs, and there is no Significantly more time-consuming costs.

At present, many path planning methods have been proposed: such as artificial force field algorithm, neural network method, genetic algorithm, random tree method, A-star algorithm. They can achieve better planning results under various obstacle conditions. However, almost all of them are based on two-dimensional maps, and there are not many researches on path planning methods under three-dimensional maps. Relatively speaking, whether it is a robot, a space probe or an electric vehicle with limited energy consumption, it will be very valuable to study a path planning method that considers energy loss.

Contents of the invention

In view of this, the main purpose of the present invention is to propose a multi-objective three-dimensional path planning method on the basis of taking into account the two goals of energy consumption and distance, so as to find an optimal path for reducing energy consumption in mountainous terrain and improve intelligent mobility. The battery life of the device.

On the one hand, the present invention provides a multi-objective three-dimensional path planning method, comprising the following steps:

Step 1, construct a multi-objective 3D path planning model, and establish a total estimated cost function F(n):

F(n)=G(n)+H(n) (1)

In the formula, G(n) is the estimated cost function from the current point to the neighboring point, and H(n) is the estimated cost function from the neighboring point to the end point, where:

G(n) is obtained by the energy consumption and straight-line distance between the current point and the neighboring point;

H(n) includes the total energy consumption and the distance, the total energy consumption is obtained by calculating the energy consumption between the adjacent point and the target node, and the distance is obtained by calculating the straight-line distance and the curved distance between the adjacent point and the target node;

Step 2, calculate the value of the total estimated cost function F(n), find the minimum value of the total estimated cost function F(n) among the adjacent points, and store it in the path list Lane;

Step 3, taking the node with the smallest total estimated cost function F(n) found in step 2 as the current point, and repeating path planning;

Step 4: When the path planning ends at the current point, the planning is stopped. At this time, the path stored in the Lane is the path planned by the multi-objective 3D path planning model.

Preferably, the step 1 is specifically expressed as:

Step 1.1, initialize the map elevation data: obtain the (x, y, z) coordinate data of each feasible node, and store it in the feasible node list Path, where the coordinates (x, y) are latitude and longitude, and the coordinates (x, y ) is mapped to the actual distance coordinates;

Step 1.2, initialize the path planning parameters: define the starting point and end point of the plan, set the search radius as factor, which is used to search the neighbors around the current node, and use the starting point as the current point for planning;

Step 1.3, use the search radius factor to find all the neighboring points around the current point, and calculate the estimated cost function G(n) from the current point to the neighboring point and the value of the estimated cost function H(n) from the neighboring point to the end point, where:

G(n)＝D(P _n ,P _n+1 )+E _G (P _n ,P _n+1 ) (2)

In the formula, E _G (P _n ,P _n+1 ) is the energy consumption between the current point and its neighbors, D(P _n ,P _n+1 ) is the Euclidean distance between the current point and its neighbors , P _n is the current point, P _n+1 is the neighbor point;

H(n)＝η ₁ *E _H +η ₂ *D′ _H +η ₃ *D″ _H (3)

In the formula, E _H is the total energy consumption from the adjacent point to the target node, D′ _H is the straight-line distance between the adjacent point and the target node, D″ _H is the curved distance between the adjacent point and the target node, η ₁ , η ₂ and η ₃ are the weights of the total energy consumption E _H , the straight line distance D′ _H and the curve distance D″ _H respectively.

Preferably, the energy consumption between the current point and neighboring points in the G(n) is obtained by establishing a slope energy consumption model, and the slope energy consumption model is established as follows:

In the formula, g ₁ (P _n ,P _n+1 ) and g ₂ (P _n ,P _n+1 ) are respectively Downhill energy consumption function per unit distance and uphill energy consumption function per unit distance when , where:

In the formula, (x _n , y _n , z _n ) are the coordinate data of the current point P _n respectively, and (x _n+1 , y _n+1 , z _n+1 ) are the coordinates of the adjacent point P _n+1 ( data;

Preferably, the unit distance downhill energy consumption function g ₁ (P _n , P _n+1 ) is established by the following method:

Select a flat road and multiple downhill roads with different slopes;

Measure the energy consumption generated by the same type of robot or car traveling the same distance on flat roads and multi-section downhill roads;

The average energy consumption of the flat road and each section of the downhill road was obtained through multiple tests;

Calculate the ratio of the average energy consumption of downhill roads with different slopes to the average energy consumption of flat roads;

Using cubic polynomial fitting, the downhill energy consumption function g ₁ (P _n ,P _n+1 ) per unit distance is obtained:

In the formula, a ₁ , b ₁ and c ₁ are undetermined coefficients.

Preferably, the uphill energy consumption function g ₂ (P _n , P _n+1 ) per unit distance is established by the following method:

Select a flat road and multiple uphill roads with different slopes;

Measure the energy consumption of the same type of robot or car traveling the same distance on flat roads and multi-segment uphill roads;

The average energy consumption of the flat road and each section of the uphill road was obtained through multiple tests;

Calculate the ratio of the average energy consumption of uphill roads with different slopes to the average energy consumption of flat roads;

Using cubic polynomial fitting, the uphill energy consumption function g ₂ (P _n ,P _n+1 ) per unit distance is obtained:

In the formula, a ₂ , b ₂ and c ₂ are undetermined coefficients.

Preferably, by adding the energy consumption of adjacent nodes, the algorithm simulates the terrain of mountains and estimates the positions of high mountains and valleys, and the energy consumption of the adjacent nodes is calculated through the slope energy consumption model, as follows:

Among them, E _H is the total energy consumption from the adjacent node to the target node, P _i and P _i+1 are the intermediate nodes from the node to the target node, ω _i is the weight from the i-th node to the i+1-th node, and ω _i is obtained from the descending curve of the sigmoid function,

Among them, b is the slope of the curve, c is the offset value, exp(-b*x(i)) is the power of -b*x(i) of e, and x(i) is the distance from the i-th node to the current point distance.

Preferably, the distance in the H(n) is obtained by calculating the straight-line distance and the curve distance between the adjacent point and the target node, and the formula is as follows:

D=D′ _H +D″ _H (11)

In the formula, D is the distance, and D′ _H is calculated by the following formula:

In the formula, P _goal is the target node, (x _goal , y _goal , z _goal ) is the coordinate data of P _goal ;

D″ _H is calculated by the following formula:

In the formula, D(P _i ,P _i+1 ) is the straight-line distance from the i-th node to the i+1-th node.

Preferably, the two objectives of total energy consumption and distance in H(n) are optimized by a multi-objective chaotic optimization algorithm, and the solution that satisfies the two objectives of the lowest total energy consumption and the shortest distance is solved, and the parameters η ₁ , η ₂ , η ₃ and c are optimized and solved to obtain the optimal value of each parameter.

Preferably, the specific steps of the multi-objective chaos optimization algorithm are as follows:

A) Initializing the population: For the above four optimization variables η ₁ , η ₂ , η ₃ and c, assign slightly different initial values to obtain 4 chaotic variables, and enlarge the scope of the chaotic variables to the corresponding optimal variables to take Value range, at the same time, the pop solutions in the population are initialized in the way of chaotic sequence;

B) selected population size;

C) Calculation of the objective function: obtain the path Lane through the multi-objective three-dimensional path planning model, and obtain the objective function value of each solution in the population;

D) Non-dominated relationship and congestion degree sorting: sort the solutions of the objective function by non-dominated relationship, keep only the solution of the first non-dominated frontier, and sort the crowding degree of the solution of the first dominant frontier, and select a lower degree of crowding solution;

E) Generation of offspring by chaotic mutation: the parent generation is mutated by the method of chaotic sequence, and the calculation formula is:

C _offspring ＝4*C _parent *(1-C _parent ) (18)

Among them, C _parent is the solution set of the parent population, and C _offspring is the solution set of the offspring population;

F) cyclic iteration: continue the calculation of the objective function in step C with the obtained offspring population, obtain the objective function value of each solution of the offspring population, and carry out loop iteration;

G) Termination criterion: When the algorithm is executed to the maximum number of generations or the value of the objective function in the population is stable, the algorithm terminates, wherein all the solutions of the first dominant front are the solutions sought, and the parameters [η ₁ , η ₂ , η ₃ ,c] solution.

In summary, the present invention first establishes the total estimated cost function F(n), F(n) from the current point to the estimated cost function G(n) of the adjacent point and the estimated cost function H(n) of the adjacent point to the end point The summation structure, in which: G(n) is obtained by the energy consumption and the straight-line distance between the current point and the neighboring point; H(n) contains the total energy consumption and the distance, and the total energy consumption is obtained by calculating the energy The distance is obtained by calculating the straight-line distance and the curve distance between the adjacent point and the target; then, find the current point by calculating the minimum value of the total estimated cost function F(n); then, repeat the path planning until the current point is the end point When stop, these nodes that minimize the total estimated cost function F(n) finally form the planned path. Compared with the existing technology, the above method can realize three-dimensional planning that takes energy consumption and distance into account, finds an optimized path to reduce energy consumption in mountainous terrain, and improves the cruising range of smart mobile devices.

In a further technical solution, the multi-objective chaotic optimization algorithm is used to optimize the total energy consumption and the distance in H(n), and the solution that satisfies the two objectives of the lowest total energy consumption and the shortest distance is solved, and the parameter η ₁ , η ₂ , η ₃ and c are optimized and solved to obtain the optimal value of each parameter, which is used to solve the multi-objective optimization solution problem of the path planning method.

Description of drawings

Fig. 1 is the flowchart of a kind of multi-target three-dimensional path planning method of the present invention;

Fig. 2 is a kind of multi-objective three-dimensional path planning model figure of the present invention;

Fig. 3 is the flowchart of multi-objective chaos optimization algorithm of the present invention;

Fig. 4 is a road network diagram of a certain mountainous city according to an embodiment of the present invention;

Fig. 5 is the front view of the route planned by the present invention based on the road network in Fig. 4;

FIG. 6 is a top view of the route planned based on the road network in FIG. 4 according to the present invention.

Detailed ways

It should be noted that, in the case of no conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and examples.

The main purpose of the present invention is to provide an energy-saving solution from the perspective of path planning. Since the energy consumption coefficient of electric vehicles uphill is significantly greater than that of flat roads, for a mountainous city, a flat and moderately long road may save energy far more than a relatively short road with many uphill and downhill roads, and There is no significant time-consuming cost. Different from the traditional path planning method that only considers the shortest distance, the present invention proposes a multi-objective three-dimensional path planning method on the basis of taking energy consumption and distance into consideration.

Referring to Fig. 1, Fig. 1 is a flow chart of a multi-objective three-dimensional path planning method, which specifically includes the following steps:

Step 1.1, initialize the map elevation data: obtain the (x, y, z) coordinate data of each feasible node, and store it in the feasible node list Path, where the coordinates (x, y) are latitude and longitude, and the coordinates (x, y ) is mapped to the actual distance coordinates;

Step 1.2, initialize the path planning parameters: define the starting point and end point of the plan, set the search radius as factor, which is used to search the neighbors around the current node, and use the starting point as the current point for planning;

It should be noted that an adjacent point is defined as a point with a certain radius around the current point;

Step 1.3, use the search radius factor to find all the neighboring points around the current point, and calculate the estimated cost function G(n) from the current point to the neighboring point and the value of the estimated cost function H(n) from the neighboring point to the end point, where:

Since the uphill energy consumption coefficient is significantly greater than the energy consumption coefficient on flat roads, and the downhill energy consumption coefficient is also different from the uphill energy consumption coefficient, it is more reasonable to use the energy consumption model to plan the path, and the planned path can avoid Continuous uphill and downhill, and because the motor is in a semi-stall state when going uphill, the operating current of the motor increases, causing the motor to heat up a lot and endangering the life of the motor. Therefore, the estimated cost function G from the current point to the adjacent point is (n) is calculated as follows:

G(n)＝D(P _n ,P _n+1 )+E _G (P _n ,P _n+1 ) (2)

In the formula, E _G (P _n ,P _n+1 ) is the energy consumption between the current point and its neighbors in kilojoules (kJ), D(P _n ,P _n+1 ) is the energy consumption between the current point and its neighbors The Euclidean distance between , P _n ( is the current point, P _n+1 is the neighbor point;

At the same time, the establishment of the estimated cost function H(n) from the adjacent point to the end point in the present invention needs to consider two aspects of energy consumption and distance, that is, the estimated cost function H(n) includes the total energy consumption E _H and the distance D, where the total energy The energy consumption E _H is obtained by calculating the energy consumption between the adjacent point and the target node, and the distance D is obtained by calculating the straight-line distance D′ _H and the curved distance D″ _H between the adjacent point and the target node. This method makes electric vehicles or robots When driving in mountainous terrain, while keeping the path short and saving energy, the general H(n) function only considers the influence of the distance. When planning a path in complex mountainous terrain, the algorithm will not be able to distinguish the mountainous terrain, resulting in repeated up and down The slope increases the energy consumption and reduces the cruising range of the battery. Therefore, the establishment of the H(n) function is established by the following formula:

H(n)H=η ₁ *E _H +η ₂ *D′ _H +η ₃ *D″ _H (3)

In the formula, E _H is the total energy consumption from the adjacent point to the target node, D′ _H is the straight-line distance between the adjacent point and the target node, D″ _H is the curved distance between the adjacent point and the target node, η ₁ , η ₂ and η ₃ are the weights of the total energy consumption E _H , the straight line distance D′ _H and the curve distance D″ _H respectively.

Step 2. Calculate the value of the total estimated cost function F(n), find the minimum value of the total estimated cost function F(n) among the adjacent points, and store it in the path list Lane. The calculation formula of F(n) is as follows:

F(n)=G(n)+H(n) (1);

FIG. 2 is a diagram of a multi-objective three-dimensional path planning model. It can also be seen from Figure 2 that the total estimated cost function F(n) includes the estimated cost function G(n) from the current point to the adjacent point and the estimated cost function H(n) from the adjacent point to the end point, and the estimated cost function G( n) and the estimated cost function H(n) are modeled separately. This is because the estimated cost from the current point to the neighboring point is certain, but the path from the neighboring point to the destination is uncertain, and only the cost from the neighboring point to the destination can be estimated.

Step 3, take the node with the smallest total estimated cost function F(n) found in step 2 as the current point, and repeat path planning.

Step 4: When the path planning ends at the current point, the planning is stopped. At this time, the path stored in the Lane is the path planned by the multi-objective 3D path planning model.

It should be noted that steps 1.1 to 1.4 in the above flow chart can be considered to be used to construct a multi-objective 3D path planning model and establish a total estimated cost function F(n).

Further, the estimated cost function G(n) from the current point to the neighboring point is obtained by calculating the slope energy consumption model, and the aforementioned slope energy consumption model is established as follows:

In the formula, g ₁ (P _n ,P _n+1 ) and g ₂ (P _n ,P _n+1 ) are respectively The downhill energy consumption function per unit distance and the uphill energy consumption function per unit distance, where the Euclidean distance D(P _n ,P _n+1 ) between the current point and the neighboring point is calculated as follows:

In the formula, (x _n , y _n , z _n ) are the coordinate data of the current point P _n respectively, and (x _n+1 , y _n+1 , z _n+1 ) are the coordinates of the adjacent point P _n+1 ( data;

At the same time, the calculation method of the slope is as follows:

In addition, in a further technical solution, the above unit distance downhill energy consumption function g ₁ (P _n ,P _n+1 ) is established by the following method:

Select a flat road and multiple downhill roads with different slopes;

Measure the energy consumption generated by the same type of robot or car traveling the same distance on flat roads and multi-section downhill roads;

The average energy consumption of the flat road and each section of the downhill road was obtained through multiple tests;

Calculate the ratio of the average energy consumption of downhill roads with different slopes to the average energy consumption of flat roads;

Using cubic polynomial fitting, the downhill energy consumption function g ₁ (P _n ,P _n+1 ) per unit distance is obtained:

In the formula, a ₁ , b ₁ and c ₁ are undetermined coefficients.

Taking a certain type of robot or car as an example, at slope α=[0°,-5°,-10°,-15°,-20°,-25°,-30°,-35° ,-40°], measure the energy consumption generated by driving 100 meters, and obtain the average energy consumption [E ₁ , E ₂ , E ₃ , E ₄ , E ₅ , E ₆ , E ₇ through multiple experiments , E ₈ , E ₉ ], calculate the ratio of energy consumption of different slopes to energy consumption of flat roads Through the analysis of the experimental data, it is found that the slope is the main factor for the change of energy consumption. Using the cubic polynomial fitting method, the values of the undetermined coefficients a ₁ , b ₁ and c ₁ can be obtained from the above experimental data, and the above-mentioned unit distance can be obtained slope energy function.

Similarly, the establishment method of the unit distance uphill energy consumption function g ₂ (P _n ,P _n+1 ) is similar to the establishment method of the unit distance downhill energy consumption function g ₁ (P _n ,P _n+1 ):

Select a flat road and multiple downhill roads with different slopes;

Measure the energy consumption generated by the same type of robot or car traveling the same distance on flat roads and multi-section downhill roads;

The average energy consumption of the flat road and each section of the downhill road was obtained through multiple tests;

Calculate the ratio of the average energy consumption of downhill roads with different slopes to the average energy consumption of flat roads;

Using cubic polynomial fitting, the downhill energy consumption function g ₁ (P _n ,P _n+1 ) per unit distance is obtained:

In the formula, a ₁ , b ₁ and c ₁ are undetermined coefficients.

Similarly, taking a certain type of robot or car as an example, measure The energy consumption generated by driving 100 meters, the average energy consumption [E ₁ ′, E ₂ ′, E ₃ ′, E ₄ ′, E ₅ ′, E ₆ ′, E ₇ ′, E ₈ ′,E ₉ ′], and calculate the ratio of energy consumption of different slopes to energy consumption of flat roads Through the analysis of the experimental data, it is found that the slope is the main factor of the change of energy consumption. Using the method of cubic polynomial fitting, the values of the undetermined coefficients a ₂ , b ₂ and c ₂ can be obtained through the above experimental data, and the above-mentioned unit distance can be obtained slope energy function.

Further, the total energy consumption E _H is calculated through the slope energy consumption model, as follows:

In the formula, the energy consumption estimation model E _H , by adding the energy consumption of adjacent nodes, the algorithm can simulate the terrain of the mountain and estimate the position of the mountain and the valley, making the algorithm more intelligent; P _i and P _{i+ 1} is the intermediate node from the node to the target node, and ω _i is the weight from the i-th node to the i+1-th node. Since people are used to paying more attention to nearby mountain peaks, when planning paths for electric vehicles or mountain robots, etc. , first consider the influence of the nearby mountain peaks on the path, so the weight ω _i is set to be larger near the weight, and as the distance increases, the weight gradually decreases. Therefore, the weight ω _i can be determined by the sigmoid function (the nonlinear action function of ) descending curve is obtained,

Among them, b is the slope of the curve, c is the offset value, exp(-b*x(i)) is the power of -b*x(i) of e, and x(i) is the distance from the i-th node to the current point distance.

In addition, the distance estimation model D is used for path length estimation, which is composed of a curved distance estimation model D″ _H and a straight line distance model D′ _H , namely:

D=D′ _H +D″ _H (11)

Since the straight-line distance model D′ _H is added with the H(n) function, the planning algorithm is more directional, and there will be no circuitous paths. The straight-line distance D′ _H is obtained by using Euclid’s calculation formula, and the unit is meter (m), that is

In the formula, P _goal is the target node, x _goal , y _goal , z _goal is the coordinate data of P _goal ;

The curve distance estimation model is calculated by the following formula, namely

Among them, D(P _i ,P _i+1 ) is the straight-line distance from the i-th node to the i+1-th node. Here, the weight ω _i is used to weight the curve distance, so that the planned path will not directly cross the nearby mountain due to the excessive weight of the distant mountain.

In addition, as mentioned above, the establishment of the estimated cost function H(n) from the adjacent point to the end point includes the energy consumption estimation, the weights η ₁ , η ₂ and η ₃ of the straight-line distance and the curve distance, and the establishment of the energy consumption model , including the calculation of the offset value c; using a multi-objective method to optimize the two objectives of the least energy consumption and the shortest distance, and optimize and solve the above coefficients.

Specifically, the calculation method for the two goals of energy consumption and distance uses the path Lane planned by the A-star algorithm to calculate the energy consumption and distance values between adjacent points in the path. The calculation formula is as follows:

Among them, n is the total number of nodes in the planned path.

The method of described chaos optimization, with the help of the pseudo-random global non-repetitive ergodicity of chaos, is more able to overcome the problem of convergence in the locality compared with optimization methods such as genetics and PSO. At the same time, the method of chaos optimization includes two steps:

a. Investigate the chaotic variables in the whole space in turn, and find the current optimal point that meets the performance index. The calculation method of the chaotic variables is:

x ^(k+1) = η*x ^(k) *(1-x ^(k) ) (16)

Among them, η is the control parameter. When η=1, the system is in a chaotic state, and the output is equivalent to a random number between [0,1], and it has ergodicity in [0,1], and any state in it will not repeated;

b. If there is no change in the current optimal point after several searches, define a subspace near the current optimal point, and then examine the chaotic variables in the entire space to find the optimal point that meets the performance index.

Preferably, the present invention optimizes and solves the two objectives of the total energy consumption E _H and the distance D through a multi-objective chaotic optimization algorithm, and adopts the multi-objective optimization algorithm, which can make the establishment of the estimated cost function H(n) from the adjacent point to the terminal more accurate. Exactly, the desired desired path is obtained, and the optimum values are simultaneously solved for the optimization variables η ₁ , η ₂ , η ₃ and c. The constraint of the optimization method is to minimize the value of the following objective function:

Specifically, the detailed steps of the above multi-objective chaos optimization algorithm are as follows:

A) Initializing the population: For the above four optimization variables η ₁ , η ₂ , η ₃ and c, assign slightly different initial values to obtain 4 chaotic variables, and enlarge the scope of the chaotic variables to the corresponding optimal variables to take Value range, at the same time, the pop solutions in the population are initialized in the way of chaotic sequence;

B) selected population size;

Preferably, in this embodiment, the population size is set to pop=50, and the maximum number of iterations g=200;

C) calculation of objective function: obtain path Lane by multi-objective three-dimensional path planning model, obtain the objective function value E _Lane and D _Lane of each solution in the population;

D) Sorting of non-dominated relationship and congestion degree: sort the solutions of the objective function by non-dominated relationship, and only keep the solution of the first non-dominated front, that is, the solution of this front is not dominated by any other solutions, and the first dominated front The solution is sorted by the degree of congestion, and the solution with a lower degree of congestion is selected, so that the obtained solutions are distributed as evenly as possible and have diversity;

E) Generation of offspring by chaotic mutation: the parent generation is mutated by the method of chaotic sequence, and the calculation formula is:

C _offspring ＝4*C _parent *(1-C _parent ) (18)

Among them, C _parent is the solution set of the parent population, and C _offspring is the solution set of the offspring population;

F) cyclic iteration: continue the calculation of the objective function in step C with the obtained offspring population, obtain the objective function value of each solution of the offspring population, and carry out loop iteration;

G) Termination criterion: When the algorithm is executed to the maximum number of generations or the value of the objective function in the population is stable, the algorithm terminates, wherein all the solutions of the first dominant front are the solutions sought, and the parameters [η ₁ , η ₂ , η ₃ ,c] solution.

Further, as shown in Figure 3, the cyclic iteration of step F is specifically manifested as: after entering the next generation, chaotic mutation generates offspring and requires the integration of the parent and offspring, and then enters the calculation of the objective function in step C, and then enters step D non-dominated Sort the relationship and degree of crowding, and then, chaotic mutation generates offspring to select a new population parent until step G is entered.

At the same time, referring to Fig. 4 to Fig. 6, Fig. 4 is a road network diagram of a certain city, Fig. 5 is a front view of the route planned based on the road network of the present invention, and Fig. 6 is a top view of the route planned based on the road network of the present invention.

Use the road network diagram of certain city, obtain elevation information and road network information therefrom, set up the map model applicable to the present invention, carry out path planning in matlab (matrix laboratory), the path planning effect that obtains is as shown in Figure 5, can It can be seen that the algorithm proposed by the present invention can quickly obtain the planned path in mountainous terrain, and the path can avoid high mountains and valleys for planning, which is in line with people's driving habits.

Therefore, in order to save energy, a multi-objective three-dimensional path planning method considering energy loss and distance is proposed. This method establishes a total estimated cost function F(n), and F(n) is estimated from the current point to the neighboring point The cost function G(n) and the estimated cost function H(n) from the adjacent point to the end point are summed to form, where: the estimated cost function G(n) is obtained by the energy consumption and the straight-line distance between the current point and the adjacent point; the estimated The cost function H(n) includes the total energy consumption and the distance. The total energy consumption is obtained by calculating the energy consumption between the adjacent point and the target node, and the distance is obtained by calculating the straight-line distance and the curved distance between the adjacent point and the target node; finally, using A multi-objective chaos optimization algorithm is used to solve the multi-objective optimization problem of the path planning method. Taking a city as an example to conduct experiments, it is proved that the algorithm can obtain a path that takes into account both energy loss and time cost. Based on experiments and principles, it can be deduced that this method can be applied to all three-dimensional terrains, and can provide path planning solutions for electric vehicles, military mountain reconnaissance robots, space star probes (Yutu, Mars probes) and other devices that consider energy consumption.

The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made by using the description of the present invention and the contents of the accompanying drawings, or directly or indirectly used in other related technical fields , are all included in the scope of patent protection of the present invention in the same way.

Claims (9)

1. a kind of multiple target three-dimensional path planning method, which comprises the steps of:

Step 1, multiple target three-dimensional path planning model is constructed, and establishes overall estimate cost function F (n):

F (n)=G (n)+H (n) (1)

In formula, G (n) is estimate cost function of the current point to adjoint point, and H (n) is estimate cost function of the adjoint point to terminal, In:

G (n) is obtained by energy consumption between current point and adjoint point and linear distance；

H (n) includes total energy consumption and distance, and total energy consumption is obtained by calculating the energy consumption between adjoint point and destination node, and distance passes through The linear distance and curve distance calculated between adjoint point and destination node obtains；

Step 2, the value for calculating overall estimate cost function F (n), finds the smallest value of overall estimate cost function F (n) in adjoint point, deposits Enter in path list Lane；

Step 3, using the smallest node of the overall estimate cost function F (n) found in step 2 as current point, repeat path Planning；

Step 4, when path planning to current point is terminal, stop planning, at this point, what is stored in Lane is that multiple target is three-dimensional Path planning model cooks up the path come.

2. multiple target three-dimensional path planning method according to claim 1, which is characterized in that step 1 specific manifestation Are as follows:

Step 1.1, it initializes map altitude data: obtaining (x, y, z) coordinate data of each feasible node, and be stored in feasible section In point list Path, wherein coordinate (x, y) is longitude and latitude, and coordinate (x, y) is mapped as actual range coordinate；

Step 1.2, initialization path projecting parameter: definition planning beginning and end position sets search radius as factor, uses Adjoint point around search present node, is planned starting point as current point；

Step 1.3, all adjoint points around current point are found using search radius factor, and calculates current point estimating to adjoint point Count cost function G (n) and adjoint point to terminal estimate cost function H (n) value, in which:

G (n)=D (P_n,P_n+1)+E_G(P_n,P_n+1) (2)

In formula, E_G(P_n,P_n+1) energy consumption between current point and adjoint point, D (P_n,P_n+1) Europe between current point and adjoint point is several In distance, P_nFor current point, P_n+1For adjoint point；

H (n) H=η₁*E_H+η₂*D′_H+η₃*D″_H (3)

In formula, E_HIt is total energy consumption of the adjoint point to destination node, D '_HLinear distance between adjoint point and destination node, D "_HFor neighbour Curve distance between point and destination node, η₁、η₂And η₃Respectively total energy consumption E_H, linear distance D '_HWith curve distance D "_HPower Weight.

3. multiple target three-dimensional path planning method according to claim 2, which is characterized in that in the G (n) current point and Energy consumption between adjoint point is obtained by establishing gradient energy consumption model, and the gradient energy consumption model is established as follows:

In formula, g₁(P_n,P_n+1) and g₂(P_n,P_n+1) it is respectively to be in the gradientWhen unit distance descending energy consumption function and Unit distance upward slope energy consumption function, in which:

In formula, (x_n,y_n,z_n) it is respectively current point P_nCoordinate data, (x_n+1,y_n+1,z_n+1) it is respectively adjoint point P_n+1Number of coordinates According to；

4. multiple target three-dimensional path planning method according to claim 3, which is characterized in that the unit distance descending energy Consume function g₁(P_n,P_n+1) establish by the following method:

Choose one section of level road descending road different with the multistage gradient；

Energy caused by the robot of same model or the level road of running car same distance and multistage descending road is measured respectively Consumption；

Repeatedly test acquires the energy consumption average value of level road and every section of descending road respectively；

Find out the ratio of different gradient descending road average energy consumption and level road average energy consumption；

By the way of cubic polynomial fitting, unit distance descending energy consumption function g is obtained₁(P_n,P_n+1):

In formula, a₁,b₁And c₁It is undetermined coefficient.

5. multiple target three-dimensional path planning method according to claim 4, which is characterized in that the unit distance upward slope energy Consume function g₂(P_n,P_n+1) establish by the following method:

Choose one section of level road upward slope road different with the multistage gradient；

Energy caused by the robot of same model or the level road of running car same distance and multistage upward slope road is measured respectively Consumption；

Repeatedly test acquires the energy consumption average value of level road and every section of upward slope road respectively；

Find out the ratio of different gradient upward slope road average energy consumption and level road average energy consumption；

By the way of cubic polynomial fitting, unit distance upward slope energy consumption function g is obtained₂(P_n,P_n+1):

In formula, a₂,b₂And c₂It is undetermined coefficient.

6. multiple target three-dimensional path planning method according to claim 5, which is characterized in that pass through the energy consumption of adjacent node It is added, so that algorithm simulation is gone out the landform in mountainous region, and estimate the position of high mountain low ebb, the energy consumption for obtaining the adjacent node passes through Gradient energy consumption model is calculated, as follows:

Wherein, E_HIt is total energy consumption of the adjoint point to destination node, P_iAnd P_i+1For the intermediate node of node to destination node, ω_iIt is i-th Weight of a node to i+1 node, and ω_iIt is acquired by the curve that sigmoid function declines,

Wherein, b is curves, and c is bias, and exp (- b*x (i)) is-b*x (i) power of e, and x (i) is i-th of node To the distance of current point.

7. multiple target three-dimensional path planning method according to claim 6, which is characterized in that distance passes through in the H (n) The linear distance and curve distance calculated between adjoint point and destination node obtains, and formula is as follows:

D=D '_H+D″_H (11)

In formula, D is distance, wherein D '_HIt is calculated by following formula:

In formula, P_goalFor destination node, (x_goal,y_goal,z_goal) it is P_goalCoordinate data；

D″_HIt is calculated by following formula:

In formula, D (P_i,P_i+1) it is linear distance of i-th of node to i+1 node.

8. multiple target three-dimensional path planning method according to claim 7, which is characterized in that optimized by multi-Objective Chaotic Algorithm optimizes two targets of total energy consumption in H (n) and distance, and solution meets that total energy consumption is minimum and most short two targets of distance Solution, and to wherein parameter η₁、η₂、η₃It is optimized with c, obtains the optimal value of each parameter.

9. multiple target three-dimensional path planning method according to claim 8, which is characterized in that the multi-Objective Chaotic optimization Specific step is as follows for algorithm:

A) initialization population: for above-mentioned 4 optimized variable η₁、η₂、η₃And c, the initial value of fine difference is assigned respectively, obtains 4 Chaos Variable, and the range of Chaos Variable is amplified to corresponding optimized variable value range respectively, meanwhile, in population Pop solution is initialized with the mode of chaos sequence respectively；

B population scale) is selected；

C) the calculating of objective function: path Lane is obtained by multiple target three-dimensional path planning model, obtains each solution in population Target function value；

D) non-dominant relationship and crowding sort: carrying out non-dominant relationship sequence to the solution of objective function, only retain the first non-branch Solution with forward position, and to the solution that first dominates forward position, crowding sequence is carried out, the lower solution of crowding is chosen；

E) chaotic mutation generates filial generation: it is made a variation by the method for chaos sequence to parent, calculation formula are as follows:

C_offspring=4*C_parent*(1-C_parent) (18)

Wherein, C_parentIt is the disaggregation of parent population, C_offspringIt is the disaggregation of progeny population；

F) loop iteration: obtained progeny population is continued to the calculating of objective function in step C, it is each to obtain progeny population The target function value of solution carries out loop iteration；

G) stop criterion: when algorithm, which goes to the target function value in maximum algebra or group, to be stablized, algorithm is terminated, In, the first all solutions for dominating forward position are all required solutions, and obtain parameter [η₁, η₂,η₃, c] solution.