A path planning method and system based on multi-source off-road potential field

Abstract

The application discloses a path planning method and system based on a multi-source off-road potential field, relates to the field of path planning, and comprises the following steps: calculating a failure condition according to vehicle performance; determining a maximum passing slope of the vehicle and a terrain slope grid map; searching for position coordinates that exceed the maximum passing slope of the vehicle and determining the position coordinates as virtual non-crossable obstacles; calculating a terrain source potential field, a crossable obstacle source potential field, a non-crossable obstacle source potential field and a virtual non-crossable obstacle source potential field, and summing up the four potential fields to obtain a multi-source off-road potential field; randomly sampling on a non-non-crossable obstacle area on the terrain slope grid map to generate a spatial topology map; calculating a passing cost of each path segment; and sequentially selecting a path segment with the minimum passing cost as a final path. The method can realize path planning for different task requirements and driving styles, and different planning strategies can be adopted for off-road vehicles with different task urgency and performance.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a path planning method and system based on multi-source off-road potential fields. Background Technology

[0002] Potential field, as a risk assessment model, is mostly used for structured roads in existing technologies, often without considering terrain factors. Even if some methods take slope factors into account, they fail to combine vehicle characteristics for failure analysis to obtain a more accurate understanding of the impact of slope.

[0003] Most existing potential fields are uniformly distributed around the obstacle, but this method proposes an oblique elliptical potential field determined by the starting and ending points when establishing the source potential field of an insurmountable obstacle. The oblique elliptical potential field allows the vehicle to maintain a greater distance from the obstacle in the longitudinal direction and to get closer to the obstacle in the lateral direction, reducing unnecessary lateral safety margins and helping the vehicle find a shorter path when passing through narrow spaces.

[0004] In this application, multi-source potential fields can be used to achieve path planning for different task requirements and driving styles, and different planning strategies can be adopted for off-road vehicles with different task urgency and performance. Summary of the Invention

[0005] The purpose of this invention is to provide a path planning method and system based on multi-source off-road potential field, which fully considers the characteristics of the vehicle and the terrain, obstacle risks of the off-road environment, realizes path planning for different task requirements and driving styles, and can adopt different planning strategies for off-road vehicles with different task urgency and performance.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] In a first aspect, the present invention provides a path planning method based on a multi-source off-road potential field, the path planning method comprising:

[0008] Failure conditions are calculated based on vehicle performance; the failure conditions include: traction / braking force failure conditions and stability failure conditions.

[0009] The maximum traversable gradient of the vehicle is determined based on the aforementioned traction / braking force failure conditions and stability failure conditions.

[0010] Determine the terrain slope raster map;

[0011] Search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual insurmountable obstacles;

[0012] Calculate the potential field generated by topographic factors, and denote it as the topographic source potential field;

[0013] Calculate the potential field generated by the traversable obstacle, and denote it as the traversable obstacle source potential field;

[0014] Calculate the potential field generated by the insurmountable obstacle, and denote it as the insurmountable obstacle source potential field;

[0015] Calculate the potential field generated by a virtual insurmountable obstacle at a location where the terrain slope exceeds the vehicle's performance limit, and denote it as the virtual insurmountable obstacle source potential field.

[0016] The potential field of terrain source, the potential field of traversable obstacles, the potential field of insurmountable obstacles, and the potential field of virtual insurmountable obstacles are summed to obtain a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk in combination with vehicle characteristics.

[0017] Random sampling is performed on non-insurmountable obstacle areas on the terrain slope grid map to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path;

[0018] The passage cost for each path segment is calculated based on the spatial topology map and the multi-source off-road potential field.

[0019] Starting from the origin, connect the path segments with the lowest travel cost in sequence until they are connected to the destination to generate the final path.

[0020] Optionally, the path planning method further includes, after the step "connecting the path segments with the lowest travel cost sequentially from the starting point until connecting to the endpoint to generate the final path", smoothing the final path according to the vehicle's minimum turning radius and ideal turning radius.

[0021] Optionally, the traction / braking force failure conditions include: critical conditions for traction failure caused by insufficient driving force and critical conditions for traction / braking force failure caused by adhesion conditions; the stability failure conditions include: critical conditions for stability failure of uphill vehicles and critical conditions for stability failure of downhill vehicles.

[0022] Optionally, the critical condition for traction failure caused by insufficient driving force is as follows:

[0023] F tmax =mgsinθ t

[0024] Among them, F tmax The maximum driving force that can be provided to the vehicle's drive wheels, m is the mass of the vehicle, g is the acceleration due to gravity, and θ is the maximum driving force that can be provided to the vehicle's drive wheels. tThe critical slope of the terrain where the vehicle fails to traction due to insufficient driving force;

[0025] The critical conditions for traction / braking force failure caused by the aforementioned adhesion conditions are as follows:

[0026] μmg cosθ f =mg sinθ f

[0027] Where μ is the road adhesion coefficient, θ f The critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions;

[0028] The critical condition for the stability failure of the vehicle going uphill is as follows:

[0029]

[0030] Where b is the distance from the center of mass to the rear axle, L is the wheelbase, and h is the distance from the center of mass to the rear axle. g It is the height of the center of mass, θ su It is the critical slope of the terrain where the vehicle's stability fails when going uphill;

