Unmanned vehicle global path planning method and system based on actual driving state

Abstract

The invention provides an unmanned vehicle global path planning method and system based on an actual driving state. The method comprises the steps of determining a passable grid of an unmanned vehicle and a vehicle angle after the unmanned vehicle reaches the passable grid based on a vehicle dynamics principle and the actual driving state of the unmanned vehicle; constructing a comprehensive passing index of the unmanned vehicle, and improving an A * algorithm by using the comprehensive passing index to obtain an improved A * algorithm; and based on a navigation map, the driverless vehicle passable grid and the vehicle angle after reaching the passable grid, adopting the improved A * algorithm to perform driverless vehicle executable path search to obtain an optimal global path planning trajectory of the driverless vehicle. The method further comprises the following steps: comprehensively evaluating the optimal global path planning trajectory of the unmanned vehicle through a driving path evaluation model; wherein the driving path evaluation model comprises a driving path safety index quantitative analysis model, a stability index quantitative analysis model and a timeliness index quantitative analysis model.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous path planning, and particularly to a global path planning method and system for an unmanned vehicle based on the actual driving state. Background Art

[0002] In recent years, with the continuous improvement of the computing power of hardware devices, industries such as autonomous driving, autonomous transportation, and intelligent logistics are gradually integrating into people's lives. The progress in these fields not only improves work efficiency but also greatly enhances people's quality of life. Mobile unmanned vehicles, robots and other intelligent devices have gradually become important roles in the operation of the entire social system. Mobile robots and unmanned intelligent systems represented by various unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs) are in rapid development and show extremely high application value and broad application prospects in various fields. An unmanned vehicle integrates functions such as positioning, environmental mapping, path planning, and motor control. The core issues in its research are positioning and path planning. In the positioning system, GPS, inertial navigation system, lidar, vision camera, wheel encoder, etc. will be installed. The GPS sensor can directly obtain the position information of the vehicle. Sensors such as radar and camera need to estimate the vehicle position information after obtaining the surrounding environment information of the unmanned vehicle itself. After obtaining the accurate position information of the vehicle, the unmanned vehicle can perform path planning and control strategies to achieve the intelligent operation of the unmanned vehicle.

[0003] Global path planning refers to, on the premise of having obtained partial map data information, combining partial local optimization information and a feedback mechanism to plan the best feasible area or path. This planning method is usually based on existing map information (such as SLAM) and aims to find an optimal path from the starting point to the target point. In the global path planning of an unmanned vehicle, factors to be considered include road structure, traffic rules, obstacle positions, etc. to ensure that the planned path is both safe and efficient. To achieve global path planning, various algorithms and technologies can be used, such as classic algorithms like Dijkstra algorithm, A* algorithm, RRT algorithm (rapidly exploring random tree algorithm), as well as intelligent algorithms like ant colony algorithm and genetic algorithm. These algorithms have their own advantages and disadvantages and are suitable for different scenarios and requirements.

[0004] Although certain achievements have been made in the global path planning method for unmanned vehicles, there are still many challenges, such as how to handle more complex road conditions, how to be closer to the actual driving mode of the unmanned vehicle, how to ensure the safety and reliability of the algorithm, and how to make the algorithm plan the path based on more factors.

[0005] In summary, the technical background of the global path planning method for driverless vehicles covers multiple aspects such as the rapid development of vehicle autonomous driving technology, the importance of path planning technology, the research status of global path planning algorithms, as well as the challenges and future development trends. The research and development in this field are of great significance for promoting the progress of driverless vehicle control technology and the construction of intelligent robot systems. However, the existing global path planning methods for driverless vehicles still have the following drawbacks:

[0006] 1) There are many existing theoretical studies and control methods for structured roads and simple environments. The actual driving path of vehicles is complex and there are numerous influencing factors. Existing research mainly focuses on improving global path planning algorithms applied to simple environments or complex obstacle environments. The improvement directions generally include increasing the operation speed, enhancing the obstacle avoidance performance, and reducing path turning, lacking consideration of the impact of terrain complexity and the vehicle's own passability parameters on the safety and other aspects of global path planning.

[0007] 2) The existing algorithms for the design of the driving path of driverless vehicles are not close enough to the actual situation. The existing algorithms all adopt the eight - direction search method, that is, there are indeed eight theoretical traveling directions for the driverless vehicle, specifically including: the four basic directions of east, south, west, and north, and the four diagonal directions of southeast, northeast, southwest, and northwest. This method simulates the natural tendency of humans when choosing a traveling path, that is, moving towards the direction closest to the destination, rather than searching all directions aimlessly. Although this method can achieve the global path planning of the driverless vehicle, based on the analysis of the actual driving situation of the driverless vehicle, it is very different from this search method. Under the condition of a small - unit grid, the direction selection of the driverless vehicle is restricted, that is, it can only search three - direction grids, and at the same time, there are multiple possibilities for the front - end direction to reach each grid, which is not certain. Therefore, how to achieve a path planning closer to the actual driving situation of the driverless vehicle has become the focus and difficulty of the research. Summary of the Invention

