Off-road environment priori knowledge based path planning method suitable for unmanned driving

Abstract

The application discloses a path planning method based on prior knowledge and suitable for unmanned driving in a cross-country environment, which comprises the following steps: importing a road network map file containing an elevation signal and marking a target point; establishing a prior knowledge base based on the cross-country capabilities of different vehicle types, confirming the maximum climbing degree and the minimum turning radius that can be reached by the current unmanned vehicle according to different vehicle types; calculating the spatial distance of each road section and recording the spatial distance into the road network file; calculating the shortest path from the starting point to the target point based on the Dijkstra algorithm; adjusting according to the slope, recalculating the shortest path; adjusting according to the turning radius, recalculating the shortest path; assigning values to corresponding indicators according to the shortest path length, the slope and the turning radius; and calculating the minimum cost function value to obtain the optimal path with the minimum cost from the starting point to the target point. The path obtained by the application is relatively good in road conditions and short in distance, and is suitable for unmanned driving.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a path planning method based on prior knowledge applicable to unmanned driving in off-road environments. Background Technology

[0002] Currently, mature path planning algorithms are mainly applied to structured urban road environments, with limited research on complex off-road environments. Most path planning algorithms combine path length with obstacle avoidance, focusing on rigid obstacles rather than impassable areas. They lack analysis of the overall terrain's traversability and are ill-suited for planning tasks on complex terrains. For autonomous vehicles, mileage, time, and safety must all be considered in complex terrain. Gradient, turning radius, and the number of turns not only affect driving speed but also safety, making them crucial factors. Summary of the Invention

[0003] The purpose of this invention is to provide a path planning method based on prior knowledge applicable to autonomous driving in off-road environments, so as to enable vehicles to reach the target point autonomously via the optimal path in off-road environments, thereby improving robustness and safety.

[0004] The technical solution for achieving the objective of this invention is: a path planning method based on prior knowledge applicable to autonomous driving in off-road environments, the method comprising:

[0005] S110: Import a road network map file containing elevation signals and mark target points. Estimate the slope of the path based on the elevation signals; estimate the minimum turning radius at sharp turns based on the road network. The starting point is the vehicle's current position.

[0006] S120: Select vehicle type, establish a priori knowledge base based on the off-road capabilities of different vehicle types, and confirm the maximum gradeability θ and minimum turning radius Υ that can be achieved by the current autonomous driving system based on different vehicle types.

[0007] S130: Calculate the spatial distance of each road segment and record it in the road network file.

[0008] S140: Calculate the shortest path from the starting point to the target point based on Dijkstra's algorithm, and record the shortest path length, maximum gradient, and minimum turning radius data in a table.

[0009] S150: Adjust according to the slope, recalculate the shortest path, and record the shortest path length, maximum slope, and minimum turning radius data in the table.

[0010] S160: Adjust according to the turning radius, recalculate the shortest path, and record the shortest path length, maximum gradient, and minimum turning radius data in the table.

[0011] S170: Assign values ​​to the corresponding markers according to the path length, slope, and turning radius in the table.

[0012] S180: Calculate the minimum cost function value to obtain the optimal path with the minimum cost from the starting point to the target point.

[0013] Furthermore, the maximum gradient of the road segment in step S110 can be estimated by the elevation change rate in the east-west direction and the elevation change rate in the north-south direction, i.e. Where h x (i), h y (i) represents the elevation change rate in the east-west direction and the elevation change rate in the north-south direction, respectively. x (i), h y (i) It is calculated using the elevation signal values ​​of two adjacent points.

[0014] Furthermore, the prior knowledge base in step S120 includes the vehicle model and its corresponding maximum gradeability θ and minimum turning radius Υ. When the minimum turning radius Υ > Υ i If, when an autonomous vehicle cannot pass through a road segment, the corresponding road segment i is deleted from the road network file. When the maximum grade gradient θ < θ i If an autonomous vehicle cannot pass through a road segment, the corresponding road segment i will be deleted from the road network file, resulting in a new road network file, denoted as osmfile.

[0015] Furthermore, in step S130, the distance calculation of the spatial path is calculated using the elevation difference and planar distance between points.