[0031] The critical conditions for vehicle stability failure on a downhill slope are as follows:

[0032]

[0033] Where a is the distance from the center of mass to the front axis, θ sd It is the critical slope of the terrain where the vehicle's stability fails downhill.

[0034] Optionally, the expression for the maximum passable gradient of the vehicle is:

[0035] θ max =min{θ t ,θ f ,θ su ,θ sd}

[0036] Where, θ max It is the maximum gradient for vehicles, θ t θ is the critical slope of the terrain where the vehicle fails to traction due to insufficient driving force. f θ is the critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions. su θ is the critical slope of the terrain where vehicle stability fails uphill. sd It is the critical slope of the terrain where the vehicle's stability fails downhill.

[0037] Optionally, the expression for the topographic source potential field is:

[0038] P t (x,y)=k t |grad(z(x,y))|

[0039] Among them, P t The topographic source potential field represents the topographic point (x, y, z); k t k is an undetermined coefficient; the more urgent the vehicle task, the higher the coefficient. t The smaller the value, the more likely the vehicle is to travel on shorter but steeper terrain; grad(z) refers to the gradient of the terrain point's height z with respect to x and y.

[0040] The expression for the potential field that can cross obstacles is as follows:

[0041]

[0042] Among them, P c This represents the potential field value of the source potential field at point (x, y) that can cross obstacles. c ,y c ) represents the coordinates of the obstacle that can be crossed, k wx R is the potential field width coefficient, which determines the degree of concentration of the potential field distribution. ec To determine the range of the potential field distribution through the obstacle, k in The potential field strength coefficient;

[0043]

[0044] Where k1 is a non-zero undetermined coefficient; γ1, γ2, and γ3 are undetermined coefficients with values ​​greater than 1, respectively measuring the influence of environmental visibility, road adhesion coefficient, and vehicle speed on the obstacle source potential field; k t Indicates the type of obstacle, k v The variable represents ambient visibility, μ represents the road adhesion coefficient, v represents vehicle speed, and k represents the vehicle speed. vr ,μ r ,v r These represent reference ambient visibility, reference road adhesion coefficient, and reference vehicle speed, respectively.

[0045] The expression for the source potential field of the insurmountable obstacle is as follows:

[0046]

[0047] Among them, P nc k represents the potential field value of the insurmountable obstacle source potential field at point (x, y). in R is the potential field intensity coefficient, β is an undetermined coefficient with a value between 0 and 1. ex R is the radius of the expanding obstacle. enc The range of the potential field distribution of the insurmountable obstacle;

[0048] The expression for the virtual insurmountable obstacle source potential field is as follows:

[0049]