[0008] The purpose of the present invention is to provide a global path planning method and system for driverless vehicles based on the actual driving state, aiming to solve the above - mentioned problems in the prior art.

[0009] The embodiment of the present invention provides a global path planning method for driverless vehicles based on the actual driving state, including:

[0010] Determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle;

[0011] Constructing a comprehensive passing index for the driverless vehicle, and using the comprehensive passing index to improve the A* algorithm to obtain an improved A* algorithm; and

[0012] Based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, the improved A* algorithm is used to search for the executable path of the driverless vehicle, and the optimal global path planning trajectory of the driverless vehicle is obtained.

[0013] An embodiment of the present invention provides a global path planning system for a driverless vehicle based on the actual driving state, including:

[0014] A passage module for determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle;

[0015] An improvement module for constructing a comprehensive passage index of the driverless vehicle, using the comprehensive passage index to improve the A* algorithm, and obtaining an improved A* algorithm; and

[0016] A planning module for searching for the executable path of the driverless vehicle using the improved A* algorithm based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, and obtaining the optimal global path planning trajectory of the driverless vehicle.

[0017] An embodiment of the present invention further provides an electronic device, including: a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps of the above-mentioned global path planning method for a driverless vehicle based on the actual driving state are implemented.

[0018] An embodiment of the present invention further provides a computer-readable storage medium, on which an implementation program for information transmission is stored. When the program is executed by a processor, the steps of the above-mentioned global path planning method for a driverless vehicle based on the actual driving state are implemented.

[0019] The adoption of the embodiment of the present invention may include the following beneficial effects: The path planning method proposed in the embodiment of the present invention performs static global path planning according to the high-precision map, starting point, and destination information, and obtains a solution with the optimal combination of three indicators: safety, smoothness, and timeliness. In path planning, in order to be closer to the actual driving situation of the driverless vehicle, the A* algorithm is improved, the quantization values of the three indicators are comprehensively considered, the path selection is optimized, and the driving direction selection function is added to achieve efficient, fast, and safe path planning for the driverless vehicle. Description of the Drawings

[0020] In order to more clearly illustrate the technical solutions in one or more embodiments of this specification or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments recorded in this specification. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0021] Figure 1 is a flowchart of the global path planning method for a driverless vehicle based on the actual driving state according to an embodiment of the present invention;

[0022] Figure 2 is a schematic diagram of subsequent state change constraints when the vehicle head orientation is 0 according to an embodiment of the present invention;

[0023] Figure 3 is a schematic diagram of subsequent state change constraints when the vehicle head orientation is 315 according to an embodiment of the present invention;

[0024] Figure 4 is a schematic diagram of the driving direction of a driverless vehicle according to an embodiment of the present invention;

[0025] Figure 5 is a flowchart of the implementation of the improved A* algorithm according to an embodiment of the present invention;

[0026] Figure 6 is a schematic diagram of a grid and adjacent grids within its 3×3 neighborhood according to an embodiment of the present invention;

[0027] Figure 7 is a schematic diagram of the relationship between the slope direction and the grid slope normal vector according to an embodiment of the present invention;

[0028] Figure 8 is a schematic diagram of path planning of the traditional A* algorithm according to an embodiment of the present invention;

[0029] Figure 9 is a schematic diagram of path planning of the improved A* algorithm according to an embodiment of the present invention;

[0030] Figure 10 is a schematic diagram of the global path planning system for a driverless vehicle based on the actual driving state according to an embodiment of the present invention. Detailed implementation manners

[0031] In order to enable those skilled in the art to better understand the technical solutions in one or more embodiments of this specification, the following will clearly and completely describe the technical solutions in one or more embodiments of this specification with reference to the accompanying drawings in one or more embodiments of this specification. Obviously, the described embodiments are only a part of the embodiments of this specification, rather than all the embodiments. Based on one or more embodiments of this specification, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of this document.

[0032] Method embodiments

[0033] According to an embodiment of the present invention, a global path planning method for a driverless vehicle based on the actual driving state is provided. Figure 1This is a flow chart of a global path planning method for an unmanned vehicle based on actual driving conditions according to an embodiment of the present invention. Figure 1 As shown, the global path planning method for an unmanned vehicle based on actual driving status according to an embodiment of the present invention specifically includes:

[0034] Step S101, based on the principles of vehicle dynamics and the actual driving state of the unmanned vehicle, determines the passable grid of the unmanned vehicle and the vehicle angle after reaching the passable grid, specifically including:

[0035] Formulate a driving direction rule for the unmanned vehicle based on the principles of vehicle dynamics and the actual driving state of the unmanned vehicle, and determine the passable grid for the unmanned vehicle and the vehicle angle after reaching the passable grid according to the driving direction rule, the starting point of the unmanned vehicle and the current state of the unmanned vehicle;

[0036] The actual driving state of the unmanned vehicle includes the grid in which the unmanned vehicle is currently located and the direction of the vehicle head;

[0037] And use formula 1 to calculate the vehicle angle after the unmanned vehicle reaches the passable grid;

