CN111815160A - Multi-dimensional node connection evaluation method - Google Patents

Abstract

The invention discloses a multi-dimensional node connection evaluation method, which comprises 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; 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; the multidimensional node connection evaluation model comprises the following steps: the communication evaluation matrix is used for evaluating the feasibility of road section communication between the nodes; the traffic cost matrix is used for evaluating the traffic cost of the road sections between the nodes; the invention can output the potential value of the environmental situation field according to the multi-dimensional cross-country environment information around the vehicle, and on the basis, a random sampling method is adopted to establish a cross-country environment space topological graph and evaluate the cross-country environment traffic risk among the nodes in the topological graph, and the risk provides favorable conditions for generating an optimized path and achieving the feasible, safe and efficient intelligent vehicle driving target.

Description

Multi-dimensional node connection evaluation method

Technical Field

The invention relates to the technical field of automatic driving, in particular to a multi-dimensional node connection evaluation method.

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 the target end point can be smoothly reached, so that 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 an intelligent vehicle path planning problem is converted into a two-point boundary value problem to be solved, a path track is generated by adopting a fixed type curve, such as a B-spline curve, a quintic polynomial curve, a three-dimensional spiral line and the like, the track generated by the geometric curve method is smooth and can meet the vehicle kinematics requirement, but has the serious defect of poor environmental condition adaptability, and the path planning requirement under the vehicle cross-country environment cannot be met; 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 a multidimensional node connection evaluation method which can output the potential value of an environment situation field according to multidimensional cross-country environment information around a vehicle, establish a cross-country environment space topological graph by adopting a random sampling method on the basis, and evaluate the cross-country environment traffic risk among nodes in the topological graph, wherein the risk provides favorable conditions for generating an optimized path and achieving a feasible, safe and efficient intelligent vehicle driving target.

In order to achieve the above object, the present invention provides a multidimensional node connection evaluation method, including:

s1, establishing a multi-level environmental situation 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;

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;

the multidimensional node connection evaluation model comprises:

connectivity evaluation matrix A_vThe system is used for evaluating the road section communication feasibility among the nodes;

the traffic cost matrix is used for evaluating the traffic cost of the road sections between the nodes;

wherein, the traffic cost matrix includes:

an extended cost evaluation matrix comprising an extended security cost evaluation matrix S_vExtended distance cost evaluation matrix D_vAnd expanding the road cost evaluation matrix P_vOne or more matrices of (a); wherein: the extended security cost evaluation matrix S_vThe evaluation matrix D is used for evaluating the traffic safety cost of a road section between two nodes_vFor evaluating the road distance cost of the road sections between the nodes, the extended road cost evaluation matrix P_vThe method is used for evaluating the passing road cost of a road section between two nodes;

a heuristic cost evaluation matrix comprising a heuristic Barrier cost evaluation matrix B_vHeuristic distance cost evaluation matrix H_vAnd initiating a road cost evaluation matrix M_v(ii) a Wherein: the heuristic obstacle cost evaluation matrix B_vFor evaluating heuristic barrier cost between a node and a target end point, the heuristic distance cost evaluation matrix H_vFor evaluating a heuristic distance cost between a node and a target end point, said heuristic road cost evaluation matrix M_vHeuristic road generation for evaluating between nodes and target end pointsAnd (4) price.

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 situation field quantization model for obstacles, threats and roads in the off-road environment, and establishes a comprehensive feasibility, safety and high efficiency traffic cost evaluation method among multiple levels of environment nodes by using the environment situation field quantization model; aiming at the problem of vehicle passing risk assessment under the complex off-road environment condition, a multi-dimensional node connection assessment model between nodes is established in an off-line learning mode, and vehicle passing risk under the complex off-road environment is assessed.

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 an off-road environment and corresponding heuristic distance cost evaluation matrices in an 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 matrix 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 may be obtained by the smart vehicle itself, and includes image information, such as a schematic diagram of the off-road environment in the image shown in fig. 2, where: 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_thrThe road layer potential energy within a certain range around the vehicle can be calculated through the road layer potential energy field modelValue U_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 over-the-field 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 given node,

is the set barrier potential energy minimum. Wherein,

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_obsRadiating outwards with 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 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 therebetween, 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

FIG. 2 shows 1 threat elementT₁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 concentric circle ring is located in a region in a shape of a Chinese character 'mi' which is a threat influence region and a passage limiting region of a threat layer, and the potential 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 feasible area of the threat layer, and the potential value is

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,

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 except 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 cross-country 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, considering mutual coupling influence among the three, and establishing a vertical (5) expressed multi-level environment state potential field model:

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_i,jThe 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 barrier 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 evaluating the risk of the 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 models provided by the equations (5) and (6) comprehensively consider the influences of environmental obstacles, off-road roads and environmental threats and consider the mutual coupling relationship among the environmental obstacles, the off-road roads and the environmental threats.

