A vehicle three-dimensional path planning method considering transverse and longitudinal slopes

Abstract

This invention discloses a vehicle 3D path planning method considering lateral and longitudinal slopes. The method comprises: first, collecting real 3D environmental information to establish a 3D grid map model; second, establishing a vehicle lateral and longitudinal slope passability model; third, constructing a diagonally segmented grid map based on the 3D grid map model; and fourth, using deep reinforcement learning as a planner to perform path planning at arbitrary angles. Beneficial effects: The planning results are more efficient and safer; simultaneously, the introduction of deep reinforcement learning to solve the arbitrary angle planning problem and the provision of a novel cost function calculation method result in higher traffic efficiency.

Description

Technical Field

[0001] This invention relates to a path planning method, and more particularly to a three-dimensional path planning method for vehicles that takes into account both lateral and longitudinal slopes. Background Technology

[0002] Currently, with increasing levels of intelligence, autonomous vehicles are gradually replacing traditional vehicles. Due to their high task execution efficiency, good operational repeatability, and low risk of accidents, they are widely used in emergency rescue, resource exploration, and military missions. For autonomous vehicles, safe and reliable path planning is one of the most important requirements for stable driving.

[0003] As autonomous vehicles penetrate deeper into military, firefighting, and scientific research fields, traditional two-dimensional path planning methods, which only consider specific traffic scenarios and structured roads and lack the ability to handle complex three-dimensional terrain scenarios, are no longer sufficient to meet the needs of the mission. Therefore, it is necessary to conduct research on three-dimensional path planning methods.

[0004] The development of current 3D path planning methods mainly faces the following problems:

[0005] (1) Lack of reliable 3D environment models. In actual operation, autonomous vehicles face problems such as continuous changes in road surface height, mixed road surface types, and complex obstacle types. It is necessary to select appropriate methods to extract key information from complex environmental characteristics for environmental modeling. At the same time, while simplifying the understanding of the environmental model, the reliability of the model should also be taken into account to ensure that the obtained path planning results are effective.

[0006] (2) Lack of suitable vehicle kinematics and dynamics constraints. In two-dimensional path planning, simple autonomous vehicle models only consider changes in heading. However, in three-dimensional path planning, the limits of vehicle pitch and roll angles must be further defined, and the range of vehicle acceleration and velocity changes must also be further restricted. At the same time, current three-dimensional path planning methods are mostly used for unmanned aerial vehicles and unmanned vessels, meaning that the research objects operate outside of terrain. Therefore, current three-dimensional path planning methods are difficult to simply transfer to autonomous vehicle planning methods.

[0007] (3) Lack of accurate and effective algorithm optimization objectives. In two-dimensional path planning, the path length or the travel time is usually used as the optimization objective. However, in three-dimensional path planning, the change in ground elevation affects the vehicle's passability and travel speed. Paths of the same length may have different maximum passing speeds due to different slopes. It is not possible to simply judge whether a path is optimal based on its length.

[0008] Current 3D path planning methods typically simplify the slope of a terrain location to a fixed value, using the vehicle's maximum longitudinal gradient to determine whether it can pass through the unit. This ignores the differences in the vehicle's state when passing at different angles, leading to sometimes overly conservative planning results that miss the optimal solution, and sometimes neglecting vehicle rollover that results in unworkable outcomes. Furthermore, the lack of a terrain-considered travel time calculation model also makes the planning results less than optimal for practical execution. Therefore, it is necessary to develop a new path planning method specifically for 3D terrain. Summary of the Invention

[0009] The purpose of this invention is to address the problems of current autonomous vehicle 3D path planning methods lacking reliable environmental models, suitable vehicle kinematics and dynamics constraints, and accurate and effective algorithm optimization objectives, and to provide a vehicle 3D path planning method that considers lateral and longitudinal slopes.

[0010] The present invention provides a vehicle three-dimensional path planning method that considers both lateral and longitudinal slopes, the method comprising the following steps:

[0011] The first step is to collect real 3D environmental information to build a 3D grid map model, including the starting point location information, ending point location information, obstacle location information and terrain elevation information of the autonomous vehicle in the current task, and to build a 3D grid map model using the grid method.

[0012] The second step is to establish a vehicle lateral and longitudinal slope passability model, and to establish a reference speed model for the vehicle under different pitch and roll angle conditions based on the vehicle parameters.