[0038] Δθ j =α1×(|θ goal -θ j |)+α2×(|θ i -θ j |) Formula 1;

[0039] Where θ goal is the angle between the grid and the end point, θ i Indicates the driving direction of the unmanned vehicle at a certain moment, θ j Indicates the next moment driving direction of the optional grid, α1 and α2 are weight coefficients, and minΔθ is taken j The corresponding θ j Direction for traveling to the optional grid;

[0040] Step S102: constructing a comprehensive traffic index for unmanned vehicles, and using the comprehensive traffic index to improve the A* algorithm to obtain an improved A* algorithm, specifically including:

[0041] Constructing evaluation indicators for the safety, stability, and timeliness of the unmanned vehicle, and obtaining a comprehensive unmanned vehicle traffic index as shown in Formula 2 based on the evaluation indicators for the safety, stability, and timeliness of the unmanned vehicle;

[0042]

[0043] Where, f g (n,n′) represents the comprehensive traffic index of the unmanned vehicle from the current grid to the sub-grid, f env(n, n′) represents the overall difficulty of the road environment from the current grid n to the sub-grid n′, f time (n, n′) represents the safety and timeliness of the theoretical traveling direction of the current driverless vehicle, C env represents f env (n, n′) the weight coefficient of, C time represents f time (n, n′) the weight coefficient of, and C env +C time = 1;

[0044] Improve the A* algorithm by using the comprehensive passing index and introducing the difficulty prediction factor shown in Formula 3 to obtain the improved A* algorithm shown in Formula 4;

[0045]

[0046] f(n′) = g(n′) + C h h(n′) Formula 4;

[0047] In the formula, f diff represents the difficulty prediction factor, n diff represents the number of f env that is 1 in each grid, f env represents the terrain factor, f(n′) represents the expression of the improved A* algorithm, g(n′) represents the actual cost value consumed by the driverless vehicle moving from the initial starting point to the current node grid, that is, the improved cost function; h(n′) is the heuristic function from the current node to the end point, that is, the improved heuristic function; C h is the heuristic coefficient;

[0048] Step S103, based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, use the improved A* algorithm to search for the executable path of the driverless vehicle to obtain the optimal global path planning trajectory of the driverless vehicle, specifically including:

[0049] Based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, use the improved A* algorithm to search for the executable path of the driverless vehicle, generate the initial driving path of the driverless vehicle, fit the initial driving path of the driverless vehicle into a smooth trajectory through a B-spline curve, and optimize the smooth trajectory through three penalties of dynamic feasibility, trajectory smoothness, and collision safety to obtain the optimal global path planning trajectory of the driverless vehicle;

[0050] The method further includes:

[0051] Comprehensively evaluate the optimal global path planning trajectory of the driverless vehicle through a driving path evaluation model;

[0052] Among them, the driving path evaluation model includes a driving path safety index quantitative analysis model, a smoothness index quantitative analysis model, and a timeliness index quantitative analysis model.

[0053] The following specifically describes the above technical solutions of the embodiments of the present invention in combination with the specific situation of the global path planning method for driverless vehicles based on the actual driving state.

[0054] In view of the deficiencies in the prior art, the embodiments of the present invention propose a global path planning method for driverless vehicles, aiming to analyze and summarize the conditional constraints of the actual driving situation of driverless vehicles, optimize the control algorithm, comprehensively consider the grid selection and driving direction selection, and at the same time, the driverless vehicle considers indicators such as the safety, smoothness, and timeliness of the path between nodes, assigns corresponding coefficients, and improves the cost function, so as to solve the problems of over-idealization of driverless vehicle driving caused by non-conformity with the actual situation and the tendency to fall into danger in complex terrain environments due to weak terrain perception ability.

[0055] A method for collaborative aerial and ground path planning of driverless vehicles proposed by the embodiments of the present invention includes the following steps:

[0056] Step 1: Based on the vehicle dynamics principle and the actual driving situation of the driverless vehicle, stipulate the driving direction rules during the driving process of the driverless vehicle, and select the passable grids and the angles after reaching the grids according to the rules, the starting point, and the state of the driverless vehicle.

[0057] Among them, the constraints on the driving direction of the driverless vehicle, that is, the grid where the driverless vehicle is currently located and the vehicle head orientation, jointly determine the position and vehicle head orientation of the next grid.

[0058] For example, when the vehicle head orientation of the driverless vehicle at a certain grid is 0, the various possible situations of the next feasible grid and its corresponding vehicle head orientation are as Figure 2 shown. For the current vehicle state, Figure 2 the shaded grids in are infeasible grids, ①, ②, and ③ are 3 feasible grids (which need to meet the passable grid conditions of the vehicle), the solid arrow represents the current vehicle head orientation, and the dashed arrow represents the corresponding subsequent vehicle head orientation that conforms to the vehicle kinematic constraints. Figure 2 There are 7 possible states for the vehicle at the next moment of the vehicle, and the situations when the vehicle head orientation is 90, 180, and 270 are similar.

[0059] When the vehicle head orientation of the driverless vehicle at a certain grid is 315, the various possible situations of the next feasible grid and its corresponding vehicle head orientation are as Figure 3 shown, a total of 7. The situations when the vehicle head orientation is 45, 135, and 225 are similar.