[0050] Among them, P vnc (x, y) represents the potential field value p of the virtual insurmountable obstacle source potential field at point (x, y). v (x,y)=((xx vnc )cosα+(yy vnc sinα) 2 +k xy ((x vnc -x)sinα+(yy vnc cosα) 2 , (x vnc y vnc ) represents the location coordinates where the terrain slope exceeds the vehicle's performance limits, α is the potential field rotation angle, and k xy R represents undetermined coefficients no greater than 1. evnc This represents the distribution range of the potential field of a virtual, insurmountable obstacle source.

[0051] Optionally, the passage cost includes: expansion cost and heuristic cost;

[0052] The expansion cost is represented by the expansion cost evaluation matrix, including the expansion risk evaluation matrix R. v and extended distance evaluation matrix D v ;

[0053] The heuristic cost is represented by a heuristic cost evaluation matrix, including a heuristic risk evaluation matrix H. v And heuristic distance evaluation matrix A v .

[0054] Optionally, the extended risk assessment matrix is ​​calculated as follows:

[0055]

[0056] Where, r ij To expand the elements of the risk assessment matrix, x kd This represents the x-coordinate of the sampling point on road segment ij. y kd This represents the ordinate of the sampling point on road segment ij. x i The x-coordinate of node i is represented by y. i d represents the ordinate of node i. x d is the lateral distance of the defined road segment sampling interval. y n is the longitudinal distance of the defined road segment sampling interval. dk is the number of points calculated by sampling on the line segment connecting node ij. d Indicates n on segment ij d The count value when the potential field values ​​of each sampling point are accumulated;

[0057] The method for calculating the extended distance evaluation matrix is ​​as follows:

[0058]

[0059] Where, d ij To expand the elements of the distance evaluation matrix, v i Let v be the coordinates of node i. j Let c be the coordinates of node j. ij This indicates the connectivity between node i and node j;

[0060] The method for calculating the heuristic risk assessment matrix is ​​as follows:

[0061]

[0062] Among them, h jg To inspire the elements of the risk assessment matrix, x kh This represents the x-coordinate of the sampling point on the jg road segment. y kh This represents the ordinate of the sampling point on the jg road segment. x j The x-coordinate of node j is represented by y. j Represents the ordinate of node j, n h k is the number of points calculated by sampling on the line segment connecting node j to endpoint g. h Indicates n on segment jg h The count value when accumulating the potential field values ​​of each sampling point;

[0063] The heuristic distance evaluation matrix is ​​calculated as follows:

[0064] a jg =||v j -v g ||

[0065] Among them, a jg For the elements of the heuristic distance evaluation matrix, v g The coordinates of the endpoint.

[0066] Optionally, the travel cost of each path is calculated based on the spatial topology map and the multi-source off-road potential field using the following formula:

[0067] F(v i )=g(v i )+h(v i )

[0068] Wherein, F(v) i Let g(v) be the cost evaluation function from the starting point to node i. i Let h(v) be the expansion cost evaluation function from the starting point to node i. i ) is the heuristic cost evaluation function from node i to the endpoint g.

[0069] Secondly, based on the above-described method of the present invention, the present invention further provides a path planning system based on a multi-source off-road potential field, the path planning system comprising:

[0070] The failure condition calculation module is used to calculate failure conditions based on vehicle performance; the failure conditions include: traction / braking force failure conditions and stability failure conditions.

[0071] The vehicle maximum travel gradient determination module is used to determine the vehicle maximum travel gradient based on the traction / braking force failure condition and the stability failure condition;

[0072] The terrain slope raster map determination module is used to determine the terrain slope raster map;

[0073] The virtual insurmountable obstacle determination module is used to search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual insurmountable obstacles;

[0074] The topographic potential field calculation module is used to calculate the potential field generated by topographic factors, denoted as the topographic potential field.

[0075] The module for calculating the potential field of traversable obstacles is used to calculate the potential field generated by traversable obstacles, denoted as the potential field of traversable obstacles.

[0076] The Uncrossable Obstacle Source Potential Field Calculation Module is used to calculate the potential field generated by an uncrossable obstacle, denoted as the Uncrossable Obstacle Source Potential Field.

[0077] The Virtual Uncrossable Obstacle Source Potential Field Calculation Module is used to calculate the potential field generated by a virtual uncrossable obstacle at a location where the terrain slope exceeds the vehicle's limit performance, denoted as the Virtual Uncrossable Obstacle Source Potential Field.

[0078] The summation module is used to sum the terrain source potential field, the traversable obstacle source potential field, the non-traversable obstacle source potential field, and the virtual non-traversable obstacle source potential field to obtain a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk in combination with vehicle characteristics.

[0079] The spatial topology map generation module is used to randomly sample areas on the terrain slope grid map that are not insurmountable obstacles to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path;

[0080] The passage cost calculation module is used to calculate the passage cost of each path segment based on the spatial topology map and the multi-source off-road potential field.

[0081] The final path selection module is used to connect the path segments with the lowest travel cost sequentially from the starting point until the destination is reached, thus generating the final path.

[0082] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0083] This invention provides a path planning method based on multi-source off-road potential fields. The path planning method includes: calculating failure conditions based on vehicle performance; the failure conditions include traction / braking force failure conditions and stability failure conditions; determining the maximum passable gradient of the vehicle based on the traction / braking force failure conditions and stability failure conditions; determining a terrain slope grid map; searching for location coordinates in the terrain slope grid map that exceed the maximum passable gradient of the vehicle, and determining the location coordinates of the maximum passable gradient as virtual insurmountable obstacles; calculating the potential field generated by terrain factors, denoted as the terrain source potential field; calculating the potential field generated by surmountable obstacles, denoted as the surmountable obstacle source potential field; and calculating the potential field generated by insurmountable obstacles, denoted as the insurmountable obstacle source potential field. The potential field is calculated based on the terrain slope exceeding the vehicle's performance limits, resulting in a virtual insurmountable obstacle source potential field. This virtual insurmountable obstacle source potential field is then used. The terrain source potential field, traversable obstacle source potential field, insurmountable obstacle source potential field, and virtual insurmountable obstacle source potential field are summed to obtain a multi-source off-road potential field that comprehensively quantifies terrain and obstacle risks based on vehicle characteristics. Random sampling is performed on non-insurmountable obstacle areas of the terrain slope grid map to generate a spatial topology map. This spatial topology map includes multiple path segments. The passage cost of each path segment is calculated based on the spatial topology map and the multi-source off-road potential field. The path segments with the lowest passage costs are sequentially selected and connected to form the final path. Therefore, this invention fully considers vehicle characteristics and off-road terrain and obstacle risks, enabling path planning tailored to different task requirements and driving styles. Different planning strategies can be adopted for off-road vehicles with varying task urgency and performance. Attached Figure Description

[0084] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0085] Figure 1 A flowchart of a path planning method based on a multi-source off-road potential field provided by the present invention;

[0086] Figure 2 This is a schematic diagram of a path planning system based on a multi-source off-road potential field, provided by the present invention. Detailed Implementation

[0087] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0088] The purpose of this invention is to provide a path planning method and system based on multi-source off-road potential field, which fully considers the characteristics of the vehicle and the terrain, obstacle risks of the off-road environment, realizes path planning for different task requirements and driving styles, and can adopt different planning strategies for off-road vehicles with different task urgency and performance.

[0089] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0090] Example 1

[0091] Figure 1 A flowchart of a path planning method based on multi-source off-road potential field provided by the present invention is shown below. Figure 1 As shown, the method in this invention includes:

[0092] Step 101: Calculate the failure conditions based on vehicle performance; the failure conditions include: traction / braking force failure conditions and stability failure conditions.

[0093] Among them, the traction / braking force failure conditions include: the critical conditions for traction failure caused by insufficient driving force and the critical conditions for traction / braking force failure caused by adhesion conditions.

