CN111879328B - Intelligent vehicle path planning method based on potential energy field probability map in cross-country environment - Google Patents

Abstract

The invention discloses an intelligent vehicle path planning method based on a potential energy field probability map in a cross-country environment, which comprises the following steps: s1, establishing a multi-level environment state potential field model for evaluating the risk of the off-road environment by adopting an artificial potential field method; s2, establishing a cross-country environment space topological graph, and generating a multi-dimensional node connection evaluation model; s3, starting from the current node, searching the expansion nodes around the current node and communicated with the current node, and evaluating the passing cost of each expansion node; s4, starting from the starting point of the path, searching new expansion nodes, and evaluating the passing cost of each new expansion node until the new expansion nodes are expanded to a target end point; and S5, generating a vehicle motion track. The invention can output the potential value of the environmental situation field according to the multi-dimensional cross-country environmental information around the vehicle, adopts a random sampling method to establish a cross-country environmental space topological graph, and generates an optimized path by evaluating the cross-country environmental traffic risk among nodes in the topological graph, thereby achieving the feasible, safe and efficient intelligent vehicle driving target.

Description

Intelligent vehicle path planning method based on potential energy field probability map in cross-country environment

Technical Field

The invention relates to the technical field of automatic driving, in particular to an intelligent vehicle path planning method based on a potential energy field probability map in a cross-country environment.

Background

The intelligent vehicle can complete tasks such as information acquisition, reconnaissance and monitoring, logistics transportation and communication transfer in a complex off-road environment, and plays a vital role in emergency rescue operations such as outbreak epidemic situation, natural disasters and accident emergency rescue. The path planning of the intelligent vehicle in the cross-country environment determines whether the intelligent vehicle can safely, efficiently and smoothly complete various driving behaviors in the driving process and smoothly reach a target terminal, and the intelligent vehicle path planning method is a key technology in the field of automatic driving of the intelligent vehicle.

In recent years, the automatic driving path planning technology of the intelligent vehicle is rapidly developed and can be divided into 5 categories: the most widely used one is a path planning method based on random sampling, such as a fast search random tree (RRT), a probability map (PRM), and a path oriented subdivision tree method (PDST), which has a problem that it cannot adapt to threat elements and off-road roads in the environment during the path planning process in the off-road environment; the path planning method based on graph search adopts an omnidirectional expansion search technology, optimizes the connection of road sections to generate a collision-free path with the shortest distance, the planning algorithm comprises Dijkstra, A, D, the variation thereof and the like, the graph search method can search the optimal path, but has the problems of long planning time and poor adaptability to the cross-country environment; the method is characterized in that the path planning problem of the intelligent vehicle is converted into a two-point boundary value problem to be solved, a path track is generated by adopting a fixed type of curve, such as a B spline curve, a quintic polynomial curve, a solid spiral line and the like, the track generated by the geometric curve method is smooth and smooth, can meet the vehicle kinematics requirement, but has the serious defect of poor environmental condition adaptability, and cannot meet the path planning requirement under the vehicle cross-country environment; the artificial potential field method abstracts the environment information into a function of an attractive force or a repulsive force field, plans a collision-free path from a starting point to a target end point through a potential energy field, has the advantages of high planning speed, smooth path and dynamic safe obstacle avoidance, and has the defects of potential energy traps and path oscillation; in addition, the bionics algorithm is developed rapidly in recent years, such as an ant colony algorithm, a genetic algorithm, a particle swarm algorithm and the like, but the bionics algorithm has the problems of long programming time and low convergence rate.

Disclosure of Invention

The invention aims to provide an intelligent vehicle path planning method based on a potential energy field probability map in an off-road environment, which can output the potential value of an environment situation field of a vehicle according to multi-dimensional off-road environment information around the vehicle, establish an off-road environment space topological map by adopting a random sampling method on the basis, generate an optimized path by evaluating the traffic risk of the off-road environment between nodes in the topological map, and achieve the feasible, safe and efficient intelligent vehicle driving target.

In order to achieve the purpose, the invention provides an intelligent vehicle path planning method based on a potential energy field probability map in a cross-country environment, which is characterized by comprising the following steps: s1, establishing a multi-level environmental state potential field model for evaluating the risk of the off-road environment by adopting an artificial potential field method according to the environmental information acquired by the intelligent vehicle; s2, generating nodes in the cross-country environment space through random sampling, establishing a cross-country environment space topological graph, and generating a multi-dimensional node connection evaluation model for evaluating the feasibility and the traffic cost of road sections among the nodes, wherein the multi-dimensional node connection evaluation model comprises a communication evaluation matrix and a traffic cost matrix; s3, starting from the current node, searching the expansion nodes around the current node and communicated with the current node, and evaluating the passing cost of each expansion node; s4, starting from the starting point of the path, adopting the method provided in S3, continuously and repeatedly starting from the node with the minimum current traffic cost, searching the new expansion nodes around the node with the minimum current traffic cost and communicated with the node with the minimum current traffic cost, and evaluating the traffic cost of each new expansion node until the node is expanded to the target end point of the path; and S5, generating a vehicle motion track by adopting a dynamic curvature smoothing method.

Due to the adoption of the technical scheme, the invention has the following advantages: the path planning method in the off-road environment provided by the invention firstly adopts a potential energy field method to respectively establish a hierarchical off-road environment potential field quantization model for obstacles, threats and roads in the off-road environment, and establishes a comprehensive feasibility, safety and high efficiency passing cost evaluation method among multi-level environment nodes by using the environment potential field quantization model; aiming at the vehicle traffic risk assessment problem under the complex off-road environment condition, a multi-dimensional node connection assessment model between nodes is established in an off-line learning mode, the vehicle traffic risk under the complex off-road environment is assessed, a path node expansion and optimization method taking traffic cost as an assessment index is provided, a weight coefficient matrix can be adjusted according to vehicle performance and task requirements, and a traffic path adaptive to the self-vehicle performance is selected; and finally, generating a vehicle motion track by adopting a dynamic curvature smoothing method. The method provides a path planning method with a multi-objective optimization function for the intelligent vehicle to run in a complex off-road environment, so that the intelligent vehicle can safely and efficiently run in the off-road environment. Therefore, the invention provides a method for planning the path of the intelligent vehicle, which combines an artificial potential energy field method and a probability map method, utilizes an environment space topological graph generated by random sampling on the basis of considering multi-dimensional cross-country environment information, and generates an optimized path by evaluating the cross-country environment traffic risk among nodes in the topological graph.

Drawings

FIG. 1 is a diagram of a framework of an intelligent vehicle path planning based on a potential energy field probability map in an off-road environment according to an embodiment of the invention.

FIG. 2 is a schematic diagram of node distribution in an off-road environment in which an intelligent vehicle is traveling in an embodiment of the invention.

Fig. 3 is a schematic diagram of optimizing the number of nodes in the embodiment of the present invention.

Fig. 4 is a flowchart of an intelligent vehicle route planning method according to an embodiment of the present invention.

FIGS. 5a and 5b are schematic diagrams of road segments and corresponding connectivity evaluation matrices between nodes of an off-road environment in an embodiment of the invention.

Fig. 6a and 6b are schematic diagrams of road segments between nodes of the off-road environment and corresponding traffic safety cost matrixes in the embodiment of the invention.

Fig. 7a and 7b are schematic diagrams of road sections between nodes of the off-road environment and corresponding traffic distance cost matrixes in the embodiment of the invention.

FIGS. 8a and 8b are schematic diagrams of road segments between nodes of the off-road environment and corresponding traffic expansion road cost evaluation matrixes in the embodiment of the invention.

Fig. 9a and 9b are schematic diagrams of road segments between nodes of the off-road environment and corresponding heuristic obstacle cost evaluation matrixes in the embodiment of the invention.

10a and 10b are schematic diagrams of road segments between nodes of the off-road environment and corresponding heuristic distance cost evaluation matrixes in the embodiment of the invention.

11a and 11b are schematic diagrams of road segments between nodes of the off-road environment and corresponding heuristic road cost evaluation matrixes in the embodiment of the invention.

FIG. 12 is a schematic diagram of an optimization node and vehicle motion trajectory smoothing method for path planning in the off-road environment of FIG. 4.

FIG. 13 is a schematic illustration of a vehicle motion profile generated by the intelligent vehicle path planning method in the off-road environment of FIG. 4.

FIG. 14 is a schematic structural diagram of a multidimensional node connection evaluation model in the embodiment of the present invention.

Detailed Description

The invention is described in detail below with reference to the figures and examples.

As shown in fig. 1 to 4, the method for planning a path of an intelligent vehicle based on a potential energy field probability map in an off-road environment provided by the embodiment of the invention includes:

and S1, establishing a multi-level environment state potential field model by adopting an artificial potential field method according to the surrounding environment information of the intelligent vehicle.

The environment information can be obtained by the intelligent vehicle, and comprises image information, as shown in fig. 2The cross-country environment schematic diagram in the image is shown, wherein: 4 architectural obstacles B in the Cross-country Environment₁～B ₄1 forest obstacle B ₄1 threat element T₁2 grassland off-road area P₁、P₂And the rest is a soil road area. The point S in the figure is the starting point of the vehicle (denoted by v)_es) The point G is the target end point of the vehicle (denoted by the symbol v)_eg)。

The multi-level environment state potential field model comprises an obstacle layer potential field model, a threat layer potential field model and a road layer potential field model. The potential energy value U of the barrier layer in a certain range around the vehicle can be calculated through the potential energy field model of the barrier layer_obsThe threat layer potential energy value U within a certain range of the periphery of the vehicle can be calculated through the threat layer potential energy field model_thrAnd the road layer potential energy value U in a certain range around the vehicle can be calculated through the road layer potential energy field model_roaAnd obtaining the off-road environment situation field potential value sigma U through summation so as to evaluate the off-road environment risk.

S1 includes the steps of:

s11, rasterizing the off-road environment in the image of the environment information to obtain a rasterized off-road environment W represented by a two-dimensional matrix, wherein W belongs to R²。

S12, establishing a barrier layer potential energy field model represented by the following formula (1) according to the distribution of the buildings, forests, mountains and other non-traversable barriers in the off-road environment W, wherein the barrier layer potential energy field model is used for dividing the off-road environment W into barrier layer forbidden areas P_obsBarrier layer node sampling limit area P_resAnd barrier layer feasible region P_freWherein: barrier layer node sampling limit area P_resThe area provided around the obstacle according to factors such as the width and turning radius of the vehicle.

In the formula of U_obsIs the barrier potential value, (x, y) is the point coordinates in the off-road environment W,

for a set maximum potential energy of the barrier layer,

the bounding region potential values are sampled for a set barrier node,

is the set barrier potential energy minimum. Wherein the content of the first and second substances,

and

the values of (b) may be set to the values shown in table 1 below, but are not limited thereto:

TABLE 1

In fig. 2 there are 4 obstacles B₁～B₄The square grid area is the forbidden area P of the barrier layer in the embodiment_obsThe potential energy field of the barrier layer is the maximum value of the potential energy of the barrier layer

Forbidden zone P_obsThe radiation is outward carried out by a certain radius to obtain an annular closed communication area as a barrier layer node sampling limit area P_res(one circle of diagonal grid area outside the square grid area) with potential energy field of

Barrier layer removing forbidden zone P in cross-country environment W_obsAnd barrier layer node sampling limit area P_resThe outer region is the feasible region of the barrier layer, and the potential energy value is

S13, establishing a threat level potential energy field model represented by the following formula (2) according to the relative position, attribute and state characteristic information between the threat elements causing loss risk to vehicle driving in the off-road environment W and the vehicle, wherein the threat level potential energy field model is used for dividing the off-road environment W into threat level forbidden areas (effective action areas of the threat elements) P_thrThreat layer restricted passage area (threat influence area) P_effAnd a threat level feasible region, wherein: the threat layer traffic-restricted area is an area for restricting traffic around the threat elements according to the loss risk of the environmental threat elements on the vehicles running in a certain distance range around the threat elements.

In the formula of U_thrIs the potential value of the threat layer, r is the forbidden area P of the threat layer corresponding to the threat element_thrAnd a running vehicle P_vehDistance between r_maxThe farthest distance of action of the threat generated for the set threat element, [ r ]_max，+∞]Potential energy value within a distance range of

r_minEffective acting distance of threat generated for set threat element, [0, r_min]Potential value within the range is set maximum value

[r_min,r_max]The potential energy value of the threat layer in the range decreases with increasing distance r, and the potential energy value thereof decreases

Determined by equation (3), k_wA threat layer forbidden zone P corresponding to the threat elements_thrIs numerically based on the threat coefficient of

Wherein r is_max、 r_min、

And

the values of (d) can be set as the values shown in Table 2 below, r_maxAnd r_minThe specific value of (b) is a setting made in the case of setting a threat of an out-of-control vehicle, but is not limited thereto:

TABLE 2

In FIG. 2, there are 1 threat element T₁In the figure, a five-pointed star is used for representing T₁. Threat element T₁In the region of closed communication (denoted by T in FIG. 2)₁As the area of circle center and black point distribution) as the environmental threat element T₁The effective threat effect area of (1), i.e. the forbidden area P of the threat layer_thrWith a potential energy field at a maximum

FIG. 2 illustrates a forbidden zone P surrounding a threat layer_thrThe region in the shape of a Chinese character 'mi' where the concentric rings are located is a threat influence region and a threat layer passage limiting region, and the potential energy value of the region is

The area except the forbidden area of the threat layer and the restricted passing area of the threat layer in the cross-country environment W is the threat layerFeasible region with potential value of

S14, establishing a road layer potential energy field model represented by the following formula (4) according to different surface attribute characteristic information of a structured road, a dirt road, a grassland, a sand land, a marsh, a mountain land and the like in the off-road environment W:

in the formula of U_roaIs the potential value of the environmental potential field road layer,

for the best structured road potential value of the set road layer traffic conditions,

is the potential value of the muddy road with the worst traffic condition of the set road layer. Wherein the content of the first and second substances,

and

the values of (b) may be set to values as shown in the following table 3, but are not limited thereto. k is a radical of_rFor road traffic coefficient, the road layer potential energy field model divides the off-road environment into 6 rank regions as shown in table 4 according to the surface attribute feature information, referring to the experimental data and expert experience of the wheeled vehicle and the tracked vehicle, but is not limited thereto.

TABLE 3

TABLE 4

As shown in fig. 2, the road layer potential energy field model divides the road layer potential energy field into 3 types of regions: meadow area P₁、P₂The black area around the grassland area is an off-road expansion area, and the area other than the green grassland area and the off-road expansion area in the off-road environment is a dirt road area. Wherein the potential energy field of the grassland area is set to

The potential energy field of the off-road expansion area is set to

The potential energy field of the dirt road area is set as

S15, fusing the barrier layer potential energy field model, the threat layer potential energy field model and the road layer potential energy field model, and establishing a vertical (5) multi-level environment state potential field model by considering mutual coupling influence among the three models:

U_bat＝∑U_obs+∑U_thr-obs+∑U_roa (5)

in the formula of U_batIs the potential value of the environmental situation field, a_ijThe corresponding element values of the matrix are evaluated for node connectivity in an off-road environment (see equation (11) below), U_thr-obsThe threat potential energy field is formed after the superposition area of the threat layer and the barrier layer is fused.

Threat elements T in FIG. 2₁The resulting influence is influenced by the obstacle B₃Is blocked while in B₃The other side of the same creates a non-threatening passage area K.

The embodiment provides a multi-level model of the off-road environment potential field, and the model can be built according to the characteristics of various environment elements, so that the intelligent vehicle can selectively avoid various obstacles, threats and off-road roads in the path planning process. The model is better than a method for sampling and simplifying obstacle model processing of various environmental elements in the environment in the traditional path planning method.

It should be noted that, the multi-level environmental state field model for assessing risk of an off-road environment in the above embodiment may also be modeled separately by using a typical general barrier layer, that is: is denoted as U_bat＝∑U_obsAnd mixed modeling of the barrier layer and the off-road layer can be adopted, namely: u shape_bat＝∑U_obs+∑U_roaOr mixed modeling of an obstacle layer, a road layer and a threat layer is adopted (the coupling relation of the threat layer and the obstacle layer is not considered), namely: u shape_bat＝∑U_obs+∑U_thr+∑U_roa. Compared with the models, the multi-level environment state potential field model provided by the formula (5) and the formula (6) comprehensively considers the influence of environmental obstacles, off-road roads and environmental threats and considers the mutual coupling relationship among the environmental obstacles, the off-road roads and the environmental threats.

S2, as shown in FIG. 14, generating nodes in the off-road environment W through random sampling, establishing an off-road environment space topological graph, and generating a multi-dimensional node connection evaluation model through off-line learning of the multi-level environment state potential field model obtained through S1 so as to evaluate the feasibility of the connection of the road sections between the nodes and evaluate the traffic cost between the connected nodes.

The "off-road environment space topological map" in S2 is represented by the following formula (7):

G_F＝(V_e,E_v) (7)

in the formula, G_FRepresenting a spatial topology, V, of an off-road environment_eRepresenting a set of nodes, E_vRepresenting a collection of connected segments between nodes.