[0060] According to the above rules, the driving direction of the driverless vehicle at a certain moment is θ i, then the grids that can be passed at the next moment are the grid in that direction and the two grids with an angle of ±45° with it, as shown in the following example: Figure 4 shown.

[0061] Set the next moment driving direction of the optional grid to θ j , j = 1, 2 or 3, the angle selection index for the next moment under comprehensive consideration is:

[0062] Δθ j =α1×(|θ goal -θ j |)+α2×(|θ i -θ j |) (1);

[0063] Where θ goal is the angle between the grid and the end point, α1 and α2 are weight coefficients, and minΔθ is taken j The corresponding θ j The direction to travel to the grid.

[0064] Step 2: Based on the safety, stability, and timeliness evaluation indicators of the unmanned vehicle, a comprehensive unmanned vehicle passability index is constructed for algorithm improvement;

[0065] 1. Terrain factor (f env )calculate

[0066] The terrain factor is affected by the ground type and the parameters of the unmanned vehicle's passability, and has nothing to do with the search direction. It mainly reflects the danger caused by the grid terrain. The terrain factors proposed in the embodiment of the present invention mainly include: slope factor (f slope ), stability factor (f stab ).

[0067] 1) Slope factor (f slope )

[0068] According to the slope of the terrain information grid map (T slope ) information and the maximum climbing grade (M slope ), introduce the slope factor (f slope ), used to measure the difficulty of the unmanned vehicle climbing the vertical slope, f slope The calculation formula is as follows:

[0069]

[0070] When the maximum climbing gradient of the unmanned vehicle is greater than the grid slope, the unmanned vehicle meets the longitudinal climbing requirements. At this time, the greater the slope of the grid, the greater the slope factor, and the greater the longitudinal climbing difficulty of the unmanned vehicle; when the maximum climbing gradient of the unmanned vehicle is less than or equal to the grid slope, the unmanned vehicle does not meet the longitudinal climbing requirements, and the slope factor is 1.

[0071] 2) Stability factor (f stab )

[0072] The stability factor measures the degree of bumpiness of the driverless vehicle during driving on this path. According to the calculation formula of the smoothness index, the calculation formula of the stability factor is obtained as follows:

[0073]

[0074] In the formula, is the average slope of two adjacent grids in the driving path; is the included angle between the normal vectors of the slopes of two adjacent grids.

[0075] Terrain factor (f env ) is composed of the slope factor and the stability factor, comprehensively reflecting the overall difficulty of the ground environment from the current grid to the grids in different adjacent directions. The larger f env , the higher the difficulty and the less passable it is.

[0076]

[0077] In the formula, n represents the current grid, n' represents the unit vector (sub-grid) of the grid that may form at the next moment, f slope (n′) and f stab (n,n′) respectively represent the slope factor and the stability factor of the grid that may be traveled at the next moment, C slope represents the weight coefficient of the slope factor, C stab represents the weight coefficient of the stability factor, and C slope +C stab =1, f env (n,n′) represents the overall difficulty of the road surface environment from the current grid n to the sub-grid n′.

[0078] 2. Calculation of safety-time factor

[0079] The safety-time factor is affected by the travel time and the time passing through the unsafe area, so as to achieve a path with both excellent safety and timeliness. The safety-time factor proposed in the embodiment of the present invention mainly includes: time factor (T), safety factor (T base ).

[0080] 1) Safety factor (T base )

[0081] The safety factor measures the driving duration of the driverless vehicle in the bad area. According to the calculation formula of the safety index, the calculation formula of the safety factor is obtained as follows:

[0082]

[0083] Where D base is the mileage within the bad area grid, v base is the driving speed within the bad area grid.

[0084] 2) Time factor (T)

[0085] The timeliness factor measures the length of time it takes for the autonomous vehicle to travel along the path. Based on the timeliness index calculation formula, the timeliness factor calculation formula is:

[0086]

[0087] Where D i is the mileage in the i-th grid, v i is the driving speed in the i-th grid.

[0088] The calculation formula of safety time factor is as follows:

[0089]

[0090] Where C T Represents the weight coefficient of the aging factor, C tbase represents the weight coefficient of the safety factor, and C T +C tbase =1, T max is the maximum time of a grid peer.

[0091] 3. Difficulty Prediction Factor Calculation

[0092] Calculate f for each grid env Then, the embodiment of the present invention introduces the difficulty prediction factor f diff , used to influence the heuristic function of the algorithm, is conducive to avoiding the grid surrounded by dangerous areas in advance during the search process, and preventing the occurrence of continuous corners and large corners. diff Let f in each grid env The number of 1s n diff The ratio to 3 is calculated as follows:

[0093]

[0094] 4. Calculation of comprehensive traffic indicators

[0095] The parameters that affect the cost function are introduced: terrain factor weight coefficient (C env ), safety-time factor weight coefficient (C time ), difficulty factor (f g ), difficulty conversion coefficient (C g ).

[0096]