[0013] The third step is to construct a diagonally segmented raster map based on the 3D raster map model and calculate the elevation value of the raster center point; and to establish a travel time cost calculator to calculate the travel time cost from one raster node to another.

[0014] The fourth step is to use deep reinforcement learning as a planner to perform arbitrary angle path planning. Arbitrary angle planning means that in a grid map, a vehicle can move from one grid node to any grid node. In other words, the movement route can follow the diagonal or side of a rectangle of any size formed by grids. Each path segment can go from a certain grid node to another arbitrary grid node. The reward for each path segment is calculated by the aforementioned time cost calculator.

[0015] The three-dimensional raster map in the first step is mainly modeled based on DEM elevation data, specifically a two-dimensional raster map with elevation information. The three-dimensional map model building process includes: building a two-dimensional map model based on real environmental information; dividing the environmental model using the raster method; marking the grid positions based on Cartesian coordinates; sampling DEM elevation data based on grid nodes and adding it to the environmental model; determining and marking the grids corresponding to the starting and ending position coordinates in the environmental model; similarly, marking the corresponding grids in the environmental model based on obstacle position information.

[0016] The second step, the vehicle's lateral and longitudinal slope passability model, specifically refers to a model that determines whether a vehicle can pass stably based on vehicle parameters and the lateral and longitudinal slope conditions it is in. Specifically, for a vehicle of known dimensions, it determines whether it will roll over based on its current pitch and roll angles. The specific judgment process uses the supporting polygon method, with the contact points of the vehicle's four wheels P1, P2, P3, and P4 as vertices to form the supporting quadrilateral S of the vehicle. The vehicle's center of gravity is denoted as P... G Assuming the vehicle's speed and acceleration change steadily, then if P G Projecting along the direction of gravity onto the plane containing the supporting quadrilateral, if the projection point falls inside S, it is assumed that the vehicle will not overturn; if the projection point falls outside S, it is assumed that the vehicle will overturn.

[0017] In actual operating conditions, the vehicle's center of gravity is not located at the vehicle's geometric center. Therefore, the projection point of the center of gravity will not fall on the geometric center of the vehicle's supporting quadrilateral. Thus, a vehicle coordinate system is established with the vehicle's geometric center as the coordinate system, the vehicle's forward direction as the X-axis, the vehicle's lateral direction as the Y-axis, and the vehicle's vertical upward direction as the Z-axis. The projection lengths of the vehicle's supporting quadrilateral S in the X and Y directions are L and W, respectively, and the coordinates of the vehicle's center of gravity are P. G (x G y G , z G The distance from the vehicle's center of gravity to each side of the vehicle's supporting quadrilateral:

[0018]

[0019]

[0020] Among them, D x1 D x2 D is the distance along the X-direction from the center of gravity to the front and rear edges of the supporting quadrilateral. y1 D y2 Let be the distance along the Y-direction from the center of gravity to the left and right edges of the supporting quadrilateral. Then, the condition for the vehicle to fall within the supporting quadrilateral can be determined by the geometric relationship between the vehicle's center of gravity and the supporting quadrilateral. That is, the range of vehicle pitch angle changes that ensures the vehicle does not roll over is:

[0021]

[0022] Where, θ max θ min These represent the upper and lower limits of the vehicle's pitch angle. And, at a certain pitch angle θ, the range of vehicle roll angle variation that ensures the vehicle does not roll over is:

[0023]

[0024] Where, φ max φ min These represent the upper and lower limits of the vehicle's roll angle, respectively. x (θ) is defined as follows:

[0025]

[0026] In summary, when the pitch angle θ and roll angle φ of an autonomous vehicle satisfy: θ min <θ <θ max φ min <φ<φ max If the projection of the vehicle's center of gravity along the direction of gravity falls within the vehicle's supporting quadrilateral, the vehicle is considered to be able to pass stably under this condition; otherwise, the vehicle is considered to be unable to pass.

[0027] When a vehicle is on a slope that allows for stable passage, a reference speed model is established for the vehicle under different pitch and roll angles. When a vehicle travels on a slope, it requires adhesion to provide a component that balances the gravitational component causing the vehicle to slide down the slope. This limits the vehicle's speed to the slope itself. The standard speed that prevents the vehicle from becoming unstable is defined as the reference speed v. r Furthermore, the reference speed of a vehicle also differs under different pitch and roll angles, as defined below:

[0028] v r (θ, φ) = v n cos ξ θ·cos ζ φ (6)