And 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, performing off-line learning on the multi-level environment state potential field model obtained through S1, generating a multi-dimensional node connection evaluation model, and evaluating the road section communication feasibility between the nodes and the traffic cost between the connected nodes by adopting a multi-dimensional node connection evaluation method through the multi-dimensional node connection evaluation model.

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 sampling limit 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) For an optimization index of the number of nodes, 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 by 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_nWhen the node proportion is 0.38 per thousand, the optimization index obtains the minimum value J (60) 1520, namely, the optimal balance point of the path 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_vMethod for evaluating communication feasibility of road sections among nodesThe method 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_vExtended 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 transit-safe 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 passing 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_e3Its corresponding extended security cost evaluation matrix 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_ij If 0, the traffic distance cost of the road section between the two nodes is setIs 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 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, meteorological environments as exemplified by the above embodimentsA cost matrix and a traffic environment cost matrix.

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_egRespectively taking n on the road section between_bEvenly distributed enlightening barrier sub-nodes v_b(1)～v_b(n_b) K is the sequence 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) Barrier potential value U of_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 including 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"expressed by the following formulae (20) and (21) and used for evaluationEstimating heuristic distance cost between the node and a target end point, and further 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 be Euclidean distance v_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 condition of a complex off-road environment 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 connected evaluation matrix starting from the current node through the multi-dimensional node connection evaluation model obtained in the step S2_eAnd evaluating the expansion cost, the heuristic cost and the traffic cost of each expansion 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_ecMultiple connected expansion nodes v_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 calculated by the 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);

in order to expand the distance-weighting factor,

in 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 road cost is obtained by calculation according to the formula (23);

in 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 cost of expanding the safety cost, the cost of expanding the distance and the cost of expanding the road 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 expansion 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 equation (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 starting point, continuously and repeatedly expanding new nodes from the current optimal node to the periphery by adopting the method provided by S3, and evaluating the traffic cost of each expanded node by using the multi-dimensional traffic cost evaluation method in S2 until the expanded nodes are expanded to the 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_{e min k}Whether it is the target end point v_egIf yes, go to S48; if not, the node v is connected_{e min k}Moving out the path candidate node set Q, namely: q → v_{e min k}And obtaining an updated path candidate node set Q.

S43, evaluating the matrix A according to the connectivity_vSearch set V_eAnd node v_{e min k}Each node v connected with each other_eiJudging node v_eiWhether or not to alreadyPresent 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 there is already a path candidate node set Q: if the node v is extended_eiNot present in the set of path candidate nodes Q, i.e.

Then node v will be expanded_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, pressing the following formula (29)Extended node set Q_addMerging with the path candidate node set Q and emptying Q_add。

S46, the node v obtained in S42_{e min k}Adding the k-th optimized node into the set C, and recording the father node of the node, namely: c ← v_{e min k}。

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

S48, all optimal nodes v in the set C_{e min k}After the links are connected in the chain table sequence, an optimized Path node sequence 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_{e min 1}＝v_esThe connected nodes 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_e1v_e2v_e3v_e6v_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_e1v_e2v_e3v_e6v_e7Each node v in_eiNot present in the set of path candidate nodes Q, i.e.

Then 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_e1v_e2v_e3v_e6v_e7},

Node v_{e min 1}＝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_e1v_e2v_e3v_e6v_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_{e min 2}＝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_e3v_e6v_e7}。

In S43, the moment is evaluated according to the communication in the embodimentArray A_vElement of line 3, node v_{e min 2}＝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_e3v_e4v_e5v_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_e3v_e4v_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_e1v_e6v_e7}。

In S45, set Q_addMerge with Q, i.e.: Q-QUQ_add＝{v_e1v_e3v_e4v_e5v_e6v_e7And clear Q_addNamely:

at S46, node v_{e min 2}＝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_e1v_e3v_e4v_e5v_e6v_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_{e min 3}＝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) }_e1v_e3v_e5v_e6v_e7}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 5, node v_{e min 3}＝v_e4Connected node 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_e1v_e3v_e5v_e6v_egAnd calculating the passage price 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-QUQ_add＝{v_e1v_e3v_e5v_e6v_e7v_egAnd clear Q_addNamely:

at S46, node v_{e min 3}＝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_e1v_e3v_e5v_e6v_e7v_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_{e min 4}＝v_e5And move it out of the waySelecting a node set Q, and updating the path candidate node set Q ═ v_e1v_e3v_e6v_e7v_eg}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 6, node v_{e min 4}＝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 }_e2v_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_e1v_e3v_e6v_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 path candidate node set Q, the nodes v in the path candidate node set Q 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 ═ Q U Q_add＝{v_e1v_e3v_e6v_e7v_egAnd clear Q_addNamely:

at S46, node v_{e min 4}＝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_e1v_e3v_e6v_e7v_egAnd (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)_e6) Node v_{e min 5}＝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) }_e1v_e3v_e7v_eg}。