[0097] In the formula, f env (n, n′) represents the overall difficulty of the road surface environment from the current grid n to the sub-grid n′, and f time (n, n′) represents the safety and timeliness of the theoretical traveling direction of the current driverless vehicle. C env represents the weight coefficient of f env (n, n′), and C time represents the weight coefficient of f time (n, n′), and C env + C time = 1; f g (n, n′) represents the comprehensive passing index of the driverless vehicle from the current grid to the sub-grid.

[0098] Step 3: According to the improved A* algorithm, perform an executable path search and determination based on the given map. Use the three comprehensive passing indexes of driving path stability, timeliness, and safety as the criteria for evaluating the path, and obtain the comprehensive optimal planning trajectory of the driverless vehicle under this map.

[0099] The core expression of the improved A* is:

[0100] f(n′) = g(n′)+ C h h(n′) (10);

[0101] In the formula, g(n′) represents the actual cost value consumed by the driverless vehicle from the initial starting point to the current node grid, that is, the improved cost function; h(n′) is the heuristic function from the current node to the end point, that is, the improved heuristic function; C h is the heuristic coefficient, which is used to affect the convergence speed of the algorithm. The larger C h is, the faster the convergence speed.

[0102] The cost function and heuristic function of the A* algorithm are related to the selection of evaluation indexes, and the main performance evaluation indexes are path timeliness, path safety, and path smoothness.

[0103] The expression of the improved cost function g(n′) is as follows:

[0104] g(n′) = g(n)+ G(n, n′) (11);

[0105] In the formula, G(n, n′) is the cost value from the current grid n to the sub-grid n′, and g(n) represents the original cost function, which is used to evaluate the evaluation index from the grid n to the sub-grid n′. The larger the cost value, the worse the performance.

[0106] In the path planning of off-road environments, not only should the actual length of the path be considered to be short, but also the safety of the path should be considered to be high. In order to comprehensively consider the actual length and safety of the path in the embodiments of the present invention, the equivalent length is introduced as the cost value that comprehensively considers the actual length and the passing difficulty between grids. At this time, the formula of G(n,n′) is as follows:

[0107]

[0108] In the formula, G(n,n′) is the cost value between grids. When f g (n′)≠1, the cost value is the equivalent length from the current grid n to the sub-grid n'. D is the side length of the grid, and f g (n,n′) is the passing difficulty factor, indicating the passing difficulty of the driverless vehicle from the current grid to the sub-grid; C g is the difficulty conversion coefficient, which is used to represent the influence degree of the safety factor on the path planning.

[0109] At this time, the formula of the improved h(n′) is as follows:

[0110]

[0111] In the formula, C d is the difficulty prediction coefficient, which reflects the search ability of the sub-grid and is used to affect the importance degree of the difficulty prediction factor (f diff ) in the heuristic function. The larger C d is, the greater the influence of the difficulty prediction factor on the heuristic function, and the higher the importance degree of the search ability of the sub-grid. n′ x and n′ y respectively represent the horizontal and vertical coordinates of the next moment point, and n goalx and n goaly respectively represent the horizontal and vertical coordinates of the current target point.

[0112] Step 4: Search for an executable path based on the navigation map, generate an initial path by the improved A* algorithm for complex terrains, fit the initial path into a smooth trajectory by a B-spline curve, and then optimize the smooth trajectory based on the optimization method with soft constraints through three penalties of dynamic feasibility, trajectory smoothness, and collision safety to obtain the planned trajectory of the driverless vehicle.

[0113] Use MATLAB to implement the improved algorithm. The overall implementation steps are as follows:

[0114] (1) Obtain map information: a matrix storing grid elevation information.

[0115] (2) Parameter setting: slope factor weight coefficient C slope , stability factor weight coefficient C stab , heuristic coefficient C h, difficulty conversion coefficient C g , difficulty prediction coefficient C d , terrain factor weight coefficient C env , safety-time factor weight coefficient C time , weight coefficient of time factor C T , weight coefficient of safety factor C tbase .

[0116] (3) Initialize the matrix: Create an open matrix, add the starting grid coordinates, angle, and F value; create a closed matrix.

[0117] (4) Take the grid corresponding to the minimum F value in the open matrix as the current grid current, and put this grid and the minimum F value into the closed table.

[0118] (5) Update the open matrix: Select the neighboring grids of the current grid current that are not in the closed matrix. If the corresponding element value of the environmental factor is not equal to 1, first use Equation (8), combined with the above steps, to calculate the traffic difficulty factor f from the current grid to the neighboring grid g , and finally store the neighboring grid in the grid set subs and store the difficulty factor in Fg.

[0119] According to the formula in the above steps, the heuristic coefficient C set in (2) h , difficulty conversion coefficient C g , difficulty prediction coefficient C d , calculate the g(sub), h(sub), and f(sub) values of each passable sub-grid sub respectively.

[0120] Determine whether the grid sub in the grid set subs exists in the open matrix: If it exists, it is necessary to judge the size of the historical F(sub) value of this grid in the open matrix and the f(sub) of the grid sub in the grid set subs. If F(sub)>f(sub), then replace F(sub) with f(sub), change the parent node of sub to current, and add sub to the open matrix; otherwise, do not change and directly add sub and f(sub) to the open matrix.