[0016]

[0017] In the formula, η(i,i-1) represents the spatial distance between the current point and the previous point; H(i) is the elevation of the current point, and H(i-1) is the elevation of the previous point; L(i,i-1) is the distance between planar path cells, which is the projection of the spatial path onto the plane. Using geodetic coordinates, the formula for calculating L(i,i-1) is as follows:

[0018]

[0019] Among them, (X) i ,Y i ), (X i-1 ,Y i-1 ) are the geodetic coordinates of the current point i and the previous point i-1, respectively, which are the longitude and latitude values ​​obtained from the road network file data; R = 6371393 meters, which is the average Earth radius of China.

[0020] The spatial distance between the current point i and the previous point i-1 is calculated using η(i,i-1). The spatial distances between all points on the road segment are summed to obtain the spatial distance of each road segment, and then recorded in the road network file.

[0021] Furthermore, in step S140, the shortest path from the starting point to the target point is calculated based on Dijkstra's algorithm. This involves using an array to store the shortest paths from the starting point to other points, initially storing the initial spatial distances of the road segments. The shortest path is found through n-1 iterations, where n is the number of road segments. Each time, the node with the smallest value in the remaining nodes is found and added to the array, and the array of remaining nodes is updated, thus obtaining the shortest path and recording the maximum slope θ along this path. max and its corresponding road segment number, minimum turning radius γ min The corresponding road segment number and the length of this path are used to obtain the optimal path from the starting point to the target point with the minimum cost, and this path is recorded in a table (including distance, maximum gradient, and minimum turning radius).

[0022] Furthermore, in step S150, adjustments are made according to the slope, namely:

[0023] Create a new road network file osmfile-θ, set osmfile-θ = osmfile, and set the maximum gradient θ in the road network file osmfile-θ. max The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max新 and its corresponding road segment number, minimum turning radius γ min And its corresponding road segment number and the length of this path.

[0024] The latest maximum slope θ will be obtained in the road network file osmfile-θ. max新 The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max新 and its corresponding road segment number, minimum turning radius γ min Record the corresponding road segment number and the length of the route. Repeat this process 9 times to obtain 9 sets of data records in the table.

[0025] Furthermore, in step S160, adjustments are made according to the turning radius, namely:

[0026] Create a new road network file osmfile-Y, and set osmfile-Y = osmfile. In the road network file osmfile-Y, set the minimum turning radius γ. min The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max and its corresponding road segment number, minimum turning radius γ min新And its corresponding road segment number and the length of this path.

[0027] The latest minimum turning radius γ will be obtained from the road network file osmfile-Υ. min新 The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max and its corresponding road segment number, minimum turning radius γ min新 Record the corresponding road segment number and the length of the route. Repeat this process 9 times to obtain 9 sets of data records in the table.

[0028] Furthermore, in step S170, values ​​are assigned to the corresponding markers according to the path length, slope, and turning radius in the table, i.e.:

[0029] The path marker S in the table is sorted by path length. 标 The values ​​are assigned as 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; the slope indicator θ is then assigned sequentially according to the slope magnitude. 标 The values ​​are assigned as 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; the turning radius flag γ is then adjusted sequentially according to the size of the turning radius. 标 The values ​​are assigned as 1, 2, 3, 4, 5, 6, 7, 8, 9, 10.

[0030] Furthermore, in step S180, the minimum cost function value is calculated, i.e.

[0031] Minimum cost function f(S) 标 ,θ 标 ,γ 标 )=ω1×S 标 +ω2×θ 标 +ω3×γ 标

[0032] Where ω1+ω2+ω3=1, ω2 and ω3 are related to the autonomous driving capability. The stronger the autonomous driving capability, the smaller the values ​​of ω2 and ω3. When the level of manual driving is reached, ω2=ω3=0 and ω1=1.