Wherein the sampling requirement of the node is expressed by equation (8), i.e.: node set V in cross-country environment space topological graph_eNode v in_efAnd v_ehGenerating in the cross-country environment W, and selecting the forbidden region P on the barrier layer_obsNode miningSample confinement region P_resForbidden region P with threat layer_thrBesides, the nodes do not overlap with each other:

randomly generated nodes are shown as the origin of the black stone in FIG. 2, and the number of nodes k is set_n＝80。

Node number k in intelligent vehicle path planning method_nThe speed and the performance of the algorithm are determined, the path planning speed is high if the number of nodes is small, and the path optimization degree is low; and if the number is large, the path optimization degree is high, and the planning speed is low. In order to balance planning speed and optimization degree, the number of nodes is optimized by adopting a simulation experiment method, and k is respectively taken as the number of the nodes _n10,20, 30.., 100, number of nodes k per node_nThe number of times of the experiment (N) is 100, and the average planning time T and the standard deviation sigma of the time of the path planning are counted_tAverage passing cost value F_nAnd standard deviation of passing cost sigma_dThe table of the node number and the planning cost is shown in table 5.

TABLE 5

As can be seen from table 5: with the number of nodes k_nIncreasing the planning time T and increasing the standard deviation sigma of the time_tEnlarging; and the planned path passing cost F_nGradually approaching the optimum value with the standard deviation sigma_dAnd becomes smaller. When the number of nodes k_nIncrease to a certain value (e.g. k)_n60), the comprehensive passing cost reduction speed of the planned path is reduced, the consumed planning time is increased rapidly, and the number k of the nodes is optimized by adopting a formula (9) to balance the passing cost and the planning time_n：

J(k_n)＝F(k_n)+λ_kT(k_n) (9)

In the formula, k_nIndicates the number of nodes, J (k)_n) Is the advantage of the number of nodesIndex of formation, F (k)_n) For experimental average traffic cost, T (k)_n) The time is planned for the experimental mean path. Lambda [ alpha ]_kFor the set optimum equalization coefficient, λ is increased, depending on the task requirements, if the trend is towards time_kAnd if the cost tends to be passed, then it is decreased. Lambda [ alpha ]_kThe values of (b) may be set as shown in the numerical values in table 6 below, but are not limited thereto.

TABLE 6

The passing cost is as follows: planning time	λk
		The passing cost is 90% in priority: 10 percent of	1
The passing cost is 70% in priority: 30 percent of	3
		Equilibrium is 50%: 50 percent of	5
Planning time priority 30%: 70 percent of	7
		Planning time priority 10%: 90 percent of	9

Specifically, the data in table 6 is processed. Setting lambda_kFig. 3 can be optimized by the number of nodes, which is 5, and it can be seen from the figure that when the number of nodes k is_n60, when the node proportion is 0.38 per mill, the optimization indicatesThe minimum value J (60) is normalized to 1520, that is, the optimal balance point of the route planning time and the traffic cost is reached.

The "multidimensional node connection evaluation model" in S2 includes a connectivity evaluation matrix A_vAnd a traffic cost matrix. The traffic cost matrix is divided into an extended cost matrix and a heuristic cost matrix. Following is a matrix A for achieving connectivity assessment_vSpecific implementation modes of the extended cost matrix and the heuristic cost matrix are explained one by one.

(one)' connectivity evaluation matrix A_v"expressed as the following expressions (10) and (11) is used to evaluate the feasibility of communication between the nodes for the road section:

A_v∈Rⁿ (10)

in the formula, RⁿIs represented by A_vIs an n-th order matrix with the element a of the ith row and the jth column_ijThe value is 1 or 0.

Evaluation of matrix A by connectivity_vThe method for evaluating the communication feasibility of the road sections among the nodes specifically comprises the following steps: if node v_eiAnd node v_ejSection e between_i,jForbidden region P of barrier layer_obsOr a forbidden zone P of the threat layer_thrBlocking, then represents node v_eiAnd node v_ejSection e between_i,jIs not connected, a_ijIs set to be 0; if the section e_i,jForbidden region P not blocked by barrier layer_obsOr a forbidden zone P of the threat layer_thrBlocking, then represents node v_eiAnd node v_ejSection e between_i,jCommunication, a_ijIs set to '1', i.e. node v_eiAnd node v_ejAnd (4) communicating. For example: FIG. 5a shows an off-road environment showing 1 obstacle B₁And 3 nodes v_e1～v_e3Its corresponding connectivity evaluation matrix A_vAs shown in fig. 5 b.

(II) "extended cost matrix" may include the following extended security cost evaluation matrix S_vExpanding, expandingSpread distance cost evaluation matrix D_vAnd expanding the road cost evaluation matrix P_vBut not limited thereto, such as a meteorological environment cost matrix:

cost matrix of traffic environment (such as road congestion, bridge, tunnel, etc.):

' extended security cost evaluation matrix S_v"is expressed as the following formula (12) and formula (13) for evaluating the traffic safety cost of the road section between two nodes:

S_v∈Rⁿ (12)

in the formula, S_vIs an n-th order positive real number matrix with the ith row and the jth column of the element s_ijRepresenting the traffic safety cost (potential value) between connected nodes.

By extending the security cost evaluation matrix S_vThe method for evaluating the traffic safety of the road sections between the nodes specifically comprises the following steps: firstly, taking n on a road section between two connected nodes in the threat layer potential energy field model_sUniformly distributed security sub-nodes v_t(1)～v_t(n_s) K is the serial number of the security child node, U_thr(k) The threat level potential value of the kth security sub-node. Then, the traffic safety cost s between two connected nodes_ijSet as each security sub-node v_t(1)～v_t(n_s) Threat layer potential value U_thr(1)～U_thr(n_s) Is added to the total value of the node, and the traffic safety cost s between two unconnected nodes_ijSet to infinity. For example: FIG. 6a shows an off-road environment comprising 1 off-road environmental threat element T₁And 3 nodes v_e1～v_e3Corresponding extended security cost assessmentMatrix S_vAs shown in fig. 6 b.

"extended distance cost evaluation matrix D_v"expressed as the following equations (14) and (15) for evaluating the traffic distance cost of the section between the nodes:

D_v∈Rⁿ (14)

in the formula, D_vIs an n-th order positive real number matrix, the ith row and the jth column of which have elements d_ijSet as node v_eiAnd node v_ejEuropean distance | | | v between_ei-v_ejAnd | l, which is used for representing the traffic distance cost of the road section between two nodes.

Evaluation of matrix D by extending distance cost_vThe method for evaluating the passing distance cost of the road sections between the nodes comprises the following steps: when node v_eiAnd node v_ejA between_ij If 1, the traffic distance of the link between two nodes is | | v_ei-v_ejL; when node v_eiAnd node v_ejA between_ijAnd if the traffic distance cost of the road section between the two nodes is set to be infinite, the traffic distance cost is set to be infinite. For example: FIG. 7a shows an off-road environment containing 1 obstacle B₁And 3 nodes v_e1～v_e3The corresponding extended distance cost evaluation matrix is shown in fig. 7 b.

' expanded road cost evaluation matrix P_v"expressed as the following equations (16) and (17) are used to estimate the road cost of a section between two nodes:

P_v∈Rⁿ (16)

in the formula, P_vIs an n-th order positive real number matrix, the ith row and the jth column of which are elements p_ijTo represent a node v_eiAnd node v_ejThe road cost (potential value) of passing in between.

Evaluating matrix P by expanding road cost_vThe method for evaluating the passing road cost of the road sections between the nodes comprises the following steps: firstly, taking n on a section between connected nodes in a road layer potential energy field model_pEvenly distributed road sub-nodes v_r(1)～v_r(n_p) K is the serial number of the road sub-node, U_roa(k) The threat level potential value of the kth road sub-node. Then, the passing road cost p between the two connected nodes_ijArranged as road sub-nodes v_r(1)～v_r(n_p) Road layer potential energy value U_roa(1)～U_roa(n_p) Of the accumulated value of (a), and the passing road cost p between two unconnected nodes_ijSet to infinity. For example: FIG. 8a shows an off-road environment comprising 2 off-road P₁～P₂And 3 nodes v_e1～v_e3And the corresponding extended road cost evaluation matrix is shown in fig. 8 b.

(III) the heuristic cost matrix comprises a heuristic obstacle cost evaluation matrix B_vHeuristic distance cost evaluation matrix H_vHeuristic road cost evaluation matrix M_vSuch as, but not limited to, the weather environment cost matrix and the traffic environment cost matrix listed in the above embodiments.