[0121] (6) Planning end flag: The open matrix is an empty set, the search fails; current is the end point, the search succeeds.

[0122] The implementation process of the improved A* algorithm is as Figure 5 shown.

[0123] Step Five: Based on the driving path stability, timeliness, and safety as the evaluation indicators of the path, evaluate the comprehensive optimal planning trajectory of the unmanned vehicle under this map.

[0124] (1) Safety Index Quantitative Analysis Model

[0125] When the Type A driverless vehicle is driving in a bad area, unexpected situations such as parking may occur due to poor road conditions, and even the mission may fail. Therefore, when evaluating the path safety, the shorter the driving time of the Type A driverless vehicle in the bad area, the better, and the safety index is directly characterized by the driving duration of the Type A driverless vehicle in the bad area.

[0126] The driving duration can be regarded as the sum of the driving durations in each grid passed through during the driving process or on a specific road section except the first grid. That is, from L0 to L n-1 The driving duration T for passing through n grids is:[[]]

[0127]

[0128] In the formula, D i is the mileage in the i-th grid on the path, and v i is the speed in the i-th grid on the path.

[0129] The mileage D in each grid i is jointly determined by the coordinates and the vehicle head orientation of the current grid, as well as the coordinates and the vehicle head orientation of the previous grid.

[0130] Denote the coordinates of the i-th grid L i in the path as (x i , y i ), and the vehicle head orientation as θ i . Correspondingly, the coordinates and the vehicle head orientation of the previous grid L i-1 are (x i-1 , y i-1 ) and θ i-1 . Denote ΔL i as the change in grid coordinates, and Δθ i as the change in vehicle head orientation, which are defined as follows:

[0131] ΔL i = |x i - x i-1 | + |y i - y i-1 | (15);

[0132] Δθ i = |θ i - θ i-1 | (16);

[0133] According to the driving rules, it can be known that ΔL i has only two possible values, 1 or 2; Δθ iThere are only 3 possible values, 0, 45, or 90. The mileage within the i-th grid L i is determined jointly by ΔL i and Δθ i , and there are 6 different cases in total, as shown in Table 1. Each case corresponds to a mileage weighting coefficient ω(ΔL i , Δθ i ). The product of this coefficient and the grid side length (cellsize, 5m in this example) is the mileage D i within this grid:

[0134] D i = ω(ΔL i , Δθ i ) × cellsize (17);

[0135] Table 1 Mileage weighting coefficient ω(ΔL i , Δθ i )

[0136]

[0137] [[ID=3�]]The speed v i refers to the average speed of the unmanned vehicle when it travels within a certain grid and the grid slope is within the specified range. v i is closely related to the grid slope, as follows:

[0138]

[0139] In the formula, s i is the slope within the i-th grid on the path.

[0140] The slope of the grid can be approximately calculated from the elevations of this grid and its adjacent grids. The approximate calculation method is as follows:

[0141] As Figure 6 shown, the approximate calculation formula for the slope of the central grid is as follows:

[0142]

[0143] In the formula, the slope is expressed in degrees, z is the elevation of the grid, is the elevation change rate.

[0144] Specifically:

[0145]

[0146] Where cellsize is the grid side length (5m in this example), and a, b, c, d, f, g, h, i are the elevations of the grid respectively. The elevation data is from the topographic map named map.tif in TIFF format, which records the elevation data of all grid center points in the entire mission area.

[0147] In summary, the L0 to L can be obtained. n-1 The driving duration T passing through n grids is required. Now it is necessary to determine which grid on the path belongs to the bad area. The coordinate information of the grids included in the bad area has been given. Establish a set B of bad area grids. Taking the coordinates as the judgment condition, judge whether (x i , y i ) belongs to B, and sort out the grids L base0 to L base(n-1) . According to the above calculation method, calculate the driving duration T base0 to L base(n-1) passing through n grids, which is the safety index of this path. base .

[0148] (2) Quantitative analysis model of ride comfort index

[0149] Ride comfort measures the degree of bumpiness of the unmanned vehicle during driving on this path. The calculation formula of the ride comfort index ε is as follows (the smaller the value, the better the ride comfort):

[0150]

[0151] Where is the average slope between two adjacent grids on the driving path; is the angle between the normal vectors of the slopes of two adjacent grids.

[0152] The relationship between the normal vector (n x , n y , n z ) of the grid slope and the slope and aspect is as Figure 7 shown. The approximate calculation formula of the aspect of the dark grid is as follows:

[0153]

[0154] Where the aspect is expressed in degrees.

[0155] According to the slope S and aspect A, the normal vector (n x , n y , n z ) of the slope of the dark grid can be obtained:

[0156] (n x , n y , n z)=(sin A·sins,cosA·sins,coss) (23);

[0157] In summary, the stability index ε can be calculated.

[0158] (3) Quantitative analysis model of timeliness indicators