[0033] Compared with existing technologies, the significant advantages of this invention are as follows: Based on the elevation information in the road network map file and the prior knowledge of the off-road capability of the current vehicle model in autonomous driving, this invention derives the shortest path using the Dijkstra algorithm, and then combines the slope and turning radius to obtain the optimal path through a cost function. This path has relatively good road conditions and a shorter distance, making it suitable for autonomous driving. This invention also adds an analysis of the overall terrain traversal difficulty and safety, that is, by introducing slope and turning radius parameters into the cost function, which improves the robustness and safety of path planning for autonomous vehicles in different environments. Attached Figure Description

[0034] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other embodiments can be obtained based on these drawings.

[0035] Figure 1 This is a schematic flowchart of a path planning method provided in an embodiment of the present invention. Detailed Implementation

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

[0037] To address the problem of finding the optimal path with good road conditions and short distance in complex off-road environments for autonomous driving, this application provides a path planning method based on prior knowledge applicable to autonomous driving in off-road environments.

[0038] See Figure 1 , Figure 1 A flowchart illustrating a path planning method for autonomous driving in off-road environments based on prior knowledge, provided in this application embodiment, includes the following steps:

[0039] S110: Import a road network map file containing elevation signals and mark target points. Estimate the slope of the path based on the elevation signals; estimate the minimum turning radius at sharp turns based on the road network. The starting point is the vehicle's current position.

[0040] Specifically, Qt programming is used to drive the map through Qwebchannel and HTML communication. Road network files are loaded, parsed to obtain road segment information, and drawn on the map. Target points are marked on the map, and formulas are used to... The maximum gradient θ of the i-th road segment is calculated. i The maximum slope of the path point is h. x (i), h y (i) represents the elevation change rate in the east-west direction and the elevation change rate in the north-south direction of the current point, respectively.

[0041] S120: Select vehicle type, establish a priori knowledge base based on the off-road capabilities of different vehicle types, and confirm the maximum gradeability θ and minimum turning radius Υ that can be achieved by the current autonomous driving system based on different vehicle types.

[0042] Specifically, when the minimum turning radius Υ>Υ i If, when an autonomous vehicle cannot pass through a road segment, the corresponding road segment i is deleted from the road network file. When the maximum grade gradient θ < θ i If an autonomous vehicle cannot pass through a road segment, the corresponding road segment i will be deleted from the road network file, resulting in a new road network file, denoted as osmfile, which will then be redrawn on the map.

[0043] S130: Calculate the spatial distance of each road segment and record it in the road network file.

[0044] Specifically, according to the formula

[0045] In the formula, η(i,i-1) represents the spatial distance between the current point and the previous point; H(i) is the elevation of the current point and H(i-1) is the elevation of the previous point; L(i,i-1) is the distance between planar path units, which is the projection of the spatial path onto the plane.

[0046] The spatial distance between the current point i and the previous point i-1 is calculated using η(i,i-1). The spatial distances between all points on the road segment are summed to obtain the spatial distance of each road segment, and then recorded in the road network file.

[0047] S140: Calculate the shortest path from the starting point to the target point based on Dijkstra's algorithm.

[0048] Specifically, a new array is created to store the shortest paths from the starting point to other points, but initially it stores the initial spatial distances of the road segments from the starting point to other points. The shortest path is found through n-1 iterations, where n is the number of road segments. Each time, the node with the smallest value in the remaining nodes is found and added to the array, and the array of remaining nodes is updated, thus obtaining the shortest path and recording the maximum slope θ along this path. max and its corresponding road segment number, minimum turning radius γ min The path includes the corresponding road segment number and the path length. This yields the optimal path from the starting point to the destination with the minimum cost, which is recorded in a table. See Table 1 for the table format.

[0049] Table 1, Data Record Table

[0050]

[0051] S150: Adjust according to the slope, recalculate the shortest path, and record the data in the table.

[0052] Specifically, create a new road network file osmfile-θ, set osmfile-θ = osmfile, and in the road network file osmfile-θ, set the maximum slope θ. maxThe corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max新 and its corresponding road segment number, minimum turning radius γ min And its corresponding road segment number and the length of this path.

[0053] The latest maximum slope θ will be obtained in the road network file osmfile-θ. max新 The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max新 and its corresponding road segment number, minimum turning radius γ min Record the corresponding road segment number and the length of the route. Repeat this process 9 times to obtain 9 sets of data records in the table.