Heuristic obstacle cost evaluation matrix B_v"is expressed as the following expression (18) and expression (19) for evaluating the heuristic barrier cost between the node and the target end point, and further for heuristic the expansion direction of the node:

B_v∈R^n×1 (18)

in the formula, B_vIs n × 1 positive real number matrix, the ith row and the jth column of the matrix are_ijRepresenting a node v_eiAnd target end point v_egHeuristic barrier costs (potential values) in between.

Matrix B is evaluated by heuristic obstacle cost_vThe method for evaluating barrier cost between a node and a target end point comprises the following steps: first, each node v in the barrier and threat layer potential energy field models_ei(including connected and unconnected nodes) and a target endpoint v_egTake n on the road section between_bEvenly distributed enlightening barrier sub-nodes v_b(1)～v_b(n_b) K is the number of the enlightening obstacle child node, U_obj(k) Barrier layer potential value, U, for the kth heuristic barrier child node_thr(k) The threat level potential value for the kth heuristic barrier child node. Then, the barrier cost b will be inspired_ijSet as heuristic Barrier child nodes v_b(1)～v_b(n_b) Potential energy value U of barrier layer_obs(1)～U_obs(n_b) Accumulated value and threat level potential energy value U_thr(1)～U_thr(n_b) The sum of the accumulated values of (a). It should be noted that there is no requirement for the connectivity status between nodes when heuristic cost evaluation is performed. For example: FIG. 9a shows an off-road environment containing 1 obstacle B ₁1 environmental threat T₁And 3 nodes v_e1～v_e3The corresponding barrier cost matrix is shown in fig. 9 b.

Heuristic distance cost evaluation matrix H_v"is expressed as the following expression (20) and expression (21) for evaluating the heuristic distance cost between the node and the target end point, and further, for heuristic the expansion direction of the node:

H_v∈R^n×1 (20)

h_ij＝||v_ei-v_eg|| (21)

in the formula, H_vIs n × 1 positive real number matrix, the ith row and the jth column of which have elements h_ijIs a node v_eiAnd target end point v_egEuropean distance | | | v between_ei-v_ejAnd | l, which represents the heuristic distance cost (potential energy value) of the road section between two nodes.

Matrix H is evaluated by heuristic distance cost_vThe method for evaluating the heuristic distance cost comprises the following steps: node v_eiAnd target end point v_egThe heuristic distance cost (including connected and unconnected nodes) is set to Euclidean distanceV is | away | |_ei-v_ejL. For example: FIG. 10a shows an off-road environment containing 1 obstacle B ₁1 environmental threat T₁And 3 nodes v_e1～v_e3The corresponding heuristic distance cost evaluation matrix is shown in fig. 10 b.

' heuristic road cost evaluation matrix M_v"is expressed as the following expression (22) and expression (23) and is used for evaluating the heuristic road cost between the node and the target end point and further for heuristic the direction of node expansion:

M_v∈R^n×1 (22)

in the formula, M_vIs n × 1 positive real number matrix, the ith row and the jth column of which have element m_ijRepresenting a node v_eiAnd target end point v_egHeuristic road cost (potential value).

Matrix M is assessed by enlightening road cost_vThe method for evaluating the heuristic road cost comprises the following steps: first, each node v in the road layer potential energy field model_eiAnd target end point v_egTake n on the road section between_mEvenly distributed road sub-nodes v_b(1)～v_b(n_b)，U_roa(k) Is the k-th road layer potential value. Then, enlighten the road cost m_ijFor each road sub-node v_b(1)～v_b(n_b) Road layer potential energy value U_roa(1)～U_roa(n_b) The accumulated value of (1). For example: FIG. 11a shows an off-road environment comprising 2 off-road regions P₁、P₂And 3 nodes v_e1～v_e3And the corresponding heuristic road cost evaluation matrix is shown in fig. 11 b.

The embodiment provides the off-road environment traffic cost evaluation method integrating feasibility, safety and high efficiency, a multi-dimensional node connection evaluation model in the off-road environment is established in an off-line learning mode, and the problem of vehicle traffic risk evaluation under the complex off-road environment condition is solved. The evaluation method is superior to the road section passing evaluation method between the route nodes taking the passing distance as a single evaluation index in the traditional route planning method, and is particularly suitable for route planning in the off-road environment.

S3, searching a node set V through the connection evaluation matrix starting from the current node through the multi-dimensional node connection evaluation model obtained in the step S2_eAnd evaluating the extension cost, the heuristic cost and the traffic cost of each extension node through the traffic cost matrix.

In particular, by the current node v_ecStarting from, using connectivity evaluation matrix A_vSearch node set V_eAnd current node v_ecA plurality of extension nodes v communicated with each other_eiAdd it to the extended node set Q_addIn, using the extended cost matrix S_v、D_v、P_vHeuristic cost matrix B_v、H_v、M_vAnd an extended weight matrix corresponding thereto

And heuristic weight matrix

Evaluating the spread cost q (v) of each node_ei) Heuristic cost h (v)_ei) And a passage cost F (v)_ei) The method comprises the following steps:

in order to explain in detail the intelligent vehicle path planning method based on the potential energy field probability map in the off-road environment, the simplified off-road environment shown in fig. 12 is taken as an example to explain the establishment of the communication evaluation matrix, the expansion cost matrix and the heuristic cost matrix between the nodes and the corresponding expansion cost, heuristic cost and traffic cost calculation method. Setting the 1-position obstacle B included in the off-road environment shown in FIG. 12₁1 environmental threat T₁2 off-road regions P₁、P₂An environmental potential field model is created as shown in fig. 12 according to S1. Including the starting point v_es(S), target end point v_eg(G) And 7 nodes v_e1～v_e7And its corresponding coordinate position.

Establishing a communication evaluation matrix A between nodes by using a multi-level environment state potential field model_vExpanding the cost matrix D_v、S_v、 P_vHeuristic cost matrix B_v、H_v、M_v。

S31, from the current node v_ecStarting from, extending to its periphery and corresponding to the current node v_ecConnected node, which becomes an extension node v_eiFurther, an extended node set Q is obtained_add＝{v_e1,v_e2,...,v_en}. Here, for simplicity of description, the current node v is set_ecIs an arbitrary node.

S32, using the expanded cost matrix S_v、D_v、P_vAnd equations (24) and (25), calculating the current node v_ecTo an extended node set Q_addIn each expansion node v_eiThe expansion cost of (2):

q(v_ei)＝q(v_ec)+q_ti(v_ec) (24)

in the formula, q (v)_ei) Is a starting point v_esTo an extension node v_eiThe cost of expansion of (2); q (v)_ec) Is a starting point v_esTo the current node v_ecThe cost of expansion of (2); q. q.s_ti(v_ec) For the current node v_ecTo an extension node v_eiIncremental expansion cost of (2); s_ciFor the current node v_ecAnd an extension node v_eiThe expanded security cost is obtained by calculation of formula (13); d_ciIs a node v_ecWith the new extension node v_eiThe cost of the extended distance between the two is obtained by calculation of formula (15); p is a radical of_ciIs a node v_ecWith the new extension node v_eiThe expanded road cost is calculated corresponding to the formula (17);

to expand the distance weight coefficient, f_s ^qIn order to extend the security weight factor,

to expand the road weight coefficient, the values thereof can be seen in table 7, but are not limited thereto:

TABLE 7

S33, adopting the formula (26), calculating an expansion node set Q_addIn each expansion node v_eiAnd target end point v_egHeuristic cost h (v) between_ei)：

In the formula, h (v)_ei) To extend a node v_eiAnd target end point v_egHeuristic cost of; b_i,gTo extend a node v_eiTo the target end point v_egThe heuristic barrier cost is obtained by calculation of formula (19); h is_i,gTo extend a node v_eiTo the target end point v_egThe heuristic distance cost is obtained by calculation of formula (21); m is_i,gTo extend a node v_eiTo the target end point v_egThe heuristic road cost is obtained by calculation of the formula (23); f. of_b ^hIn order to enlighten the barrier weight coefficient,

in order to enlighten the distance weight coefficient,

to enlighten the road weight coefficient, it is shown in table 7.

S34, calculating an extended node set Q by adopting the formula (27)_addIn each expansion node v_eiPassing cost F (v)_ei)：

F(v_ei)＝q(v_ei)+h(v_ei) (27)

In the formula, q (v)_ei) For the starting point v calculated according to equation (24)_esTo an extension node v_eiExtended cost of h (v)_ei) For the extended node v calculated according to equation (26)_eiAnd target end point v_egThe heuristic cost of (c).