[0094] Stability failure conditions include: critical failure conditions for uphill vehicles and critical failure conditions for downhill vehicles. The specific calculation process is as follows:

[0095] Traction failure caused by insufficient driving force:

[0096] Traction failure occurs when the vehicle's driving force is insufficient to overcome the tangential component of the vehicle's weight along the slope. The critical condition for traction failure due to insufficient driving force is:

[0097] F tmax =mg sinθ t

[0098] Among them, F tmax The maximum driving force that can be provided to the vehicle's drive wheels, m is the mass of the vehicle, g is the acceleration due to gravity, and θ is the maximum driving force that can be provided to the vehicle's drive wheels. t This is the critical slope of the terrain where the vehicle fails to traction due to insufficient driving force.

[0099] Traction / braking force failure caused by adhesion conditions:

[0100] When the adhesion provided by the road surface is insufficient to overcome the tangential component of the vehicle's weight along the slope, traction failure will occur if the vehicle is going uphill, and braking failure will occur if the vehicle is going downhill. The critical conditions for traction / braking failure due to imperfect adhesion conditions are as follows:

[0101] μmg cosθ f =mg sinθ f

[0102] Where μ is the road adhesion coefficient, θ f This refers to the critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions.

[0103] Stability failure

[0104] The vehicle travels uphill:

[0105] When a vehicle is going uphill, if the normal force exerted on the ground on the front wheels of the vehicle is not greater than zero, it is considered a stability failure. The critical condition for stability failure of an uphill vehicle is:

[0106]

[0107] Where b is the distance from the center of mass to the rear axle, L is the wheelbase, and h is the distance from the center of mass to the rear axle. g It is the height of the center of mass, θ su It is the critical slope of the terrain where the vehicle's stability fails when going uphill.

[0108] The vehicle travels downhill:

[0109] When a vehicle is going downhill, if the normal force exerted on the ground on the rear wheels of the vehicle is not greater than zero, it is considered a stability failure. The critical condition for stability failure of a vehicle going downhill is:

[0110]

[0111] Where a is the distance from the center of mass to the front axis, θ sd It is the critical slope of the terrain where the vehicle's stability fails downhill.

[0112] Step 102: Determine the maximum traversable gradient of the vehicle based on the traction / braking force failure condition and the stability failure condition.

[0113] The expression for the maximum passable gradient for vehicles is:

[0114] θ max =min{θ t ,θ f ,θ su ,θ sd}

[0115] Where, θ max It is the maximum gradient for vehicles, θ t θ is the critical slope of the terrain where the vehicle fails to traction due to insufficient driving force. f This refers to the critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions.

[0116] Step 103: Determine the terrain slope raster map.

[0117] Specifically, the terrain slope is calculated based on the terrain raster map, and a terrain slope raster map of the same size is generated.

[0118] Step 104: Search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual insurmountable obstacles.

[0119] Search for slopes exceeding θ in a terrain slope raster map. max Position coordinates (x) vnc y vnc ), considering the position (x) vnc y vnc There are virtual, insurmountable obstacles at that location.

[0120] Step 105: Calculate the potential field generated by topographic factors, denoted as the topographic source potential field.

[0121] The potential field generated by topographic factors is calculated and denoted as the topographic source potential field. The design principle of the topographic source potential field is: the steeper the terrain, the larger the value of the topographic source potential field; the gentler the terrain, the smaller the value of the topographic source potential field; when the terrain slope is 0, the value of the topographic source potential field is also 0. The topographic source potential field is defined as:

[0122] P t (x,y)=k t |grad(z(x,y))|

[0123] Where Pt represents the topographic potential field at the topographic point (x, y, z); k t k is an undetermined coefficient; the more urgent the vehicle task, the higher the coefficient. t The smaller the value, the more likely the vehicle is to travel on shorter but steeper terrain; grad(z) refers to the gradient of the terrain point height z with respect to x and y.

[0124] Step 106: Calculate the potential field generated by the traversable obstacle, denoted as the traversable obstacle source potential field.

[0125] The potential field generated by a traversable obstacle is calculated and denoted as the traversable obstacle source potential field. The design principle of the traversable obstacle source potential field is: the potential field value increases closer to the obstacle, and the rate of increase decreases as the vehicle approaches the obstacle, until the rate of increase reaches zero at the maximum potential field value. This means that the maximum value of the traversable obstacle source potential field is finite, ensuring that the vehicle tends to move away from the obstacle but can still traverse it. This type of potential field is defined as:

[0126]

[0127] Among them, P c This represents the potential field value of the source potential field at point (x, y) that can cross obstacles. c ,y c ) represents the coordinates of the obstacle that can be crossed, k wx R is the potential field width coefficient, which determines the degree of concentration of the potential field distribution. ec k represents the range of the potential field distribution that can pass through the obstacle. in It is the potential field strength coefficient, which is related to the type of obstacle, environmental visibility, road adhesion coefficient, and vehicle speed.

[0128] k in The expression is as follows:

[0129]