[0054] S160: Adjust according to the turning radius, recalculate the shortest path, and record the data in the table.

[0055] Specifically, create a new road network file osmfile-Y, set osmfile-Y = osmfile, and in the road network file osmfile-Y, set the minimum turning radius γ. min The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max and its corresponding road segment number, minimum turning radius γ min新 And its corresponding road segment number and the length of this path.

[0056] The latest minimum turning radius γ will be obtained from the road network file osmfile-Υ. min新 The corresponding road segment is deleted, the shortest path is recalculated, and the maximum slope θ in the new shortest path is recorded. max and its corresponding road segment number, minimum turning radius γ min新 Record the corresponding road segment number and the length of the route. Repeat this process 9 times to obtain 9 sets of data records in the table.

[0057] S170: Assign values ​​to the corresponding markers according to the path length, slope, and turning radius in the table.

[0058] Specifically, the path marker S in the table is sorted by path length. 标 The values ​​are assigned as 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; the slope indicator θ is then assigned sequentially according to the slope magnitude. 标 The values ​​are assigned as 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; the turning radius flag γ is then adjusted sequentially according to the size of the turning radius. 标 The values ​​are assigned as 1, 2, 3, 4, 5, 6, 7, 8, 9, 10.

[0059] S180: Calculate the minimum cost function value to obtain the optimal path with the minimum cost from the starting point to the target point.

[0060] Minimum cost function f(S) 标 ,θ 标 ,γ 标 )=ω1×S 标 +ω2×θ 标 +ω3×γ 标

[0061] Where ω1+ω2+ω3=1, ω2 and ω3 are related to the autonomous driving capability. The stronger the autonomous driving capability, the smaller the values ​​of ω2 and ω3. When the level of manual driving is reached, ω2=ω3=0 and ω1=1. In this embodiment, ω1=0.6, ω2=0.25, and ω3=0.15 are taken.

[0062] Due to the poor road conditions in off-road environments, high demands are placed on autonomous driving systems. This invention, based on elevation information from road network map files and prior knowledge of the off-road capabilities of the current vehicle model under autonomous driving conditions, uses the Dijkstra algorithm to derive the shortest path. Then, combining this with factors such as gradient and turning radius, a cost function is used to derive the optimal path. This path has relatively good road conditions and a shorter distance, making it suitable for autonomous driving.

[0063] The above-described embodiments are merely one implementation of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the scope of protection of the present invention. Therefore, the scope of protection of this invention should be determined by the appended claims.

Claims

1. A path planning method based on prior knowledge for autonomous driving in off-road environments, characterized in that, Including the following steps: S110: Import the road network map file containing elevation signals, mark the target points, and estimate the slope of each path based on the elevation signals; Estimate the minimum turning radius at sharp turns based on the road network map file, starting from the vehicle's current position; S120: Select vehicle type, establish a prior knowledge base based on the off-road capabilities of different vehicle types, and confirm the maximum climbing gradient and minimum turning radius that can be achieved by the current autonomous driving system based on different vehicle types; S130: Calculate the spatial distance of each road segment and record it in the road network map file; S140: Based on the starting point, target point, and road segment spatial distance in the road network map file, calculate the shortest path from the starting point to the target point using the Dijkstra algorithm, and record the shortest path length, maximum gradient, and minimum turning radius; S150: Adjust according to the slope, recalculate the shortest path, and record the shortest path length, maximum slope, and minimum turning radius; S160: Adjust according to the minimum turning radius, recalculate the shortest path, and record the shortest path length, maximum gradient, and minimum turning radius; S170: Assign values ​​to the corresponding flag quantities according to the shortest path length, maximum gradient, and minimum turning radius in steps S140, S150, and S160. S180: Calculate the minimum cost function value to obtain the optimal path with the minimum cost from the starting point to the target point.

2. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, The road network map file consists of various road segments. Each road segment has a corresponding road segment number, maximum gradient, and minimum turning radius. Each road segment is composed of path points, which contain latitude and longitude information and corresponding elevation signals.