For example: current node v_ecIs a starting point v_esThus from the initial node v_esStarting, extending to an initial node v_esThe peripheral nodes communicated with the peripheral nodes and the expansion nodes are combined into Q_add＝{v_e1,v_e2,v_e3,v_e6,v_e7}. Using an extended cost matrix S_v、D_v、P_vCalculating the current node v_ecTo an extended node set Q_add＝{v_e1,v_e2,v_e3,v_e6,v_e7The expansion cost of each node in the } is calculated. The current node is a starting point v_esQ (v) is_ec)＝q(v_es) 0; set Q_addIn each expansion node v_eiWith the current node v_esThe safety cost, the distance cost and the road cost are respectively as follows: s_s1＝50，s_s2＝30，s_s3＝0，s_s6＝0，s_s7＝0； d_s1＝380，d_s2＝440，d_s3＝890，d_s6＝490，d_s7＝940；p_s1＝0，p_s2＝0，p_s3＝60， p_s6＝0，p_s7＝60。

According to the protective performance, off-road performance and mission constraint of the vehicle

The current node v can be obtained from equation (25)_esTo each expanderExhibition node v_eiOf the incremental cost q_ti(v_ec) Respectively as follows: q. q.s_t1(v_ec)＝430， q_t2(v_ec)＝470，q_t3(v_ec)＝950，q_t6(v_ec)＝490，q_t7(v_ec) 1000. Due to q (v)_ec)＝q(v_es) 0, so obtained according to formula (25): q (v)_e1)＝430，q(v_e2)＝470，q(v_e3)＝950，q(v_e6)＝490，q(v_e7) 1000. The heuristic cost of the nodes is as follows: b_1g＝120，b_2g＝70，b_3g＝40，b_6g＝20，b_7g＝0； h_1g＝1000，h_2g＝930，h_3g＝640，h_6g＝1000，h_7g＝640；m_1g＝0，m_2g＝0，m_3g＝0， m_6g＝20，m _7g0. Obtaining h (v) according to equation (26)_ei) Namely: h (v)_e1)＝1120，h(v_e2)＝1000， h(v_e3)＝700，h(v_e6)＝1040，h(v_e7) 640. Computing the extended node set Q by equation (27)_addMiddle 5 nodes v_e1、v_e2、v_e3、v_e6、v_e7The traffic cost of (a), namely: f (v)_e1)＝1550，F(v_e2)＝1470，F(v_e3)＝1650， F(v_e6)＝1530，F(v_e7)＝1640。

The embodiment is based on a multi-dimensional node connection evaluation model, adopts an online optimization mode, selects the weight coefficient matrix according to vehicle performance and task requirements, and performs path node optimization by taking the expansion cost and the heuristic cost as evaluation indexes. The optimization method is superior to a general commonality optimization method taking a general vehicle model as a target object in the traditional path planning method, takes the protection performance, the off-road performance and the task requirement of the vehicle into consideration, introduces an individualized optimization method taking a weight coefficient matrix as a characteristic, and has better applicability.

S4, from the starting point v_esStarting from the method provided in S3, new nodes are repeatedly expanded from the current optimal node to the periphery, and more than one node is used in S2Evaluating the traffic cost of each expansion node by using a dimension traffic cost evaluation method until the traffic cost is expanded to a target terminal point v_egUntil now.

S41, establishing a path optimization node set C, a path candidate node set Q and an expansion node set Q_addWherein, the set Q_addC is an empty set, i.e.:

placing an initial node v in the set Q_esI.e. Q ═ v_es}。

S42, calculating each node v in the path candidate node set Q by adopting the formula (27)_eiAnd judging the node v with the minimum passing cost in the path candidate node set Q_eminkWhether it is the target end point v_egIf yes, go to S48; if not, the node v is connected_eminkMoving out the path candidate node set Q, namely: q → v_eminkAnd obtaining an updated path candidate node set Q.

S43, evaluating the matrix A according to the connectivity_vSearch set V_eAnd node v_eminkEach node v connected with each other_eiJudging node v_eiWhether it already exists in set C; if within set C, that is: v. of_eiE is C, then node v_eiNot added to the set Q_addIf not in set C, then:

then node v_eiJoining to an extended node set Q_addAt this time, in

Finally, the current Q is calculated by adopting the formula (27)_addIn each node v_eiPassing cost F (v)_ei)。

S44, judging the current expansion node set Q obtained in S43_addIn each expansion node v_eiWhether or not already present in the path candidate node set Q: if the node v is extended_eiNot present in the set of path candidate nodes Q, i.e.

v_ei∈Q_addThen expand node v_eiRemaining in the extended node set Q_addPerforming the following steps; if the node v is extended_eiAlready present in the set of path candidate nodes Q, i.e. v_ei∈Q,v_ei∈Q_addThen compare the nodes v in the path candidate node set Q_ei(orig) passage cost F (v)_ei(orig)) and an extended node set Q_addNode v in_ei(cur) passage cost F (v)_ei(cur))：

If F (v)_ei(cur))＜F(v_ei(orig)), node v is assigned_ei(orig) move out of path candidate node set Q, i.e.: q → v_ei(orig) and node v_ei(cur) remaining in the set of expansion nodes Q_addIn (1).

If F (v)_ei(cur))＞F(v_ei(orig)), node v is assigned_ei(cur) Shift out extended node set Q_addNamely: q_add→v_ei(cur), and node v_ei(orig) remains in the path candidate node set Q.

S45, expanding the node set Q according to the following formula (29)_addMerging with the path candidate node set Q and emptying Q_add。

S46, the node v obtained in S42_eminkAdding the k-th optimized node into the set C, and recording the father node of the node, namely: c ← v_emink。

S47, returning to S42, executing S42-S47 successively.

S48, all optimal nodes v in the set C_eminkAfter the links are connected in the order of the linked list, an optimized Path node order set Path is obtained, namely: path ← C [ v ]_e,min1,v_e,min2,...,v_e,mink]。

An off-road environment as shown in FIG. 12, with obstacle B₁Environmental threat T₁Off-road region P₁、P₂Setting initial point S as node v_esThe target end point G is a node v_eg. The environment modeling is performed according to the method of S1. Generating 7 sampling nodes v in the graph according to the method of S2_e1～v_e7Forming a set of sampling nodes V_eAnd establishing a connection matrix A between the nodes_vAnd multidimensional node connection evaluation model S_v、D_v、P_v、B_v、H_v、M_v. Starting from a starting point v_esOptimizing peripheral expansion nodes by using an evaluation matrix until the peripheral expansion nodes are expanded to a target end point v_eg。

During cycle 1, matrix A is evaluated according to the connectivity in FIG. 12_vAnd node v_emin1＝v_esThe nodes connected are v_e1、v_e2、v_e3、v_e6、v_e7. And is currently

Thus for each node

Comprises the following steps: extended node set Q_add＝{v_e1 v_e2 v_e3 v_e6 v_e7}，Q_addThe traffic cost of each extension node in the system is respectively as follows: f (v)_e1)＝1550，F(v_e2)＝1470，F(v_e3)＝1650，F(v_e6)＝1530，F(v_e7) 1640. Extended node set Q_add＝{v_e1 v_e2 v_e3v_e6 v_e7Each node v in_eiNot present in the set of path candidate nodes Q, i.e.

v_ei∈Q_addThen expand the node set Q_addIn each node v_eiRemaining in the extended node set Q_addIn (1). Set Q_addAfter merging with the set Q, Q is added_addEmptying to obtain: q ═ v_e1 v_e2 v_e3 v_e6 v_e7},

Node v_emin1＝v_esI.e. the starting point v_esThe node is added into the set C as the 1 st optimization node, and no father node exists because the node is the starting node. Namely: c ═ v_es，0}。

During cycle 2:

in S42, in this embodiment, the current path candidate node set Q ═ v_e1 v_e2 v_e3 v_e6 v_e7And F (v) according to the passing cost F (v) of each node in the path candidate node set Q_ei) Obtaining the minimum passing cost F (v)_e2) And (4) nodes.

v_emin2＝v_e2And shifting the path candidate node set Q out of the path candidate node set Q, and the updated path candidate node set Q is { v { (v) }_e1v_e3 v_e6 v_e7}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 3, node v_emin2＝v_e2Connected nodes v_es、v_e3、v_e4、v_e5、v_e7. Because of node v_esAlready in the path optimization node set C, i.e. v_esE C, thus v will be_e3、v_e4、v_e5、v_e74 nodes are added to the candidate set Q one by one_addAnd calculating the passing cost F (v) by adopting the formula (27)_ei). To distinguish from F (v) calculated in the first loop in the set Q of path candidate nodes_ei) Newly added to the set Q_addNode in F (v) traffic cost_ei(cur)), and F (v) in the path candidate node set Q_ei) With F (v)_ei(orig)), namely: q_add＝{v_e3 v_e4 v_e5 v_e7}，F(v_e3(cur))＝1645,F(v_e7(cur))＝1870, F(v_e4)＝1440,F(v_e5)＝1485。