In S43, the matrix A is evaluated according to the connectivity in the present embodiment_vElement of line 7, node v_{e min 5}＝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 }_esv_e2v_e4v_e5E.c, so will v_e1、v_e32 nodes successively join the candidate set Q_addIn (1), namely: q_add＝{v_e1v_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 ═ Q U Q_add＝{v_e1v_e3v_e7v_eg}，

At S46, node v_{e min 5}＝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，0v_e2,esv_e6,esv_e1,esv_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_e1v_e3v_e7v_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_{e min 6}＝v_eg. Finding 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_esv_e2v_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 concrete 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 repeated 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_{e min k}←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_obsA forbidden area 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 there is no interference.

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 subjected to the path planning 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 (6)

1. A multidimensional node connection evaluation method 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;

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;

the multidimensional node connection evaluation model comprises:

connectivity evaluation matrix A_vThe system is used for evaluating the road section communication feasibility among the nodes;

the traffic cost matrix is used for evaluating the traffic cost of the road sections between the nodes;

wherein, the traffic cost matrix includes:

an extended cost evaluation matrix comprising an extended security cost evaluation matrix S_vExtended distance cost evaluation matrix D_vAnd expanding the road cost evaluation matrix P_vOne or more matrices of (a); wherein: the extended security cost evaluation matrix S_vThe evaluation matrix D is used for evaluating the traffic safety cost of a road section between two nodes_vFor evaluating the traffic distance cost of the road sections between the nodes, the extended road cost evaluation matrix P_vThe method is used for evaluating the passing road cost of a road section between two nodes;

a heuristic cost evaluation matrix comprising a heuristic Barrier cost evaluation matrix B_vHeuristic distance cost evaluation matrix H_vHeuristic road cost evaluation matrix M_v(ii) a Wherein: the heuristic obstacle cost evaluation matrix B_vFor evaluating heuristic Barrier costs between a node and a target endpoint, said heuristic distance cost evaluation matrix H_vFor evaluating a heuristic distance cost between a node and a target end point, said heuristic road cost evaluation matrix M_vFor evaluating a heuristic road cost between a node and a target end point.

2. The multi-dimensional node connection evaluation method of claim 1, wherein the "multi-level environment potential field model" in S1 is represented by the following formula (5):

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

where (x, y) are point coordinates in an off-road environment,

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 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 set threat layer,

for a set minimum value of the potential energy of the threat zone,

for a set maximum potential of the threat zone,

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, 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 multi-dimensional node connection evaluation method according to claim 2, wherein the "off-road environment space topology map" in S2 is represented by the following formula (7):

G_F＝(V_e,E_v) (7)

set of nodes V_eThe sampling requirement of the middle node is expressed as equation (8):

optimizing a node set V by adopting a formula (9)_eNumber of intermediate nodes k_n：

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

In the formula, G_FRepresenting a spatial topology, V, of an off-road environment_eRepresenting a set of nodes, E_vRepresenting a set of connected segments between nodes, v_efAnd v_ehRepresents V_eTwo different nodes in (A), W represents a rasterized off-road environment represented by a two-dimensional matrix, J (k)_n) For an optimization index of the number of nodes, F (k)_n) For experimental average traffic cost, T (k)_n) Planning time of the mean path of the experiment, lambda_kThe equalization coefficients are optimized for the settings.

4. The multi-dimensional node connection evaluation method of claim 3, wherein the connectivity evaluation matrix A in S2_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,

5. the multi-dimensional node connection evaluation method of claim 4, wherein the extended security cost evaluation matrix S_vExpressed as the following formula (13), is used for evaluating the traffic safety cost of the road section between two nodes:

the extended distance cost evaluation matrix D_vExpressed as the following equation (15), for evaluating the distance cost of the road section between the nodes:

the extended road cost evaluation matrix 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.

6. The multi-dimensional node connection evaluation method of claim 5, wherein the heuristic obstacle cost evaluation matrix B_vExpressed as (19) below, for evaluating the heuristic penalty between the node and the target end point:

heuristic distance cost evaluation matrix 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)

heuristic road cost evaluation matrix 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_egEuclidean 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