[0130] Where k1 is a non-zero undetermined coefficient; if the task is urgent and it is desired that the vehicle tends to cross traversable obstacles to find a shorter path for quick passage, k1 can be taken as a smaller value; γ1, γ2, and γ3 are undetermined coefficients with values ​​greater than 1, respectively measuring the influence of environmental visibility, road adhesion coefficient, and vehicle speed on the potential field of the obstacle source; the larger the value, the greater the influence on the potential field; k t Indicates the type of obstacle, k v The variable represents ambient visibility, μ represents the road adhesion coefficient, v represents vehicle speed, and k represents the vehicle speed. vr ,μr ,v r These represent the reference ambient visibility, reference road adhesion coefficient, and reference vehicle speed, respectively.

[0131] Step 107: Calculate the potential field generated by the insurmountable obstacle, and denote it as the insurmountable obstacle source potential field.

[0132] The potential field generated by an insurmountable obstacle is calculated and denoted as the insurmountable obstacle source potential field. The design principle for the insurmountable obstacle source potential field is that the potential field value increases closer to the obstacle, and the rate of increase increases further with increasing proximity to the obstacle, until it reaches infinity, ensuring that vehicles will not intend to cross the obstacle. This type of potential field is defined as:

[0133]

[0134] Among them, P nc k represents the potential field value of the insurmountable obstacle source potential field at point (x, y). in β is the potential field strength coefficient, calculated using the same method as described above. β is an undetermined coefficient, with a value between 0 and 1; smaller values ​​can be selected for more aggressive driving styles and more urgent missions.

[0135] R ex The radius of the expanded obstacle is calculated as R0, which is equal to the obstacle radius and the distance R from the vehicle's center of gravity to both sides of the vehicle. la The sum is:

[0136] R ex =R0+R la

[0137] p(x,y) is defined as follows:

[0138] p(x,y)=((xx nc )cosα+(yy nc sinα) 2 +k xy ((x nc -x)sinα+(yy nc cosα) 2

[0139] Where (x) nc y nc ) represents the position coordinates of the insurmountable obstacle, and α is the potential field rotation angle, which represents the angle by which the potential field rotates counterclockwise around the geometric center of the obstacle. Its value is equal to the angle between the line connecting the planning start and end points and the y-axis in the global coordinate system. The counter-clockwise selection is considered positive. xy A coefficient not greater than 1 is used to determine the degree of stretching deformation of the potential field; it can be taken as 0.25. Renc To determine the range of the source potential field distribution that cannot be crossed by obstacles, R can be selected as twice the value of the source potential field. ex .

[0140] Step 108: Calculate the potential field generated by the virtual insurmountable obstacle at the location where the terrain slope exceeds the vehicle's limit performance, and denote it as the virtual insurmountable obstacle source potential field.

[0141] This calculation calculates the potential field of an insurmountable obstacle source generated by a virtual insurmountable obstacle at a location where the terrain slope exceeds the vehicle's performance limits. The calculation method is consistent with the method for calculating the potential field of an insurmountable obstacle source. The only difference is that R0 = 0 here, i.e., R... ex Size equal to R la .

[0142]

[0143] Where p v (x,y)=((xx vnc )cosα+(yy vnc sinα) 2 +k xy ((x vnc -x)sinα+(yy vnc cosα) 2 , (x vnc y vnc () represents the coordinates of the location where the terrain slope exceeds the vehicle's performance limits.

[0144] Step 109: Summing the terrain source potential field, the traversable obstacle source potential field, the insurmountable obstacle source potential field, and the virtual insurmountable obstacle source potential field yields a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk based on vehicle characteristics.

[0145] By summing the potential field of the terrain source, the potential field of the traversable obstacle source, the potential field of the insurmountable obstacle source, and the potential field of the virtual insurmountable obstacle source, a multi-source off-road potential field is obtained that can comprehensively quantify the terrain risk and obstacle risk in combination with vehicle characteristics.

[0146]

[0147] Where P(x,y) is the potential field value of the multi-source off-road potential field at the position (x,y), and n n n represents the number of insurmountable obstacles. v n represents the number of virtual, insurmountable obstacles. c The number of obstacles that can be crossed.

[0148] Step 110: Randomly sample areas on the terrain slope grid map that are not insurmountable obstacles to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path.

[0149] The expansion radius R la The area occupied by the insurmountable obstacle is denoted as B. Random sampling is performed on the non-insurmountable obstacle areas within the planned grid map area W to generate a spatial topology map G = (V, E), where V is the set of sampling nodes v, and E is the set of connecting road segments e between the sampling nodes. V can be represented as:

[0150]

[0151] The road segments connecting the nodes in V may pass through insurmountable obstacles. These road segments are not allowed as planned paths. Therefore, a matrix needs to be built to represent the connectivity between nodes. Let this matrix be C. v The elements c of the connected matrix ij c represents the connectivity property of nodes i and j. If the path segment connecting nodes i and j does not pass through any inaccessible obstacles, then c ij and c ji The value is 1, otherwise it is 0.

[0152] Step 111: Calculate the passage cost of each path segment based on the spatial topology map and the multi-source off-road potential field.

[0153] The access cost includes expansion cost and heuristic cost. The expansion cost is represented by an expansion cost evaluation matrix, which includes an expansion risk evaluation matrix R. v and extended distance evaluation matrix D v The heuristic cost is represented by the heuristic cost assessment matrix, which includes the heuristic risk assessment matrix H. v And heuristic distance evaluation matrix A v .

[0154] The calculation method for the extended risk assessment matrix is ​​as follows:

[0155]

[0156] Where, r ij To expand the elements of the risk assessment matrix, x i The x-coordinate of node i is represented by y. i d represents the ordinate of node i. x The lateral distance of the defined road segment sampling interval, d, measures how far apart the sampling points are to be calculated laterally when calculating each road segment. yThe defined longitudinal distance of the road segment sampling interval measures how far apart the sampling points are longitudinally when calculating each road segment. d The number of sampling points calculated on the line segment connecting node ij is determined based on the length of the line segment and the sampling interval d. x d y calculate.

[0157] The method for calculating the extended distance evaluation matrix is ​​as follows:

[0158]

[0159] Where, d ij To expand the elements of the distance evaluation matrix, v i Let v be the coordinates of node i. j Let be the coordinates of node j.

[0160] The method for calculating the heuristic risk assessment matrix is ​​as follows:

[0161]

[0162] Among them, h ij To inspire the elements of the risk assessment matrix, x kh =x j +k h *d x y kh =y j +k h *d y n h The number of sampling points calculated on the line segment connecting node j to endpoint g is determined based on the length of the line segment and the sampling interval d. x d y calculate.

[0163] The heuristic distance evaluation matrix is ​​calculated as follows:

[0164] a jg =||v j -v g ||

[0165] Among them, a jg For the elements of the heuristic distance evaluation matrix, v g The coordinates of the endpoint.

[0166] Step 112: Starting from the starting point, connect the path segments with the lowest travel cost in sequence until they are connected to the destination to generate the final path.

[0167] Specifically, path search is based on the A* algorithm. The passage cost evaluation function is defined as follows:

[0168] F(v i )=g(vi )+h(v i )

[0169] Wherein, F(v) i Let g(v) be the cost evaluation function from the starting point to node i. i Let h(v) be the expansion cost evaluation function from the starting point to node i. i ) is the heuristic cost evaluation function from node i to the endpoint g.

[0170] When using the A* algorithm to search for a path in set E, whenever the next node to be traversed needs to be selected, F(v) will always be chosen. i The node with the lowest value.

[0171] Step 113: Smooth the final path according to the vehicle's minimum turning radius and ideal turning radius.

[0172] The corners of the searched path are replaced with arcs. After the search, the resulting path is a broken line, which is not conducive to vehicle controller tracking. Therefore, the corners need to be processed. This invention uses arcs to replace corners, connecting two road segments with arcs. The radius of the arc is selected as an ideal radius R. o If the ideal radius R is used o If an alternative bend in the path leads to a collision with an obstacle, the radius should be appropriately reduced, ensuring that the reduced radius is larger than the vehicle's minimum turning radius. If a collision still cannot be avoided even when the radius is reduced to the vehicle's minimum turning radius, the path segment needs to be replanned.

[0173] Example 2

[0174] To implement the method corresponding to Embodiment 1 above and achieve the corresponding functions and technical effects, a path planning system based on a multi-source off-road potential field is provided below, specifically including:

[0175] The failure condition calculation module 201 is used to calculate failure conditions based on vehicle performance; the failure conditions include: traction / braking force failure conditions and stability failure conditions.

[0176] The vehicle maximum passing gradient determination module 202 is used to determine the vehicle maximum passing gradient based on the traction / braking force failure condition and the stability failure condition.

[0177] The terrain slope raster map determination module 203 is used to determine the terrain slope raster map.

[0178] The virtual uncrossable obstacle determination module 204 is used to search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual uncrossable obstacles.

[0179] The topographic potential field calculation module 205 is used to calculate the potential field generated by topographic factors, denoted as the topographic potential field.

[0180] The traversable obstacle source potential field calculation module 206 is used to calculate the potential field generated by the traversable obstacle, denoted as the traversable obstacle source potential field.

[0181] The insurmountable obstacle source potential field calculation module 207 is used to calculate the potential field generated by the insurmountable obstacle, denoted as the insurmountable obstacle source potential field.

[0182] The virtual insurmountable obstacle source potential field calculation module 208 is used to calculate the potential field generated by the virtual insurmountable obstacle at the location where the terrain slope exceeds the vehicle's limit performance, denoted as the virtual insurmountable obstacle source potential field.

[0183] The summation module 209 is used to sum the terrain source potential field, the traversable obstacle source potential field, the non-traversable obstacle source potential field, and the virtual non-traversable obstacle source potential field to obtain a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk in combination with vehicle characteristics.

[0184] The spatial topology map generation module 210 is used to randomly sample areas on the terrain slope grid map that are not insurmountable obstacles to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path.

[0185] The passage cost calculation module 211 is used to calculate the passage cost of each path segment based on the spatial topology map and the multi-source off-road potential field.

[0186] The final path selection module 212 is used to connect the path segments with the lowest travel cost sequentially from the starting point until the destination is reached, thus generating the final path.

[0187] The path smoothing module 213 is used to smooth the final path according to the vehicle's minimum turning radius and ideal turning radius.

[0188] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0189] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A path planning method based on multi-source off-road potential fields, characterized in that, The path planning method includes: Failure conditions are calculated based on vehicle performance; the failure conditions include: traction / braking force failure conditions and stability failure conditions; the traction / braking force failure conditions include: critical conditions for traction failure caused by insufficient driving force and critical conditions for traction / braking force failure caused by adhesion conditions; the stability failure conditions include: critical conditions for stability failure of vehicles going uphill and critical conditions for stability failure of vehicles going downhill. The critical conditions for traction failure caused by insufficient driving force are as follows: in, The maximum driving force that can be provided to the vehicle's drive wheels. m Let g be the mass of the vehicle, and g be the acceleration due to gravity. The critical slope of the terrain where the vehicle fails to traction due to insufficient driving force; The critical conditions for traction / braking force failure caused by the aforementioned adhesion conditions are as follows: in, The road adhesion coefficient, The critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions; The critical condition for the stability failure of the vehicle going uphill is as follows: in, b It is the distance from the center of gravity to the rear axle. L It's the wheelbase. h g It is the height of the center of mass. θ su It is the critical slope of the terrain where the vehicle's stability fails when going uphill; The critical conditions for vehicle stability failure on a downhill slope are as follows: in, a It is the distance from the center of gravity to the front axle. θ sd It is the critical slope of the terrain where the vehicle's stability fails downhill; The maximum traversable gradient of the vehicle is determined based on the aforementioned traction / braking force failure conditions and stability failure conditions. Determine the terrain slope raster map; Search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual insurmountable obstacles; Calculate the potential field generated by topographic factors, and denote it as the topographic source potential field; Calculate the potential field generated by the traversable obstacle, and denote it as the traversable obstacle source potential field; Calculate the potential field generated by the insurmountable obstacle, and denote it as the insurmountable obstacle source potential field; Calculate the potential field generated by a virtual insurmountable obstacle at a location where the terrain slope exceeds the vehicle's performance limit, and denote it as the virtual insurmountable obstacle source potential field. The potential field of terrain source, the potential field of traversable obstacles, the potential field of insurmountable obstacles, and the potential field of virtual insurmountable obstacles are summed to obtain a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk in combination with vehicle characteristics. Random sampling is performed on non-insurmountable obstacle areas on the terrain slope grid map to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path; The passage cost for each path segment is calculated based on the spatial topology map and the multi-source off-road potential field. Starting from the origin, connect the path segments with the lowest travel cost in sequence until they are connected to the destination to generate the final path.

2. The path planning method based on multi-source off-road potential field according to claim 1, characterized in that, The path planning method further includes, after the step "connecting the path segments with the lowest travel cost sequentially from the starting point until connecting to the endpoint to generate the final path", smoothing the final path according to the vehicle's minimum turning radius and ideal turning radius.

3. The path planning method based on multi-source off-road potential field according to claim 1, characterized in that, The expression for the maximum passable gradient of the vehicle is: in, θ max This is the maximum gradient for vehicles to pass. This refers to the critical slope of the terrain where the vehicle fails to traction due to insufficient driving force. This refers to the critical slope of the terrain where the vehicle's traction / braking force fails due to adhesion conditions. θ su It is the critical slope of the terrain where the vehicle's stability fails when going uphill. θ sd It is the critical slope of the terrain where the vehicle's stability fails downhill.

4. The path planning method based on multi-source off-road potential field according to claim 1, characterized in that, The expression for the topographic source potential field is: in, The topographic source potential field represents the topographic point (x, y, z); The coefficient is undetermined; the more urgent the vehicle's mission, the more... The smaller the value, the more willing the vehicle is to travel on shorter but steeper slopes; The gradient of the elevation z of a terrain point with respect to x and y; The expression for the potential field that can cross obstacles is as follows: in, This represents the potential field value of the source potential field at point (x, y) that can cross obstacles. Coordinates for crossing obstacles. The potential field width coefficient determines the degree of concentration of the potential field distribution. To determine the range of the potential field distribution that can pass through obstacles, The potential field strength coefficient; in, These are undetermined coefficients that are not equal to 0. These are undetermined coefficients with values ​​greater than 1, which respectively measure the influence of environmental visibility, road adhesion coefficient, and vehicle speed on the obstacle source potential field. Indicates the type of obstacle. Indicates environmental visibility. Indicates the road adhesion coefficient. Indicates vehicle speed. , , These represent reference ambient visibility, reference road adhesion coefficient, and reference vehicle speed, respectively. The expression for the source potential field of the insurmountable obstacle is as follows: in, This represents the potential field value of the source potential field at point (x, y) of the insurmountable obstacle. The potential field strength coefficient, These are undetermined coefficients, with values ​​between 0 and 1. Let the radius of the expanding obstacle be _____. The range of the potential field distribution of the insurmountable obstacle; The expression for the virtual insurmountable obstacle source potential field is as follows: in, P vnc ( x , y ) represents the potential field of a virtual, insurmountable obstacle source in ( x , y Potential field value at point ) , ( x vnc , y vnc () represents the coordinates of the location where the terrain slope exceeds the vehicle's performance limits. Let the potential field rotation angle be , The coefficients are undetermined and no greater than 1. R evnc This represents the distribution range of the potential field of a virtual, insurmountable obstacle source.

5. The path planning method based on multi-source off-road potential field according to claim 1, characterized in that, The passage cost includes: expansion cost and heuristic cost; The expansion cost is represented by an expansion cost evaluation matrix, including an expansion risk evaluation matrix. R v and extended distance evaluation matrix D v ; The heuristic cost is represented by a heuristic cost evaluation matrix, including a heuristic risk evaluation matrix. H v and heuristic distance evaluation matrix A v .

6. The path planning method based on multi-source off-road potential field according to claim 5, characterized in that, The calculation method for the extended risk assessment matrix is ​​as follows: in, To expand the elements of the risk assessment matrix, x kd express ij The x-coordinate of the sampling points on the road section , y kd express ij The ordinates of the sampling points on the road section , Represents a node i x-coordinate Represents a node i The ordinate, d x The lateral distance of the defined road segment sampling interval. d y The longitudinal distance of the defined road segment sampling interval. For the node ij The number of sampling points calculated on the connection segment. k d express ij On the road section n d The count value when the potential field values ​​of each sampling point are accumulated; The method for calculating the extended distance evaluation matrix is ​​as follows: in, To expand the elements of the distance evaluation matrix, For nodes i coordinates For nodes j coordinates c ij Represents a node i and nodes j The connectivity property; The method for calculating the heuristic risk assessment matrix is ​​as follows: in, h jg To inspire the elements of the risk assessment matrix, x kh express jg The x-coordinate of the sampling points on the road section , y kh express jg The ordinates of the sampling points on the road section , x j Represents a node j x-coordinate y j Represents a node j The ordinate, For the node j To the finish line g The number of sampling points calculated on the connection segment. k h express jg On the road section n h The count value when accumulating the potential field values ​​of each sampling point; The heuristic distance evaluation matrix is ​​calculated as follows: in, To inspire the elements of the distance evaluation matrix, v g The coordinates of the endpoint.

7. The path planning method based on multi-source off-road potential field according to claim 1, characterized in that, The travel cost of each path is calculated based on the spatial topology map and the multi-source off-road potential field using the following formula: in, From the starting point to the node i The passage cost evaluation function, From the starting point to the node i Extended cost evaluation function, For the node i To the finish line g The heuristic cost evaluation function.

8. A path planning system based on multi-source off-road potential fields, characterized in that, The path planning system includes: The failure condition calculation module is used to calculate failure conditions based on vehicle performance. The failure conditions include: traction / braking force failure conditions and stability failure conditions. The traction / braking force failure conditions include: critical conditions for traction failure caused by insufficient driving force and critical conditions for traction / braking force failure caused by adhesion conditions. The stability failure conditions include: critical conditions for stability failure of uphill vehicles and critical conditions for stability failure of downhill vehicles. The critical conditions for traction failure caused by insufficient driving force are as follows: in, The maximum driving force that can be provided to the vehicle's drive wheels. m Let g be the mass of the vehicle, and g be the acceleration due to gravity. The critical slope of the terrain where the vehicle fails to traction due to insufficient driving force; The critical conditions for traction / braking force failure caused by the aforementioned adhesion conditions are as follows: in, The road adhesion coefficient, The critical slope of the terrain where the vehicle fails to exert traction / braking force due to adhesion conditions; The critical condition for the stability failure of the vehicle going uphill is as follows: in, b It is the distance from the center of gravity to the rear axle. L It's the wheelbase. h g It is the height of the center of mass. θ su It is the critical slope of the terrain where the vehicle's stability fails when going uphill; The critical conditions for vehicle stability failure on a downhill slope are as follows: in, a It is the distance from the center of gravity to the front axle. θ sd It is the critical slope of the terrain where the vehicle's stability fails downhill; The vehicle maximum travel gradient determination module is used to determine the vehicle maximum travel gradient based on the traction / braking force failure condition and the stability failure condition; The terrain slope raster map determination module is used to determine the terrain slope raster map; The virtual insurmountable obstacle determination module is used to search for location coordinates in the terrain slope grid map that exceed the maximum passable slope of the vehicle, and determine the location coordinates of the maximum passable slope as virtual insurmountable obstacles; The topographic potential field calculation module is used to calculate the potential field generated by topographic factors, denoted as the topographic potential field. The module for calculating the potential field of traversable obstacles is used to calculate the potential field generated by traversable obstacles, denoted as the potential field of traversable obstacles. The Uncrossable Obstacle Source Potential Field Calculation Module is used to calculate the potential field generated by an uncrossable obstacle, denoted as the Uncrossable Obstacle Source Potential Field. The Virtual Uncrossable Obstacle Source Potential Field Calculation Module is used to calculate the potential field generated by a virtual uncrossable obstacle at a location where the terrain slope exceeds the vehicle's limit performance, denoted as the Virtual Uncrossable Obstacle Source Potential Field. The summation module is used to sum the terrain source potential field, the traversable obstacle source potential field, the non-traversable obstacle source potential field, and the virtual non-traversable obstacle source potential field to obtain a multi-source off-road potential field that can comprehensively quantify terrain risk and obstacle risk in combination with vehicle characteristics. The spatial topology map generation module is used to randomly sample areas on the terrain slope grid map that are not insurmountable obstacles to generate a spatial topology map; the spatial topology map includes multiple path segments, which together form a complete path; The passage cost calculation module is used to calculate the passage cost of each path segment based on the spatial topology map and the multi-source off-road potential field. The final path selection module is used to connect the path segments with the lowest travel cost sequentially from the starting point until the destination is reached, thus generating the final path.

Images