In S44, node v is found_e3、v_e7Having been located in the set Q of path candidate nodes, the nodes v in the set Q of path candidate nodes are compared_ei(orig) and the current path candidate node set Q_addInner node v_ei(cur) of passage costs.

From S34, it can be seen that: f (v)_e3(orig))＝1650，F(v_e7(orig)) -1640. Thus, according to F (v)_e7(cur)) ＞F(v_e7(orig)), thus node v is reserved_e7Within set Q, v is_e7Removing Q_addNamely: q_add＝{v_e3 v_e4 v_e5}; according to F (v)_e3(cur))＜F(v_e3(orig)), so node v will be_e3Shift out set Q and put v_e3Remain in set Q_addIn the updating, the passing cost is F (v)_e3(cur)), namely: q ═ v_e1 v_e6 v_e7}。

In S45, set Q_addMerge with Q, i.e.: q ═ U Q-_add＝{v_e1 v_e3 v_e4 v_e5 v_e6 v_e7And clear Q_addNamely:

at S46, node v_emin2＝v_e2I.e. node v_e2Adding the node as the 2 nd optimization node into the set C, and recording the father node of the node, namely: c ═ v_es，0，v_e2,es}。

S47, returning to S42, and executing S42-S47 successively.

In cycle 3:

in S42, in this embodiment, the current path candidate node set Q ═ v_e1 v_e3 v_e4 v_e5 v_e6 v_e7According to the passing cost F (v) of each node in the path candidate node set Q_ei) Obtaining the minimum passing cost F (v)_e4) Node v_emin3＝v_e4And shifting the path candidate node set Q out of the path candidate node set Q, and the updated path candidate node set Q is { v { (v) }_e1 v_e3 v_e5 v_e6 v_e7}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 5, node v_emin3＝v_e4Connected nodes v_e1、v_e2、v_e3、v_e5、v_e6、v_eg. Because of node v_e2E C, thus v will be_e1、v_e3、v_e5、v_e6、v_eg5 nodes are successively added into a candidate set Q_addIn (1), namely: q_add＝{v_e1 v_e3 v_e5 v_e6 v_egAnd calculating the passing cost F (v) by adopting the formula (27)_ei) The method specifically comprises the following steps: f (v)_e1(cur))＝2290，F(v_e3(cur))＝2480，F(v_e5(cur))＝2490， F(v_e6(cur))＝2465，F(v_eg)＝1545。

In S44, node v is found_e1、v_e3、v_e5、v_e6Having been located in the set Q of path candidate nodes, the nodes v in the set Q of path candidate nodes are compared_ei(orig) with the current node v_ei(cur) of passage costs.

From S34 and cycle 2S 43: f (v)_e1(orig))＝1550，F(v_e3(orig))＝1645， F(v_e5(orig))＝1485，F(v_e6(orig)) 1530 and hence according to F (v)_e1(cur))＞F(v_e1(orig))， F(v_e3(cur))＞F(v_e3(orig))，F(v_e5(cur))＞F(v_e5(orig))，F(v_e6(cur))＞F(v_e6(orig)), thus retaining v_e1、v_e3、v_e5、v_e6Within the path candidate node set Q, and v_e1、v_e3、v_e5、v_e6Removing Q_addNamely: q_add＝{v_g}。

In S45, set Q_addMerge with Q, i.e.: q ═ U Q-_add＝{v_e1 v_e3 v_e5 v_e6 v_e7 v_egAnd clear Q_addNamely:

at S46, node v_emin3＝v_e4I.e. node v_e4Adding the node as the 3 rd optimization node into the set C, and recording the father node of the node, namely: c ═ v_es，0，v_e2,es，v_e4,e2}。

S47, returning to S42, and executing S42-S47 successively.

During cycle 4:

in S42, in this embodiment, the current path candidate node set Q ═ v_e1 v_e3 v_e5 v_e6 v_e7 v_egAccording to the passing cost F (v) of each node in the path candidate node set Q_ei) Obtaining the minimum passing cost F (v)_e5) Node v_emin4＝v_e5And shifting the path candidate node set Q out of the path candidate node set Q, and the updated path candidate node set Q is { v { (v) }_e1 v_e3 v_e6 v_e7 v_eg}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 6, node v_emin4＝v_e5Connected nodes v_e1、v_e2、v_e3、v_e4、v_e6、v_e7. Because of node v_e2、v_e4Already in the optimization set C, i.e. { v }_e2 v_e4E.c, so will v_e1、v_e3、v_e6、v_e74 nodes are added to the candidate set Q one by one_addIn (1), namely: q_add＝{v_e1 v_e3 v_e6 v_e7Calculating the passing cost F (v) by adopting the formula (27)_ei)，F(v_e1(cur))＝2170，F(v_e3(cur))＝1770， F(v_e6(cur))＝2165，F(v_e7(cur))＝1890。

In S44, node v is found_e1、v_e3、v_e6、v_e7Having been located in the set Q of path candidate nodes, the nodes v in the set Q of path candidate nodes are compared_ei(orig) with node v in the current node set Q_ei(cur) of passage costs.

From S34, it can be seen that: f (v)_e1(orig))＝1550，F(v_e3(orig))＝1645，F(v_e6(orig))＝1530， F(v_e7(orig)) -1640. Thus, according to F (v)_e1(cur))＞F(v_e1(orig))，F(v_e3(cur))＞F(v_e3(orig))， F(v_e6(cur))＞F(v_e6(orig))，F(v_e7(cur))＞F(v_e7(orig)), thus retaining v_e1、v_e3、v_e6、v_e7Within set Q, and v is_e1、v_e3、v_e6、v_e7Removing Q_addNamely:

in S45, set Q_addMerge with Q, i.e.: q ═ U Q-_add＝{v_e1 v_e3 v_e6 v_e7 v_egAnd clear Q_addNamely:

at S46, node v_emin4＝v_e5I.e. node v_e5Adding the node as the 4 th optimization node into the set C, and recording the father node of the node, namely: c ═ v_es，0，v_e2,es，v_e6,es，v_e5,e2}。

S47, returning to S42, and executing S42-S47 successively.

During cycle 5:

in S42, in this embodiment, the current path candidate node set Q ═ v_e1 v_e3 v_e6 v_e7 v_egAccording to the passing cost F (v) of each node in the path candidate node set Q_ei) To obtain the minimum passing costF(v_e6) Node v_emin5＝v_e6And shifting the path candidate node set Q out of the path candidate node set Q, and the updated path candidate node set Q is { v { (v) }_e1 v_e3 v_e7 v_eg}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 7, node v_emin5＝v_e6Node v for taking minimum passing cost_e6Connected nodes v_es、v_e1、v_e3、v_e4、v_e5、v_e7. Because of node v_es、v_e2、v_e4、v_e5Already in the optimization set C, i.e. { v }_es v_e2 v_e4 v_e5E.c, so will v_e1、v_e32 nodes successively join the candidate set Q_addIn (1), namely: q_add＝{v_e1 v_e3Calculating the passing cost F (v) by adopting the formula (27)_ei)，F(v_e1(cur))＝1760， F(v_e3(cur))＝2350。

In S44, node v is found_e1、v_e3Having been located in the set Q of path candidate nodes, the nodes v in the set Q of path candidate nodes are compared_ei(orig) with the current node v_ei(cur) of passage costs.

From S34, it can be seen that: f (v)_e1(orig))＝1550，F(v_e3(orig)) -1645, and thus according to F (v)_e1(cur))＞F(v_e1(orig))，F(v_e3(cur))＞F(v_e3(orig)), thus retaining v_e1、v_e3Within set Q, and v is_e1、v_e3Removing Q_addNamely:

in S45, set Q_addMerge with Q and empty Q_add. Namely: q ═ U Q-_add＝{v_e1 v_e3 v_e7 v_eg}，

At S46, node v_emin5＝v_e6I.e. node v_e6Adding the 5 th optimized node into the set C, and recording the father node of the node, namely: c ═ v_es，0 v_e2,es v_e6,es v_e1,es v_e6,es}。

S47, returning to S42, and executing S42-S47 successively.

During cycle 6:

in S42, in this embodiment, the current path candidate node set Q ═ v_e1 v_e3 v_e7 v_egAccording to the passing cost F (v) of each node in the path candidate node set Q_ei) Obtaining the minimum passing cost F (v)_eg) Node v_emin6＝v_eg. Discovering current minimum traffic cost node v_egIs a target end point, and a current target end point v_egIs node v in loop 5S 43_e4Then, the process goes to S48.