[0029] Among them, v n ξ represents the vehicle reference speed in a slope-free scenario, ξ represents the influence coefficient of the vehicle pitch angle, and ζ represents the influence coefficient of the vehicle roll angle.

[0030] The third step, diagonal raster map segmentation, refers to further diagonally segmenting the raster map built based on DEM data. Specifically, this involves connecting the diagonals of each raster to divide each raster into four small triangular units. This is done because a raster with elevation information is enclosed by four points in space. Since these four points are not on the same plane, it is impossible to estimate the slope of vehicles passing through the raster by constructing a plane. Diagonal segmentation divides the plane into four small triangular units, each on the same plane, allowing the slope of the plane to be obtained for effectively estimating the state of vehicles passing through the terrain. The elevation value of the raster center point is then calculated using the following formula:

[0031]

[0032] Where, k c k1, k2, k3, and k4 represent the elevation values ​​of the center point of the grid, and k1, k2, k3, and k4 represent the elevation values ​​of the four vertices of the grid.

[0033] In the third step, the travel time cost calculator is used to calculate the travel time cost between two grid nodes, which is then used as a heuristic function for subsequent planning. Arbitrary angle planning is used to obtain the optimal solution in the global space. Arbitrary angle planning means that in the grid map, the vehicle's movement route can follow the line connecting one grid node to any other grid node, not just adjacent grid nodes. That is, the movement route can follow the diagonal or side of a rectangle of any size, instead of only following the side of the smallest grid. The reason for doing this is to enrich the solution space of the path planning result to improve the optimality of the solution. The line connecting two grid nodes is called a path segment, and the planned path will consist of several path segments.

[0034] The travel time cost calculator calculates the travel time cost of a path segment connecting two given grid nodes. The path segment traverses multiple grids, and the slope within each grid affects the travel time of vehicles. The impact of terrain undulation cannot be simply reduced to the elevation difference between the starting and ending points. To further improve the model accuracy, the path segment is divided into several smaller segments using a diagonally split grid map. These smaller segments are called path nodes. The travel time cost of a path segment is equal to the sum of the travel time costs of all path nodes that make up the path segment.

[0035] The process for calculating the travel time cost of each path segment is as follows:

[0036] Step 1: Calculate the node coordinates of the path segment. Since the path segment is formed by dividing the map diagonally, the nodes of the path segment must exist on the grid edge or diagonal, but their specific coordinates may be unknown. They need to be obtained geometrically for subsequent calculations. First, calculate the x and y coordinates of the path segment nodes. Nodes on the grid edge can be solved using the x and y coordinates of the start and end points of the path segment through the graphical similarity method. Nodes on the grid diagonal can be solved using the x and y coordinates of nodes on adjacent grid edges and the graphical similarity method. After knowing the x and y coordinates of each node, the elevation value of the node can be obtained from the elevation values ​​of the two endpoints of the edge or diagonal segment it is located on. The calculation formula is as follows:

[0037]