[0159] Timeliness measures the length of time the autonomous vehicle takes to travel along the route, and is directly characterized by the autonomous vehicle's travel time. The shorter the travel time, the higher the timeliness of the route.

[0160] The safety analysis model has given the path L0 to L n-1 The formula and method for calculating the driving time T passing through n grids. The calculated T at this time is the timeliness index of the path.

[0161] By comparing with the path planning method based on the traditional A* algorithm, such as Figure 8 、 Figure 9 As shown, it can be found that the global path planning method based on the improved algorithm proposed in the embodiment of the present invention can comprehensively consider terrain information and plan a path with the optimal comprehensive passability index. Although the path timeliness is greater than that of the path planning method based on the traditional A* algorithm, the overall path security is optimal, there is no safety risk, and the stability is also very good. Taking all factors into consideration, the embodiment of the present invention is superior to the path planning method based on the traditional A* algorithm. The comparison results are shown in Table 2.

[0162] Table 2 Comparison of two planning methods

[0163]

[0164] In summary, the embodiment of the present invention comprehensively considers grid selection and driving direction selection by optimizing the control algorithm. Compared with the existing algorithm, the embodiment of the present invention makes the path of the unmanned vehicle during driving closer to reality in the optimal path planning of the unmanned vehicle. At the same time, through improvements, the unmanned vehicle can perform intelligent analysis and decision-making on complex terrain and plan the optimal driving path for the vehicle. This technology can fully consider factors such as the undulations, slopes, and unfavorable areas of the terrain to ensure that the vehicle can travel stably and safely in an off-road environment. Through precise path planning, the off-road unmanned vehicle can overcome various terrain obstacles and improve its off-road capabilities in complex environments. At the same time, considering factors related to safety, the safety of the off-road unmanned vehicle is enhanced and the probability of accidents is reduced.

[0165] System Example

[0166] According to an embodiment of the present invention, a global path planning system for an unmanned vehicle based on actual driving status is provided. Figure 10It is a schematic diagram of the global path planning system for a driverless vehicle based on the actual driving state according to an embodiment of the present invention. As Figure 10 shown, the global path planning system for a driverless vehicle based on the actual driving state according to an embodiment of the present invention specifically includes:

[0167] A passing module 1000, configured to determine the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle;

[0168] An improvement module 1002, configured to construct a comprehensive passing index for the driverless vehicle, and use the comprehensive passing index to improve the A* algorithm to obtain an improved A* algorithm; and

[0169] A planning module 1004, configured to search for an executable path of the driverless vehicle by using the improved A* algorithm based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, so as to obtain an optimal global path planning trajectory of the driverless vehicle;

[0170] The system further includes:

[0171] An evaluation module, configured to comprehensively evaluate the optimal global path planning trajectory of the driverless vehicle through a driving path evaluation model;

[0172] Wherein, the driving path evaluation model includes a quantitative analysis model for the driving path safety index, a quantitative analysis model for the smoothness index, and a quantitative analysis model for the timeliness index.

[0173] The embodiment of the present invention is a system embodiment corresponding to the above method embodiment. The specific operations of each module can be understood with reference to the description of the method embodiment, and will not be elaborated here.

[0174] In summary, the embodiment of the present invention has the following beneficial effects:

[0175] 1. Based on the vehicle dynamics principle and the actual driving situation of the driverless vehicle, the embodiment of the present invention optimizes the control algorithm, comprehensively considers the grid selection and the driving direction selection. Compared with the existing algorithms, in the optimal path planning of the driverless vehicle, the path during the driving of the driverless vehicle in the embodiment of the present invention is closer to the actual situation.

[0176] 2. The existing path planning methods lack consideration of the influence of the terrain complexity and the vehicle's own passability parameters on the global path planning in terms of safety and other aspects. The embodiment of the present invention considers the safety, smoothness, timeliness and other indexes of the path between nodes of the driverless vehicle, assigns corresponding coefficients, and improves the cost function, thereby effectively solving the problems of over-idealization of the driving of the driverless vehicle caused by non-conformity with the actual situation and the problem of being prone to danger in a complex terrain environment due to weak terrain perception ability.

[0177] 3. The embodiments of the present invention can be applied to scenarios with complex terrains. Through algorithm optimization, the application range of the driverless vehicle is wider, suitable for terrain conditions such as flat ground and off-road, demonstrating extensive applicability.

[0178] Embodiment 1 of the device

[0179] The embodiments of the present invention provide an electronic device, including: a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps described in the method embodiments are implemented.

[0180] Embodiment 2 of the device

[0181] The embodiments of the present invention provide a computer-readable storage medium, on which an implementation program for information transmission is stored. When the program is executed by a processor, the steps described in the method embodiments are implemented.

[0182] The computer-readable storage medium described in this embodiment includes, but is not limited to: ROM, RAM, magnetic disk, optical disc, etc.

[0183] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A global path planning method for an autonomous vehicle based on the actual driving state, characterized in that Including: Determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle; Constructing a comprehensive passing index for the driverless vehicle, and using the comprehensive passing index to improve the A* algorithm to obtain an improved A* algorithm; And Based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, using the improved A* algorithm to search for the executable path of the driverless vehicle, and obtaining the optimal global path planning trajectory of the driverless vehicle.