S48, taking the target end point v in the set C_egV of parent node_e4Get v_e4V of parent node_e2Get v_e2V of parent node_esNode v will be_es→v_e2→v_e4→v_egAfter the connection in order, an optimized Path node sequence set Path ═ v is obtained_es v_e2 v_e4v_egThe flow is shown in FIG. 4, and the planned optimized path node sequence is connected as shown by node v in FIG. 12_es→v_e2→v_e4→v_egThe broken lines formed by connecting in sequence are shown.

For simplicity, in the above embodiment, there are only 7 sampling nodes, and the target endpoint is found after 6 cycles. It should be noted that, for convenience of understanding, the specific implementation manner of step 4 is described by taking 7 sampling nodes as an example in the above embodiment, and the target endpoint is found by cycling 6 times. In essence, the number of cycles is determined mainly by the number of sampling nodes used, and the cycle is performed until the target endpoint is found.

S5, generating a vehicle motion track by adopting a dynamic curvature smoothing method, comprising the following steps:

s51, according to the kinematics of the vehicle, determining the minimum turning radius R when the vehicle turns at the minimum speed_minAnd the ideal turning radius R when the vehicle turns at a constant speed_i。

Specifically, in the present embodiment, the minimum turning radius R of the vehicle is taken_min16m, ideal turning radius R_i＝720m。

S52, successively taking the starting point v in the optimized Path node sequence set Path_esAnd target endpoint v_egEach intermediate node outside the ideal turning radius R is taken as the current node_iAnd planning a motion trail at the current node.

v_emink←Path (30)

Specifically, in the present embodiment, the successive node v is fetched_e2、v_e4With ideal turning radius R_iThe motion trajectory at the node is planned 720 m. As shown in fig. 12, at node v_e2、v_e4And replacing broken line broken lines with thick solid line arcs to generate a vehicle motion track.

S53, checking whether the movement track at each node planned by the S52 is in the forbidden area P of the barrier layer_obsAnd a forbidden zone P of the threat layer_thrInterference occurs, if the interference does not occur, the motion track is confirmed to be a final motion track planning result; otherwise, go to S54 for replanning.

Specifically, the first planning takes the turning radius R_iAt node v, 20m_e2、v_e4And (4) the generated track is not interfered, and the motion track is confirmed to be a final motion track planning result.

S54, adopting formula (4) for the nodes interfered in S53, continuously reducing the planned turning radius of the vehicle, and replanning the motion trail of the vehicle until the motion trail and the adjacent forbidden zone P of the barrier layer_obsAnd a forbidden zone P of the threat layer_thrUntil no interference exists.

R＝R_i-λ_R(R_i-R_min)，λ_R∈(0.1,0.5) (31)

And S55, sequentially connecting all the optimized path nodes according to the smoothed motion trail, and processing the trail at the nodes by using a dynamic curvature smoothing method to obtain the final optimized path.

Specifically, in the off-road environment embodiment shown in fig. 2, the optimized path of the vehicle, which is planned by the intelligent vehicle path planning method, is shown in fig. 13. S4 proposes a dynamic curvature smoothing method to optimize the node path. Aiming at the problems that a hard inflection point exists at a path node and a motion track generated by path planning is difficult to meet the requirement of vehicle kinematics characteristics, a dynamic curvature smoothing method is adopted to obtain a proper smooth motion track at each path optimization node according to the vehicle kinematics characteristics.

The embodiment of the invention carries out path planning based on the potential energy field probability map method, so that the intelligent vehicle can evaluate the multi-dimensional traffic cost among spatial nodes of the off-road environment by utilizing a multi-level model of the environmental state potential field in the off-road environment, and plan a feasible, safe and efficient driving path under the off-road environment condition by taking the traffic cost as an evaluation index according to the vehicle performance and task requirements.

Finally, it should be pointed out that: the above examples are only for illustrating the technical solutions of the present invention, and are not limited thereto. Those of ordinary skill in the art will understand that: modifications can be made to the technical solutions described in the foregoing embodiments, or some technical features may be equivalently replaced; such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (4)

1. An intelligent vehicle path planning method based on a potential energy field probability map in a cross-country environment is characterized by comprising the following steps:

s1, establishing a multi-level environmental state potential field model for evaluating the risk of the off-road environment by adopting an artificial potential field method according to the environmental information around the intelligent vehicle; the multi-level environment state potential field model comprises an obstacle layer potential field model, a threat layer potential field model and a road layer potential field model, and the obstacle passing modelThe layer potential energy field model can calculate the potential energy value U of the barrier layer in a certain range around the vehicle_obsThe threat layer potential energy value U within a certain range of the periphery of the vehicle can be calculated through the threat layer potential energy field model_thrAnd the road layer potential energy value U in a certain range around the vehicle can be calculated through the road layer potential energy field model_roaObtaining the potential value sigma U of the off-road environment situation field through summation so as to evaluate the risk of the off-road environment;

s2, generating nodes in the cross-country environment space through random sampling, establishing a cross-country environment space topological graph, and generating a multi-dimensional node connection evaluation model, wherein the multi-dimensional node connection evaluation model comprises a connection evaluation matrix for evaluating the connection feasibility of the road sections among the nodes and a traffic cost matrix for evaluating the traffic cost of the road sections among the nodes;

wherein, the cross-country environment space topological graph is represented as the following formula (7):

G_F＝(V_e,E_v) (7)

in the formula, G_FRepresenting a spatial topology, V, of an off-road environment_eRepresenting a set of nodes, E_vRepresenting a collection of connection segments between nodes,

wherein the sampling requirement of the node is expressed by equation (8), i.e.: node set V in cross-country environment space topological graph_eNode v in_efAnd v_ehGenerating in the cross-country environment W, and selecting the forbidden region P on the barrier layer_obsNode sampling limit region P_resForbidden region P with threat layer_thrBesides, the nodes do not overlap with each other:

optimizing the number of nodes k using equation (9)_n：

J(k_n)＝F(k_n)+λ_kT(k_n) (9)

In the formula, k_nIndicates the number of nodes, J (k)_n) For an optimization index of the number of nodes, F (k)_n) Is an average of experimentsPassing cost, T (k)_n) Planning time of the mean path of the experiment, lambda_kOptimizing the equalization coefficient for the setting;

the connectivity evaluation matrix A_vRepresented by formula (11):

in the formula, A_vRow i and column j in (1)_ijRepresenting node v when the value is 1_eiAnd node v_ejCommunicating; when the value is 0, the node v is represented_eiAnd node v_ejSection e between_i,jIs not communicated when the road section e_ijAnd region P_obs、P_thrAt the time of intersection, e_ij∈P_obs、e_ij∈P_thr(ii) a When the section e_ijAnd region P_obs、P_thrWhen there is no intersection, the two signals are,

the traffic cost matrix is divided into an extended cost matrix and a heuristic cost matrix;

s3, starting from the current node, searching the expansion nodes around the current node and communicated with the current node, and evaluating the passing cost of each expansion node; s3 specifically includes:

s31, from the current node v_ecStarting from, extending to its periphery and corresponding to the current node v_ecConnected node, which becomes an extension node v_eiFurther, an extended node set Q is obtained_add＝{v_e1,v_e2,...,v_en}；

S32, calculating the current node v by using the expanded cost matrix and the formula (24) and the formula (25)_ecTo an extended node set Q_addIn each expansion node v_eiThe expansion cost of (2):

q(v_ei)＝q(v_ec)+q_ti(v_ec) (24)

in the formula, q (v)_ei) Is a starting point v_esTo an extension node v_eiThe cost of expansion of (2); q (v)_ec) Is a starting point v_esTo the current node v_ecThe cost of expansion of (2); q. q.s_ti(v_ec) For the current node v_ecTo an extension node v_eiThe incremental cost of (2); s_ciFor the current node v_ecAnd an extension node v_eiAn extended security cost therebetween; d_ciIs a node v_ecWith the new extension node v_eiAn extended distance cost therebetween; p is a radical of_ciIs a node v_ecWith the new extension node v_eiExtended road cost therebetween;

to expand the distance weight coefficient, f_s ^qIn order to extend the security weight factor,

to expand the road weight coefficients;

s33, adopting the formula (26), calculating an expansion node set Q_addIn each expansion node v_eiAnd target end point v_egHeuristic cost h (v) between_ei)：

In the formula, h (v)_ei) To extend a node v_eiAnd target end point v_egHeuristic cost of; b_i,gTo extend a node v_eiTo the target end point v_egHeuristic barrier cost in between; h is_i,gTo extend a node v_eiTo the target end point v_egHeuristic distance cost therebetween; m is_i,gTo extend a node v_eiTo the target end point v_egHeuristic road cost in between;