[0038] Where p a =(i a j a k a ) is the node to be found, p m =(i m j m k m ) and p n =(i n j n k n These are the two endpoints of the edge or diagonal segment to which it lies. From this, the coordinates of all nodes in the path segment can be calculated.

[0039] Step 2: Calculate the pitch and roll angles of the vehicle as it travels on each path segment; calculate the pitch and roll angles of the vehicle separately according to the location of each path segment.

[0040] When a path segment is located inside a triangular cell in a diagonally segmented map, the spatial angle between the triangular cell and the horizontal plane is considered the slope of the terrain in that area. The slope characteristic is represented by the upward normal vector of the triangular cell, and the calculation formula is as follows:

[0041]

[0042]

[0043] Where, p l =(i l j l k l ) and P l+1 =(i l+1 j l+1 k l+1 ) are the two endpoints of a certain path segment, p c =(i c j c k cThe right-angle vertex of the triangular cell where the path segment is located is also the center point of the grid.

[0044] The pitch angle θ of vehicles traveling on this path segment l，l+1 The calculation formula is as follows:

[0045]

[0046] The roll angle φ of vehicles traveling on this path segment l，l+1 The calculation formula is as follows:

[0047]

[0048] When the path segment is on the edge or diagonal of the grid, the vehicle pitch angle is calculated in the same way as above, and the vehicle roll angle is simplified to the average roll angle of the vehicle traveling along the current path on the left and right slopes. The roll angle is calculated in the same way as above.

[0049] Step 3: Calculate the travel time cost of the path segment based on the pitch and roll angles of the vehicle; based on Step 2, which can calculate the pitch and roll angles of the vehicle traveling on any path segment, the reference speed of the path segment can be obtained according to the reference speed model:

[0050] v r(l，l+1) =v n cos ξ θ l，l+1 cos ζ φ l，l+1 (13)

[0051] The formula for calculating travel time cost is as follows:

[0052]

[0053] In the fourth step, deep reinforcement learning is used as a planner to plan paths at arbitrary angles. The reason for this is that in step three, when calculating the time cost from one grid node to another, different paths will pass through different terrains, resulting in different time costs. This makes the time cost from one grid node to another a function with a minimum value with the path as the independent variable, requiring the planner to have the ability to sample multiple times and iterate optimally.

[0054] The state space of deep reinforcement learning is designed to include all grid nodes, where the starting position, ending position, and obstacle positions need to be recorded. The action space is designed so that the next action can be transferred to any grid node. The reward function is designed based on the ending position, obstacle position, and terrain information. Reaching the ending point provides a large positive reward, and colliding with an obstacle provides a large negative reward. Small negative rewards are designed based on the time cost of travel. The deep reinforcement learning agent explores the space to find positive rewards and updates its policy. Through policy iteration, the cumulative reward obtained by the agent decreases, thus finding the optimal path. Projecting this onto the real environment completes the path planning process.

[0055] The beneficial effects of this invention are:

[0056] The present invention provides a three-dimensional path planning method for autonomous vehicles that considers lateral and longitudinal slopes. First, a three-dimensional grid map model based on DEM data, a vehicle lateral and longitudinal slope passability model, and a pass reference speed model are established. Then, a diagonally segmented grid model is further established, and a slope passage time calculation method is provided based on this model as the cost function for planning. Finally, deep reinforcement learning is used as the planner to perform optimal path planning at any angle.

[0057] Compared to traditional methods that simply calculate terrain slope as a single value and compare it with vehicle climbing gradient, this method establishes a more accurate model to solve the vehicle 3D path planning problem, making the planning results more effective and safer. At the same time, it introduces deep reinforcement learning to solve the planning problem at arbitrary angles and provides a novel cost function calculation method to make the results more efficient. Attached Figure Description

[0058] Figure 1 This is a schematic diagram of the overall process of the method described in this invention.

[0059] Figure 2 This is a schematic diagram comparing the arbitrary angle planning method described in this invention with traditional methods.

[0060] Figure 3 This is a schematic diagram of a diagonally segmented grid map as described in this invention.

[0061] Figure 4 This is a schematic diagram of the state transition in the arbitrary angle planning of deep reinforcement learning as described in this invention. Detailed Implementation

[0062] Please see Figures 1 to 4 As shown:

[0063] The vehicle three-dimensional path planning method considering lateral and longitudinal slopes provided by this invention specifically includes the following steps:

[0064] The first step is to collect real 3D environmental information to build a 3D grid map model, including the starting point location information, ending point location information, obstacle location information, terrain elevation information, etc. of the autonomous vehicle in the current task, and to build a 3D grid map model using the grid method.

[0065] The second step is to establish a vehicle lateral and longitudinal slope passability model, and to establish a reference speed model for the vehicle under different pitch and roll angle conditions based on the vehicle parameters.

[0066] The third step is to construct a diagonally segmented raster map based on the 3D raster map model and calculate the elevation value of the raster center point; and to establish a travel time cost calculator to calculate the travel time cost from one raster node to another.

[0067] The fourth step is to use deep reinforcement learning as a planner to plan paths at arbitrary angles. That is, each path segment can go from a certain grid node to another arbitrary grid node, and the reward for each path segment is calculated by the aforementioned time cost calculator.

[0068] The first step involves modeling the 3D raster map primarily based on DEM (Digital Elevation Model) elevation data, specifically a 2D raster map containing elevation information. The 3D map model creation process includes: establishing a 2D map model based on the actual environmental information; dividing the environmental model using a raster method; marking grid positions based on Cartesian coordinates; sampling DEM elevation data from grid nodes and adding it to the environmental model; determining and marking the grids corresponding to the start and end point coordinates within the environmental model; and similarly, marking the corresponding grids based on obstacle location information within the environmental model.

[0069] Specifically, in this example, the location of an obstacle in the simulation environment is represented by the area covered by the graphic bounded by its boundary. This example creates a two-dimensional grid map with elevation information; the grid dimensions are 100*100, specifically represented as (x, y). A matrix Grid(x, y) is created to represent the state of each grid cell, and matrix G... r The ID dimension is consistent with the grid dimension, which is 100*100. The grid state includes obstacle occupied position, free position, end position, and start position. The Grid matrix is ​​initialized according to each grid state. For example, if position (x1, y1) is free, then Grid(x1, y1) = 0. The specific grid state assignment rules are as follows:

[0070] Table 1. Rules for Assigning Values ​​to the Grid Matrix

[0071] Grid status Obstacles occupying space Free space Finish line Starting position Grid(x, y) -1 0 1 2

[0072] Create a matrix H(x, y) representing the elevation values ​​of each grid cell, with the same dimensions as the grid cells, which is 100*100.

[0073] The second step, the vehicle's lateral and longitudinal slope passability model, specifically refers to a model that determines whether a vehicle can pass stably based on vehicle parameters and the slope conditions it is on. Specifically, for a vehicle of known size, it determines whether it will roll over based on its current pitch and roll angles. The specific judgment process uses the supporting polygon method, with the contact points of the vehicle's four wheels P1, P2, P3, and P4 as vertices to form the supporting quadrilateral S of the vehicle. The vehicle's center of gravity is denoted as P. G Assuming the vehicle's speed and acceleration change steadily, then if P G Projecting along the direction of gravity onto the plane containing the supporting quadrilateral, if the projection point falls within S, it is assumed that the vehicle will not overturn; if the projection point falls outside S, it is assumed that the vehicle will overturn.

[0074] In actual operating conditions, the vehicle's center of gravity is not located at the vehicle's geometric center. Therefore, the projection point of the center of gravity will not fall on the geometric center of the vehicle's supporting quadrilateral. Thus, a vehicle coordinate system is established with the vehicle's geometric center as the coordinate system, the vehicle's forward direction as the X-axis, the vehicle's lateral direction as the Y-axis, and the vehicle's vertical upward direction as the Z-axis. The projection lengths of the vehicle's supporting quadrilateral S in the X and Y directions are L and W, respectively, and the coordinates of the vehicle's center of gravity are P. G (x G y G , z G The distance from the vehicle's center of gravity to each side of the vehicle's supporting quadrilateral:

[0075]

[0076]

[0077] Among them, D x1 D x2 D is the distance along the X-direction from the center of gravity to the front and rear edges of the supporting quadrilateral. y1 D y2 Let be the distance along the Y-direction from the center of gravity to the left and right edges of the supporting quadrilateral. Then, the condition for the vehicle to fall within the supporting quadrilateral can be determined by the geometric relationship between the vehicle's center of gravity and the supporting quadrilateral. That is, the range of vehicle pitch angle changes that ensures the vehicle does not roll over is:

[0078]

[0079] Where, θ max θ min These represent the upper and lower limits of the vehicle's pitch angle. And, at a certain pitch angle θ, the range of vehicle roll angle variation that ensures the vehicle does not roll over is:

[0080]

[0081] Where, φ max φ min These represent the upper and lower limits of the vehicle's roll angle, respectively. x (θ) is defined as follows:

[0082]

[0083] In summary, when the pitch angle θ and roll angle φ of an autonomous vehicle satisfy: θ min <θ <θ max φ min <φ<φ max If the projection of the vehicle's center of gravity along the direction of gravity falls within the vehicle's supporting quadrilateral, the vehicle is considered to be able to pass stably under this condition; otherwise, the vehicle is considered to be unable to pass.

[0084] When the vehicle is on a slope where it can travel stably, a reference speed model is established for the vehicle under different pitch and roll angles. The standard speed at which the vehicle does not lose stability is defined as the reference speed v. r Furthermore, the reference speed of a vehicle also differs under different pitch and roll angles, as defined below:

[0085] v r (θ, φ) = v n cos ξ θ·cos ζ φ (6)

[0086] Among them, v n ξ represents the vehicle reference speed in a slope-free scenario, ξ represents the influence coefficient of the vehicle pitch angle, and ζ represents the influence coefficient of the vehicle roll angle.

[0087] The third step, diagonal raster map segmentation, refers to further diagonally segmenting the raster map built based on DEM data. Specifically, this involves connecting the diagonals of each raster to divide each raster into four smaller triangular units. The elevation value of the raster center point is then calculated using the following formula:

[0088]

[0089] Where, k c k1, k2, k3, and k4 represent the elevation values ​​of the center point of the grid, while k1, k2, k3, and k4 represent the elevation values ​​of the four vertices of the grid.

[0090] In the third step, a travel time cost calculator is established to calculate the travel time cost between two grid nodes, which is used as a heuristic function for subsequent planning. The travel time cost calculation process for each path node is as follows:

[0091] Step 1: Calculate the node coordinates of the path segment. First, determine the x and y coordinates of the path segment nodes. Nodes on the grid edges can be determined using the x and y coordinates of the start and end points of the path segment via a similarity method. Nodes on the grid diagonal can be determined using the x and y coordinates of nodes on adjacent grid edges and a similarity method. Figure 3 As shown in the diagram, the red line represents a path to be calculated. The node of the bottom left path node is located at the intersection of the path and the diagonal. A perpendicular line can be drawn from this node to the grid edge, resulting in the triangle similarity formula:

[0092]

[0093] Then the x and y coordinates of the node can be determined. Once the x and y coordinates of each node are known, the elevation value of the node can be obtained from the elevation values ​​of the two endpoints of the side or diagonal segment it belongs to, using the following formula:

[0094]

[0095] Where, p a =(i a j a k a ) is the node to be found, p m =(i m j m k m ) and p n =(i n j n k n ( ) represent the two endpoints of the edge or diagonal segment to which it is located. From this, the coordinates of all nodes in the path segment can be calculated.

[0096] Step 2: Calculate the pitch and roll angles of the vehicle as it travels on each path segment; calculate the pitch and roll angles of the vehicle according to the different locations of the path segments.

[0097] When a path segment is located inside a triangular cell in a diagonally segmented map, the spatial angle between the triangular cell and the horizontal plane is considered the slope of the terrain in that area. The slope characteristic is represented by the upward normal vector of the triangular cell, and the calculation formula is as follows:

[0098]

[0099]

[0100] Where, p l =(i l j l k l ) and p l+1 =(i l+1 j l+1 kl+1 ) are the two endpoints of a certain path segment, p c =(i c j c k c The right-angle vertex of the triangular cell where the path segment is located is also the center point of the grid.

[0101] The pitch angle θ of vehicles traveling on this path segment l，l+1 The calculation formula is as follows:

[0102]

[0103] The roll angle φ of vehicles traveling on this path segment l，l+1 The calculation formula is as follows:

[0104]

[0105] When the path segment is on the edge or diagonal of the grid, the vehicle pitch angle is calculated in the same way as above, and the vehicle roll angle is simplified to the average roll angle of the vehicle traveling along the current path on the left and right slopes. The roll angle is calculated in the same way as above.

[0106] Step 3: Calculate the travel time cost of the path segment based on the pitch and roll angles of the vehicle; based on Step 2, the pitch and roll angles of the vehicle traveling on any path segment can be calculated, and the reference speed of the path segment can be obtained according to the reference speed model:

[0107] v r(l，l+1) =v m cos ξ θ l，l+1 cos ξ φ l，l+1 (13)

[0108] The formula for calculating travel time cost is as follows:

[0109]

[0110] The fourth step uses deep reinforcement learning as a planner for path planning at arbitrary angles. In this example, the Dueling Double Deep Q Network (D3QN) is used as the planner.

[0111] In deep reinforcement learning, both the state space S and the action space A are designed as the coordinates of all grid nodes, as follows:

[0112]

[0113]

[0114] Where u and v represent the horizontal and vertical dimensions of the raster, and in this example, u and v are both 100.

[0115] The reward function in this example is designed as follows:

[0116]

[0117] Where cost is the total travel time cost of the path segment from the previous grid to this grid, and is the sum of the travel time costs of the path segments.

[0118] Specifically, the D3QN method used in this example consists of two networks: an estimation network and a target network. The input to both networks is the current state (the coordinates of the current grid node), and the output is the action to be performed (the coordinates of the next grid node). The output of the estimation network is updated using gradient descent with the output of the target network as the target value. After a certain number of updates, the parameters of the estimation network are fed into the target network for temporal difference learning. Simultaneously, the D3QN method employs empirical replay to ensure that the samples are independent and identically distributed.

[0119] Both networks in this example are Duel Depth Q-networks. Each Duel Depth Q-network consists of two parts: one responsible for calculating the state value function and the other responsible for calculating the action advantage function. Both consist of four fully connected layers and one linear transformation layer. The network outputs the sum of the state value function and the weighted advantage function as the network estimate.

[0120] The D3QN method performs action selection, observes the next state and reward and adds them to the experience replay library, performs state transitions, samples observations, and updates the neural network. This process is repeated until the agent reaches the goal, marking the end of one round. Multiple rounds are executed. As the estimation network updates and the loss decreases, the cumulative reward for each round gradually diminishes, causing the agent's actions to converge to the optimal policy, thus obtaining the optimal path.

Claims

1. A vehicle three-dimensional path planning method considering transverse and longitudinal slopes, characterized in that: The method includes the following steps: The method includes the following steps: The first step is to collect real 3D environmental information to build a 3D grid map model, including the starting point location information, ending point location information, obstacle location information and terrain elevation information of the autonomous vehicle in the current task, and to build a 3D grid map model using the grid method. The second step is to establish a vehicle lateral and longitudinal slope passability model, and to establish a reference speed model for the vehicle under different pitch and roll angles based on the vehicle parameters. The vehicle lateral and longitudinal slope passability model specifically refers to a model that determines whether a vehicle can pass stably based on vehicle parameters and the slope conditions it is on. Specifically, for a vehicle of known dimensions, it determines whether it will roll over based on its current pitch and roll angles. The specific judgment process uses a supporting polygon method, with the contact points of the vehicle's four wheels... , , , The vehicle is supported by a quadrilateral with vertices. The vehicle's center of gravity is recorded as Assuming the vehicle's speed and acceleration change steadily, then if If the projection point falls on the plane containing the supporting quadrilateral along the direction of gravity, then... The interior assumes the vehicle will not overturn if the projection point falls on... The outsiders believed the vehicle would overturn; In actual working conditions, the vehicle's center of gravity is not located at the vehicle's geometric center, and therefore the projection point of the center of gravity will not fall on the geometric center of the vehicle's supporting quadrilateral. Therefore, a vehicle coordinate system is established with the vehicle's geometric center as the reference point, and the vehicle's forward direction is... Axle, lateral direction of the vehicle Axle, the direction of the vehicle vertically upward Axle, vehicle support quadrilateral exist , The projected lengths of the directions are respectively and The vehicle's center of gravity coordinates are The distance from the vehicle's center of gravity to each side of the vehicle's supporting quadrilateral: (1) (2) in, , The distance from the center of gravity to the front and back edges of the supporting quadrilateral Distance in direction , The distance from the center of gravity to the left and right edges of the supporting quadrilateral The directional distance can be determined by the geometric relationship between the vehicle's center of gravity and the supporting quadrilateral, which establishes the condition that the vehicle falls within the supporting quadrilateral. This means the range of vehicle pitch angle changes required to prevent the vehicle from rolling over is: (3) wherein, , are respectively the upper and lower limits of the vehicle pitch angle, and the range of the vehicle roll angle when the vehicle is at a certain pitch angle is guaranteed not to roll, and the range of the vehicle roll angle is (4) wherein , are respectively the upper and lower limits of the vehicle roll angle, are defined as follows: (5) In summary, when the pitch angle of an autonomous vehicle... and roll angle satisfy: , If the projection of the vehicle's center of gravity along the direction of gravity falls within the vehicle's supporting quadrilateral, the vehicle can pass stably under this condition; otherwise, the vehicle cannot pass. When a vehicle is on a slope that allows for stable passage, a reference speed model is established for the vehicle under different pitch and roll angles. When a vehicle travels on a slope, it requires traction force to provide a component that balances the gravitational component causing the vehicle to slide down the slope. This limits the vehicle's speed to the slope itself. The standard speed that prevents the vehicle from becoming unstable is defined as the reference speed. Furthermore, the reference speed of a vehicle also differs under different pitch and roll angles, as defined below: (6) wherein, is the vehicle reference speed for a no-gradient scenario, is the influence coefficient of the vehicle pitch angle, is the influence coefficient of the vehicle roll angle; The third step is to construct a diagonally segmented raster map based on the 3D raster map model and calculate the elevation value of the raster center point; and to establish a travel time cost calculator to calculate the travel time cost from one raster node to another. Diagonally segmented raster maps refer to further diagonally segmenting a raster map built based on DEM data. Specifically, this involves connecting the diagonals of each raster to divide each raster into four smaller triangular units, and then calculating the elevation value of the raster's center point. The formula is as follows: (7) in, This represents the elevation value of the center point of the grid. , , , This represents the elevation values ​​of the four vertices of the grid; In the third step, the travel time cost calculator is used to calculate the travel time cost between two grid nodes, which is then used as a heuristic function for subsequent planning. Arbitrary angle planning is used to obtain the optimal solution in the global space. Arbitrary angle planning means that in the grid map, the vehicle's movement route can follow the line connecting one grid node to any other grid node, not just adjacent grid nodes. That is, the movement route can follow the diagonal or side of a rectangle of any size, instead of only following the side of the smallest grid. The line connecting two grid nodes is called a path segment, and the planned path will consist of several path segments. The travel time cost calculator calculates the travel time cost of a path segment connecting two given grid nodes. The path segment traverses multiple grids, and the slope within each grid affects the travel time of vehicles. The impact of terrain undulation cannot be simply reduced to the elevation difference between the starting and ending points. To further improve the model accuracy, the path segment is divided into several smaller segments using a diagonally split grid map. These smaller segments are called path nodes. The travel time cost of a path segment is equal to the sum of the travel time costs of all path nodes that make up the path segment. The process for calculating the travel time cost of each path segment is as follows: Step 1: Calculate the node coordinates of the path segment. Path segments are formed by dividing a map diagonally. First, calculate the x and y coordinates of the path segment nodes. Nodes on the grid edges can be calculated using the x and y coordinates of the start and end points of the path segment through a similarity method. Nodes on the grid diagonals can be calculated using the x and y coordinates of nodes on adjacent grid edges and a similarity method. Once the x and y coordinates of each node are known, the node's elevation value can be obtained from the elevation values ​​of the two endpoints of the edge or diagonal segment it belongs to. The calculation formula is as follows: (8) wherein, is the node to be sought, and are the two end points of the edge or diagonal segment, respectively, on which the node lies, whereby all node coordinates of the path segment can be calculated; Step 2: Calculate the pitch and roll angles of the vehicle as it travels on each path segment; calculate the pitch and roll angles of the vehicle separately according to the location of each path segment. When a path segment is located inside a triangular cell in a diagonally segmented map, the spatial angle between the triangular cell and the horizontal plane is considered the slope of the terrain in that area. The slope characteristic is represented by the upward normal vector of the triangular cell, and the calculation formula is as follows: (9) (10) in, and These are the two endpoints of a certain path segment. The right-angle vertex of the triangular cell where this path segment is located, which is also the center point of the grid; Pitch angle of a vehicle passing over the path section The calculation formula is as follows: (11) The roll angle of a vehicle passing over the path section The calculation formula is as follows: (12) When the path segment is on the edge or diagonal of the grid, the vehicle pitch angle is calculated in the same way as above, and the vehicle roll angle is simplified to the average roll angle of the vehicle traveling along the current path on the left and right slopes. The roll angle is calculated in the same way as above. Step 3: Calculate the travel time cost of the path segment based on the pitch and roll angles of the vehicle; based on Step 2, which can calculate the pitch and roll angles of the vehicle traveling on any path segment, the reference speed of the path segment can be obtained according to the reference speed model: (13) The formula for calculating travel time cost is as follows: (14)； Step 4: Use deep reinforcement learning as a planner for arbitrary angle path planning. Arbitrary angle planning means that in a grid map, the vehicle can move from one grid node to any other grid node. That is, the movement route can follow the diagonal or edge of a rectangle of any size formed by grids. Each path segment can go from one grid node to another arbitrary grid node. The reward for each path segment is calculated by the aforementioned time cost calculator. Using deep reinforcement learning as a planner for arbitrary angle path planning, the state space of deep reinforcement learning is designed to include all grid nodes, where the starting position, ending position, and obstacle position need to be recorded. The action space is designed so that the next action can be transferred to any grid node. The reward function is designed based on the ending position, obstacle position, and terrain information. The vehicle reaches the ending point and provides a large positive reward, and the vehicle collides with an obstacle and provides a large negative reward. A small negative reward is designed based on the travel time cost. The deep reinforcement learning agent explores in the space to find positive rewards and updates the policy. Through the iteration of the policy, the cumulative reward obtained by the agent decreases, thereby finding the optimal path. Projecting this onto the real environment completes the path planning process.

Images