3. The path planning method based on prior knowledge for unmanned driving in off-road environments according to claim 1, characterized in that, Step S110, which estimates the slope of the path based on the elevation signal, specifically includes: calculating the gradient of the elevation signal using the partial derivative method to estimate the slope corresponding to each path point, and the maximum slope of the i-th road segment. The maximum slope of the path point is, i.e. in , These represent the elevation change rates in the east-west and north-south directions, respectively, for the current point.

4. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, Step S110, which estimates the minimum turning radius at sharp turns based on the road network map file, specifically includes: estimating the turning radius based on the latitude and longitude coordinates of adjacent path points, and determining the minimum turning radius ϒ of the i-th road segment. i This represents the minimum turning radius of the path point.

5. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, The prior knowledge base in S120 includes the vehicle model and its corresponding maximum gradeability θ and minimum turning radius ϒ. When the minimum turning radius ϒ > ϒ... i If, when an autonomous vehicle cannot pass through a road segment, the corresponding road segment i is deleted from the road network map file; if the maximum grade gradient θ < θ i If an autonomous vehicle cannot pass through a certain road segment, the corresponding road segment i will be deleted from the road network map file, resulting in a new road network map file, denoted as osmfile.

6. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, Step S130 specifically includes Calculate the spatial distance between any two adjacent points using the elevation difference and planar distance between them: In the formula, This represents the spatial distance between the current point i and the previous point i-1; The elevation of the current point. The elevation of the point above; It is the planar path unit distance, which is the projection of the spatial path onto the plane, using geodetic coordinates. The calculation formula is as follows: in,( , ), ( , ) are the geodetic coordinates of the current point i and the previous point i-1, respectively, which are the longitude and latitude values ​​obtained from the road network file data; R is the average Earth radius of China. pass Calculate the spatial distance between the current point i and the previous point i-1, sum the spatial distances between all points on the road segment, obtain the spatial distance of each road segment, and record it in the road network map file.

7. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, The adjustment based on slope and recalculation of the shortest path specifically includes: Create a new road network file osmfile-θ, set osmfile-θ=osmfile, and set the maximum gradient in the road network file osmfile-θ. The corresponding road segment is deleted, the shortest path is recalculated, and the maximum gradient in the new shortest path is recorded. and its corresponding road section number and minimum turning radius And its corresponding road segment number, and the length of this path; The latest maximum gradient will be obtained in the road network file osmfile-θ. The corresponding road segment is deleted, the shortest path is recalculated, and the maximum gradient in the new shortest path is recorded. and its corresponding road section number and minimum turning radius The corresponding road segment number and the length of this path are recorded. This process is repeated 9 times to obtain 9 sets of data.

8. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, The adjustment based on the minimum turning radius and recalculation of the shortest path specifically includes: Create a new road network file osmfile-ϒ, and set osmfile-ϒ=osmfile. In the road network file osmfile-ϒ, set the minimum turning radius. The corresponding road segment is deleted, the shortest path is recalculated, and the maximum gradient in the new shortest path is recorded. and its corresponding road section number and minimum turning radius And its corresponding road segment number, and the length of this path; The latest maximum gradient will be obtained in the road network file osmfile-θ. The corresponding road segment is deleted, the shortest path is recalculated, and the maximum gradient in the new shortest path is recorded. and its corresponding road section number and minimum turning radius The corresponding road segment number and the length of this path are recorded. This process is repeated 9 times to obtain 9 sets of data.

9. The path planning method for unmanned driving based on prior knowledge in off-road environments according to claim 1, characterized in that, Step S170 specifically includes: assigning path markers sequentially according to path length and size. The values ​​are assigned as 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; the slope markers are then adjusted sequentially according to their magnitude. Assign values ​​of 10, 9, 8, 7, 6, 5, 4, 3, 2, 1; adjust the turning radius flags sequentially according to their size. The values ​​are assigned as 1, 2, 3, 4, 5, 6, 7, 8, 9, 10.

10. The path planning method based on prior knowledge for unmanned driving in off-road environments according to claim 1, characterized in that, The minimum cost function is: Among them, the weighting coefficient + + =1, , Related to autonomous driving capabilities, when a set level of manual driving is reached, = =0, =1.

Images