in order to enlighten the barrier weight coefficient,

in order to enlighten the distance weight coefficient,

enlightening a road weight coefficient;

s34, calculating an extended node set Q by adopting the formula (27)_addIn each expansion node v_eiPassing cost F (v)_ei)：

F(v_ei)＝q(v_ei)+h(v_ei) (27)

In the formula, q (v)_ei) For the starting point v calculated according to equation (24)_esTo an extension node v_eiExtended cost of h (v)_ei) For the extended node v calculated according to equation (26)_eiAnd target end point v_egHeuristic cost of;

s4, starting from the starting point of the path, adopting the method provided in S3, continuously and repeatedly starting from the node with the minimum current traffic cost, searching the new expansion nodes around the node with the minimum current traffic cost and communicated with the node with the minimum current traffic cost, and evaluating the traffic cost of each new expansion node until the node is expanded to the target end point of the path; s4 specifically includes:

s41, establishing a path optimization node set C, a path candidate node set Q and an expansion node set Q_add，

Q＝{v_es}；

S42, calculating each node v in the set Q by adopting the formula (27)_eiAnd judging the node v with the minimum traffic cost in the set Q_{emin k}Whether it is the target end point v_egIf so, thenJumping to S48; if not, Q → v_{emin k}Obtaining an updated set Q;

s43, evaluating the matrix A according to the connectivity_vSearch set V_eAnd node v_{emin k}Each expansion node v connected with each other_eiJudging the extension node v_eiWhether already present in set C: if it is not

Then the extension node v_eiJoining to an extended node set Q_addAnd calculating the current Q using equation (27)_addIn each expansion node v_eiPassing cost F (v)_ei)；

S44, judging the current set Q obtained in S43_addIn each expansion node v_eiWhether already present in set Q: if it is

v_ei∈Q_addThen expand node v_eiRemain in set Q_addPerforming the following steps; if v is_ei∈Q,v_ei∈Q_addThen compare nodes v in set Q_ei(orig) passage cost F (v)_ei(orig)) and the group Q_addMiddle node v_ei(cur) passage cost F (v)_ei(cur))：

If F (v)_ei(cur))＜F(v_ei(orig)), then Q → v_ei(orig) and node v_ei(cur) remaining in set Q_addPerforming the following steps;

if F (v)_ei(cur))＞F(v_ei(orig)) then Q_add→v_ei(cur), and node v_ei(orig) remains in set Q;

s45, collecting Q_addMerging with the set Q and emptying the set Q_add；

S46, the node v obtained in S42_{emin k}Adding the k-th optimized node into the set C and recording a father node of the node;

s47, returning to S42, and executing S42-S47 successively;

s48, all optimal nodes v in the set C_{emin k}After the links are connected in sequence according to the linked list, an optimized Path node sequence set Path is obtained;

s5, generating a vehicle motion track by adopting a dynamic curvature smoothing method, wherein S5 specifically comprises the following steps:

s51, according to the kinematics of the vehicle, determining the minimum turning radius R when the vehicle turns at the minimum speed_minAnd the ideal turning radius R when the vehicle turns at a constant speed_i；

S52, successively taking the starting point v in the optimized Path node sequence set Path_esAnd target endpoint v_egEach intermediate node outside the ideal turning radius R is taken as the current node_iPlanning a motion track at a current node;

s53, checking whether the movement track at each node planned by the S52 is in the forbidden area P of the barrier layer_obsAnd a forbidden zone P of the threat layer_thrInterference occurs, if the interference does not occur, the motion track is confirmed to be a final motion track planning result; otherwise, jumping to S54 for replanning;

s54, adopting a formula (31) for the nodes interfered in the S53, continuously reducing the planned turning radius of the vehicle, and replanning the motion trail of the vehicle until the motion trail and the adjacent forbidden area P of the barrier layer_obsAnd a forbidden zone P of the threat layer_thrUntil there is no interference;

R＝R_i-λ_R(R_i-R_min)，λ_R∈(0.1,0.5) (31)

wherein R represents the planned turning radius of the vehicle after the movement track of the vehicle is re-planned, lambda_RParameters are not set;

and S55, sequentially connecting all the optimized path nodes according to the smoothed motion trail, and processing the trail at the nodes by using a dynamic curvature smoothing method to obtain the final optimized path.

2. The method for planning the intelligent vehicle path based on the potential energy field probability map in the off-road environment as claimed in claim 1, wherein the "multi-level environment state potential field model" in S1 is expressed as the following formula (5):

U_bat＝∑U_obs+∑U_thr-obs+∑U_roa (5)

in the formula of U_obsIs potential value of barrier layer, U_thrPotential value of threat layer, U_roaIs the potential energy value of the environmental potential field road layer, U_batIs the potential value of the environmental situation field, U_thr-obsIs a threat potential energy field after the superposition area of the threat layer and the barrier layer is fused,

is [ r ]_min,r_max]In-range threat zone potential value, A_vFor the connectivity evaluation matrix, (x, y) are point coordinates in the off-road environment,

is the maximum potential energy value of the barrier layer,

the bounding region potential values are sampled for barrier nodes,

is the potential energy minimum value of the barrier layer, r is the distance between the forbidden area of the threat layer corresponding to the threat element and the running vehicle, r_min、r_maxRespectively generating effective action distance and farthest action distance of the threat for the threat layer,

is the minimum potential energy of the threat zone,

is the maximum potential of the threat zone,

for the best structured road potential value of the road layer traffic conditions,

is the potential value of the muddy road with the worst road layer traffic condition, a_ijFor the element values, k, corresponding to the connectivity evaluation matrix_wA threat coefficient, k, of a threat element_rAs road traffic factor, P_obs、P_res、P_fre、P_thr、P_effThe method is characterized by comprising the following steps of respectively forming a barrier layer forbidden zone, a barrier layer node sampling limit zone, a barrier layer feasible zone, a threat layer forbidden zone and a threat layer restricted passing zone in the cross-country environment.

3. The method for intelligent vehicle path planning based on potential energy field probability map in off-road environment according to claim 1, wherein the traffic cost in S2 comprises an extended cost, and the matrix S is evaluated by extending a safety cost_vExtended distance cost evaluation matrix D_vAnd expanding the road cost evaluation matrix P_vOne or more ofEvaluating the expansion cost by various matrixes;

S_vexpressed as the following formula (13), is used for evaluating the traffic safety cost of the road section between two nodes:

D_vexpressed as the following equation (15), for evaluating the distance cost of the road section between the nodes:

P_vexpressed as the following equation (17), is used for evaluating the road cost of the road section between two nodes:

in the formula, S_vRow i and column j of (1)_ijFor setting child nodes v between two connected nodes_t(1)～v_t(n_s) Threat layer potential value U_thr(1)～U_thr(n_s) Accumulated value of, D_vRow i and column j in (1)_ijIs the Euclidean distance v between two connected nodes_ei-v_ej||，P_vRow i and column j of (1)_ijFor setting child nodes v between two connected nodes_r(1)～v_r(n_p) Road layer potential energy value U_roa(1)～U_roa(n_p) Accumulated value of s_ij、d_ijAnd p_ijAnd is set to infinity at two unconnected nodes.

4. The method for intelligent vehicle routing in an off-road environment based on potential energy field probability map as claimed in claim 1, wherein the traffic cost in S2 further comprises a heuristic cost, moment is evaluated by heuristic barrier costArray B_vHeuristic distance cost evaluation matrix H_vHeuristic road cost evaluation matrix M_vEvaluating the heuristic cost with one or more matrices;

B_vexpressed as (19) below, for evaluating the heuristic penalty between the node and the target end point:

H_vexpressed as (21) below, for evaluating the heuristic distance cost between the node and the target end point:

h_ij＝||v_ei-v_eg|| (21)

M_vexpressed as (23) below, for evaluating the heuristic road cost between the node and the target end point:

in the formula, B_vRow i and column j in (1)_ijFor each node v in the potential energy field model of the barrier layer and threat layer_eiAnd target end point v_egSet in between child nodes v_b(1)～v_b(n_b) Potential energy value U of barrier layer_obj(1)～U_obj(n_b) Accumulated value and threat level potential energy value U_thr(1)～U_thr(n_b) Sum of accumulated values of H_vRow i and column j in (1)_ijIs a node v_eiAnd target end point v_egEuropean distance | | | v between_ei-v_ej||，M_vRow i and column j in (1)_ijFor each node v in the road layer potential energy field model_eiAnd target end point v_egSet child node v between_b(1)～v_b(n_b) Road layer potential energy value U_roa(1)～U_roa(n_b) The accumulated value of (1).

Images