2. The method according to claim 1, characterized in that, The method further includes: Comprehensively evaluating the optimal global path planning trajectory of the driverless vehicle through a driving path evaluation model; Among them, the driving path evaluation model includes a quantitative analysis model for driving path safety indicators, a quantitative analysis model for smoothness indicators, and a quantitative analysis model for timeliness indicators.

3. The method according to claim 1, wherein Determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle specifically includes: Formulating a driving direction rule for the driverless vehicle based on the vehicle dynamics principle and the actual driving state of the driverless vehicle, and determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids according to the driving direction rule of the driverless vehicle, the starting point of the driverless vehicle, and the current state of the driverless vehicle; Among them, the actual driving state of the driverless vehicle includes the grid where the driverless vehicle is currently located and the heading of the vehicle; And calculating the vehicle angle after the driverless vehicle reaches the passable grid using Formula 1; Δθ j = α1×(|θ goal - θ j |)+ α2×(|θ i - θ j |) Formula 1; where θ goal is the angle between the grid and the end point, θ i represents the vehicle driving direction at a certain moment of the driverless vehicle, θ j represents the driving direction of the next moment of the optional grid, and α1 and α2 are weight coefficients, taking minΔθ j corresponding θ j is the direction to drive to the optional grid.

4. The method according to claim 1, wherein Constructing a comprehensive passing index for the driverless vehicle specifically includes: Constructing safety, smoothness, and timeliness evaluation indicators for the driverless vehicle, and obtaining the comprehensive passing index of the driverless vehicle as shown in Formula 2 according to the safety, smoothness, and timeliness evaluation indicators of the driverless vehicle; where f g (n, n′) represents the comprehensive passing index of the driverless vehicle from the current grid to the sub-grid, f env (n, n′) represents the overall difficulty of the road surface environment from the current grid n to the sub-grid n′, f time (n, n′) represents the safety and timeliness of the theoretical traveling direction of the current driverless vehicle, C env represents the weight coefficient of f env (n, n′), C time represents the weight coefficient of f time (n, n′), and C env + C time = 1.

5. The method according to claim 4, characterized in that, Using the comprehensive passing index to improve the A* algorithm to obtain an improved A* algorithm specifically includes: Using the comprehensive passing index and introducing a difficulty prediction factor as shown in Formula 3 to improve the A* algorithm to obtain an improved A* algorithm as shown in Formula 4; f(n′) = g(n′) + C h h(n′) formula 4; where f diff represents the difficulty prediction factor, n diff represents the number of 1s in each grid for f env , f env represents the terrain factor, f(n′) represents the expression of the improved A* algorithm, g(n′) represents the actual cost consumed by the unmanned vehicle moving from the initial starting point to the current node grid, that is, the improved cost function; h(n′) is the heuristic function from the current node to the end point, that is, the improved heuristic function; C h is the heuristic coefficient.

6. The method according to claim 1, characterized in that Based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, using the improved A* algorithm to search for the executable path of the driverless vehicle to obtain the optimal global path planning trajectory of the driverless vehicle specifically includes: Based on the navigation map, the passable grids of the driverless vehicle, and the vehicle angle after reaching the passable grids, using the improved A* algorithm to search for the executable path of the driverless vehicle, generating an initial driving path of the driverless vehicle, fitting the initial driving path of the driverless vehicle into a smooth trajectory through a B-spline curve, and optimizing the smooth trajectory through three penalties of dynamic feasibility, trajectory smoothness, and collision safety to obtain the optimal global path planning trajectory of the driverless vehicle.

7. A global path planning system for driverless vehicles based on the actual driving state, characterized in that Including: A passing module for determining the passable grids of the driverless vehicle and the vehicle angle after reaching the passable grids based on the vehicle dynamics principle and the actual driving state of the driverless vehicle; An improvement module for constructing a comprehensive passing index for the driverless vehicle and using the comprehensive passing index to improve the A* algorithm to obtain an improved A* algorithm; And A planning module, configured to use the improved A* algorithm to search for an executable path for the driverless vehicle based on a navigation map, passable grids for the driverless vehicle, and the vehicle angle after reaching the passable grids, so as to obtain an optimal global path planning trajectory for the driverless vehicle.

8. The system according to claim 7, wherein The system further includes: An evaluation module, configured to comprehensively evaluate the optimal global path planning trajectory of the driverless vehicle through a driving path evaluation model; wherein, the driving path evaluation model includes a quantitative analysis model for driving path safety indicators, a quantitative analysis model for stability indicators, and a quantitative analysis model for timeliness indicators.

9. An electronic device, characterized in that, It includes: A memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps of the method for global path planning of a driverless vehicle based on the actual driving state as described in any one of claims 1-6 are implemented.

10. A computer-readable storage medium, characterized in that, An implementation program for information transmission is stored on the computer-readable storage medium. When the program is executed by the processor, the steps of the method for global path planning of a driverless vehicle based on the actual driving state as described in any one of claims 1-6 are implemented.

Images